ROBONOVA-1 with Arduino Mega

1//Project ROBONOVA-1
2#include <VarSpeedServo.h> //Ddeclare to insert  Servo.h library
3VarSpeedServo servo[10]; // Create an array object of class servo
4float  q[6];//Declaration of global variables to work with through functions in radians
5int  l1=6,l2=5; //Leg lengths
6int a[6]; //Angles to work with through equations in  degrees
7int posactual[10]; //Define actual position of Servos.
8int oq[10]={93,68,15,93,117,170,135,93,47,93};  //Ofsets calibration definition for each joint.
9const float Pi=3.1416;
10
11void  setup() {
12  Serial.begin(9600); //Serial comunication with PC.
13  //---Attaching  SERVOS---//
14    //LEFT Leg
15     servo[0].attach(5);
16     servo[1].attach(4);
17     servo[2].attach(3);
18    //LEFT Feet
19     servo[6].attach(2);
20     servo[7].attach(12);
21    //RIGHT Leg
22     servo[3].attach(9);
23     servo[4].attach(8);
24     servo[5].attach(7);
25    //RIGHT Feet
26     servo[8].attach(6);
27     servo[9].attach(11);
28  //Servos  setting initial position
29   for (int i = 0; i <= 9; i++)
30   {
31     Serial.print("Setup  Joint :   ");
32     Serial.println(i);
33     posactual[i]=oq[i];
34   }
35}
36
37///-------------------!!BUCLE  PRINCIPAL!!-------------------///
38
39void loop() {
40  if (Serial.read() >=  0) {
41      rightstep();
42      leftstep();
43  }
44}
45
46///-------------------!!FUNCIONES!!-------------------///
47void  rightstep() {
48Serial.print("Right Step");
49  delay(1000);
50     gestion  (8,8,   0, 0,   0,0,  0,0,0);       //Depart from leveled
51     gestion (7,7,  -1, 0,   0,0,  1,0,0);      //Leveling over 1=LEFT
52     gestion (7,9,  -2,-1,  -1,1,  1,0,0);    
53     gestion (7,9,  -2,-2,  -1,1,  1,5,5);    
54     gestion  (7,8,  -3,-3,  -1,1,  1,2,10);   // Weight in one foot
55     gestion (7,7,  -3,-3,  -1,3,  1,-3,10);  //Taking foot in the air to the front
56     gestion (7,7,  -3,-3,  -1,4,  1,-3,10);  //
57     gestion (7,9,  -3,-3,   0,4,  1,-3,10);  
58     gestion (7,9,  -2,-2,   0,4,  1,0,10);   
59     gestion (8,8,  -1,-1,   0,4,  0,5,0);   
60     gestion (8,8,  -1,-1,   0,4,  2,5,5);    
61     gestion (8,8,  -1,-1,   0,4,  2,10,10);     //Passing weight to leg moved
62     gestion (6,8,   0, 0,   4,4,  2,10,10);     
63     gestion (8,8,   0, 0,   4,4,  2,10,10);  
64     gestion (8,8,   0, 0,   0,0,  0,5,0);       
65  delay(1000);
66}
67
68
69void  leftstep() {
70  Serial.print("Left step");
71  delay(1000);
72     gestion  (8,8,   0, 0,   0, 0,   0,0,0);       //Depart from leveled
73     gestion (7,7,   0, 0,   0, 0,   2,0,5);      //Leveling over 2=RIGHT
74     gestion (9,7,   1,  1,   1, 0,   2,0,5);    
75     gestion (9,7,   3, 3,   1,-1,   2,5,10);    
76     gestion (8,7,   3, 3,   1,-1,   2,2,10);   // Weight in one foot
77     gestion  (7,7,   2, 3,   3,-1,   2,-3,10);  //Taking foot in the air to the front
78     gestion  (7,7,   3, 3,   4,-1,   2,-3,10);  
79     gestion (9,7,   3, 3,   4,-1,   2,-3,10);  
80     gestion (9,7,   2, 2,   4,-1,   2,0,10);   
81     gestion (7,8,   2,  0,   4, 0,   1,5,5);   
82     gestion (8,8,   0, 0,   4, 0,   1,5,5);    
83     gestion (7,8,   0, 0,   4, 0,   1,10,10);  //Passing weight to leg moved
84     gestion (8,6,   0, 0,   4, 4,   1,10,10); 
85     gestion (8,8,   0, 0,   4,  4,   1,10,10); 
86     gestion (8,8,   0, 0,   0, 0,   0,0,0);       
87  delay(1000);
88}
89void  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.
90  piernaIzq(x1,y1,z1);
91  piernaDer(x2,y2,z2);
92  switch (acc) {
93    case 0:   //For leveling the feets
94      Serial.print("Leveling");
95      //Left Foot
96       postogo[6]=-a[2]+a[1]+oq[6]-bal;
97       postogo[7]=a[0]+oq[7];
98      //Right Foot 
99       postogo[8]=a[5]-a[4]+oq[8]+bal;
100       postogo[9]=a[3]+oq[9];
101    break;
102    case 1: //Balance over the LEFT  leg
103      Serial.print("Balance LEFT");
104      //Position to go Left Foot
105       postogo[6]=-a[2]+a[1]+oq[6]-bal;
106       postogo[7]=oq[7]+off;
107      //Positioning  Right Foot 
108       postogo[8]=a[5]-a[4]+oq[8]+bal;
109       postogo[9]=oq[9];
110    break;
111    case 2:  //Balance over RIGHT leg
112      Serial.print("Balance  RIGHT");
113      //Position to go Left Foot
114       postogo[6]=-a[2]+a[1]+oq[6]-bal;
115       postogo[7]=oq[7];
116      //Positioning Right Foot 
117       postogo[8]=a[5]-a[4]+oq[8]+bal;
118       postogo[9]=oq[9]-off;
119    break;
120  }
121  servosend();
122  delay(1000);
123}
124
125void  servosend(){
126  for (int i = 0; i <= 9; i++)  //Until 9 inclusive
127    {servo[i].write(postogo[i],4,false);  //We can set the speed of the movement.
128    Serial.print("Poisitioning :   ");
129    Serial.println(i);
130    delay(15);
131  }
132}
133
134void piernaIzq(double  x, double y, double z){ //,int veloc, bool wait){ Para cuando utilicemos las variables  de VarSpeedServo
135  //Solving angles
136  Serial.println("Calculo Pierna pierna  izquierda");
137  q[0]=atan(y/x);
138  q[2]=acos((x*x+y*y+z*z-l1*l1-l2*l2)/(2*l1*l2));
139  q[1]=atan((l2*sin(q[2]))/(l1+l2*cos(q[2])))+atan(z/sqrt(x*x+y*y));
140  a[0]=(int)(q[0]*180/Pi);
141  a[1]=(int)(q[1]*180/Pi);
142  a[2]=(int)(q[2]*180/Pi);
143  //Position to go leg  servos 
144      postogo[0]=a[0]+oq[0];
145      postogo[1]=a[1]+oq[1];
146      postogo[2]=a[2]+oq[2];
147}
148
149void  piernaDer(double x, double y, double z){
150  Serial.println("Calculate right leg");
151  //Solving angles
152  q[3]=atan(y/x);
153  q[5]=acos((x*x+y*y+z*z-l1*l1-l2*l2)/(2*l1*l2));
154  q[4]=atan((l2*sin(q[5]))/(l1+l2*cos(q[5])))+atan(z/sqrt(x*x+y*y));
155  a[3]=(int)(q[3]*180/Pi);
156  a[4]=(int)(q[4]*180/Pi);
157  a[5]=(int)(q[5]*180/Pi);
158  //Position to go  leg  servos
159      postogo[3]=a[3]+oq[3];
160      postogo[4]=-a[4]+oq[4];
161      postogo[5]=-a[5]+oq[5];
162}
163