openDogV3

1
//ODrive
2
#include <ODriveArduino.h>
3

4
// IMU
5
#include "Wire.h"
6
#include "I2Cdev.h"
7
#include "MPU6050.h"
8

9
// Radio
10
#include <SPI.h>
11
#include <nRF24L01.h>
12
#include <RF24.h>
13

14
//I2C LCD
15
#include <LiquidCrystal_I2C.h>
16
LiquidCrystal_I2C lcd(0x27, 16, 2); // set the LCD address to 0x27 for a 16 chars and 2 line display
17

18
// ramp lib
19
#include <Ramp.h>
20

21
RF24 radio(9, 10); // CE, CSN
22
const byte addresses[][6] = {"00001", "00002"};
23

24
// Printing with stream operator
25
template<class T> inline Print& operator <<(Print &obj,     T arg) {
26
  obj.print(arg);
27
  return obj;
28
}
29
template<>        inline Print& operator <<(Print &obj, float arg) {
30
  obj.print(arg, 4);
31
  return obj;
32
}
33

34

35
//ODrive Objects
36
ODriveArduino odrive1(Serial1);
37
ODriveArduino odrive2(Serial2);
38
ODriveArduino odrive3(Serial3);
39
ODriveArduino odrive4(Serial4);
40
ODriveArduino odrive5(Serial5);
41
ODriveArduino odrive6(Serial6);
42

43
//**************remote control****************
44
struct RECEIVE_DATA_STRUCTURE {
45
  //put your variable definitions here for the data you want to send
46
  //THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
47
  int16_t menuDown;
48
  int16_t Select;
49
  int16_t menuUp;
50
  int16_t toggleBottom;
51
  int16_t toggleTop;
52
  int16_t toggle1;
53
  int16_t toggle2;
54
  int16_t mode;
55
  int16_t RLR;
56
  int16_t RFB;
57
  int16_t RT;
58
  int16_t LLR;
59
  int16_t LFB;
60
  int16_t LT;
61
};
62

63
RECEIVE_DATA_STRUCTURE mydata_remote;
64

65
int toggleTopOld;
66
int remoteState;
67
int remoteStateOld;
68

69
float RLR = 0;
70
float RFB = 0;
71
float RT = 340;
72
float LLR = 0;
73
float LFB = 0;
74
float LT = 0;
75

76
float RLRFiltered = 0;
77
float RFBFiltered = 0;
78
float RTFiltered = 340;
79
float LLRFiltered = 0;
80
float LFBFiltered = 0;
81
float LTFiltered = 0;
82
int filterFlag1 = 0;
83

84
unsigned long currentMillis;
85
long previousMillis = 0;    // set up timers
86
long interval = 10;        // time constant for timer
87
unsigned long count;
88

89
long previousIMUMillis = 0;    // set up timers
90

91
long previousInterpMillis = 0;    // set up timers
92
int interpFlag = 0;
93

94
unsigned long remoteMillis;
95

96
int loopTime;
97
int prevLoopTime;
98

99
int stepFlag = 0;
100
long previousStepMillis = 0;
101
int stepStartFlag = 0;
102

103
int requested_state;
104
int mode;
105
int modeOld;
106
int modeFlag;
107
int menuFlag;
108
int modeConfirm;
109
int modeConfirmFlag = 0;
110
int runMode = 0;
111

112
float longLeg1;
113
float shortLeg1;
114
float legLength1;
115
float longLeg2;
116
float shortLeg2;
117
float legLength2;
118
float footOffset;
119

120
float fr_RFB;
121
float fl_RFB;
122
float bl_RFB;
123
float br_RFB;
124
float fr_RLR;
125
float fl_RLR;
126
float bl_RLR;
127
float br_RLR;;
128
float fr_LT;
129
float fl_LT;
130
float bl_LT;
131
float br_LT;
132

133
float fr_LLR;
134
float fl_LLR;
135
float br_LLR;
136
float bl_LLR;
137

138
int timer1;   // FB gait timer
139
int timer2;   // LR gait timer
140
int timer3;   // Yaw gait timer
141
float timerScale;   // resulting timer after calcs
142
float timerScale2;   // multiplier
143

144
// ODrive offsets from power up
145
// ratio is 10:1 so 1 'turn' is 36'.
146

147

