Please subscribe click here

Program Arduino

//2 = ultrasonic
//3 body kiri depan
//4 body kiri belakang
//5 kaki kiri depan
//6 body kanan depan
//7 kaki kanan belakang
//8 kaki kanan depan
//9 body kanan belakang
//10 kaki kiri belakang
#include <Servo.h>
Servo kaki_l_1,kaki_l_2,kaki_r_1,kaki_r_2,servo_us,body_l_1,body_l_2,body_r_1,body_r_2; // create servo object to control a servo
// twelve servo objects can be created on most boards
int pos = 0; // variable to store the servo position
int kl1=60,kl2=130,kr1=60,kr2=110,kp=40,bp=20,dmaju=75;
int bl1=110,bl2=100,br1=80,br2=100,cu=77;
int dserial=0,dser,toleh=0;
void kaki1(int ddel,int sud)
{
kaki_l_1.write(kl1+kp); //100); // tell servo to go to position in variable ‘pos’
delay(ddel);
kaki_l_1.write(kl1+kp-sud); //100); // tell servo to go to position in variable ‘pos’
delay(ddel);

}
void kaki2(int ddel,int sud)
{
kaki_l_2.write(kl2-kp); //100); // tell servo to go to position in variable ‘pos’
delay(ddel);
kaki_l_2.write(kl2-kp+sud); //100); // tell servo to go to position in variable ‘pos’
delay(ddel);
}
void kaki3(int ddel,int sud)
{
kaki_r_1.write(kr1+kp); //100); // tell servo to go to position in variable ‘pos’
delay(ddel);
kaki_r_1.write(kr1+kp-sud); //100); // tell servo to go to position in variable ‘pos’
delay(ddel);
}
void kaki4(int ddel,int sud)
{
kaki_r_2.write(kr2-kp); //100); // tell servo to go to position in variable ‘pos’
delay(ddel);
kaki_r_2.write(kr2-kp+sud); //100); // tell servo to go to position in variable ‘pos’
delay(ddel);
}
void body1(int ddel,int sud)
{
body_l_1.write(bl1); //+ mundur
delay(ddel);
body_l_1.write(bl1-sud); //+ mundur
delay(ddel);
}
void body2(int ddel,int sud)
{
body_l_2.write(bl2); //+ mundur
delay(ddel);
body_l_2.write(bl2+sud); //+ mundur
delay(ddel);
}
void body3(int ddel,int sud)
{
body_r_1.write(br1); //+ mundur
delay(ddel);
body_r_1.write(br1+sud); //+ mundur
delay(ddel);
}
void body4(int ddel,int sud)
{
body_r_2.write(br2); //+ mundur
delay(ddel);
body_r_2.write(br2-sud); //+ mundur
delay(ddel);
}

void normal()
{
servo_us.write(cu); // tell servo to go to position in variable ‘pos’
kaki_l_1.write(kl1+kp); //100); // tell servo to go to position in variable ‘pos’
kaki_l_2.write(kl2-kp);//90); // tell servo to go to position in variable ‘pos’
kaki_r_1.write(kr1+kp);//100); // tell servo to go to position in variable ‘pos’
kaki_r_2.write(kr2-kp);//80); // tell servo to go to position in variable ‘pos’
body_l_1.write(bl1); //+ mundur
body_l_2.write(bl2); //+mundur
body_r_1.write(br1); //+maju
body_r_2.write(br2); //+maju
}
void normalm()
{
servo_us.write(cu); // tell servo to go to position in variable ‘pos’
kaki_l_1.write(kl1+kp); //100); // tell servo to go to position in variable ‘pos’
kaki_l_2.write(kl2-kp);//90); // tell servo to go to position in variable ‘pos’
kaki_r_1.write(kr1+kp);//100); // tell servo to go to position in variable ‘pos’
kaki_r_2.write(kr2-kp);//80); // tell servo to go to position in variable ‘pos’
body_l_1.write(bl1-(1.5*bp)); //+ mundur
body_l_2.write(bl2-(1.5*bp)); //+mundur
body_r_1.write(br1+(1.5*bp)); //+maju
body_r_2.write(br2+(1.5*bp)); //+maju
}
void istirahat()
{
kaki1(0,70);
body1(0,45);
kaki2(0,70);
body2(0,45);
kaki3(0,70);
body3(0,45);
kaki4(0,70);
body4(0,45);

delay(2*dmaju);
}
void normalb()
{
servo_us.write(cu); // tell servo to go to position in variable ‘pos’
kaki_l_1.write(kl1+kp); //100); // tell servo to go to position in variable ‘pos’
kaki_l_2.write(kl2-kp);//90); // tell servo to go to position in variable ‘pos’
kaki_r_1.write(kr1+kp);//100); // tell servo to go to position in variable ‘pos’
kaki_r_2.write(kr2-kp);//80); // tell servo to go to position in variable ‘pos’
body_l_1.write(bl1+(bp)); //+ mundur
body_l_2.write(bl2+(bp)); //+mundur
body_r_1.write(br1-(bp)); //+maju
body_r_2.write(br2-(bp)); //+maju
}

