This arduino code can be used on any small bi-ped robot with 4 degrees of freedom and an Arduino based microcontroller. You can understand the code by reading the comments parallel to the code.
Here is a picture of a 4DOF robot available commercially:
You can also make your own bi-ped robot very easily. (I will add tutorials very soon)
Do not freakout looking at the size of the code. Most of the code is actually repeating itself. You can modify thhis code or work your own code if you like, but please feel free to share your experiences in here....
IDE used - Arduino 1.0.5
Board - Arduino Mega2560
Sensors - One regular IR sensor each on left and right foot. (Ref Pic below)
We will just use the digital output of the IR sensor. Before you test your code, make sure that you have configured the sensors correctly using the potentiometer on the module.
I have used functions extensively in this code. If you understand how a functio works in c programmings, this code is a piece of cake for ya guys. Good luck.
Arduino Code:
----starts here----
#include<Servo.h>
int pos1; // indicates the positions of each of the servo motors
int pos2;
int pos3;
int pos4;
int i; // instead of moving from one angle to another at once, I increment servo angle in a for loop and
// i is th for loop variable.
int a; // variables a and b are used to store sensor readings
int b;
int q = 5; // determines the speed of robot. Smaller the value, faster the robot.
int m; // extra variables, just in case!
int n;
Servo foot_left; // initializing servo motors
Servo foot_right;
Servo hip_left;
Servo hip_right;
void setup()
{
foot_left.attach(2); // indicates the PWM pins I attached my servo motors (you may need to change this)
foot_right.attach(4);
hip_left.attach(3);
hip_right.attach(5);
pinMode(12, INPUT); // declaring the state of IR input pins
pinMode(13, INPUT);
pos1 = 90; // it is really important to ensure that the initial position of all the servo motors is
pos2 = 90; // 90 degrees to avoid confusion. Here 90 is the start position of all my servos.
pos3 = 90; // you can first initialize this position and connect the components of the robot.
pos4 = 90;
// writing initial positions
foot_left.write(pos1);
foot_right.write(pos2);
hip_left.write(pos3);
hip_right.write(pos4);
bend_right_start();
move_forward_start(); //change to forward leap right
bend_right_end();
}
void loop()
{
a = digitalRead(12); // reading the sensor values
b = digitalRead(13);
if( (a==1) && (b==1) ) // conditional statements
{
walk(); // function declarations after void loop()
}
a = digitalRead(12);
b = digitalRead(13);
if( (a==1) && (b==0) && ( n==1 ) )
//yet to be tested
{
deflect_right();
}
a = digitalRead(12);
b = digitalRead(13);
if( (a==0) && (b==1) && ( n==0 ) )
{
deflect_left();
}
a = digitalRead(12);
b = digitalRead(13);
if( (a==0) && (b==0) )
{
walk();
}
else
{
walk();
}
}
void walk()
{
m = m + 1 ; // condition to allow the robot to take 1 step at a time
n = m % 2 ;
if( n == 1)
{
bend_left_start();
move_forward_leap_left();
bend_left_end();
}
if( n == 0)
{
bend_right_start();
move_forward_leap_right();
bend_right_end();
}
}
void move_forward_start()
{
for(i = 0; i < 20; i += 1)
{
pos3 = pos3 - 1;
pos4 = pos4 - 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
}
void move_forward_end()
{
for(i = 20; i>0; i-=1)
{
pos3 = pos3 + 1;
pos4 = pos4 + 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
}
void move_forward_leap_right()
{
for(i = 0; i < 40; i += 1)
{
pos3 = pos3 - 1;
pos4 = pos4 - 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
}
void move_forward_leap_left()
{
for(i = 40; i>0; i-=1)
{
pos3 = pos3 + 1;
pos4 = pos4 + 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
}
void bend_right_start()
{
for(i = 0; i < 90; i += 1)
{
pos1 = pos1 + 1;
if( (i%2) == 1)
{
pos2 = pos2 + 1;
}
foot_left.write(pos1);
foot_right.write(pos2);
delay(q);
}
}
void bend_right_end()
{
for(i = 90; i>0; i-=1)
{
pos1 = pos1 - 1;
if( (i%2) == 1)
{
pos2 = pos2 - 1;
}
foot_left.write(pos1);
foot_right.write(pos2);
delay(q);
}
}
void bend_left_start()
{
for(i = 0; i < 90; i += 1)
{
pos2 = pos2 - 1;
if( (i%2) == 1)
{
pos1 = pos1 - 1;
}
foot_left.write(pos1);
foot_right.write(pos2);
delay(q);
}
}
void bend_left_end()
{
for(i = 90; i>0; i-=1)
{
pos2 = pos2 + 1;
if( (i%2) == 1)
{
pos1 = pos1 + 1;
}
foot_left.write(pos1);
foot_right.write(pos2);
delay(q);
}
}
void deflect_right()
{
bend_left_start();
for(i = 20; i>0; i-=1)
{
pos4 = pos4 + 1;
hip_right.write(pos4);
delay(q);
}
bend_left_end();
bend_right_start();
for(i = 20; i>0; i-=1)
{
pos3 = pos3 + 1;
hip_left.write(pos3);
delay(q);
}
for(i = 0; i < 60; i += 1)
{
pos3 = pos3 - 1;
pos4 = pos4 - 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
bend_right_end();
m = m+1;
}
void deflect_left()
{
bend_right_start();
for(i = 20; i>0; i-=1)
{
pos3 = pos3 - 1;
hip_left.write(pos3);
delay(q);
}
bend_right_end();
bend_left_start();
for(i = 20; i>0; i-=1)
{
pos4 = pos4 - 1;
hip_right.write(pos4);
delay(q);
}
for(i = 0; i < 60; i += 1)
{
pos3 = pos3 + 1;
pos4 = pos4 + 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
bend_left_end();
m = m+1;
}
----END----
This code is kind of big, but it is really simple. Ask me if you have any doubts :)
Here is a picture of a 4DOF robot available commercially:
 |
| 4DOF bi-ped robot |
Here is the pic of my biped line following robot:
 |
| Qubert |
Do not freakout looking at the size of the code. Most of the code is actually repeating itself. You can modify thhis code or work your own code if you like, but please feel free to share your experiences in here....
IDE used - Arduino 1.0.5
Board - Arduino Mega2560
Sensors - One regular IR sensor each on left and right foot. (Ref Pic below)
 |
| IR sensor |
I have used functions extensively in this code. If you understand how a functio works in c programmings, this code is a piece of cake for ya guys. Good luck.
Arduino Code:
----starts here----
#include<Servo.h>
int pos1; // indicates the positions of each of the servo motors
int pos2;
int pos3;
int pos4;
int i; // instead of moving from one angle to another at once, I increment servo angle in a for loop and
// i is th for loop variable.
int a; // variables a and b are used to store sensor readings
int b;
int q = 5; // determines the speed of robot. Smaller the value, faster the robot.
int m; // extra variables, just in case!
int n;
Servo foot_left; // initializing servo motors
Servo foot_right;
Servo hip_left;
Servo hip_right;
void setup()
{
foot_left.attach(2); // indicates the PWM pins I attached my servo motors (you may need to change this)
foot_right.attach(4);
hip_left.attach(3);
hip_right.attach(5);
pinMode(12, INPUT); // declaring the state of IR input pins
pinMode(13, INPUT);
pos1 = 90; // it is really important to ensure that the initial position of all the servo motors is
pos2 = 90; // 90 degrees to avoid confusion. Here 90 is the start position of all my servos.
pos3 = 90; // you can first initialize this position and connect the components of the robot.
pos4 = 90;
// writing initial positions
foot_left.write(pos1);
foot_right.write(pos2);
hip_left.write(pos3);
hip_right.write(pos4);
bend_right_start();
move_forward_start(); //change to forward leap right
bend_right_end();
}
void loop()
{
a = digitalRead(12); // reading the sensor values
b = digitalRead(13);
if( (a==1) && (b==1) ) // conditional statements
{
walk(); // function declarations after void loop()
}
a = digitalRead(12);
b = digitalRead(13);
if( (a==1) && (b==0) && ( n==1 ) )
//yet to be tested
{
deflect_right();
}
a = digitalRead(12);
b = digitalRead(13);
if( (a==0) && (b==1) && ( n==0 ) )
{
deflect_left();
}
a = digitalRead(12);
b = digitalRead(13);
if( (a==0) && (b==0) )
{
walk();
}
else
{
walk();
}
}
void walk()
{
m = m + 1 ; // condition to allow the robot to take 1 step at a time
n = m % 2 ;
if( n == 1)
{
bend_left_start();
move_forward_leap_left();
bend_left_end();
}
if( n == 0)
{
bend_right_start();
move_forward_leap_right();
bend_right_end();
}
}
void move_forward_start()
{
for(i = 0; i < 20; i += 1)
{
pos3 = pos3 - 1;
pos4 = pos4 - 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
}
void move_forward_end()
{
for(i = 20; i>0; i-=1)
{
pos3 = pos3 + 1;
pos4 = pos4 + 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
}
void move_forward_leap_right()
{
for(i = 0; i < 40; i += 1)
{
pos3 = pos3 - 1;
pos4 = pos4 - 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
}
void move_forward_leap_left()
{
for(i = 40; i>0; i-=1)
{
pos3 = pos3 + 1;
pos4 = pos4 + 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
}
void bend_right_start()
{
for(i = 0; i < 90; i += 1)
{
pos1 = pos1 + 1;
if( (i%2) == 1)
{
pos2 = pos2 + 1;
}
foot_left.write(pos1);
foot_right.write(pos2);
delay(q);
}
}
void bend_right_end()
{
for(i = 90; i>0; i-=1)
{
pos1 = pos1 - 1;
if( (i%2) == 1)
{
pos2 = pos2 - 1;
}
foot_left.write(pos1);
foot_right.write(pos2);
delay(q);
}
}
void bend_left_start()
{
for(i = 0; i < 90; i += 1)
{
pos2 = pos2 - 1;
if( (i%2) == 1)
{
pos1 = pos1 - 1;
}
foot_left.write(pos1);
foot_right.write(pos2);
delay(q);
}
}
void bend_left_end()
{
for(i = 90; i>0; i-=1)
{
pos2 = pos2 + 1;
if( (i%2) == 1)
{
pos1 = pos1 + 1;
}
foot_left.write(pos1);
foot_right.write(pos2);
delay(q);
}
}
void deflect_right()
{
bend_left_start();
for(i = 20; i>0; i-=1)
{
pos4 = pos4 + 1;
hip_right.write(pos4);
delay(q);
}
bend_left_end();
bend_right_start();
for(i = 20; i>0; i-=1)
{
pos3 = pos3 + 1;
hip_left.write(pos3);
delay(q);
}
for(i = 0; i < 60; i += 1)
{
pos3 = pos3 - 1;
pos4 = pos4 - 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
bend_right_end();
m = m+1;
}
void deflect_left()
{
bend_right_start();
for(i = 20; i>0; i-=1)
{
pos3 = pos3 - 1;
hip_left.write(pos3);
delay(q);
}
bend_right_end();
bend_left_start();
for(i = 20; i>0; i-=1)
{
pos4 = pos4 - 1;
hip_right.write(pos4);
delay(q);
}
for(i = 0; i < 60; i += 1)
{
pos3 = pos3 + 1;
pos4 = pos4 + 1;
hip_left.write(pos3);
hip_right.write(pos4);
delay(q);
}
bend_left_end();
m = m+1;
}
----END----
This code is kind of big, but it is really simple. Ask me if you have any doubts :)
No comments:
Post a Comment