148
float offSet20 = -0.1;      //ODrive 2, axis 0      // knee - right front
149
float offSet30 = -0.45;      //ODrive 3, axis 0     // knee - right rear
150
float offSet50 = -0.05;      //ODrive 5, axis 0     // knee - left front
151
float offSet60 = -0.4;      //ODrive 6, axis 0       // knee - left rear
152

153
float offSet21 = -0.1;      //ODrive 2, axis 1     // shoulder - right front
154
float offSet31 = 0.45;      //ODrive 3, axis 1     // shoulder - right rear
155
float offSet51 = 0.66;      //ODrive 5, axis 0     // shoulder - left front
156
float offSet61 =  -0.08;      //ODrive 6, axis 1     // shoulder - left rear
157

158
float offSet10 = 0.27;      //ODrive 1, axis 0     // hips - right front
159
float offSet11 = 0.1;      //ODrive 1, axis 1     // hips - right back
160
float offSet40 = 0.07;      //ODrive 4, axis 0     // hips - left front
161
float offSet41 = 0.35;      //ODrive 4, axis 1     // hips - left back
162

163
// IMU variables
164

165
MPU6050 accelgyro;
166
int16_t ax, ay, az;
167
int16_t gx, gy, gz;
168

169
#define Gyr_Gain 0.00763358 
170

171
float AccelX;
172
float AccelY;
173
float AccelZ;
174

175
float GyroX;
176
float GyroY;
177
float GyroZ;
178

179
float mixX;
180
float mixY;
181

182
float pitchAccel, rollAccel;
183

184
float IMUpitch;
185
float IMUroll;
186

187
// Dynamics stability variables
188

189
float legTransX;
190
float legTransY;
191
float legTransXFiltered;
192
float legTransYFiltered;
193

194
float legRoll;
195
float legPitch;
196
float legRollFiltered;
197
float legPitchFiltered;
198

199

200

201
//***********************************
202
//***********************************
203
class Interpolation {
204
  public:
205
    rampInt myRamp;
206
    int interpolationFlag = 0;
207
    int savedValue;
208

209
    int go(int input, int duration) {
210

211
      if (input != savedValue) {   // check for new data
212
        interpolationFlag = 0;
213
      }
214
      savedValue = input;          // bookmark the old value
215

216
      if (interpolationFlag == 0) {                                        // only do it once until the flag is reset
217
        myRamp.go(input, duration, LINEAR, ONCEFORWARD);              // start interpolation (value to go to, duration)
218
        interpolationFlag = 1;
219
      }
220

221
      int output = myRamp.update();
222
      return output;
223
    }
224
};    // end of class
225

226
Interpolation interpFRX;        // interpolation objects
227
Interpolation interpFRY;
228
Interpolation interpFRZ;
229
Interpolation interpFRT;
230

231
Interpolation interpFLX;        // interpolation objects
232
Interpolation interpFLY;
233
Interpolation interpFLZ;
234
Interpolation interpFLT;
235

236
Interpolation interpBRX;        // interpolation objects
237
Interpolation interpBRY;
238
Interpolation interpBRZ;
239
Interpolation interpBRT;
240

241
Interpolation interpBLX;        // interpolation objects
242
Interpolation interpBLY;
243
Interpolation interpBLZ;
244
Interpolation interpBLT;
245

246
//***********************************
247
//***********************************
248

249
// ****************** SETUP ******************************
250

251
void setup() {
252

253

254
  // initialize serial communication
255
  Serial.begin(115200);
256

257
  Serial1.begin(115200);
258
  Serial2.begin(115200);
259
  Serial3.begin(115200);
260
  Serial4.begin(115200);
261
  Serial5.begin(115200);
262
  Serial6.begin(115200);
263

264

265
  radio.begin();
266
  radio.openWritingPipe(addresses[0]); // 00002
267
  radio.openReadingPipe(1, addresses[1]); // 00001
268
  radio.setPALevel(RF24_PA_MIN);
269

270
  radio.startListening();
271

272
  //LCD
273
  lcd.init();
274
  lcd.backlight();
275
  lcd.setCursor(0, 0);
276
  lcd.print("openDog V3");
277
  lcd.setCursor(0, 1);
278
  lcd.print("S:0  C:0");
279

280
}   // end of setup
281

282
// ********************* MAIN LOOP *******************************
283