void mundur()
{
kaki2(dmaju,40);
body2(dmaju,5);
kaki2(dmaju,0);
kaki4(dmaju,40);
body4(dmaju,5);
kaki4(dmaju,0);

kaki1(dmaju,40);
body1(dmaju,-5);
kaki1(dmaju,0);
kaki3(dmaju,40);
body3(dmaju,-5);
kaki3(dmaju,0);
normalm();
delay(dmaju);
}
void maju()
{
kaki1(dmaju,40);
body1(dmaju,15);
kaki1(dmaju,0);

kaki3(dmaju,40);
body3(dmaju,15);
kaki3(dmaju,0);

kaki2(dmaju,40);
body2(dmaju,-10);
kaki2(dmaju,0);

kaki4(dmaju,40);
body4(dmaju,-10);
kaki4(dmaju,0);

normalb();
delay(dmaju);
}
void putarkiri()
{
kaki2(dmaju,40);
body2(dmaju,30);
kaki2(dmaju,0);

kaki1(dmaju,40);
body1(dmaju,-30);
kaki1(dmaju,0);
kaki3(dmaju,40);
body3(dmaju,30);
kaki3(dmaju,0);

kaki4(dmaju,40);
body4(dmaju,-30);
kaki4(dmaju,0);

body1(0,20);
body2(0,-20);
body3(0,-20);
body4(0,20);

//normal();
delay(dmaju);

}
void putarkanan()
{
kaki1(dmaju,40);
body1(dmaju,30);
kaki1(dmaju,0);

kaki2(dmaju,40);
body2(dmaju,-30);
kaki2(dmaju,0);
kaki4(dmaju,40);
body4(dmaju,30);
kaki4(dmaju,0);

kaki3(dmaju,40);
body3(dmaju,-30);
kaki3(dmaju,0);
body1(0,-20);
body2(0,20);
body3(0,20);
body4(0,-20);

//normal();
delay(dmaju);

}
void kekiri()
{
kaki1(0,0);
body1(0,0);
kaki2(0,0);
body2(0,0);
kaki3(0,0);
body3(0,0);
kaki4(0,0);
body4(0,0);

body1(dmaju,30);
kaki1(dmaju,40);
body1(dmaju,0);

body2(dmaju,30);
kaki2(dmaju,40);
body2(dmaju,0);

body3(dmaju,30);
kaki3(dmaju,-40);
body3(dmaju,0);

body4(dmaju,30);
kaki4(dmaju,-40);
body4(dmaju,0);
}

void kekanan()
{
kaki1(0,0);
body1(0,0);
kaki2(0,0);
body2(0,0);
kaki3(0,0);
body3(0,0);
kaki4(0,0);
body4(0,0);

body3(dmaju,30);
kaki3(dmaju,40);
body3(dmaju,0);

body4(dmaju,30);
kaki4(dmaju,40);
body4(dmaju,0);

body1(dmaju,30);
kaki1(dmaju,-40);
body1(dmaju,0);

body2(dmaju,30);
kaki2(dmaju,-40);
body2(dmaju,0);

}
void pushup()
{
body1(0,0);
body2(0,0);
body3(0,0);
body4(0,0);
kaki1(0,0);
kaki2(0,0);
kaki3(0,0);
kaki4(0,0);
delay(400);
kaki1(0,45);
kaki2(0,45);
kaki3(0,45);
kaki4(0,45);
delay(400);
}
void salam()
{
kaki3(0,0);
body3(0,85);
kaki2(0,0);
kaki4(0,0);

body1(0,70);
kaki1(0,80);
delay(3*dmaju);
kaki1(0,110);
delay(3*dmaju);

body1(0,90);
kaki1(0,80);
delay(3*dmaju);
kaki1(0,110);
delay(3*dmaju);

}

