Project in progress
ROBONOVA-1 with Arduino Mega

ROBONOVA-1 with Arduino Mega

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

  • 622 views
  • 1 comment
  • 3 respects

Components and supplies

Apps and online services

About this project

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

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() {
  if (Serial.read() >= 0) {
      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);
  piernaDer(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);
}

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

Comments

Similar projects you might like

Bluetooth controlled car with arduino mega

Project showcase by DANNY003

  • 1,709 views
  • 2 comments
  • 8 respects

Mega Bread - Atari Joystick Linking for Robotics Control

Project in progress by Pigeon-Kicker

  • 1,721 views
  • 3 comments
  • 10 respects

Arduino Mega Chess

Project showcase by

  • 29,047 views
  • 66 comments
  • 144 respects

Arduino MEGA Guitar Pedal

Project tutorial by electrosmash

  • 19,747 views
  • 2 comments
  • 114 respects

Mega Solar Tracker

Project in progress by Team Trouble

  • 5,827 views
  • 9 comments
  • 43 respects

Arduino Mega Chess II

Project showcase by

  • 4,237 views
  • 9 comments
  • 37 respects
Add projectSign up / Login