ROBONOVA-1 with Arduino Mega

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 leg

This 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() function

This 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() function

This 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() function

This 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);
}