Simulasi Robot Hexapod Kaki 3DOF Berbasis Kurva Sinusoidal

Project ini dikerjakan dengan software Visual C# Express 2010.

Berlatar belakang pergerakan kaki dari robot hexapod yang bisa dikatakan mempunyai pola perulangan yang mirip dengan kurva sinusoidal maka dibuatlah program simulasi ini. Pergerakan sendi ke-1 (yang dekat dengan badan robot) mempunyai pola sinusoidal dan sendi ke-2 dan ke-3 mempunyai beda hanya aktif saat nilai sinus positif saja atau negatif saja dengan beda fasa 90 derajat.

Algoritma yang digunakan mereduksi variabel-variabel pergerakan robot menjadi dua, yaitu jarak ayunan kaki sebelah kanan dan kiri. Dengan kata lain, jika amplitudo ayunan tersebut dengan kecepatan respon gerak yang sama adalah akan menentukan kecepatan perpindahan posisi robot. Sehingga dapat mempermudah navigasi pergerakan robot seperti mengontrol robot beroda yang berkonstruksi dua roda, kanan dan kiri. Pergerakan seperti maju, belok kanan, belok kiri, putar kanan, putar kiri, mundur akan lebih mudah dikontrol dengan mengisi dua variabel saja, kecepatan kaki sebelah kanan dan sebelah kiri yang akan menentukan arah vektor pergerakan robot.

Berikut kodingan saya.

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

