KITtyBot

1/* An IR controlled version of the KITtyBot 2.
2    It uses Arduino   Pro Mini and the PCB board designed by me (Fritzing sketch Kittybotmini.fzz)
3     It is based on the previous robots KITtyBot and KITtyBot mini using an IR remote   to control the robot
4    It uses a NEC (Adafruit) remote and the IRLib2 libraries,   see https://github.com/cyborg5/IRLib2.
5    Download IRLib2 libraries from the   repository and install them according to the instructions.
6    The general dimensions   are similar to the original KITtyBot but there is a displacment
7    between the   gamma and alfa axis of 12 mm (the servos mounted on top of each other)
8    I   have conitiously tweeked the gaits for walking and turning but I so far feel this   has given the most stable behaviour.
9    Created by Staffan Ek 2017
10*/
11
12#include   <IRLibRecv.h>
13#include <Servo.h>
14#include <IRLibDecodeBase.h> // First include   the decode base
15#include <IRLib_P01_NEC.h>  // Include only the protocol you   are using
16
17#define MY_PROTOCOL NEC //Defines the IR control (NEC)
18
19long   Previous;
20
21IRrecv My_Receiver(A1);//Receive on pin A0
22IRdecodeNEC My_Decoder;
23
24const   int servonum = 12; // The amount of servos
25
26Servo servo[servonum]; // Create   servo object
27const float servodeg0[12] = {90, 90, 90, 90, 90, 90, 90, 90, 90,   90, 90, 90};
28// Neutral positions for the servos adjusted from nominal 90 degrees   (a calibration is needed to adjust these values)
29float servodegnew[servonum];   // The desired servo position in degrees
30float servodegold[servonum]; // The   old (or current) servo position
31// Update values below to the KITtyBot mini
32const   int servodir[12] = {  +1, +1, -1, -1, -1, +1, -1, -1, -1, +1, +1, +1}; // Turning   direction (positive is servo counter-clockwise)
33const float pi = 3.1416;
34const   float alfa0 = pi / 6; // The neutral position of alfa (30 deg)
35const float beta0   = pi / 3; // The neutral position of beta (60 deg)
36const float jointlength =   50; // The length of a leg part (both have the same length)
37const float width   = 120; // The width (distance between feet in y direction, with toeout0 added)
38const   float leng = 120; // The length (disatnce between feet in x direction)
39const   float distag = 12; // Distance between alfa and gamma axis
40const float toeout0   = 20; // The outward distance of feet from the gamma servo centre (the distance   the foot is pointed outwards)
41const float leglength0 = 2 * jointlength * cos(alfa0);
42const   float gamma0 = asin(toeout0 / (leglength0 + distag)); // The neutral position of   gamma (due to toe out 20 mm and distag 12 mm)
43const float bodyradius = sqrt(pow((width   / 2), 2) + pow((leng / 2), 2)); // The length of diagonal (distance from centre   to foot corner)
44const float phi0 = atan(width / leng); // The angle of bodyradius   vs the x (forward pointing) axis
45const float height0 = sqrt(pow(leglength0 +   distag, 2) - pow(toeout0, 2)); // The normal height of robot (if any angles or distances   are changed this must be updated)
46float leglength [4] = {sqrt(pow(height0, 2)   + pow(toeout0, 2)), sqrt(pow(height0, 2) + pow(toeout0, 2)),
47                       sqrt(pow(height0,   2) + pow(toeout0, 2)), sqrt(pow(height0, 2) + pow(toeout0, 2))
48                      };
49//   Start values of leglength
50unsigned long timestep = 500; // Time taken by each   sequence (when using servomove())
51int steplength = 40; //The length of a step   in x direction during walking (forward and reverse creep)
52float phi = 20; //   turnangle during turning (in degrees, not radians!)
53// Variable for movement
54float   footpos[12]; // Foot positions, order LeftFrontxyz, LeftRearxyz, RightFrontxyz,   RightRearxyz
55float stepturn[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Foot   movement in case of a turn
56// The foot positions are calibrated with their respective   start positions
57const float jointangle0[12] = {alfa0, beta0, 0, alfa0, beta0,   0, alfa0, beta0, 0, alfa0, beta0, 0};
58float jointangle[12]; //Using a vector   for angles, order LeftFrontAlfaBetaGamma etc
59
60const int voltagepin = A0; //   The assigned pin for voltage meassure
61int lowvolt = 0; // A variable that stops   the robot if the voltage goew <7.0 V
62
63int mode = 0; // The current ordered   walking mode; forward, reverse, left, right
64
65
66void setup() {
67  Serial.begin(9600);
68   Serial.println("KITtyBot mini"); //These lines are just to check the configuration.   Can be deleted.
69  Serial.print("Gamma0: ");
70  Serial.println(gamma0);
71   Serial.print("Leglength0: ");
72  Serial.println(leglength0);
73  Serial.print("Bodyradius:   ");
74  Serial.println(bodyradius);
75  Serial.print("Phi0: ");
76  Serial.println(phi0);
77   Serial.print("Height0: ");
78  Serial.println(height0);
79
80  servo[0].attach(3);
81   servo[1].attach(4);
82  servo[2].attach(5);
83  servo[3].attach(6);
84  servo[4].attach(7);
85   servo[5].attach(8);
86  servo[6].attach(2);
87  servo[7].attach(A3);
88  servo[8].attach(12);
89   servo[9].attach(11);
90  servo[10].attach(10);
91  servo[11].attach(9);
92
93   for (int i = 0; i < servonum; i++) { // Centre all values and the output to the   serovs
94    servodegnew[i] = servodeg0[i];
95    servodegold[i] = servodegnew[i];
96     servo[i].write(servodegnew[i]);
97  }
98  delay(5000);
99  My_Receiver.enableIRIn();   // Start the receiver
100
101}
102
103void loop() {
104  voltmeasure(); // Check   voltage at least here
105  while (lowvolt == 0) { // Proceed only if there is enough   power
106    bodyxyz(0, 0, 0); // Just make sure everything is centered
107    servomove();
108     mode = 0;
109    IRread();
110    Serial.print("Mode: "); // Only for monitoring   in the serial console, can be deleted
111    Serial.println(mode);
112    switch   (mode) {
113      case 1: forwardcreep(); break;
114      case 2: reversecreep();   break;
115      case 3: leftturn(); break;
116      case 4: rightturn(); break;
117     }
118    voltmeasure();
119    delay(500);
120  }
121
122  if (lowvolt == 1)   { // Got to "rest". A clear signal that battery needs charging
123    bodyxyz   (0, 0, -30); // Lower body, a clear signal that it has gone to rest
124    servomove();
125   }
126}
127
128// Below are the functions called in correct order in order to calculate   new angles
129void lengthangles() {
130  // Front left foot
131  jointangle[2] =   gammaleft(footpos[1], footpos[2]);
132  leglength[0] = legleft(footpos[0], footpos[2],   jointangle[2]);
133  jointangle[1] = beta(leglength[0]);
134  jointangle[0] = alfafront(footpos[0],   jointangle[1], leglength[0]);
135  // Rear left foot
136  jointangle[5] = gammaleft(footpos[4],   footpos[5]);
137  leglength[1] = legleft(footpos[3], footpos[5], jointangle[5]);
138   jointangle[4] = beta(leglength[1]);
139  jointangle[3] = alfarear(footpos[3],   jointangle[4], leglength[1]);
140  // Front rigth foot
141  jointangle[8] = gammaright(footpos[7],   footpos[8]);
142  leglength[2] = legright(footpos[6], footpos[8], jointangle[8]);
143   jointangle[7] = beta(leglength[2]);
144  jointangle[6] = alfafront(footpos[6],   jointangle[7], leglength[2]);
145  // Rear right foot
146  jointangle[11] = gammaright(footpos[10],   footpos[11]);
147  leglength[3] = legright(footpos[9], footpos[11], jointangle[11]);
148   jointangle[10] = beta(leglength[3]);
149  jointangle[9] = alfarear(footpos[9],   jointangle[10], leglength[3]);
150}
151
152// Functions used to calculate IK
153
154//   Gamma, the hip servo "on top"
155float gammaleft (float dy, float dz) {
156  float   gresult = atan((toeout0 + dy) / (height0 - dz)) - gamma0;
157  return gresult;
158}
159
160float   gammaright(float dy, float dz) {
161  float gresult = gamma0 - atan((toeout0 - dy)   / (height0 - dz));
162  return gresult;
163}
164
165//Calculating leg length (distance   alfa axis to toe)
166float legleft(float dx, float dz, float gamma) {
167  float   lresult = sqrt(pow(leglength0 - (dz / cos(gamma0 + gamma)), 2) + pow(dx, 2));
168   if (lresult > 2 * jointlength) lresult = 2 * jointlength; // If leglength is higher   than possible some following functions become unstable
169  return lresult;
170}
171
172float   legright(float dx, float dz, float gamma) {
173  float lresult = sqrt(pow(leglength0   - (dz / cos(gamma0 - gamma)), 2) + pow(dx, 2));
174  if (lresult > 2 * jointlength)   lresult = 2 * jointlength; // If leglength is higher than possible some following   functions become unstable
175  return lresult;
176}
177
178// Beta, the "knee joint"
179float   beta(float leg) {
180  float bresult = 2 * acos(leg / (2 * jointlength));
181  return   bresult;
182}
183
184// Alfa, The other hip servo
185float alfafront(float dx, float   beta, float leg) {
186  float aresult = (beta / 2) - asin(dx / leg);
187  return   aresult;
188}
189
190float alfarear(float dx, float beta, float leg) {
191  float   aresult = (beta / 2) + asin(dx / leg);
192  return aresult;
193}
194
195// Giving   foot positions based on a turning angle f (in degrees). Stepturn is the used to   make footpos values
196void turnpos(float f) {
197  stepturn[0] = bodyradius * cos(phi0   + (f * pi / 180)) - leng / 2;
198  stepturn[1] = bodyradius * sin(phi0 + (f * pi   / 180)) - width / 2;
199  stepturn[3] = bodyradius * cos(pi - phi0 + (f * pi / 180))   + leng / 2;
200  stepturn[4] = bodyradius * sin(pi - phi0 + (f * pi / 180)) - width   / 2;
201  stepturn[6] = bodyradius * cos(2 * pi - phi0 + (f * pi / 180)) - leng   / 2;
202  stepturn[7] = bodyradius * sin(2 * pi - phi0 + (f * pi / 180)) + width   / 2;
203  stepturn[9] = bodyradius * cos(pi + phi0 + (f * pi / 180)) + leng / 2;
204   stepturn[10] = bodyradius * sin(pi + phi0 + (f * pi / 180)) + width / 2;
205}
206
207//   Calculates servo positions (in degrees) based on joint angles in the fucntion above
208void   servopos() {
209  for (int i = 0; i < 12; i++) {
210    servodegnew[i] = servodeg0[i]   + servodir[i] * (jointangle[i] - jointangle0[i]) * 180 / pi;
211  }
212}
213
214//   The servo algorithm for controlled and syncronized movements. All servos should   reach their end position at the end of a timestep
215void servomove() {
216  int   servotimeold[servonum]; // Local variable for time of last servo position
217  int   servotimenew[servonum]; // Local variable for the current time when the servo i   positioned
218  int SERVOPULSE[servonum]; // Local variable to write to the servo   driver
219  float servodeg[servonum]; // Local variable for the current servo position
220   float servodegspeed[servonum]; // Local variable for the desired servo speed degress   per millisecond
221  unsigned long starttime = millis(); // Time stamp the start   of the algorithm
222  unsigned long timenow = starttime; // Resetting time now
223   for (int i = 0; i < servonum; i++) {
224    servodegspeed[i] = (servodegnew[i]   - servodegold[i]) / timestep; // Calculate the desired servo speed
225    servodeg[i]   = servodegold[i]; // Resetting the servo position
226    servotimeold[i] = starttime;   // Resetting the time
227  }
228  while ((timenow - starttime) < timestep) { //   Loop continues until the time step is fulfilled
229    for (int i = 0; i < servonum;   i++) { // Iterate through each servo
230      servotimenew[i] = millis(); // Get   a time stamp
231      servodeg[i] += servodegspeed[i] * (servotimenew[i] - servotimeold[i]);
232       // Calculate a new position based on the desired speed and elapsed time
233       servo[i].write(servodeg[i]); // Position servo
234      servotimeold[i] =   servotimenew[i]; // Resetting the old servo time for the next iteration
235    }
236     timenow = millis();
237    // Get a time stamp after all servos has been iterated   to use in the while case.
238  }
239  for (int i = 0; i < servonum; i++) { // Make   on last iteration to assure that the servos reached their end positions
240    servo[i].write(servodegnew[i]);   // Position servo
241    servodegold[i] = servodegnew[i]; // Resetting the current   position for future iterations
242  }
243}
244
245// A servomove without timing,   use when no synchronous moving is needed, i.e. lifting/moving one leg
246void servomovefast()   {
247  for (int i = 0; i < servonum; i++) { // Make on last iteration to assure   that the servos reached their end positions
248    servo[i].write(servodegnew[i]);   // Position servo
249    servodegold[i] = servodegnew[i]; // Resetting the current   position for future iterations
250  }
251  delay(100); // Just give a reasonable   time for servos to reach endpoint before moving on.
252}
253
254// Calculates a   foot position (xyz coordiantes)
255void footxyz(int i, float x, float y, float z)   {
256  footpos[3 * i] = x;
257  footpos[3 * i + 1] = y;
258  footpos[3 * i + 2]   = z;
259  lengthangles();
260  servopos();
261}
262
263// Calculates foot movement,   adding desired value to current position
264void footmovexyz(int i, float x, float   y, float z) {
265  footpos[3 * i] += x;
266  footpos[3 * i + 1] += y;
267  footpos[3   * i + 2] += z;
268  lengthangles();
269  servopos();
270}
271
272// Calculates body   positioning according to xyz coordinates.
273void bodyxyz(float x, float y, float   z ) {
274  //Note: Moving body means moving the feet in the other direction, hence   minus signs in all foot positions
275  for (int i = 0; i < 4; i++) {
276    footpos[3   * i] = -x;
277    footpos[3 * i + 1] = -y;
278    footpos[3 * i + 2] = -z;
279  }
280   lengthangles();
281  servopos();
282}
283
284// Calculates body movement, adding   cooridinate to existing position.
285void bodymovexyz(float x, float y, float z   ) {
286  //Note: Body move mean moving the feet in the other direction, hence minus   signs in all foot positions
287  for (int i = 0; i < 4; i++) {
288    footpos[3   * i] -= x;
289    footpos[3 * i + 1] -= y;
290    footpos[3 * i + 2] -= z;
291  }
292   lengthangles();
293  servopos();
294}
295
296// Calculates a twist on the body   the desired angle phi
297void bodytwist(float f) {
298  // Again here the movement   is in minus driection from the foot positions
299  turnpos(-f);
300  for (int i   = 0; i < 12; i++) {
301    footpos[i] += stepturn[i];
302  }
303  lengthangles();
304   servopos();
305}
306
307// Does a footmovement; lifts move xy and puts down foot
308void   footstep (int i, float x, float y) {
309  footmovexyz(i, 0, 0, 30);
310  servomovefast();
311   footmovexyz(i, x, y, 0);
312  servomovefast();
313  footmovexyz(i, 0, 0, -30);
314   servomovefast();
315}
316
317// Does a footmovement based on the disired turning   angle, moves the foot along the turning arc
318void (footstepangle(int i, float   f)) {
319  turnpos(f);
320  footmovexyz(i, 0, 0, 30);
321  servomovefast();
322   footmovexyz(i, stepturn[3 * i], stepturn [3 * i + 1], 0);
323  servomovefast();
324   footmovexyz(i, 0, 0, -30);
325  servomovefast();
326}
327
328// Checks voltage,   in case of low battery lowvolt variable changes
329void voltmeasure() {
330  /*   Note: The 7.6 V battery is conneced via a 2.2k resistance from BAT to voltagepin   and 1.0k to GND
331    This gives the 5 V analog input a 16 volt measure range*/
332   float voltsig = analogRead(voltagepin);
333  float voltage = voltsig * 16 / 1023.0;
334   Serial.print("Battery: ");
335  Serial.println(voltage);
336  if (voltage <   7.0) {
337    lowvolt = 1;
338  }
339  else {
340    lowvolt = 0;
341  }
342}
343
344//   The IR read function, based on Adafruits example sketch
345void IRread() {
346  if   (My_Receiver.getResults()) {
347    Serial.print("Recieving ");
348    Serial.println   (My_Decoder.value);
349    if (My_Decoder.decode()) {
350      if (My_Decoder.value   == 0xFFFFFFFF) { // Detects if the button is still pressed and keeps the value
351         My_Decoder.value = Previous;
352      }
353      switch (My_Decoder.value)   { //Detects if an arrow button is pressed and sets mode parameter
354        case   0xfda05f:   mode = 1; break;
355        case 0xfdb04f:   mode = 2; break;
356        case   0xfd10ef:   mode = 3; break;
357        case 0xfd50af:   mode = 4; break;
358      }
359       Previous = My_Decoder.value;
360    }
361    My_Receiver.enableIRIn();
362   }
363  else {
364    mode = 0;
365  }
366}
367
368// A gait for forward creeping
369void   forwardcreep() {
370  bodymovexyz(steplength / 4, -toeout0, 0); // Starts to position   for forward walking, leaning to the right
371  servomove();
372  footstep(1, steplength   / 2, 0); // Moving rear left leg one half step length
373  footstep(0, steplength   / 2, 0); // And the front left
374  bodymovexyz(steplength / 4, 2 * toeout0, 0);   // Shifting body forward and to the left (in order to move the right feet later)
375   servomove();
376  while (mode == 1) {
377    // Here the while loop starts, repeaetd   as long as fwd is ordered (mode 1)
378    footstep(3, steplength, 0); // Moving   rear right forward
379    footstep(2, steplength, 0); // Moving front right forward
380     bodymovexyz(steplength / 2, -2 * toeout0, 0); // Shifting body forward and to   the right
381    servomove();
382    footstep(1, steplength, 0); // Moving rear   left forward
383    footstep(0, steplength, 0); // Moving front left forward
384     bodymovexyz(steplength / 2, 2 * toeout0, 0); // Shifting body forward and to   the left
385    servomove();
386    // The robot has the same position as before   the while loop but has moved on steplength forward.
387    IRread(); // If there   is still a forward command (mode ==1) the sequence should be repeated
388  }
389   // The while loop ends and it assumes normal postion
390  /* bodymovexyz(0, 10,   0);*/
391  footstep(3, steplength / 2, 0); // Taking half steps to make all legs   neutral
392  footstep(2, steplength / 2, 0);
393  bodyxyz(0, 0, 0); // Centering   body
394  servomove();
395  // Leaving gait mode
396}
397
398// A similar gait for   reverse walking (not commented as much look at forwardcreep
399void reversecreep()   {
400  bodymovexyz(-steplength / 4, -toeout0, 0); // Starts to position for forward   walking
401  servomove();
402  footstep(0, -steplength / 2, 0);
403  footstep(1,   -steplength / 2, 0);
404  bodymovexyz(-steplength / 4, 2 * toeout0, 0);
405  servomove();
406   while (mode == 2) {
407    // Here the while loop starts, repeaetd as long as   reverse is ordered (mode 2)
408    footstep(2, -steplength, 0);
409    footstep(3,   -steplength, 0);
410    bodymovexyz(-steplength / 2, -2 * toeout0, 0);
411    servomove();
412     footstep(0, -steplength, 0);
413    footstep(1, -steplength, 0);
414    bodymovexyz(-steplength   / 2, 2 * toeout0, 0);
415    servomove();
416    IRread(); // If mode == 2 the while   loop continues
417  }
418  // The while loop ends and it assumes normal postion
419   /*  bodymovexyz(0, 10, 0);*/
420  footstep(2, -steplength / 2, 0);
421  footstep(3,   -steplength / 2, 0);
422  bodyxyz(0, 0, 0);
423  servomove();
424  // Leaving gait   mode
425}
426
427// Doing a turn to the left the desired phi angle
428void leftturn()   {
429  while (mode == 3) {
430    // While loop as long as the left button is pressed
431     bodyxyz(toeout0 / 2, toeout0, 0); // Lean left before doing anything
432    servomove();
433     footstepangle(3, phi); // Move rear right foot into new position
434    footstepangle(2,   phi); // Move front right foot into new position
435    footxyz(0, -toeout0 / 2   - stepturn[0], toeout0 - stepturn[1], 0);
436    footxyz(1, -toeout0 / 2 - stepturn[3],   toeout0 - stepturn[4], 0);
437    footxyz(2, -toeout0 / 2, toeout0, 0);
438    footxyz(3,   -toeout0 / 2, toeout0, 0);
439    // Twisting body and lean left. Written in absolute   coordinates to minmize errors.
440    servomove(); // Do the actual servo command
441     footstepangle(0, phi); // Move front left foot
442    footstepangle(1, phi);   // Move rear left foot
443    IRread(); // Check is left button is still pressed   (mode == 3), repeat while loop
444  }
445  bodyxyz(0, 0, 0); // Centre body when   turning is finished
446  servomove();
447}
448
449//Doing a right turn. Should be   identical to left turn but with different commands. Testing both at the moment.
450void   rightturn() {
451  while (mode == 4) {
452    // While loop as long as the right   button is pressed
453    bodyxyz(-toeout0 / 2, toeout0, 0); // Lean left before   doing anything
454    servomove();
455    footstepangle(2, -phi); //Move front right   foot
456    footstepangle(3, -phi); //Move rear right foot
457    footxyz(0, toeout0   / 2 - stepturn[0], toeout0 - stepturn[1], 0);
458    footxyz(1, toeout0 / 2 - stepturn[3],   toeout0 - stepturn[4], 0);
459    footxyz(2, toeout0 / 2, toeout0, 0);
460    footxyz(3,   toeout0 / 2, toeout0, 0);
461    // Twisting body and lean left. Written in absolute   coordinates to minmize errors.
462    servomove(); // Do the actual servo command
463     footstepangle(1, -phi); //Move rear left foot
464    footstepangle(0, -phi);   //Move front left foot
465    IRread(); // Check is rightt button is still pressed   (mode == 4), repeat while loop
466  }
467  bodyxyz(0, 0, 0);
468  servomove();
469}
470

1/* An IR controlled version of the KITtyBot 2.
2    It uses Arduino  Pro Mini and the PCB board designed by me (Fritzing sketch Kittybotmini.fzz)
3    It is based on the previous robots KITtyBot and KITtyBot mini using an IR remote  to control the robot
4    It uses a NEC (Adafruit) remote and the IRLib2 libraries,  see https://github.com/cyborg5/IRLib2.
5    Download IRLib2 libraries from the  repository and install them according to the instructions.
6    The general dimensions  are similar to the original KITtyBot but there is a displacment
7    between the  gamma and alfa axis of 12 mm (the servos mounted on top of each other)
8    I  have conitiously tweeked the gaits for walking and turning but I so far feel this  has given the most stable behaviour.
9    Created by Staffan Ek 2017
10*/
11
12#include  <IRLibRecv.h>
13#include <Servo.h>
14#include <IRLibDecodeBase.h> // First include  the decode base
15#include <IRLib_P01_NEC.h>  // Include only the protocol you  are using
16
17#define MY_PROTOCOL NEC //Defines the IR control (NEC)
18
19long  Previous;
20
21IRrecv My_Receiver(A1);//Receive on pin A0
22IRdecodeNEC My_Decoder;
23
24const  int servonum = 12; // The amount of servos
25
26Servo servo[servonum]; // Create  servo object
27const float servodeg0[12] = {90, 90, 90, 90, 90, 90, 90, 90, 90,  90, 90, 90};
28// Neutral positions for the servos adjusted from nominal 90 degrees  (a calibration is needed to adjust these values)
29float servodegnew[servonum];  // The desired servo position in degrees
30float servodegold[servonum]; // The  old (or current) servo position
31// Update values below to the KITtyBot mini
32const  int servodir[12] = {  +1, +1, -1, -1, -1, +1, -1, -1, -1, +1, +1, +1}; // Turning  direction (positive is servo counter-clockwise)
33const float pi = 3.1416;
34const  float alfa0 = pi / 6; // The neutral position of alfa (30 deg)
35const float beta0  = pi / 3; // The neutral position of beta (60 deg)
36const float jointlength =  50; // The length of a leg part (both have the same length)
37const float width  = 120; // The width (distance between feet in y direction, with toeout0 added)
38const  float leng = 120; // The length (disatnce between feet in x direction)
39const  float distag = 12; // Distance between alfa and gamma axis
40const float toeout0  = 20; // The outward distance of feet from the gamma servo centre (the distance  the foot is pointed outwards)
41const float leglength0 = 2 * jointlength * cos(alfa0);
42const  float gamma0 = asin(toeout0 / (leglength0 + distag)); // The neutral position of  gamma (due to toe out 20 mm and distag 12 mm)
43const float bodyradius = sqrt(pow((width  / 2), 2) + pow((leng / 2), 2)); // The length of diagonal (distance from centre  to foot corner)
44const float phi0 = atan(width / leng); // The angle of bodyradius  vs the x (forward pointing) axis
45const float height0 = sqrt(pow(leglength0 +  distag, 2) - pow(toeout0, 2)); // The normal height of robot (if any angles or distances  are changed this must be updated)
46float leglength [4] = {sqrt(pow(height0, 2)  + pow(toeout0, 2)), sqrt(pow(height0, 2) + pow(toeout0, 2)),
47                       sqrt(pow(height0,  2) + pow(toeout0, 2)), sqrt(pow(height0, 2) + pow(toeout0, 2))
48                      };
49//  Start values of leglength
50unsigned long timestep = 500; // Time taken by each  sequence (when using servomove())
51int steplength = 40; //The length of a step  in x direction during walking (forward and reverse creep)
52float phi = 20; //  turnangle during turning (in degrees, not radians!)
53// Variable for movement
54float  footpos[12]; // Foot positions, order LeftFrontxyz, LeftRearxyz, RightFrontxyz,  RightRearxyz
55float stepturn[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Foot  movement in case of a turn
56// The foot positions are calibrated with their respective  start positions
57const float jointangle0[12] = {alfa0, beta0, 0, alfa0, beta0,  0, alfa0, beta0, 0, alfa0, beta0, 0};
58float jointangle[12]; //Using a vector  for angles, order LeftFrontAlfaBetaGamma etc
59
60const int voltagepin = A0; //  The assigned pin for voltage meassure
61int lowvolt = 0; // A variable that stops  the robot if the voltage goew <7.0 V
62
63int mode = 0; // The current ordered  walking mode; forward, reverse, left, right
64
65
66void setup() {
67  Serial.begin(9600);
68  Serial.println("KITtyBot mini"); //These lines are just to check the configuration.  Can be deleted.
69  Serial.print("Gamma0: ");
70  Serial.println(gamma0);
71  Serial.print("Leglength0: ");
72  Serial.println(leglength0);
73  Serial.print("Bodyradius:  ");
74  Serial.println(bodyradius);
75  Serial.print("Phi0: ");
76  Serial.println(phi0);
77  Serial.print("Height0: ");
78  Serial.println(height0);
79
80  servo[0].attach(3);
81  servo[1].attach(4);
82  servo[2].attach(5);
83  servo[3].attach(6);
84  servo[4].attach(7);
85  servo[5].attach(8);
86  servo[6].attach(2);
87  servo[7].attach(A3);
88  servo[8].attach(12);
89  servo[9].attach(11);
90  servo[10].attach(10);
91  servo[11].attach(9);
92
93  for (int i = 0; i < servonum; i++) { // Centre all values and the output to the  serovs
94    servodegnew[i] = servodeg0[i];
95    servodegold[i] = servodegnew[i];
96    servo[i].write(servodegnew[i]);
97  }
98  delay(5000);
99  My_Receiver.enableIRIn();  // Start the receiver
100
101}
102
103void loop() {
104  voltmeasure(); // Check  voltage at least here
105  while (lowvolt == 0) { // Proceed only if there is enough  power
106    bodyxyz(0, 0, 0); // Just make sure everything is centered
107    servomove();
108    mode = 0;
109    IRread();
110    Serial.print("Mode: "); // Only for monitoring  in the serial console, can be deleted
111    Serial.println(mode);
112    switch  (mode) {
113      case 1: forwardcreep(); break;
114      case 2: reversecreep();  break;
115      case 3: leftturn(); break;
116      case 4: rightturn(); break;
117    }
118    voltmeasure();
119    delay(500);
120  }
121
122  if (lowvolt == 1)  { // Got to "rest". A clear signal that battery needs charging
123    bodyxyz  (0, 0, -30); // Lower body, a clear signal that it has gone to rest
124    servomove();
125  }
126}
127
128// Below are the functions called in correct order in order to calculate  new angles
129void lengthangles() {
130  // Front left foot
131  jointangle[2] =  gammaleft(footpos[1], footpos[2]);
132  leglength[0] = legleft(footpos[0], footpos[2],  jointangle[2]);
133  jointangle[1] = beta(leglength[0]);
134  jointangle[0] = alfafront(footpos[0],  jointangle[1], leglength[0]);
135  // Rear left foot
136  jointangle[5] = gammaleft(footpos[4],  footpos[5]);
137  leglength[1] = legleft(footpos[3], footpos[5], jointangle[5]);
138  jointangle[4] = beta(leglength[1]);
139  jointangle[3] = alfarear(footpos[3],  jointangle[4], leglength[1]);
140  // Front rigth foot
141  jointangle[8] = gammaright(footpos[7],  footpos[8]);
142  leglength[2] = legright(footpos[6], footpos[8], jointangle[8]);
143  jointangle[7] = beta(leglength[2]);
144  jointangle[6] = alfafront(footpos[6],  jointangle[7], leglength[2]);
145  // Rear right foot
146  jointangle[11] = gammaright(footpos[10],  footpos[11]);
147  leglength[3] = legright(footpos[9], footpos[11], jointangle[11]);
148  jointangle[10] = beta(leglength[3]);
149  jointangle[9] = alfarear(footpos[9],  jointangle[10], leglength[3]);
150}
151
152// Functions used to calculate IK
153
154//  Gamma, the hip servo "on top"
155float gammaleft (float dy, float dz) {
156  float  gresult = atan((toeout0 + dy) / (height0 - dz)) - gamma0;
157  return gresult;
158}
159
160float  gammaright(float dy, float dz) {
161  float gresult = gamma0 - atan((toeout0 - dy)  / (height0 - dz));
162  return gresult;
163}
164
165//Calculating leg length (distance  alfa axis to toe)
166float legleft(float dx, float dz, float gamma) {
167  float  lresult = sqrt(pow(leglength0 - (dz / cos(gamma0 + gamma)), 2) + pow(dx, 2));
168  if (lresult > 2 * jointlength) lresult = 2 * jointlength; // If leglength is higher  than possible some following functions become unstable
169  return lresult;
170}
171
172float  legright(float dx, float dz, float gamma) {
173  float lresult = sqrt(pow(leglength0  - (dz / cos(gamma0 - gamma)), 2) + pow(dx, 2));
174  if (lresult > 2 * jointlength)  lresult = 2 * jointlength; // If leglength is higher than possible some following  functions become unstable
175  return lresult;
176}
177
178// Beta, the "knee joint"
179float  beta(float leg) {
180  float bresult = 2 * acos(leg / (2 * jointlength));
181  return  bresult;
182}
183
184// Alfa, The other hip servo
185float alfafront(float dx, float  beta, float leg) {
186  float aresult = (beta / 2) - asin(dx / leg);
187  return  aresult;
188}
189
190float alfarear(float dx, float beta, float leg) {
191  float  aresult = (beta / 2) + asin(dx / leg);
192  return aresult;
193}
194
195// Giving  foot positions based on a turning angle f (in degrees). Stepturn is the used to  make footpos values
196void turnpos(float f) {
197  stepturn[0] = bodyradius * cos(phi0  + (f * pi / 180)) - leng / 2;
198  stepturn[1] = bodyradius * sin(phi0 + (f * pi  / 180)) - width / 2;
199  stepturn[3] = bodyradius * cos(pi - phi0 + (f * pi / 180))  + leng / 2;
200  stepturn[4] = bodyradius * sin(pi - phi0 + (f * pi / 180)) - width  / 2;
201  stepturn[6] = bodyradius * cos(2 * pi - phi0 + (f * pi / 180)) - leng  / 2;
202  stepturn[7] = bodyradius * sin(2 * pi - phi0 + (f * pi / 180)) + width  / 2;
203  stepturn[9] = bodyradius * cos(pi + phi0 + (f * pi / 180)) + leng / 2;
204  stepturn[10] = bodyradius * sin(pi + phi0 + (f * pi / 180)) + width / 2;
205}
206
207//  Calculates servo positions (in degrees) based on joint angles in the fucntion above
208void  servopos() {
209  for (int i = 0; i < 12; i++) {
210    servodegnew[i] = servodeg0[i]  + servodir[i] * (jointangle[i] - jointangle0[i]) * 180 / pi;
211  }
212}
213
214//  The servo algorithm for controlled and syncronized movements. All servos should  reach their end position at the end of a timestep
215void servomove() {
216  int  servotimeold[servonum]; // Local variable for time of last servo position
217  int  servotimenew[servonum]; // Local variable for the current time when the servo i  positioned
218  int SERVOPULSE[servonum]; // Local variable to write to the servo  driver
219  float servodeg[servonum]; // Local variable for the current servo position
220  float servodegspeed[servonum]; // Local variable for the desired servo speed degress  per millisecond
221  unsigned long starttime = millis(); // Time stamp the start  of the algorithm
222  unsigned long timenow = starttime; // Resetting time now
223  for (int i = 0; i < servonum; i++) {
224    servodegspeed[i] = (servodegnew[i]  - servodegold[i]) / timestep; // Calculate the desired servo speed
225    servodeg[i]  = servodegold[i]; // Resetting the servo position
226    servotimeold[i] = starttime;  // Resetting the time
227  }
228  while ((timenow - starttime) < timestep) { //  Loop continues until the time step is fulfilled
229    for (int i = 0; i < servonum;  i++) { // Iterate through each servo
230      servotimenew[i] = millis(); // Get  a time stamp
231      servodeg[i] += servodegspeed[i] * (servotimenew[i] - servotimeold[i]);
232      // Calculate a new position based on the desired speed and elapsed time
233      servo[i].write(servodeg[i]); // Position servo
234      servotimeold[i] =  servotimenew[i]; // Resetting the old servo time for the next iteration
235    }
236    timenow = millis();
237    // Get a time stamp after all servos has been iterated  to use in the while case.
238  }
239  for (int i = 0; i < servonum; i++) { // Make  on last iteration to assure that the servos reached their end positions
240    servo[i].write(servodegnew[i]);  // Position servo
241    servodegold[i] = servodegnew[i]; // Resetting the current  position for future iterations
242  }
243}
244
245// A servomove without timing,  use when no synchronous moving is needed, i.e. lifting/moving one leg
246void servomovefast()  {
247  for (int i = 0; i < servonum; i++) { // Make on last iteration to assure  that the servos reached their end positions
248    servo[i].write(servodegnew[i]);  // Position servo
249    servodegold[i] = servodegnew[i]; // Resetting the current  position for future iterations
250  }
251  delay(100); // Just give a reasonable  time for servos to reach endpoint before moving on.
252}
253
254// Calculates a  foot position (xyz coordiantes)
255void footxyz(int i, float x, float y, float z)  {
256  footpos[3 * i] = x;
257  footpos[3 * i + 1] = y;
258  footpos[3 * i + 2]  = z;
259  lengthangles();
260  servopos();
261}
262
263// Calculates foot movement,  adding desired value to current position
264void footmovexyz(int i, float x, float  y, float z) {
265  footpos[3 * i] += x;
266  footpos[3 * i + 1] += y;
267  footpos[3  * i + 2] += z;
268  lengthangles();
269  servopos();
270}
271
272// Calculates body  positioning according to xyz coordinates.
273void bodyxyz(float x, float y, float  z ) {
274  //Note: Moving body means moving the feet in the other direction, hence  minus signs in all foot positions
275  for (int i = 0; i < 4; i++) {
276    footpos[3  * i] = -x;
277    footpos[3 * i + 1] = -y;
278    footpos[3 * i + 2] = -z;
279  }
280  lengthangles();
281  servopos();
282}
283
284// Calculates body movement, adding  cooridinate to existing position.
285void bodymovexyz(float x, float y, float z  ) {
286  //Note: Body move mean moving the feet in the other direction, hence minus  signs in all foot positions
287  for (int i = 0; i < 4; i++) {
288    footpos[3  * i] -= x;
289    footpos[3 * i + 1] -= y;
290    footpos[3 * i + 2] -= z;
291  }
292  lengthangles();
293  servopos();
294}
295
296// Calculates a twist on the body  the desired angle phi
297void bodytwist(float f) {
298  // Again here the movement  is in minus driection from the foot positions
299  turnpos(-f);
300  for (int i  = 0; i < 12; i++) {
301    footpos[i] += stepturn[i];
302  }
303  lengthangles();
304  servopos();
305}
306
307// Does a footmovement; lifts move xy and puts down foot
308void  footstep (int i, float x, float y) {
309  footmovexyz(i, 0, 0, 30);
310  servomovefast();
311  footmovexyz(i, x, y, 0);
312  servomovefast();
313  footmovexyz(i, 0, 0, -30);
314  servomovefast();
315}
316
317// Does a footmovement based on the disired turning  angle, moves the foot along the turning arc
318void (footstepangle(int i, float  f)) {
319  turnpos(f);
320  footmovexyz(i, 0, 0, 30);
321  servomovefast();
322  footmovexyz(i, stepturn[3 * i], stepturn [3 * i + 1], 0);
323  servomovefast();
324  footmovexyz(i, 0, 0, -30);
325  servomovefast();
326}
327
328// Checks voltage,  in case of low battery lowvolt variable changes
329void voltmeasure() {
330  /*  Note: The 7.6 V battery is conneced via a 2.2k resistance from BAT to voltagepin  and 1.0k to GND
331    This gives the 5 V analog input a 16 volt measure range*/
332  float voltsig = analogRead(voltagepin);
333  float voltage = voltsig * 16 / 1023.0;
334  Serial.print("Battery: ");
335  Serial.println(voltage);
336  if (voltage <  7.0) {
337    lowvolt = 1;
338  }
339  else {
340    lowvolt = 0;
341  }
342}
343
344//  The IR read function, based on Adafruits example sketch
345void IRread() {
346  if  (My_Receiver.getResults()) {
347    Serial.print("Recieving ");
348    Serial.println  (My_Decoder.value);
349    if (My_Decoder.decode()) {
350      if (My_Decoder.value  == 0xFFFFFFFF) { // Detects if the button is still pressed and keeps the value
351        My_Decoder.value = Previous;
352      }
353      switch (My_Decoder.value)  { //Detects if an arrow button is pressed and sets mode parameter
354        case  0xfda05f:   mode = 1; break;
355        case 0xfdb04f:   mode = 2; break;
356        case  0xfd10ef:   mode = 3; break;
357        case 0xfd50af:   mode = 4; break;
358      }
359      Previous = My_Decoder.value;
360    }
361    My_Receiver.enableIRIn();
362  }
363  else {
364    mode = 0;
365  }
366}
367
368// A gait for forward creeping
369void  forwardcreep() {
370  bodymovexyz(steplength / 4, -toeout0, 0); // Starts to position  for forward walking, leaning to the right
371  servomove();
372  footstep(1, steplength  / 2, 0); // Moving rear left leg one half step length
373  footstep(0, steplength  / 2, 0); // And the front left
374  bodymovexyz(steplength / 4, 2 * toeout0, 0);  // Shifting body forward and to the left (in order to move the right feet later)
375  servomove();
376  while (mode == 1) {
377    // Here the while loop starts, repeaetd  as long as fwd is ordered (mode 1)
378    footstep(3, steplength, 0); // Moving  rear right forward
379    footstep(2, steplength, 0); // Moving front right forward
380    bodymovexyz(steplength / 2, -2 * toeout0, 0); // Shifting body forward and to  the right
381    servomove();
382    footstep(1, steplength, 0); // Moving rear  left forward
383    footstep(0, steplength, 0); // Moving front left forward
384    bodymovexyz(steplength / 2, 2 * toeout0, 0); // Shifting body forward and to  the left
385    servomove();
386    // The robot has the same position as before  the while loop but has moved on steplength forward.
387    IRread(); // If there  is still a forward command (mode ==1) the sequence should be repeated
388  }
389  // The while loop ends and it assumes normal postion
390  /* bodymovexyz(0, 10,  0);*/
391  footstep(3, steplength / 2, 0); // Taking half steps to make all legs  neutral
392  footstep(2, steplength / 2, 0);
393  bodyxyz(0, 0, 0); // Centering  body
394  servomove();
395  // Leaving gait mode
396}
397
398// A similar gait for  reverse walking (not commented as much look at forwardcreep
399void reversecreep()  {
400  bodymovexyz(-steplength / 4, -toeout0, 0); // Starts to position for forward  walking
401  servomove();
402  footstep(0, -steplength / 2, 0);
403  footstep(1,  -steplength / 2, 0);
404  bodymovexyz(-steplength / 4, 2 * toeout0, 0);
405  servomove();
406  while (mode == 2) {
407    // Here the while loop starts, repeaetd as long as  reverse is ordered (mode 2)
408    footstep(2, -steplength, 0);
409    footstep(3,  -steplength, 0);
410    bodymovexyz(-steplength / 2, -2 * toeout0, 0);
411    servomove();
412    footstep(0, -steplength, 0);
413    footstep(1, -steplength, 0);
414    bodymovexyz(-steplength  / 2, 2 * toeout0, 0);
415    servomove();
416    IRread(); // If mode == 2 the while  loop continues
417  }
418  // The while loop ends and it assumes normal postion
419  /*  bodymovexyz(0, 10, 0);*/
420  footstep(2, -steplength / 2, 0);
421  footstep(3,  -steplength / 2, 0);
422  bodyxyz(0, 0, 0);
423  servomove();
424  // Leaving gait  mode
425}
426
427// Doing a turn to the left the desired phi angle
428void leftturn()  {
429  while (mode == 3) {
430    // While loop as long as the left button is pressed
431    bodyxyz(toeout0 / 2, toeout0, 0); // Lean left before doing anything
432    servomove();
433    footstepangle(3, phi); // Move rear right foot into new position
434    footstepangle(2,  phi); // Move front right foot into new position
435    footxyz(0, -toeout0 / 2  - stepturn[0], toeout0 - stepturn[1], 0);
436    footxyz(1, -toeout0 / 2 - stepturn[3],  toeout0 - stepturn[4], 0);
437    footxyz(2, -toeout0 / 2, toeout0, 0);
438    footxyz(3,  -toeout0 / 2, toeout0, 0);
439    // Twisting body and lean left. Written in absolute  coordinates to minmize errors.
440    servomove(); // Do the actual servo command
441    footstepangle(0, phi); // Move front left foot
442    footstepangle(1, phi);  // Move rear left foot
443    IRread(); // Check is left button is still pressed  (mode == 3), repeat while loop
444  }
445  bodyxyz(0, 0, 0); // Centre body when  turning is finished
446  servomove();
447}
448
449//Doing a right turn. Should be  identical to left turn but with different commands. Testing both at the moment.
450void  rightturn() {
451  while (mode == 4) {
452    // While loop as long as the right  button is pressed
453    bodyxyz(-toeout0 / 2, toeout0, 0); // Lean left before  doing anything
454    servomove();
455    footstepangle(2, -phi); //Move front right  foot
456    footstepangle(3, -phi); //Move rear right foot
457    footxyz(0, toeout0  / 2 - stepturn[0], toeout0 - stepturn[1], 0);
458    footxyz(1, toeout0 / 2 - stepturn[3],  toeout0 - stepturn[4], 0);
459    footxyz(2, toeout0 / 2, toeout0, 0);
460    footxyz(3,  toeout0 / 2, toeout0, 0);
461    // Twisting body and lean left. Written in absolute  coordinates to minmize errors.
462    servomove(); // Do the actual servo command
463    footstepangle(1, -phi); //Move rear left foot
464    footstepangle(0, -phi);  //Move front left foot
465    IRread(); // Check is rightt button is still pressed  (mode == 4), repeat while loop
466  }
467  bodyxyz(0, 0, 0);
468  servomove();
469}
470