284
void loop() {
285

286
  currentMillis = millis();
287
  if (currentMillis - previousMillis >= 10) {  // start timed event
288

289
    previousMillis = currentMillis;
290

291
    if (mydata_remote.toggleBottom == 1) {
292
        // read IMU
293
        Wire.begin();   
294
        accelgyro.initialize(); 
295
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
296
        
297
        AccelX = ax;
298
        AccelY = ay;
299
        AccelZ = az;
300
        GyroX = Gyr_Gain * (gx);
301
        GyroY = Gyr_Gain * (gy)*-1;
302
        GyroZ = Gyr_Gain * (gz);
303
      
304
        AccelY = (atan2(AccelY, AccelZ) * 180 / PI);
305
        AccelX = (atan2(AccelX, AccelZ) * 180 / PI);
306
    
307
        float dt = 0.01;
308
        float K = 0.9;
309
        float A = K / (K + dt);
310
      
311
        mixX = A *(mixX+GyroX*dt) + (1-A)*AccelY;    
312
        mixY = A *(mixY+GyroY*dt) + (1-A)*AccelX; 
313
    
314
        IMUpitch = mixX + 2.7;      // trim IMU to zero
315
        IMUroll = mixY - 5;      
316
    }
317
    else {
318
        // ignore IMU data
319
        Wire.end();
320
        IMUpitch = 0;      // IMU data is zeero, do not read IMU 
321
        IMUroll = 0;
322
    }
323

324
    //check loop is actually running the speed we want
325
    //loopTime = currentMillis - prevLoopTime;
326
    //prevLoopTime = currentMillis;
327
    //Serial.println(loopTime);
328

329
    
330

331

332
    // check for radio data
333
    if (radio.available()) {
334
      radio.read(&mydata_remote, sizeof(RECEIVE_DATA_STRUCTURE));
335
      remoteMillis = currentMillis;
336
    }
337

338
    // is the remote disconnected for too long ?
339
    if (currentMillis - remoteMillis > 500) {
340
      remoteState = 0;
341
    }
342
    else {
343
      remoteState = 1;
344
    }    
345

346
    // threshold remote data
347
    // some are reversed based on stick wiring in remote
348
    RFB = (thresholdStick(mydata_remote.RFB)) * -1;
349
    RLR = (thresholdStick(mydata_remote.RLR));
350
    RT = thresholdStick(mydata_remote.RT);
351
    LFB = (thresholdStick(mydata_remote.LFB)) * -1;
352
    LLR = thresholdStick(mydata_remote.LLR);
353
    LT = thresholdStick(mydata_remote.LT);
354

355
    
356
    // stop the dog if the remote becomes disconnected
357
    if (remoteState == 0) {     
358
      RFB = 0;
359
      RLR = 0;
360
      RT = 0;
361
      LFB = 0;
362
      LLR = 0;
363
      LT = 0;
364
    }
365

366

367
    // mode select
368

369
    if (mydata_remote.menuUp == 1 && menuFlag == 0) {
370
      menuFlag = 1;
371
      mode = mode + 1;
372
      mode = constrain(mode, 0, 10);
373
    }
374
    else if (mydata_remote.menuDown == 1 && menuFlag == 0) {
375
      menuFlag = 1;
376
      mode = mode - 1;
377
      mode = constrain(mode, 0, 10);
378
    }
379
    else if (mydata_remote.menuDown == 0 && mydata_remote.menuUp == 0) {
380
      menuFlag = 0;
381
    }
382

383
    if (mode != modeOld) {            // only update the LCD if the data has changed
384
      lcd.setCursor(0, 1);
385
      lcd.print("    ");
386
      lcd.setCursor(0, 1);
387
      lcd.print("S:");
388
      lcd.print(mode);
389
    }
390
    modeOld = mode;       // bookmark previous data so we can see if it's changed
391

392
    if (mydata_remote.Select == 1) {
393
      modeConfirm = mode;             // make the actual mode be the potential mode when the select button is pressed
394
      lcd.setCursor(5, 1);            // display current mode on LCD
395
      lcd.print("    ");
396
      lcd.setCursor(5, 1);
397
      lcd.print("C:");
398
      lcd.print(modeConfirm);
399
    }
400

401
    // display remote connection state
402
    if (remoteState != remoteStateOld) {
403
      if (remoteState == 0) {
404
        lcd.setCursor(13, 0);
405
        lcd.print("N");
406
      }
407
      else if (remoteState == 1) {
408
        lcd.setCursor(13, 0);
409
        lcd.print("C");
410
      }
411
    }
412
    remoteStateOld = remoteState;
413

414
    // display motor enable state
415

416
    if (mydata_remote.toggleTop != toggleTopOld) {
417
      if (mydata_remote.toggleTop == 1) {
418
        lcd.setCursor(15, 0);
419
        lcd.print("E");
420
      }
421
      else if (mydata_remote.toggleTop == 0) {
422
        lcd.setCursor(15, 0);
423
        lcd.print("D");
424
      }
425
    }
426
    toggleTopOld = mydata_remote.toggleTop;
427

428
    // ****** MAIN MENU ******* 
429

430
    if (mydata_remote.Select == 0) {                            // make sure the button is released before it can be pressed again
431
              modeConfirmFlag = 0;
432
    }
433

434
    if (modeConfirm == 1 && modeConfirmFlag == 0 && mydata_remote.Select == 1) {             // init ODrives with low gains
435
      Serial.println("Init Odrives mode 1");
436
      OdriveInit1();
437
      modeConfirmFlag = 1;
438
    }
439

440
    else if (modeConfirm == 2 && modeConfirmFlag == 0 && mydata_remote.Select == 1) {       // default to 45' shoulder and knees
441
      Serial.println("Knees mode 2");
442
      applyOffsets1();
443
      modeConfirmFlag = 1;
444
    }
445

446
    else if (modeConfirm == 3 && modeConfirmFlag == 0 && mydata_remote.Select == 1) {       // default to hip position
447
      Serial.println("Shoulders mode 3");
448
      applyOffsets2();
449
      modeConfirmFlag = 1;
450
    }
451

452
    else if (modeConfirm == 4 && modeConfirmFlag == 0 && mydata_remote.Select == 1) {       // turn up gains
453
      Serial.println("Modify Gains mode 4");
454
      modifyGains();
455
      modeConfirmFlag = 1;
456
    }
457

458
    else if (modeConfirm == 5 && modeConfirmFlag == 0 && mydata_remote.Select == 1) {       // turn up gains
459
      Serial.println("Kinematics mode 5");
460
      interpFlag = 0;
461
      previousInterpMillis = currentMillis;
462
      runMode = 1;
463
      modeConfirmFlag = 1;
464
    }
465

466
    else if (modeConfirm == 6 && modeConfirmFlag == 0 && mydata_remote.Select == 1) {       // turn up gains
467
      Serial.println("Walking Mode 6");
468
      interpFlag = 0;
469
      previousInterpMillis = currentMillis;
470
      runMode = 2;
471
      modeConfirmFlag = 1;
472
    }
473

474
    else if (modeConfirm == 10 && modeConfirmFlag == 0 && mydata_remote.Select == 1) {       // turn up gains
475
      Serial.println("Going Home Mode 10");
476
      interpFlag = 0;
477
      previousInterpMillis = currentMillis;
478
      runMode = 10;
479
      modeConfirmFlag = 1;
480
    }
481

482
    else if (modeConfirm == 9 && modeConfirmFlag == 0 && mydata_remote.Select == 1) {       // turn up gains
483
      Serial.println("Interp Test");
484
      interpFlag = 0;
485
      previousInterpMillis = currentMillis;
486
      runMode = 9;
487
      modeConfirmFlag = 1;
488
    }
489
    
490
    // **** END OF MAIN MENU **** 
491
 
492

493
    if (runMode == 10) {         // put the legs back on the stand
494

495
        int offset1 = 70;
496

497
        kinematics (1, -offset1, 0, 270, 0, 0, 0, 0, 0);   // front right
498
        kinematics (2, -offset1, 0, 270, 0, 0, 0, 0, 0);   // front left
499
        kinematics (3, offset1, 0, 270, 0, 0, 0, 0, 0);   // back left
500
        kinematics (4, offset1, 0, 270, 0, 0, 0, 0, 0);   // back right
501
    }
502

503
    else if (runMode == 1) {
504
      
505
    // ** inverse kinematics demo ** 
506

507
        // scale sticks to mm
508
        RFB = map(RFB,-462,462,-100,100);
509
        RLR = map(RLR,-462,462,-100,100);        
510
        RT = map(RT,-462,462,240,440);
511
        RT = constrain(RT,240,380);      
512
        LFB = map(LFB,-462,462,-15,15);        
513
        LLR = map(LLR,-462,462,-15,15);        
514
        LT = map(LT,-462,462,-20,20);      
515

516
        // filter sticks
517
        RFBFiltered = filter(RFB, RFBFiltered, 40);
518
        RLRFiltered = filter(RLR, RLRFiltered, 40);
519
        RTFiltered = filter(RT, RTFiltered, 40);
520
        LFBFiltered = filter(LFB, LFBFiltered, 40);
521
        LLRFiltered = filter(LLR, LLRFiltered, 40);
522
        LTFiltered = filter(LT, LTFiltered, 40);
523
                  
524
        kinematics (1, RFBFiltered, RLRFiltered, RTFiltered, LLRFiltered, LFBFiltered, LTFiltered, 0, 0);   // front right
525
        kinematics (2, RFBFiltered, RLRFiltered, RTFiltered, LLRFiltered, LFBFiltered, LTFiltered, 0, 0);   // front left
526
        kinematics (3, RFBFiltered, RLRFiltered, RTFiltered, LLRFiltered, LFBFiltered, LTFiltered, 0, 0);   // back left
527
        kinematics (4, RFBFiltered, RLRFiltered, RTFiltered, LLRFiltered, LFBFiltered, LTFiltered, 0, 0);   // back right
528
      
529
 }
530

531
   else if (runMode == 2) {
532
      // simple walking
533
      
534
      RFB = map(RFB,-462,462,-50,50);  // mm
535
      RLR = map(RLR,-462,462,-25,25);  // mm
536
      LT = map(LT,-462,462,-25,25);      // degrees 
537

538
      RFBFiltered = filter(RFB, RFBFiltered, 15);
539
      RLRFiltered = filter(RLR, RLRFiltered, 15);
540
      LTFiltered = filter(LT, LTFiltered, 15);
541

542
      longLeg1 = 340;
543
      shortLeg1 = 200;
544
      longLeg2 = 340;
545
      shortLeg2 = 200;
546

547
      footOffset = 0;
548
      timer1 = 80;   // FB gait timer  80
549
      //timer2 = 75;   // LR gait timer
550
      //timer3 = 75;   // LR gait timer
551

552
      if (RFBFiltered > -0.1 && RFBFiltered < 0.1 && RLRFiltered > -0.1 && RLRFiltered < 0.1  && LTFiltered > -0.1 && LTFiltered < 0.1 ) {    // controls are centred or near enough
553
      
554
      // position legs a default standing positionS
555
          legLength1 = longLeg1;
556
          legLength2 = longLeg2;
557
          fr_RFB = 0;
558
          fl_RFB = 0;
559
          bl_RFB = 0;
560
          br_RFB = 0;
561
          fr_RLR = footOffset;
562
          fl_RLR = -footOffset;
563
          bl_RLR = -footOffset;
564
          br_RLR = footOffset;
565
          fr_LT = 0;
566
          fl_LT = 0;
567
          bl_LT = 0;
568
          br_LT = 0;        
569
      }
570
      
571
      //walking
572
      else {
573
       
574
          if (stepFlag == 0 && currentMillis - previousStepMillis > timerScale) {
575
              legLength1  = shortLeg1;
576
              legLength2 = longLeg2; 
577
              fr_RFB = 0-RFBFiltered;
578
              fl_RFB = RFBFiltered;
579
              bl_RFB = 0-RFBFiltered;
580
              br_RFB = RFBFiltered;
581
              fr_RLR = (footOffset -RLRFiltered) + LT;
582
              fl_RLR = (-footOffset +RLRFiltered) - LT;
583
              bl_RLR = (-footOffset - RLRFiltered) - LT;
584
              br_RLR = (footOffset + RLRFiltered) + LT;
585
              //fr_RLR = LT;
586
              //fl_RLR = 0-LT;
587
              //bl_RLR = 0-LT;
588
              //br_RLR = LT;
589
              stepFlag = 1;              
590
              previousStepMillis = currentMillis;
591
          }
592

593
          else if (stepFlag == 1 && currentMillis - previousStepMillis > timerScale) {
594
              legLength1 = longLeg1;
595
              legLength2 = longLeg2;
596
              fr_RFB = 0-RFBFiltered;
597
              fl_RFB = RFBFiltered;
598
              bl_RFB = 0-RFBFiltered;
599
              br_RFB = RFBFiltered;
600
              fr_RLR = (footOffset -RLRFiltered) + LT;
601
              fl_RLR = (-footOffset +RLRFiltered) - LT;
602
              bl_RLR = (-footOffset -RLRFiltered) - LT;
603
              br_RLR = (footOffset +RLRFiltered) + LT;
604
              //fr_RLR = LT;
605
              //fl_RLR = 0-LT;
606
              //bl_RLR = 0-LT;
607
              //br_RLR = LT;                        
608

609
              stepFlag = 2;              
610
              previousStepMillis = currentMillis;
611
          }
612

613
          else if (stepFlag == 2 && currentMillis - previousStepMillis > timerScale) {
614
              legLength1 = longLeg1;
615
              legLength2 = shortLeg2;
616
              fr_RFB = RFBFiltered;
617
              fl_RFB = 0-RFBFiltered;
618
              bl_RFB = RFBFiltered;
619
              br_RFB = 0-RFBFiltered;
620
              fr_RLR = (footOffset +RLRFiltered) - LT;
621
              fl_RLR = (-footOffset -RLRFiltered) + LT;
622
              bl_RLR = (-footOffset +RLRFiltered) + LT;
623
              br_RLR = (footOffset -RLRFiltered) - LT;
624
              //fr_RLR = 0-LT;
625
              //fl_RLR = LT;
626
              //bl_RLR = LT;
627
              // br_RLR = 0-LT; 
628
              stepFlag = 3;              
629
              previousStepMillis = currentMillis;
630
          }
631

632
          else if (stepFlag == 3 && currentMillis - previousStepMillis > timerScale) {
633
              legLength1 = longLeg1;
634
              legLength2 = longLeg2;
635
              fr_RFB = RFBFiltered;
636
              fl_RFB = 0-RFBFiltered;
637
              bl_RFB = RFBFiltered;
638
              br_RFB = 0-RFBFiltered;
639
              fr_RLR = (footOffset +RLRFiltered) - LT;
640
              fl_RLR = (-footOffset -RLRFiltered) + LT;
641
              bl_RLR = (-footOffset +RLRFiltered) + LT;
642
              br_RLR = (footOffset -RLRFiltered) - LT;
643
              //fr_RLR = 0-LT;
644
              //fl_RLR = LT;
645
              //bl_RLR = LT;
646
              //br_RLR = 0-LT; 
647
              stepFlag = 0;              
648
              previousStepMillis = currentMillis;
649
          }
650

651
          float stepLength;
652
          float stepWidth;
653
          float stepAngle;
654
          float stepHyp;
655

656
          // timer calcs
657

658
          stepLength = abs(fr_RFB);
659
          stepWidth = abs(fr_RLR);
660

661
          if (stepLength == 0.0) {
662
            stepLength = 0.01;   // avoid divide by zero
663
          }
664

665
          stepAngle = atan(stepLength/stepWidth);  // radians       // work out actual distance of step
666
          stepHyp = abs(stepLength/sin(stepAngle));    // mm
667

668
          timerScale =  timer1 + (stepHyp/3.5);              
669
      }
670

671
      legTransX = IMUpitch * -2;
672
      legTransY = IMUroll * -2;
673

674
      legTransXFiltered = filter(legTransX,legTransXFiltered,50);
675
      legTransYFiltered = filter(legTransY,legTransYFiltered,50);
676

677
      legRoll = IMUroll * -0.5;
678
      legPitch = IMUpitch * 0.5;
679

680
      legRollFiltered = filter(legRoll,legRollFiltered,60);
681
      legPitchFiltered = filter(legPitch,legPitchFiltered,60);      
682

683
   
684
      kinematics (1, fr_RFB - legTransXFiltered, fr_RLR - legTransYFiltered, legLength1, legRollFiltered, legPitchFiltered, 0, 1, (timerScale*0.8));   // front right
685
      kinematics (2, fl_RFB - legTransXFiltered, fl_RLR - legTransYFiltered, legLength2, legRollFiltered, legPitchFiltered, 0, 1, (timerScale*0.8));   // front left
686
      kinematics (3, bl_RFB - legTransXFiltered, bl_RLR - legTransYFiltered, legLength1, legRollFiltered, legPitchFiltered, 0, 1, (timerScale*0.8));   // back left
687
      kinematics (4, br_RFB - legTransXFiltered, br_RLR - legTransYFiltered, legLength2, legRollFiltered, legPitchFiltered, 0, 1, (timerScale*0.8));   // back right             
688
   }
689

690

691

692
  }     // end of timed loop
693

694
}       // end  of main loop
695