namespace WindowsFormsApplication2
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }

        double amp_ka1
            , amp_ka2
            , amp_ka3
            , amp_ki1
            , amp_ki2
            , amp_ki3
            , amp_paha
            , pjg_kaki = 30
            , pjg_paha = 50
            , res_kanan
            , res_kiri
            , res_paha_kiri = 10;
        int i;
        private void timer1_Tick(object sender, EventArgs e)
        {
            Graphics g = this.pictureBox1.CreateGraphics();
            Pen lantai = new Pen(Color.Brown, 3);
            Pen p = new Pen(Color.Green, 3);
            Pen k1 = new Pen(Color.Red, 3);
            Pen k2 = new Pen(Color.Orange, 3);
            Pen erase = new Pen(Color.White, 3);
            Pen line_kurva = new Pen(Color.Black, 3);
            Pen erase_kurva = new Pen(Color.GreenYellow, 3);
            Pen base_kurva = new Pen(Color.Gray, 1);

            if (i == 0) g.Clear(Color.White);

            g.DrawRectangle(p,200,250,50,100);
            g.DrawRectangle(p, 200, 130, 50, 30);
            g.DrawLine(lantai, new Point(0, 168), new Point(500, 168));

            //kurva
            g.DrawLine(erase_kurva, new Point((int)(i / 5) + 400, (int)(-20 * Math.Sin(i * 0.0174532925)) + 40), new Point((int)(i / 5) + 400, (int)(-20 * Math.Sin(i * 0.0174532925)) + 43));
            g.DrawLine(base_kurva, new Point(400, 40), new Point(760, 40));

            //kiri
            g.DrawLine(erase, new Point(200, 250), new Point((200 - (int)(pjg_kaki * (Math.Cos((45 - amp_ki1) * 0.0174532925)))), (250 - (int)(pjg_kaki * (Math.Sin((45 - amp_ki1) * 0.0174532925))))));
            g.DrawLine(erase, new Point(200, 300), new Point((200 - (int)(pjg_kaki * (Math.Cos(amp_ki2 * 0.0174532925))))       , (300 - (int)(pjg_kaki * (Math.Sin(amp_ki2 * 0.0174532925))))));
            g.DrawLine(erase, new Point(200, 350), new Point((200 - (int)(pjg_kaki * (Math.Cos((45 - amp_ki3) * 0.0174532925)))), (350 + (int)(pjg_kaki * (Math.Sin((45 - amp_ki3) * 0.0174532925))))));
            //kanan
            g.DrawLine(erase, new Point(250, 250), new Point((250 + (int)(pjg_kaki * (Math.Cos((45 - amp_ka1) * 0.0174532925)))), (250 - (int)(pjg_kaki * (Math.Sin((45 - amp_ka1) * 0.0174532925))))));
            g.DrawLine(erase, new Point(250, 300), new Point((250 + (int)(pjg_kaki * (Math.Cos(amp_ka2 * 0.0174532925))))       , (300 - (int)(pjg_kaki * (Math.Sin(amp_ka2 * 0.0174532925))))));
            g.DrawLine(erase, new Point(250, 350), new Point((250 + (int)(pjg_kaki * (Math.Cos((45 - amp_ka3) * 0.0174532925)))), (350 + (int)(pjg_kaki * (Math.Sin((45 - amp_ka3) * 0.0174532925))))));
            //_________________________________________________________________
            if (Math.Sin((double)(i + 90) * 0.0174532925) >= 0)
            {
                //kiri
                g.DrawLine(erase, new Point(200, 150), new Point((200 - (int)(pjg_paha * (Math.Cos((45 - amp_paha) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((45 - amp_paha) * 0.0174532925))))));
                g.DrawLine(erase, new Point((200 - (int)(pjg_paha * (Math.Cos((45 - amp_paha) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((45 - amp_paha) * 0.0174532925))))), new Point((200 - (int)(pjg_paha * (Math.Cos((45 - amp_paha) * 0.0174532925)))), (200 - (int)(pjg_paha * (Math.Sin((45 - amp_paha) * 0.0174532925))))));
            }
            else
            {
                //kanan
                g.DrawLine(erase, new Point(250, 150), new Point((250 - (int)(pjg_paha * (Math.Cos((135 - amp_paha) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((135 - amp_paha) * 0.0174532925))))));
                g.DrawLine(erase, new Point((250 - (int)(pjg_paha * (Math.Cos((135 - amp_paha) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((135 - amp_paha) * 0.0174532925))))), new Point((250 - (int)(pjg_paha * (Math.Cos((135 - amp_paha) * 0.0174532925)))), (200 - (int)(pjg_paha * (Math.Sin((135 - amp_paha) * 0.0174532925)))))); 

            }

            res_kiri = ((double)trackBar2.Value - 5) * 4;
            res_kanan = ((double)trackBar3.Value - 5)* 4;
            label2.Text = string.Format("Kecepatan (Amplitudo) kiri={0}", res_kiri);
            label3.Text = string.Format("Kecepatan (Amplitudo) kanan={0}", res_kanan);

            amp_ka1 = res_kanan * Math.Sin(i * 0.0174532925);
            amp_ka2 = res_kanan * Math.Sin(i * 0.0174532925);
            amp_ka3 = -res_kanan * Math.Sin(i * 0.0174532925);
            amp_ki1 = -res_kiri * Math.Sin(i * 0.0174532925);
            amp_ki2 = -res_kiri * Math.Sin(i * 0.0174532925);
            amp_ki3 = res_kiri * Math.Sin(i * 0.0174532925);

            if (res_kiri >= 0)
            {
                amp_paha = -res_paha_kiri * Math.Sin((i + 90) * 0.0174532925);
            }
            else
            {
                amp_paha = res_paha_kiri * Math.Sin((i - 90) * 0.0174532925);
            }

            label1.Text = string.Format("Resolusi kecepatan={0}",trackBar1.Value*2);
            i += (trackBar1.Value * 2);
            if (i >= 360) i = 0;

            //kurva
            g.DrawLine(line_kurva, new Point((int)(i / 5) + 400, (int)(-20 * Math.Sin(i * 0.0174532925)) + 40), new Point((int)(i / 5) + 400, (int)(-20 * Math.Sin(i * 0.0174532925)) + 43));

            //kiri
            g.DrawLine(k2, new Point(200, 250), new Point((200 - (int)(pjg_kaki * (Math.Cos((45 - amp_ki1) * 0.0174532925)))), (250 - (int)(pjg_kaki * (Math.Sin((45 - amp_ki1) * 0.0174532925))))));
            g.DrawLine(k1, new Point(200, 300), new Point((200 - (int)(pjg_kaki * (Math.Cos(amp_ki2 * 0.0174532925))))       , (300 - (int)(pjg_kaki * (Math.Sin(amp_ki2 * 0.0174532925))))));
            g.DrawLine(k2, new Point(200, 350), new Point((200 - (int)(pjg_kaki * (Math.Cos((45 - amp_ki3) * 0.0174532925)))), (350 + (int)(pjg_kaki * (Math.Sin((45 - amp_ki3) * 0.0174532925))))));
            //kanan
            g.DrawLine(k1, new Point(250, 250), new Point((250 + (int)(pjg_kaki * (Math.Cos((45 - amp_ka1) * 0.0174532925)))), (250 - (int)(pjg_kaki * (Math.Sin((45 - amp_ka1) * 0.0174532925))))));
            g.DrawLine(k2, new Point(250, 300), new Point((250 + (int)(pjg_kaki * (Math.Cos(amp_ka2 * 0.0174532925))))       , (300 - (int)(pjg_kaki * (Math.Sin(amp_ka2 * 0.0174532925))))));
            g.DrawLine(k1, new Point(250, 350), new Point((250 + (int)(pjg_kaki * (Math.Cos((45 - amp_ka3) * 0.0174532925)))), (350 + (int)(pjg_kaki * (Math.Sin((45 - amp_ka3) * 0.0174532925))))));
            //_________________________________________________________________
            if (Math.Sin((double)(i + 90) * 0.0174532925) >= 0)
            {
                //kiri
                g.DrawLine(k2, new Point(200, 150), new Point((200 - (int)(pjg_paha * (Math.Cos((45 - amp_paha) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((45 - amp_paha) * 0.0174532925))))));
                g.DrawLine(k2, new Point((200 - (int)(pjg_paha * (Math.Cos((45 - amp_paha) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((45 - amp_paha) * 0.0174532925))))), new Point((200 - (int)(pjg_paha * (Math.Cos((45 - amp_paha) * 0.0174532925)))), (200 - (int)(pjg_paha * (Math.Sin((45 - amp_paha) * 0.0174532925))))));
                //kanan
                g.DrawLine(k1, new Point(250, 150), new Point((250 - (int)(pjg_paha * (Math.Cos((135) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((135) * 0.0174532925))))));
                g.DrawLine(k1, new Point((250 - (int)(pjg_paha * (Math.Cos((135) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((135) * 0.0174532925))))), new Point((250 - (int)(pjg_paha * (Math.Cos((135) * 0.0174532925)))), (200 - (int)(pjg_paha * (Math.Sin((135) * 0.0174532925)))))); 

            }
            else
            {
                //kiri
                g.DrawLine(k2, new Point(200, 150), new Point((200 - (int)(pjg_paha * (Math.Cos((45) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((45) * 0.0174532925))))));
                g.DrawLine(k2, new Point((200 - (int)(pjg_paha * (Math.Cos((45) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((45) * 0.0174532925))))), new Point((200 - (int)(pjg_paha * (Math.Cos((45) * 0.0174532925)))), (200 - (int)(pjg_paha * (Math.Sin((45) * 0.0174532925))))));
                //kanan
                g.DrawLine(k1, new Point(250, 150), new Point((250 - (int)(pjg_paha * (Math.Cos((135 - amp_paha) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((135 - amp_paha) * 0.0174532925))))));
                g.DrawLine(k1, new Point((250 - (int)(pjg_paha * (Math.Cos((135 - amp_paha) * 0.0174532925)))), (150 - (int)(pjg_paha * (Math.Sin((135 - amp_paha) * 0.0174532925))))), new Point((250 - (int)(pjg_paha * (Math.Cos((135 - amp_paha) * 0.0174532925)))), (200 - (int)(pjg_paha * (Math.Sin((135 - amp_paha) * 0.0174532925)))))); 

            }
        }

        private void Form1_Load(object sender, EventArgs e)
        {
            button1.Enabled = true;
            button2.Enabled = false;
            timer1.Enabled = false;
        }

        private void button1_Click(object sender, EventArgs e)
        {
            button2.Enabled = true;
            button1.Enabled = false;
            timer1.Enabled = true;
        }

        private void button2_Click(object sender, EventArgs e)
        {
            button1.Enabled = true;
            button2.Enabled = false;
            timer1.Enabled = false;
        }
    }
}

Berikut link download installer dari project di atas http://www.mediafire.com/file/5v3kwvyz3klc2bh/hexapod_install.rar. Terlebih dahulu install Framework 4.0.

Berikut contoh kode hasil percobaan robot hexapod dengan servo AX-12.


/*****************************************************
Chip type : ATmega128
Program type : Application
AVR Core Clock frequency: 16.000000 MHz
Memory model : Small
External RAM size : 0
Data Stack size : 1024
*****************************************************/

#include mega128.h>
#include delay.h>
#include stdio.h>

// Alphanumeric LCD Module functions
#include alcd.h>

#ifndef RXB8
#define RXB8 1
#endif

#ifndef TXB8
#define TXB8 0
#endif

#ifndef UPE
#define UPE 2
#endif

#ifndef DOR
#define DOR 3
#endif

#ifndef FE
#define FE 4
#endif

#ifndef UDRE
#define UDRE 5
#endif

#ifndef RXC
#define RXC 7
#endif

#define P_MODEL_NUMBER_L 0
#define P_MODEL_NUMBER_H 1
#define P_VERSION 2
#define P_ID 3
#define P_BAUD_RATE 4
#define P_RETURN_DELAY_TIME 5
#define P_CW_ANGLE_LIMIT_L 6
#define P_CW_ANGLE_LIMIT_H 7
#define P_CCW_ANGLE_LIMIT_L 8
#define P_CCW_ANGLE_LIMIT_H 9
#define P_SYSTEM_DATA2 10
#define P_LIMIT_TEMPERATURE 11
#define P_DOWN_LIMIT_VOLTAGE 12
#define P_UP_LIMIT_VOLTAGE 13
#define P_MAX_TORQUE_L 14
#define P_MAX_TORQUE_H 15
#define P_RETURN_LEVEL 16
#define P_ALARM_LED 17
#define P_ALARM_SHUTDOWN 18
#define P_OPERATING_MODE 19
#define P_DOWN_CALIBRATION_L 20
#define P_DOWN_CALIBRATION_H 21
#define P_UP_CALIBRATION_L 22
#define P_UP_CALIBRATION_H 23
#define P_TORQUE_ENABLE (24)
#define P_LED (25)
#define P_CW_COMPLIANCE_MARGIN (26)
#define P_CCW_COMPLIANCE_MARGIN (27)
#define P_CW_COMPLIANCE_SLOPE (28)
#define P_CCW_COMPLIANCE_SLOPE (29)
#define P_GOAL_POSITION_L (30)
#define P_GOAL_POSITION_H (31)
#define P_GOAL_SPEED_L (32)
#define P_GOAL_SPEED_H (33)
#define P_TORQUE_LIMIT_L (34)
#define P_TORQUE_LIMIT_H (35)
#define P_PRESENT_POSITION_L (36)
#define P_PRESENT_POSITION_H (37)
#define P_PRESENT_SPEED_L (38)
#define P_PRESENT_SPEED_H (39)
#define P_PRESENT_LOAD_L (40)
#define P_PRESENT_LOAD_H (41)
#define P_PRESENT_VOLTAGE (42)
#define P_PRESENT_TEMPERATURE (43)
#define P_REGISTERED_INSTRUCTION (44)
#define P_PAUSE_TIME (45)
#define P_MOVING (46)
#define P_LOCK (47)
#define P_PUNCH_L (48)
#define P_PUNCH_H (49)

//— Instruction —
#define INST_PING 0x01
#define INST_READ 0x02
#define INST_WRITE 0x03
#define INST_REG_WRITE 0x04
#define INST_ACTION 0x05
#define INST_RESET 0x06
#define INST_DIGITAL_RESET 0x07
#define INST_SYSTEM_READ 0x0C
#define INST_SYSTEM_WRITE 0x0D
#define INST_SYNC_WRITE 0x83
#define INST_SYNC_REG_WRITE 0x84

#define DEFAULT_RETURN_PACKET_SIZE 6
#define BROADCASTING_ID 0xfe

#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<DOR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)

// USART1 Receiver buffer
#define RX_BUFFER_SIZE1 8
char rx_buffer1[RX_BUFFER_SIZE1];

#if RX_BUFFER_SIZE1 <= 256
unsigned char rx_wr_index1,rx_rd_index1,rx_counter1;
#else
unsigned int rx_wr_index1,rx_rd_index1,rx_counter1;
#endif

// This flag is set on USART1 Receiver buffer overflow
bit rx_buffer_overflow1;

// USART1 Receiver interrupt service routine
interrupt [USART1_RXC] void usart1_rx_isr(void)
{
char status,data;
status=UCSR1A;
data=UDR1;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
{
rx_buffer1[rx_wr_index1++]=data;
#if RX_BUFFER_SIZE1 == 256
// special case for receiver buffer size=256
if (++rx_counter1 == 0)
{
#else
if (rx_wr_index1 == RX_BUFFER_SIZE1) rx_wr_index1=0;
if (++rx_counter1 == RX_BUFFER_SIZE1)
{
rx_counter1=0;
#endif
rx_buffer_overflow1=1;
}
}
}

// Get a character from the USART1 Receiver buffer
#pragma used+
char getchar1(void)
{
char data;
while (rx_counter1==0);
data=rx_buffer1[rx_rd_index1++];
#if RX_BUFFER_SIZE1 != 256
if (rx_rd_index1 == RX_BUFFER_SIZE1) rx_rd_index1=0;
#endif
#asm(“cli”)
–rx_counter1;
#asm(“sei”)
return data;
}
#pragma used-
// USART1 Transmitter buffer
#define TX_BUFFER_SIZE1 8
char tx_buffer1[TX_BUFFER_SIZE1];

#if TX_BUFFER_SIZE1 <= 256
unsigned char tx_wr_index1,tx_rd_index1,tx_counter1;
#else
unsigned int tx_wr_index1,tx_rd_index1,tx_counter1;
#endif

// USART1 Transmitter interrupt service routine
interrupt [USART1_TXC] void usart1_tx_isr(void)
{
if (tx_counter1)
{
–tx_counter1;
UDR1=tx_buffer1[tx_rd_index1++];
#if TX_BUFFER_SIZE1 != 256
if (tx_rd_index1 == TX_BUFFER_SIZE1) tx_rd_index1=0;
#endif
}
}

// Write a character to the USART1 Transmitter buffer
#pragma used+
void putchar1(char c)
{
while (tx_counter1 == TX_BUFFER_SIZE1);
#asm(“cli”)
if (tx_counter1 || ((UCSR1A & DATA_REGISTER_EMPTY)==0))
{
tx_buffer1[tx_wr_index1++]=c;
#if TX_BUFFER_SIZE1 != 256
if (tx_wr_index1 == TX_BUFFER_SIZE1) tx_wr_index1=0;
#endif
++tx_counter1;
}
else
UDR1=c;
#asm(“sei”)
}
#pragma used-

#include

unsigned char panjangTX,sl,sh,parameter[128],transfer[128];

unsigned char packet_kirim (unsigned char ID,unsigned char perintah,unsigned char parameterlength)
{
unsigned char txpacketlength,checksum,i;

transfer[0]=0xFF;
transfer[1]=0xFF;
transfer[2]=ID;
transfer[3]=parameterlength+2;
transfer[4]=perintah;
for(i=0;i<parameterlength;i++)
{
transfer[i+5]= parameter[i];

}
checksum=0;
txpacketlength=parameterlength+6;
for(i=2;i<txpacketlength-1;i++)
{
checksum+=transfer[i];
}
transfer[i]=~checksum;

for(i=0;i
#include stdlib.h>

unsigned char kata[16];

unsigned char posisi_0(unsigned int data)
{
unsigned char buff;
buff = (unsigned char)(data & 0b0000000011111111);
return(buff);
}

unsigned char posisi_1(unsigned int data)
{
unsigned char buff;
buff = (unsigned char)((data & 0b1111111100000000)>>8);
return(buff);
}

unsigned int spg_full(unsigned int nilai_tengah,int Amplitud,int degree)
{
unsigned int buff;
buff = nilai_tengah + (unsigned int)((float)Amplitud * sin(0.0174532925*(float)degree));
return(buff);
}

unsigned int spg_full_abs(unsigned int nilai_tengah,int Amplitud,int degree)
{
unsigned int buff;
buff = nilai_tengah + (unsigned int)((float)Amplitud * ((float)(abs(100*sin(0.0174532925*(float)degree)))/100));
return(buff);
}

unsigned int spg_half(unsigned int nilai_tengah,int Amplitud,int degree)
{
unsigned int buff;
if(((degree>180)&&(degree-180)&&(degree0)&&(degree-360)&&(degree>4;
speed_kiri = (int)buff – 7;
}

void forward_kanan()
{
//=============kanan tengah=======================
parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(511,Amp04,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(511,Amp04,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x04,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(597,Amp05,(sudut+90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(597,Amp05,(sudut+90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x05,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full_abs(631,Amp06,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full_abs(631,Amp06,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x06,INST_WRITE,5); //115 u/ 13

//=============kanan belakang=======================

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(358,Amp07,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(358,Amp07,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x07,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(597,Amp08,(sudut-90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(597,Amp08,(sudut-90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x08,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(631,Amp09,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(631,Amp09,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x09,INST_WRITE,5); //115 u/ 13

//=============kanan depan=======================

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(667,Amp01,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(667,Amp01,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x01,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(597,Amp02,(sudut-90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(597,Amp02,(sudut-90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x02,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(631,Amp03,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(631,Amp03,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x03,INST_WRITE,5); //115 u/ 13
}

void forward_kiri()
{
//=============kiri tengah=======================
parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(511,Amp0D,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(511,Amp0D,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0D,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(420,Amp0E,(sudut-90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(420,Amp0E,(sudut-90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0E,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full_abs(409,Amp12,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full_abs(409,Amp12,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0F,INST_WRITE,5); //115 u/ 13

//=============kiri belakang=======================
parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(667,Amp10,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(667,Amp10,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x10,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(407,Amp11,(sudut+90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(407,Amp11,(sudut+90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x11,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(369,Amp0F,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(369,Amp0F,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x12,INST_WRITE,5); //115 u/ 13

//=============kiri depan=======================

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(358,Amp0A,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(358,Amp0A,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0A,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(407,-Amp0B,(sudut+90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(407,-Amp0B,(sudut+90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0B,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(381,Amp0C,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(381,Amp0C,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0C,INST_WRITE,5); //115 u/ 13
}

void backward_kanan()
{
//=============kanan tengah=======================
parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(511,-Amp04,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(511,-Amp04,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x04,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(597,Amp05,(sudut+90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(597,Amp05,(sudut+90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x05,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full_abs(631,Amp06,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full_abs(631,Amp06,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x06,INST_WRITE,5); //115 u/ 13

//=============kanan belakang=======================

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(358,-Amp07,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(358,-Amp07,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x07,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(597,Amp08,(sudut-90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(597,Amp08,(sudut-90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x08,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(631,-Amp09,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(631,-Amp09,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x09,INST_WRITE,5); //115 u/ 13

//=============kanan depan=======================

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(667,-Amp01,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(667,-Amp01,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x01,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(597,Amp02,(sudut-90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(597,Amp02,(sudut-90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x02,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(631,-Amp03,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(631,-Amp03,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x03,INST_WRITE,5); //115 u/ 13
}

void backward_kiri()
{
//=============kiri tengah=======================
parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(511,-Amp0D,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(511,-Amp0D,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0D,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(420,Amp0E,(sudut-90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(420,Amp0E,(sudut-90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0E,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full_abs(409,Amp12,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full_abs(409,Amp12,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0F,INST_WRITE,5); //115 u/ 13

//=============kiri belakang=======================
parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(668,-Amp10,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(668,-Amp10,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x10,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(407,Amp11,(sudut+90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(407,Amp11,(sudut+90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x11,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(369,-Amp0F,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(369,-Amp0F,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x12,INST_WRITE,5); //115 u/ 13

//=============kiri depan=======================

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(343,-Amp0A,sudut)); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(343,-Amp0A,sudut)); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0A,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_half(407,-Amp0B,(sudut+90))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_half(407,-Amp0B,(sudut+90))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0B,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = posisi_0(spg_full(381,-Amp0C,(sudut))); //Writing Data P_GOAL_POSITION_L
parameter[2] = posisi_1(spg_full(381,-Amp0C,(sudut))); //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0C,INST_WRITE,5); //115 u/ 13
}

unsigned int dmin=0;
unsigned int dplus=0;

void diam()
{
parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0xFF; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x04,INST_WRITE,5); //kaki 4&13 sudut 150
panjangTX = packet_kirim(0x0D,INST_WRITE,5);

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0x9B; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x01,INST_WRITE,5); //1 & 16 sudut 180
panjangTX = packet_kirim(0x10,INST_WRITE,5);

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0x66; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x07,INST_WRITE,5); //10 & 7 sudut 120
panjangTX = packet_kirim(0x0A,INST_WRITE,5);

delay_ms(dplus);

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0x55; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x03,INST_WRITE,5);
panjangTX = packet_kirim(0x06,INST_WRITE,5);
panjangTX = packet_kirim(0x09,INST_WRITE,5);

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0xAB; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0C,INST_WRITE,5);
panjangTX = packet_kirim(0x0F,INST_WRITE,5);
panjangTX = packet_kirim(0x12,INST_WRITE,5);

delay_ms(dplus);

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0x55; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x02,INST_WRITE,5);
panjangTX = packet_kirim(0x05,INST_WRITE,5);
panjangTX = packet_kirim(0x08,INST_WRITE,5);

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0xAB; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0B,INST_WRITE,5);
panjangTX = packet_kirim(0x0E,INST_WRITE,5);
panjangTX = packet_kirim(0x11,INST_WRITE,5);
}

void kiri()
{
parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xFF; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x08,INST_WRITE,5); //kaki 8&2 mengangkat sebesar 100
panjangTX = packet_kirim(0x02,INST_WRITE,5);
panjangTX = packet_kirim(0x0E,INST_WRITE,5); //kaki 14 mengangkat sebesar 200

delay_ms(dmin); //mulai maju diudara

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x55; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x01,INST_WRITE,5); //150 untuk 1

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x22; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x07,INST_WRITE,5); //90 untuk 7

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xBC; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0D,INST_WRITE,5); //180 untuk 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version ke belakang tripod 2
parameter[1] = 0xAB; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0A,INST_WRITE,5); //10 untuk 115

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version ke belakang
parameter[1] = 0x44; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x04,INST_WRITE,5); //4 untuk 185

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version ke belakang
parameter[1] = 0xDE; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x10,INST_WRITE,5); //16 untuk 150

delay_ms(dmin); //sekarang mulai menginjak

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0x55; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x02,INST_WRITE,5);
panjangTX = packet_kirim(0x08,INST_WRITE,5);

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0xAB; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0E,INST_WRITE,5); //150 untuk 14

delay_ms(dplus); //mulai mengangkat tripod baru atau tripod 2

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xFF; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x05,INST_WRITE,5); // kaki 5 mengangkat sebesar 100
panjangTX = packet_kirim(0x0B,INST_WRITE,5); //
panjangTX = packet_kirim(0x11,INST_WRITE,5); //kaki 11 & 17 mengangkat sebesar 200

delay_ms(dmin); //mengayuh tripod 1

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xDE; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x01,INST_WRITE,5); //180 untuk 1

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x44; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0D,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xAB; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x07,INST_WRITE,5); //150 untuk 7

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xBC; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x04,INST_WRITE,5); //120 untuk 4

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x22; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0A,INST_WRITE,5); //150 untuk 10

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x55; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x10,INST_WRITE,5); //210 untuk 16

delay_ms(dmin); //tripod 2 turun

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x55; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x05,INST_WRITE,5); //150 untuk 5

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0xAB; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0B,INST_WRITE,5);
panjangTX = packet_kirim(0x11,INST_WRITE,5); //150 untuk 5

delay_ms(dplus);// tripod 1 mengangkat
}

void kanan()
{
parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xFF; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x08,INST_WRITE,5); //kaki 8&2 mengangkat sebesar 100
panjangTX = packet_kirim(0x02,INST_WRITE,5);
panjangTX = packet_kirim(0x0E,INST_WRITE,5); //kaki 14 mengangkat sebesar 200

delay_ms(dmin); //mulai maju diudara

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x55; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x01,INST_WRITE,5); //150 untuk 1

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x22; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x07,INST_WRITE,5); //90 untuk 7

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xBC; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0D,INST_WRITE,5); //180 untuk 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version ke belakang tripod 2
parameter[1] = 0xAB; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0A,INST_WRITE,5); //10 untuk 115

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version ke belakang
parameter[1] = 0x44; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x04,INST_WRITE,5); //4 untuk 185

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version ke belakang
parameter[1] = 0xDE; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x10,INST_WRITE,5); //16 untuk 150

delay_ms(dmin); //sekarang mulai menginjak

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0x55; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x02,INST_WRITE,5);
panjangTX = packet_kirim(0x08,INST_WRITE,5);

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0xAB; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0E,INST_WRITE,5); //150 untuk 14

delay_ms(dplus); //mulai mengangkat tripod baru atau tripod 2

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xFF; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x05,INST_WRITE,5); // kaki 5 mengangkat sebesar 100
panjangTX = packet_kirim(0x0B,INST_WRITE,5); //
panjangTX = packet_kirim(0x11,INST_WRITE,5); //kaki 11 & 17 mengangkat sebesar 200

delay_ms(dmin); //mengayuh tripod 1

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xDE; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x01,INST_WRITE,5); //180 untuk 1

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x44; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0D,INST_WRITE,5); //115 u/ 13

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xAB; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x07,INST_WRITE,5); //150 untuk 7

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0xBC; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x04,INST_WRITE,5); //120 untuk 4

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x22; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0A,INST_WRITE,5); //150 untuk 10

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x55; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x10,INST_WRITE,5); //210 untuk 16

delay_ms(dmin); //tripod 2 turun

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
parameter[1] = 0x55; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x05,INST_WRITE,5); //150 untuk 5

parameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version BERDIRI
parameter[1] = 0xAB; //Writing Data P_GOAL_POSITION_L
parameter[2] = 0x01; //Writing Data P_GOAL_POSITION_H
parameter[3] = sl; //Writing Data P_GOAL_SPEED_L
parameter[4] = sh; //Writing Data P_GOAL_SPEED_H
panjangTX = packet_kirim(0x0B,INST_WRITE,5);
panjangTX = packet_kirim(0x11,INST_WRITE,5); //150 untuk 5

delay_ms(dplus);// tripod 1 mengangkat
}

void putar_kiri()
{
//————–kanan
Amp06 = 0;
Amp09 = 0;
Amp03 = 0;
//—————-kiri
Amp12 = 0;
Amp0F = 0;
Amp0C = 0;

forward_kanan();
backward_kiri();
}

void putar_kanan()
{
//————–kanan
Amp06 = 0;
Amp09 = 0;
Amp03 = 0;
//—————-kiri
Amp12 = 0;
Amp0F = 0;
Amp0C = 0;

forward_kiri();
backward_kanan();
}

void belok_kanan()
{
//————–kanan
Amp04 = 0;
Amp07 = 0;
Amp01 = 0;
Amp06=0;
Amp09=0;
Amp03=0;

//—————-kiri
Amp0D = -70;
Amp10 = 70;
Amp0A = 70;

forward_kiri();
forward_kanan();
}

void belok_kiri()
{
//————–kanan
Amp04 = -70;
Amp07 = 70;
Amp01 = 70;
//—————-kiri
Amp0D = 0;
Amp10 = 0;
Amp0A = 0;
Amp12=0;
Amp0F=0;
Amp0C=0;

forward_kiri();
forward_kanan();
}

void looping(unsigned int speed)
{
for(sudut=0;sudut<360;sudut+=10)
{
if((sudut==0)||(sudut==180))get_velocity();

//————–kanan
Amp04 = (int)((float)(-50)*(float)((float)abs(speed_kanan)/7)); //tengah
Amp05 = -100;
Amp06 = 11;
if(Amp04==0)Amp06=0;

Amp07 = (int)((float)50*(float)((float)abs(speed_kanan)/7)); //belakang
Amp08 = -100;
Amp09 = 50;
if(Amp07==0)Amp09=0;

Amp01 = (int)((float)50*(float)((float)abs(speed_kanan)/7)); //depan
Amp02 = -100;
Amp03 = -50;
if(Amp01==0)Amp03=0;

//—————-kiri
Amp0D = (int)((float)-50*(float)((float)abs(speed_kiri)/7)); //tengah
Amp0E = 100;
Amp12 = -11;
if(Amp0D==0)Amp12=0;

Amp10 = (int)((float)50*(float)((float)abs(speed_kiri)/7)); //belakang
Amp11 = 100;
Amp0F = 50;
if(Amp10==0)Amp0F=0;

Amp0A = (int)((float)50*(float)((float)abs(speed_kiri)/7)); //depan
Amp0B = -100;
Amp0C = -50;
if(Amp0A==0)Amp0C=0;

if((speed_kanan == (-7)) && (speed_kiri == (-7)))
belok_kanan();
else if ((speed_kanan == (-6)) && (speed_kiri == (-6)))
belok_kiri();
else if((speed_kanan == (-7)) && (speed_kiri == (-6)))
putar_kanan();
else if ((speed_kanan == (-6)) && (speed_kiri == (-7)))
putar_kiri();
else
{
if(speed_kanan == 8)
diam();
else if(speed_kanan<=0)
backward_kanan();
else
forward_kanan();

if(speed_kiri == 8)
diam();
else if(speed_kiri<=0)
backward_kiri();
else
forward_kiri();
}
delay_ms(1000/speed);
}
}

void main(void)
{
// Declare your local variables here

// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x00;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD=0x00;
DDRD=0x00;

// Port E initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTE=0x00;
DDRE=0x00;

// Port F initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTF=0x00;
DDRF=0x00;

// Port G initialization
// Func4=In Func3=In Func2=In Func1=In Func0=In
// State4=T State3=T State2=T State1=T State0=T
PORTG=0x00;
DDRG=0x00;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0 output: Disconnected
ASSR=0x00;
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer1 Stopped
// Mode: Normal top=0xFFFF
// OC1A output: Discon.
// OC1B output: Discon.
// OC1C output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
// Compare C Match Interrupt: Off
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;
OCR1CH=0x00;
OCR1CL=0x00;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2 output: Disconnected
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

// Timer/Counter 3 initialization
// Clock source: System Clock
// Clock value: Timer3 Stopped
// Mode: Normal top=0xFFFF
// OC3A output: Discon.
// OC3B output: Discon.
// OC3C output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer3 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
// Compare C Match Interrupt: Off
TCCR3A=0x00;
TCCR3B=0x00;
TCNT3H=0x00;
TCNT3L=0x00;
ICR3H=0x00;
ICR3L=0x00;
OCR3AH=0x00;
OCR3AL=0x00;
OCR3BH=0x00;
OCR3BL=0x00;
OCR3CH=0x00;
OCR3CL=0x00;

// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
// INT3: Off
// INT4: Off
// INT5: Off
// INT6: Off
// INT7: Off
EICRA=0x00;
EICRB=0x00;
EIMSK=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;

ETIMSK=0x00;

// USART0 initialization
// USART0 disabled
UCSR0B=0x00;

// USART1 initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART1 Receiver: On
// USART1 Transmitter: On
// USART1 Mode: Asynchronous
// USART1 Baud Rate: 1000000
UCSR1A=0x00;
UCSR1B=0xD8;
UCSR1C=0x06;
UBRR1H=0x00;
UBRR1L=0x00;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// ADC initialization
// ADC disabled
ADCSRA=0x00;

// SPI initialization
// SPI disabled
SPCR=0x00;

// TWI initialization
// TWI disabled
TWCR=0x00;

// Alphanumeric LCD initialization
// Connections specified in the
// Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu:
// RS – PORTA Bit 0
// RD – PORTA Bit 1
// EN – PORTA Bit 2
// D4 – PORTA Bit 4
// D5 – PORTA Bit 5
// D6 – PORTA Bit 6
// D7 – PORTA Bit 7
// Characters/line: 16
lcd_init(16);

// Global enable interrupts
#asm(“sei”)

sl=0x00;sh=0x03;

while (1)
{

looping(300);

}
}

3 Responses to Simulasi Robot Hexapod Kaki 3DOF Berbasis Kurva Sinusoidal

  1. faris says:

    gan…program diatas kan untuk pengontrol servo dengan serial..
    kalau mengontrolnya menggunakan pwm bagaimana cara mereduksi variabel2 pergerakan robot menjadi cuma 2 variabel jarak ayunan kaki kanan dan kiri??

  2. ada yang versi servo biasa gak

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s