This work has been my final project for a subject at the university. Didn't have enough time to make it do more stuff, but I hope the programation is useful for anyone trying to make something similar. This is my first project with Arduino, so probably you will find some mistakes, hope to hear and learn from them.
The ROBONOVA-1 is a human body robot composed with 16 servos, here I leave you it's manual:
http://www.flexwatch.es/archivos/ROBONOVA-Manual-ES.pdf
If for anyone is easier to read the pdf I uploaded it's report here, and most of the mathematics is explained over there :
https://drive.google.com/open?id=1EcAuCnE-ognE42eKcJgjzBTF5YuyyAgy
Mainly what I did is to solve the inverse kinematics of the robot from it's waist until the ankle. And then with the orientation of the final point (ankle) defined, I am able to define the position of the ankle wanted according to the action desired (either balance over one leg or to level the robot body).
The conecction is the same as a normal servo control, with the difference that as there are a lot of servos, you won't be able to power them with the Arduino, so you will need to get and external source. (AND DON'T FORGET TO CONECT BOTH GND FROM THE ARDUINO AND THE EXTERNAL SOURCE!).
The program is separated in a few blocks, so that is easier to read. The program works jus for the legs, but if you have enough PMW pins you can make them work!
Reference sistem for each legThis is the reference sistem of each leg defined for the movements:
And considering the right hand rule the movement of each joint and it's name are:
Leg() function
The first function, and most basic, is theone that calculates the angles through inverse kinematics so that each legreaches the given position. It need three double variables that are related tothe reference system of each leg, then it calculates the angles for the 3joints and then fills the elements of the array related to each joint. And, asthe variables that returns are golbar variables defined at the beginning of theprogram, the angles can be used in other functions without needing to bedeclared the call of the variables.
void leftleg(double x, double y, double z){ //,int veloc, bool wait){ Para cuandoutilicemos las variables de VarSpeedServo
//Solving angles
Serial.println("CalculoPierna pierna izquierda");
q[0]=atan(y/x);
q[2]=acos((x*x+y*y+z*z-l1*l1-l2*l2)/(2*l1*l2));
q[1]=atan((l2*sin(q[2]))/(l1+l2*cos(q[2])))+atan(z/sqrt(x*x+y*y));
a[0]=(int)(q[0]*180/Pi);
a[1]=(int)(q[1]*180/Pi);
a[2]=(int)(q[2]*180/Pi);
//Position to goleg servos
postogo[0]=a[0]+oq[0];
postogo[1]=a[1]+oq[1];
postogo[2]=a[2]+oq[2];
}
void rightleg(double x, double y, double z){
Serial.println("Calculateright leg");
//Solving angles
q[3]=atan(y/x);
q[5]=acos((x*x+y*y+z*z-l1*l1-l2*l2)/(2*l1*l2));
q[4]=atan((l2*sin(q[5]))/(l1+l2*cos(q[5])))+atan(z/sqrt(x*x+y*y));
a[3]=(int)(q[3]*180/Pi);
a[4]=(int)(q[4]*180/Pi);
a[5]=(int)(q[5]*180/Pi);
//Position togo leg servos
postogo[3]=a[3]+oq[3];
postogo[4]=-a[4]+oq[4];
postogo[5]=-a[5]+oq[5];
}
Servosend() functionThis function sends each servo to each ofthe positions defined between the leg and gestion functions. In order to make it visual through the movement the actioned servo is printed through the serial monitor. Note that the VarSpeedServo.h
library is used and not the common Servo.h
. This allows us to controll the speed of the servos defining a numer between 0-255 where 255 is full speed. Therefore, a speed of 4 is very low, but this lowers the shake of the robot in each movement.
void servosend(){
for (int i = 0; i <= 9; i++) //Until 9 inclusive
{servo[i].write(postogo[i],4,false); //We can set the speed of the movement.
Serial.print("Poisitioning : ");
Serial.println(i);
delay(15);
}
}
Gestion() functionThis function merges the actions of the legfunctions, takes in consideration the action to be implemented and it containsthe back and forth balance. The first variables that calls are to define theposition of each leg, being the first 3 coordintes related to the left leg andthe second group for the right leg (the order of the variables are ment so itis easier to compare the position of each feet). Next the variable “acc”, defines the action to be performedand it is related in how to position the ankles. A 0 is for leveling the robot, a 1 is for balancing over the left leg, and 2 is for balancing over the rightleg. Then the “bal” variable is forthe back and forth balance of the robot, whether the robot is leveling otbalancing it is possible to set this balance. If this variable is positive therobot leans forward, and if it’s negative it leans backwards. And at last, but not least, the “off”variable (stands for offset), is to increase or decrease the roll of the feetover the one the robot is balancing. If this variable is positive the foot roll’sso its legs withstand more weight of the robot. This variable only modifies themovement when “acc” is set to 1 or 2.
void gestion(double x1,double x2,double y1,double y2,double z1,double z2,int acc,int bal,int off){ //Variables 1 are for the left feet and 2 forthe right one.
leftleg(x1,y1,z1);
rightleg(x2,y2,z2);
switch (acc) {
case 0: //For leveling the feets
Serial.print("Leveling");
//Left Foot
postogo[6]=-a[2]+a[1]+oq[6]-bal;
postogo[7]=a[0]+oq[7];
//Right Foot
postogo[8]=a[5]-a[4]+oq[8]+bal;
postogo[9]=a[3]+oq[9];
break;
case 1: //Balance over the LEFT leg
Serial.print("Balance LEFT");
//Position to go Left Foot
postogo[6]=-a[2]+a[1]+oq[6]-bal;
postogo[7]=oq[7]+off;
//Positioning Right Foot
postogo[8]=a[5]-a[4]+oq[8]+bal;
postogo[9]=oq[9];
break;
case 2: //Balance over RIGHT leg
Serial.print("Balance RIGHT");
//Position to go Left Foot
postogo[6]=-a[2]+a[1]+oq[6]-bal;
postogo[7]=oq[7];
//Positioning Right Foot
postogo[8]=a[5]-a[4]+oq[8]+bal;
postogo[9]=oq[9]-off;
break;
}
servosend();
delay(1000);
}
Step() functionThis functions contains each of the pointsof motion defined for the robot to accomplish one step. It is intended to mimicthe movement of the human body with each step given in real life. There is adifferences between each step motion due to the position of the cables thatinfluenced in a way for one movement, but opposite for the other.
void rightstep() {
Serial.print("Right Step");
delay(1000);
gestion (8,8, 0, 0, 0,0, 0,0,0); //Depart from leveled
gestion (7,7, -1, 0, 0,0, 1,0,0); //Leveling over 1=LEFT
gestion (7,9, -2,-1, -1,1, 1,0,0);
gestion (7,9, -2,-2, -1,1, 1,5,5);
gestion (7,8, -3,-3, -1,1, 1,2,10); // Weight in one foot
gestion (7,7, -3,-3, -1,3, 1,-3,10); //Taking foot in the air tothe front
gestion (7,7, -3,-3, -1,4, 1,-3,10); //
gestion (7,9, -3,-3, 0,4, 1,-3,10);
gestion (7,9, -2,-2, 0,4, 1,0,10);
gestion (8,8, -1,-1, 0,4, 0,5,0);
gestion (8,8, -1,-1, 0,4, 2,5,5);
gestion (8,8, -1,-1, 0,4, 2,10,10); //Passing weight to legmoved
gestion (6,8, 0, 0, 4,4, 2,10,10);
gestion (8,8, 0, 0, 4,4, 2,10,10);
gestion (8,8, 0, 0, 0,0, 0,5,0);
delay(1000);
}
Comments