void hello()
{
kaki1(0,0);
kaki2(0,0);
kaki4(0,0);
kaki1(0,0);
body1(0,85);
body2(0,0);
body4(0,0);
body3(0,90);
kaki3(2.5*dmaju,110);
kaki3(0,50);
if(toleh==0)
{
servo_us.write(cu-40);
toleh=1;
}
else
{
servo_us.write(cu+40);
toleh=0;

}
}

void setup() {
// myservo.attach(5); // attaches the servo on pin 9 to the servo object
Serial.begin(9600);
servo_us.attach(2); // attaches the servo on pin 9 to the servo object
kaki_l_1.attach(5); // attaches the servo on pin 9 to the servo object
kaki_l_2.attach(10); // attaches the servo on pin 9 to the servo object
kaki_r_1.attach(8); // attaches the servo on pin 9 to the servo object
kaki_r_2.attach(7); // attaches the servo on pin 9 to the servo object
body_l_1.attach(3);
body_l_2.attach(4);
body_r_1.attach(6);
body_r_2.attach(9);
normal();
//istirahat();
}
void loop() {
//buka
/*
servo_us.write(cu-4); // tell servo to go to position in variable ‘pos’
//kaki_l_1.write(kl1); // tell servo to go to position in variable ‘pos’
//kaki_l_2.write(kl2); // tell servo to go to position in variable ‘pos’
//kaki_r_1.write(kr1); // tell servo to go to position in variable ‘pos’
//kaki_r_2.write(kr2); // tell servo to go to position in variable ‘pos’
//body_l_1.write(110-20); //+ mundur
//body_l_2.write(100+20); //+mundur
//body_r_1.write(80+20); //+maju
//body_r_2.write(100-20); //+maju
delay(1000);
//tutup
servo_us.write(cu+4); // tell servo to go to position in variable ‘pos’
kaki_l_1.write(kl1+kp); //100); // tell servo to go to position in variable ‘pos’
kaki_l_2.write(kl2-kp);//90); // tell servo to go to position in variable ‘pos’
kaki_r_1.write(kr1+kp);//100); // tell servo to go to position in variable ‘pos’
kaki_r_2.write(kr2-kp);//80); // tell servo to go to position in variable ‘pos’
body_l_1.write(bl1); //+ mundur
body_l_2.write(bl2); //+mundur
body_r_1.write(br1); //+maju
body_r_2.write(br2); //+maju

delay(1000); */
//putarkiri();
//putarkanan();
//body1(200,90);
//body2(200,90);
//body3(200,90);
//body4(200,90);
//servo_us.write(cu+40); // tell servo to go to position in variable ‘pos’
//mundur();
//kekiri();
//pkiri();
//hello();
//maju();
//servo_us.write(cu-40); // tell servo to go to position in variable ‘pos’
//maju();
//mundur();
//maju();
//kekanan();
//normalm();
//normal();
//aa:
//goto aa;
if(Serial.available())
{
servo_us.write(cu);
normal();
dser=Serial.read();
if(dser==’w’)dserial=dser;
if(dser==’s’)dserial=dser;
if(dser==’a’)dserial=dser;
if(dser==’d’)dserial=dser;
if(dser==’r’)dserial=dser;
if(dser==’l’)dserial=dser;
if(dser==’h’)dserial=dser;
if(dser==’n’)dserial=dser;
if(dser==’i’)dserial=dser;
if(dser==’b’)dserial=dser;
if(dser==’p’)dserial=dser;

Serial.println(dserial);
Serial.print(dser);
}
if(dserial==’w’)maju();
if(dserial==’s’)mundur();
if(dserial==’a’)kekiri();
if(dserial==’d’)kekanan();
if(dserial==’r’)putarkanan();
if(dserial==’l’)putarkiri();
if(dserial==’h’)hello();
if(dserial==’n’)normal();
if(dserial==’i’)istirahat();
if(dserial==’b’)salam();
if(dserial==’p’)pushup();

}