Project in progress

# ROBONOVA-1 with Arduino Mega

The objective of this project is to control the ROBONOVA-1 and make a few steps whith it.

• 1,456 views
• 1 comment
• 4 respects

## Components and supplies

 Arduino Mega 2560 & Genuino Mega 2560
×1
 SG90 Micro-servo motor
×10

## Apps and online services

 Arduino IDE

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 :

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

## Code

##### Robonova-1Arduino
The most important feature is the gestion function. This one need the positions of the XYZ1 (Left) and XYZ2 (Right) leg over a refernce sistem definet from the joint of each leg and the waist of the robot, where X goes below the robot, Z goes in front of the robot, and Y looks to the right of the robot. Then "acc" can be 0 (level the robot), 1 (balance over Left foot) or 2 (balance over Right foot). Bal is the back and forth angle wanted for the robot to balance (positive front). And "off" is in case we need to adjust balance over the leg.
//Project ROBONOVA-1
#include <VarSpeedServo.h> //Ddeclare to insert Servo.h library
VarSpeedServo servo[10]; // Create an array object of class servo
float q[6];//Declaration of global variables to work with through functions in radians
int l1=6,l2=5; //Leg lengths
int a[6]; //Angles to work with through equations in degrees
int posactual[10]; //Define actual position of Servos.
int oq[10]={93,68,15,93,117,170,135,93,47,93}; //Ofsets calibration definition for each joint.
const float Pi=3.1416;

void setup() {
Serial.begin(9600); //Serial comunication with PC.
//---Attaching SERVOS---//
//LEFT Leg
servo[0].attach(5);
servo[1].attach(4);
servo[2].attach(3);
//LEFT Feet
servo[6].attach(2);
servo[7].attach(12);
//RIGHT Leg
servo[3].attach(9);
servo[4].attach(8);
servo[5].attach(7);
//RIGHT Feet
servo[8].attach(6);
servo[9].attach(11);
//Servos setting initial position
for (int i = 0; i <= 9; i++)
{
Serial.print("Setup Joint :   ");
Serial.println(i);
posactual[i]=oq[i];
}
}

///-------------------!!BUCLE PRINCIPAL!!-------------------///

void loop() {
rightstep();
leftstep();
}
}

///-------------------!!FUNCIONES!!-------------------///
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 to the 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 leg moved
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);
}

void leftstep() {
Serial.print("Left step");
delay(1000);
gestion (8,8,   0, 0,   0, 0,   0,0,0);       //Depart from leveled
gestion (7,7,   0, 0,   0, 0,   2,0,5);      //Leveling over 2=RIGHT
gestion (9,7,   1, 1,   1, 0,   2,0,5);
gestion (9,7,   3, 3,   1,-1,   2,5,10);
gestion (8,7,   3, 3,   1,-1,   2,2,10);   // Weight in one foot
gestion (7,7,   2, 3,   3,-1,   2,-3,10);  //Taking foot in the air to the front
gestion (7,7,   3, 3,   4,-1,   2,-3,10);
gestion (9,7,   3, 3,   4,-1,   2,-3,10);
gestion (9,7,   2, 2,   4,-1,   2,0,10);
gestion (7,8,   2, 0,   4, 0,   1,5,5);
gestion (8,8,   0, 0,   4, 0,   1,5,5);
gestion (7,8,   0, 0,   4, 0,   1,10,10);  //Passing weight to leg moved
gestion (8,6,   0, 0,   4, 4,   1,10,10);
gestion (8,8,   0, 0,   4, 4,   1,10,10);
gestion (8,8,   0, 0,   0, 0,   0,0,0);
delay(1000);
}
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 for the right one.
piernaIzq(x1,y1,z1);
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);
}

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

void piernaIzq(double x, double y, double z){ //,int veloc, bool wait){ Para cuando utilicemos las variables de VarSpeedServo
//Solving angles
Serial.println("Calculo Pierna 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 go leg servos
postogo[0]=a[0]+oq[0];
postogo[1]=a[1]+oq[1];
postogo[2]=a[2]+oq[2];
}

void piernaDer(double x, double y, double z){
Serial.println("Calculate right 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 to go  leg servos
postogo[3]=a[3]+oq[3];
postogo[4]=-a[4]+oq[4];
postogo[5]=-a[5]+oq[5];
}

• 0 projects
• 1 follower

May 20, 2019

#### Members who respect this project

and 3 others

See similar projects
you might like

#### Bluetooth Controlled Car with Arduino Mega

Project showcase by DANNY003

• 3,210 views
• 11 respects

Project in progress by Pigeon-Kicker

• 2,146 views
• 10 respects

#### Arduino Mega Chess

Project showcase by Sergey_Urusov

• 34,039 views
• 162 respects

#### Arduino MEGA Guitar Pedal

Project tutorial by electrosmash

• 22,081 views
• 124 respects

#### Mega Solar Tracker

Project in progress by Team Trouble

• 8,576 views
• 51 respects

#### Arduino Mega Chess II

Project showcase by Sergey_Urusov

• 5,892 views