Patrick Clary / Mbed 2 deprecated WalkingRobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   CircularBuffer Servo Terminal mbed Radio

main.cpp@9:a6d1502f0f20, 2013-01-31 (annotated)

Committer:
pclary
Date:
Thu Jan 31 23:51:15 2013 +0000
Revision:
9:a6d1502f0f20
Parent:
8:db453051f3f4
Child:
10:dc1ba352667e
Leg coordination scheme based on distance moved

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pclary 0:449568595ed9 1 #include "mbed.h"
pclary 0:449568595ed9 2 #include "RobotLeg.h"
pclary 3:6fa07ceb897f 3 #include "Matrix.h"
pclary 2:caf73a1d7827 4 #include "CircularBuffer.h"
pclary 5:475f67175510 5 #include "Radio.h"
pclary 6:0163f2737cc6 6 #include "Terminal.h"
pclary 9:a6d1502f0f20 7 #include "utility.h"
pclary 0:449568595ed9 8 #include <cstring>
pclary 6:0163f2737cc6 9 #include <cmath>
pclary 0:449568595ed9 10
pclary 6:0163f2737cc6 11 #define MAXSPEED 0.1f
pclary 6:0163f2737cc6 12 #define MAXTURN 1.0f
pclary 8:db453051f3f4 13 #define RESET_STEP_TIME 0.4f
pclary 8:db453051f3f4 14 #define DIM_A 0.125f
pclary 8:db453051f3f4 15 #define DIM_B 0.11f
pclary 8:db453051f3f4 16 #define DIM_C 0.0025f
pclary 8:db453051f3f4 17 #define DIM_D 0.025f
pclary 8:db453051f3f4 18 #define CIRCLE_X 0.095f
pclary 8:db453051f3f4 19 #define CIRCLE_Y 0.095f
pclary 8:db453051f3f4 20 #define CIRCLE_Z -0.12f
pclary 8:db453051f3f4 21 #define CIRCLE_R 0.09f
pclary 3:6fa07ceb897f 22
pclary 6:0163f2737cc6 23 enum state_t
pclary 6:0163f2737cc6 24 {
pclary 6:0163f2737cc6 25 walk,
pclary 6:0163f2737cc6 26 reset
pclary 6:0163f2737cc6 27 };
pclary 6:0163f2737cc6 28
pclary 6:0163f2737cc6 29 enum legstate_t
pclary 6:0163f2737cc6 30 {
pclary 6:0163f2737cc6 31 A,
pclary 6:0163f2737cc6 32 B,
pclary 6:0163f2737cc6 33 C,
pclary 6:0163f2737cc6 34 D
pclary 6:0163f2737cc6 35 };
pclary 0:449568595ed9 36
pclary 3:6fa07ceb897f 37 CircularBuffer<float,16> dataLog;
pclary 9:a6d1502f0f20 38 Radio radio(p5, p6, p7, p16, p17, p18);
pclary 9:a6d1502f0f20 39 Timer stepTimer;
pclary 8:db453051f3f4 40 RobotLeg legA(p26, p29, p30, false); // Start the legs disabled
pclary 8:db453051f3f4 41 RobotLeg legB(p13, p14, p15, false);
pclary 8:db453051f3f4 42 RobotLeg legC(p12, p11, p8, false);
pclary 8:db453051f3f4 43 RobotLeg legD(p23, p24, p25, false);
pclary 6:0163f2737cc6 44 state_t state;
pclary 6:0163f2737cc6 45 legstate_t legState;
pclary 9:a6d1502f0f20 46 float stepDistance, stepDistanceTarget;
pclary 6:0163f2737cc6 47
pclary 6:0163f2737cc6 48
pclary 6:0163f2737cc6 49
pclary 6:0163f2737cc6 50 CmdHandler* log(Terminal* terminal, const char* input)
pclary 6:0163f2737cc6 51 {
pclary 6:0163f2737cc6 52 int start = 0;
pclary 6:0163f2737cc6 53 int end = 15;
pclary 6:0163f2737cc6 54 char output[256];
pclary 6:0163f2737cc6 55
pclary 6:0163f2737cc6 56 if (sscanf(input, "log %d %d", &start, &end) == 1)
pclary 6:0163f2737cc6 57 {
pclary 6:0163f2737cc6 58 // Print only one item
pclary 6:0163f2737cc6 59 sprintf(output, "%4d: %f\n", start, dataLog[start]);
pclary 6:0163f2737cc6 60 terminal->write(output);
pclary 6:0163f2737cc6 61 }
pclary 6:0163f2737cc6 62 else
pclary 6:0163f2737cc6 63 {
pclary 6:0163f2737cc6 64 // Print a range of items
pclary 6:0163f2737cc6 65 for (int i = start; i <= end; i++)
pclary 6:0163f2737cc6 66 {
pclary 6:0163f2737cc6 67 sprintf(output, "%4d: %f\n", i, dataLog[i]);
pclary 6:0163f2737cc6 68 terminal->write(output);
pclary 6:0163f2737cc6 69 }
pclary 6:0163f2737cc6 70 }
pclary 6:0163f2737cc6 71
pclary 6:0163f2737cc6 72 return NULL;
pclary 8:db453051f3f4 73 } // log()
pclary 6:0163f2737cc6 74
pclary 6:0163f2737cc6 75
pclary 6:0163f2737cc6 76
pclary 6:0163f2737cc6 77 CmdHandler* read(Terminal* terminal, const char* input)
pclary 6:0163f2737cc6 78 {
pclary 6:0163f2737cc6 79 char output[256];
pclary 6:0163f2737cc6 80 uint32_t data;
pclary 6:0163f2737cc6 81
pclary 6:0163f2737cc6 82 data = radio.rx_controller;
pclary 6:0163f2737cc6 83 sprintf(output, "%d%d%d%d%d%d%d%d %d%d%d%d%d%d%d%d %d%d%d%d%d%d%d%d %d%d%d%d%d%d%d%d : %4d %4d %4d %4d",
pclary 6:0163f2737cc6 84 (data>>31)&1, (data>>30)&1, (data>>29)&1, (data>>28)&1, (data>>27)&1, (data>>26)&1, (data>>25)&1, (data>>24)&1,
pclary 6:0163f2737cc6 85 (data>>23)&1, (data>>22)&1, (data>>21)&1, (data>>20)&1, (data>>19)&1, (data>>18)&1, (data>>17)&1, (data>>16)&1,
pclary 6:0163f2737cc6 86 (data>>15)&1, (data>>14)&1, (data>>13)&1, (data>>12)&1, (data>>11)&1, (data>>10)&1, (data>>9)&1, (data>>8)&1,
pclary 6:0163f2737cc6 87 (data>>7)&1, (data>>6)&1, (data>>5)&1, (data>>4)&1, (data>>3)&1, (data>>2)&1, (data>>1)&1, (data>>0)&1,
pclary 6:0163f2737cc6 88 (int8_t)((data>>24)&0xff), (int8_t)((data>>16)&0xff), (int8_t)((data>>8)&0xff), (int8_t)((data)&0xff));
pclary 6:0163f2737cc6 89 terminal->write(output);
pclary 6:0163f2737cc6 90
pclary 6:0163f2737cc6 91 return NULL;
pclary 8:db453051f3f4 92 } // read()
pclary 6:0163f2737cc6 93
pclary 6:0163f2737cc6 94
pclary 6:0163f2737cc6 95
pclary 9:a6d1502f0f20 96 CmdHandler* resetc(Terminal* terminal, const char* input)
pclary 9:a6d1502f0f20 97 {
pclary 9:a6d1502f0f20 98 char output[256];
pclary 9:a6d1502f0f20 99 float f;
pclary 9:a6d1502f0f20 100 vector3 v;
pclary 9:a6d1502f0f20 101
pclary 9:a6d1502f0f20 102 if (sscanf(input, "reset %f", &f) == 1)
pclary 9:a6d1502f0f20 103 {
pclary 9:a6d1502f0f20 104 v = legA.reset(f);
pclary 9:a6d1502f0f20 105 sprintf(output, "reset: %f %f %f", v.x, v.y, v.z);
pclary 9:a6d1502f0f20 106 terminal->write(output);
pclary 9:a6d1502f0f20 107 }
pclary 9:a6d1502f0f20 108 else
pclary 9:a6d1502f0f20 109 {
pclary 9:a6d1502f0f20 110 sprintf(output, "syntax error");
pclary 9:a6d1502f0f20 111 terminal->write(output);
pclary 9:a6d1502f0f20 112 }
pclary 9:a6d1502f0f20 113
pclary 9:a6d1502f0f20 114 return NULL;
pclary 9:a6d1502f0f20 115 } // reset()
pclary 9:a6d1502f0f20 116
pclary 9:a6d1502f0f20 117
pclary 9:a6d1502f0f20 118
pclary 9:a6d1502f0f20 119 CmdHandler* rndp(Terminal* terminal, const char* input)
pclary 9:a6d1502f0f20 120 {
pclary 9:a6d1502f0f20 121 char output[256];
pclary 9:a6d1502f0f20 122
pclary 9:a6d1502f0f20 123 sprintf(output, "%f %f %f", legA.nDeltaPosition.x, legA.nDeltaPosition.y, legA.nDeltaPosition.z);
pclary 9:a6d1502f0f20 124 terminal->write(output);
pclary 9:a6d1502f0f20 125
pclary 9:a6d1502f0f20 126 return NULL;
pclary 9:a6d1502f0f20 127 } // step()
pclary 9:a6d1502f0f20 128
pclary 9:a6d1502f0f20 129
pclary 9:a6d1502f0f20 130
pclary 6:0163f2737cc6 131 int deadzone(int input, int zone)
pclary 6:0163f2737cc6 132 {
pclary 6:0163f2737cc6 133 if (input > zone) return input;
pclary 6:0163f2737cc6 134 else if (input < -zone) return input;
pclary 6:0163f2737cc6 135 else return 0;
pclary 8:db453051f3f4 136 } // deadzone()
pclary 6:0163f2737cc6 137
pclary 6:0163f2737cc6 138
pclary 6:0163f2737cc6 139
pclary 6:0163f2737cc6 140 void resetLegs()
pclary 6:0163f2737cc6 141 {
pclary 6:0163f2737cc6 142 legA.reset(-0.5f);
pclary 6:0163f2737cc6 143 legB.reset(0.0f);
pclary 6:0163f2737cc6 144 legC.reset(0.5f);
pclary 6:0163f2737cc6 145 legD.reset(1.0f);
pclary 9:a6d1502f0f20 146 stepTimer.reset();
pclary 6:0163f2737cc6 147 state = reset;
pclary 6:0163f2737cc6 148 legState = D;
pclary 8:db453051f3f4 149 } // resetLegs()
pclary 8:db453051f3f4 150
pclary 8:db453051f3f4 151
pclary 8:db453051f3f4 152
pclary 8:db453051f3f4 153 void walkLegs()
pclary 8:db453051f3f4 154 {
pclary 8:db453051f3f4 155 state = walk;
pclary 8:db453051f3f4 156 legState = A;
pclary 9:a6d1502f0f20 157 stepDistanceTarget = CIRCLE_R / 2.0f;
pclary 9:a6d1502f0f20 158 stepDistance = 0;
pclary 9:a6d1502f0f20 159 } // walkLegs()
pclary 0:449568595ed9 160
pclary 0:449568595ed9 161
pclary 0:449568595ed9 162
pclary 0:449568595ed9 163 int main()
pclary 0:449568595ed9 164 {
pclary 9:a6d1502f0f20 165 Timer deltaTimer;
pclary 6:0163f2737cc6 166 float xaxis, yaxis, turnaxis, speed, angle;
pclary 8:db453051f3f4 167 float deltaTime, cycleTime;
pclary 2:caf73a1d7827 168 vector3 v;
pclary 5:475f67175510 169 matrix4 T;
pclary 5:475f67175510 170 matrix4 PA, QA;
pclary 5:475f67175510 171 matrix4 PB, QB;
pclary 5:475f67175510 172 matrix4 PC, QC;
pclary 6:0163f2737cc6 173 matrix4 PD, QD;
pclary 0:449568595ed9 174 Terminal terminal;
pclary 6:0163f2737cc6 175 bool freeA, freeB, freeC, freeD;
pclary 2:caf73a1d7827 176
pclary 2:caf73a1d7827 177 terminal.addCommand("log", &log);
pclary 5:475f67175510 178 terminal.addCommand("read", &read);
pclary 9:a6d1502f0f20 179 terminal.addCommand("reset", &resetc);
pclary 9:a6d1502f0f20 180 terminal.addCommand("ndp", &rndp);
pclary 9:a6d1502f0f20 181
pclary 9:a6d1502f0f20 182
pclary 9:a6d1502f0f20 183 DigitalOut debug1(LED1);
pclary 9:a6d1502f0f20 184 DigitalOut debug2(LED2);
pclary 9:a6d1502f0f20 185 DigitalOut debug3(LED3);
pclary 9:a6d1502f0f20 186 DigitalOut debug4(LED4);
pclary 9:a6d1502f0f20 187
pclary 2:caf73a1d7827 188
pclary 6:0163f2737cc6 189 radio.reset();
pclary 6:0163f2737cc6 190
pclary 0:449568595ed9 191 // Set leg parameters
pclary 8:db453051f3f4 192 legA.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
pclary 8:db453051f3f4 193 legB.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
pclary 8:db453051f3f4 194 legC.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
pclary 8:db453051f3f4 195 legD.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
pclary 5:475f67175510 196 legA.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
pclary 5:475f67175510 197 legB.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
pclary 5:475f67175510 198 legC.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
pclary 5:475f67175510 199 legD.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
pclary 8:db453051f3f4 200 legA.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
pclary 8:db453051f3f4 201 legB.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
pclary 8:db453051f3f4 202 legC.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
pclary 8:db453051f3f4 203 legD.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
pclary 5:475f67175510 204 legA.theta.calibrate(1000, 2000, 45.0f, -45.0f);
pclary 5:475f67175510 205 legA.phi.calibrate(1000, 2000, 70.0f, -45.0f);
pclary 7:aac5f901bd76 206 legA.psi.calibrate(2000, 1000, 70.0f, -60.0f);
pclary 5:475f67175510 207 legB.theta.calibrate(1000, 2000, 45.0f, -45.0f);
pclary 5:475f67175510 208 legB.phi.calibrate(1000, 2000, 70.0f, -45.0f);
pclary 7:aac5f901bd76 209 legB.psi.calibrate(2000, 1000, 70.0f, -60.0f);
pclary 5:475f67175510 210 legC.theta.calibrate(2000, 1000, 45.0f, -45.0f);
pclary 5:475f67175510 211 legC.phi.calibrate(2000, 1000, 70.0f, -45.0f);
pclary 7:aac5f901bd76 212 legC.psi.calibrate(1000, 2000, 70.0f, -60.0f);
pclary 5:475f67175510 213 legD.theta.calibrate(2000, 1000, 45.0f, -45.0f);
pclary 5:475f67175510 214 legD.phi.calibrate(2000, 1000, 70.0f, -45.0f);
pclary 7:aac5f901bd76 215 legD.psi.calibrate(1000, 2000, 70.0f, -60.0f);
pclary 6:0163f2737cc6 216
pclary 6:0163f2737cc6 217 // Initialize leg position deltas
pclary 6:0163f2737cc6 218 legA.nDeltaPosition = vector3(0.0f, 0.01f, 0.0f);
pclary 6:0163f2737cc6 219 legB.nDeltaPosition = vector3(0.0f, -0.01f, 0.0f);
pclary 6:0163f2737cc6 220 legC.nDeltaPosition = vector3(0.0f, 0.01f, 0.0f);
pclary 6:0163f2737cc6 221 legD.nDeltaPosition = vector3(0.0f, -0.01f, 0.0f);
pclary 5:475f67175510 222
pclary 2:caf73a1d7827 223 // Create matrices to change base from robot coordinates to leg coordinates
pclary 5:475f67175510 224 QA.translate(vector3(0.1f, 0.1f, 0.0f));
pclary 5:475f67175510 225 PA = QA.inverse();
pclary 5:475f67175510 226 QB.translate(vector3(-0.1f, -0.1f, 0.0f));
pclary 5:475f67175510 227 QB.a11 = -1.0f; QB.a22 = -1.0f;
pclary 5:475f67175510 228 PB = QB.inverse();
pclary 5:475f67175510 229 QC.translate(vector3(0.1f, -0.1f, 0.0f));
pclary 5:475f67175510 230 QC.a11 = -1.0f;
pclary 5:475f67175510 231 PC = QC.inverse();
pclary 5:475f67175510 232 QD.translate(vector3(-0.1f, 0.1f, 0.0f));
pclary 5:475f67175510 233 QD.a22 = -1.0f;
pclary 5:475f67175510 234 PD = QD.inverse();
pclary 8:db453051f3f4 235
pclary 8:db453051f3f4 236 // Start timers
pclary 8:db453051f3f4 237 deltaTimer.start();
pclary 9:a6d1502f0f20 238 stepTimer.start();
pclary 8:db453051f3f4 239
pclary 2:caf73a1d7827 240 // Go to initial position
pclary 9:a6d1502f0f20 241 legA.move(vector3(0.15f, 0.15f, 0.05f));
pclary 9:a6d1502f0f20 242 legB.move(vector3(0.15f, 0.15f, 0.05f));
pclary 9:a6d1502f0f20 243 legC.move(vector3(0.15f, 0.15f, 0.05f));
pclary 9:a6d1502f0f20 244 legD.move(vector3(0.15f, 0.15f, 0.05f));
pclary 8:db453051f3f4 245 legA.theta.enable(); wait(0.1f);
pclary 8:db453051f3f4 246 legB.theta.enable(); wait(0.1f);
pclary 8:db453051f3f4 247 legC.theta.enable(); wait(0.1f);
pclary 8:db453051f3f4 248 legD.theta.enable(); wait(0.1f);
pclary 8:db453051f3f4 249 legA.phi.enable(); wait(0.1f);
pclary 8:db453051f3f4 250 legB.phi.enable(); wait(0.1f);
pclary 8:db453051f3f4 251 legC.phi.enable(); wait(0.1f);
pclary 8:db453051f3f4 252 legD.phi.enable(); wait(0.1f);
pclary 8:db453051f3f4 253 legA.psi.enable(); wait(0.1f);
pclary 8:db453051f3f4 254 legB.psi.enable(); wait(0.1f);
pclary 8:db453051f3f4 255 legC.psi.enable(); wait(0.1f);
pclary 8:db453051f3f4 256 legD.psi.enable(); wait(0.1f);
pclary 9:a6d1502f0f20 257 wait(0.4f);
pclary 6:0163f2737cc6 258 resetLegs();
pclary 2:caf73a1d7827 259
pclary 2:caf73a1d7827 260 /*
pclary 2:caf73a1d7827 261 // Dump debug info
pclary 2:caf73a1d7827 262 sprintf(output, "T =\t%f\t%f\t%f\t%f\n\t%f\t%f\t%f\t%f\n\t%f\t%f\t%f\t%f\n\t0\t\t0\t\t0\t\t1\n",
pclary 2:caf73a1d7827 263 T.a11, T.a12, T.a13, T.a14,
pclary 2:caf73a1d7827 264 T.a21, T.a22, T.a23, T.a24,
pclary 2:caf73a1d7827 265 T.a31, T.a32, T.a33, T.a34);
pclary 8:db453051f3f4 266 terminal.write(output);
pclary 8:db453051f3f4 267 */
pclary 2:caf73a1d7827 268
pclary 0:449568595ed9 269
pclary 1:a5447cf54fba 270 while(true)
pclary 1:a5447cf54fba 271 {
pclary 6:0163f2737cc6 272 switch (state)
pclary 2:caf73a1d7827 273 {
pclary 6:0163f2737cc6 274 case walk:
pclary 9:a6d1502f0f20 275 // Debug stuff
pclary 9:a6d1502f0f20 276 switch (legState)
pclary 9:a6d1502f0f20 277 {
pclary 9:a6d1502f0f20 278 case A: debug1 = 1; debug2 = 0; debug3 = 0; debug4 = 0; break;
pclary 9:a6d1502f0f20 279 case B: debug1 = 0; debug2 = 1; debug3 = 0; debug4 = 0; break;
pclary 9:a6d1502f0f20 280 case C: debug1 = 0; debug2 = 0; debug3 = 1; debug4 = 0; break;
pclary 9:a6d1502f0f20 281 case D: debug1 = 0; debug2 = 0; debug3 = 0; debug4 = 1; break;
pclary 9:a6d1502f0f20 282 }
pclary 9:a6d1502f0f20 283
pclary 9:a6d1502f0f20 284 // Get delta-time
pclary 8:db453051f3f4 285 deltaTime = deltaTimer.read();
pclary 8:db453051f3f4 286 deltaTimer.reset();
pclary 8:db453051f3f4 287
pclary 6:0163f2737cc6 288 // Read controller input
pclary 6:0163f2737cc6 289 xaxis = 0.0078125f * deadzone((int8_t)((radio.rx_controller>>0)&0xff), 8); // Convert to +/-1.0f range
pclary 6:0163f2737cc6 290 yaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>8)&0xff), 8);
pclary 6:0163f2737cc6 291 turnaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>16)&0xff), 8);
pclary 6:0163f2737cc6 292
pclary 6:0163f2737cc6 293 // Compute delta movement vector and delta angle
pclary 9:a6d1502f0f20 294 speed = sqrt(xaxis*xaxis + yaxis*yaxis) * MAXSPEED;
pclary 8:db453051f3f4 295 v.x = -xaxis;
pclary 8:db453051f3f4 296 v.y = -yaxis;
pclary 6:0163f2737cc6 297 v.z = 0;
pclary 8:db453051f3f4 298 v = v * MAXSPEED * deltaTime;
pclary 8:db453051f3f4 299 angle = -turnaxis * MAXTURN * deltaTime;
pclary 9:a6d1502f0f20 300 stepDistance += deltaTime * (speed + fabs(angle * 0.141f));
pclary 6:0163f2737cc6 301
pclary 6:0163f2737cc6 302 // Compute movement transformation in robot coordinates
pclary 6:0163f2737cc6 303 T.identity().rotateZ(angle).translate(v).inverse();
pclary 6:0163f2737cc6 304
pclary 6:0163f2737cc6 305 // Update legs
pclary 6:0163f2737cc6 306 freeA = legA.update(PA*T*QA);
pclary 6:0163f2737cc6 307 freeB = legB.update(PB*T*QB);
pclary 6:0163f2737cc6 308 freeC = legC.update(PC*T*QC);
pclary 6:0163f2737cc6 309 freeD = legD.update(PD*T*QD);
pclary 6:0163f2737cc6 310
pclary 6:0163f2737cc6 311 // Update state
pclary 6:0163f2737cc6 312 switch (legState)
pclary 6:0163f2737cc6 313 {
pclary 6:0163f2737cc6 314 case A:
pclary 6:0163f2737cc6 315 if (!freeB || !freeC || !freeD) resetLegs();
pclary 9:a6d1502f0f20 316 else if (!freeA || stepDistance > stepDistanceTarget)
pclary 6:0163f2737cc6 317 {
pclary 6:0163f2737cc6 318 legA.reset(1.0f);
pclary 6:0163f2737cc6 319 legState = B;
pclary 9:a6d1502f0f20 320 stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
pclary 9:a6d1502f0f20 321 stepDistance = 0;
pclary 6:0163f2737cc6 322 }
pclary 6:0163f2737cc6 323 break;
pclary 6:0163f2737cc6 324
pclary 6:0163f2737cc6 325 case B:
pclary 6:0163f2737cc6 326 if (!freeA || !freeC || !freeD) resetLegs();
pclary 9:a6d1502f0f20 327 else if (!freeB || stepDistance > stepDistanceTarget)
pclary 6:0163f2737cc6 328 {
pclary 6:0163f2737cc6 329 legB.reset(1.0f);
pclary 6:0163f2737cc6 330 legState = C;
pclary 9:a6d1502f0f20 331 stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
pclary 9:a6d1502f0f20 332 stepDistance = 0;
pclary 6:0163f2737cc6 333 }
pclary 6:0163f2737cc6 334 break;
pclary 6:0163f2737cc6 335
pclary 6:0163f2737cc6 336 case C:
pclary 6:0163f2737cc6 337 if (!freeA || !freeB || !freeD) resetLegs();
pclary 9:a6d1502f0f20 338 else if (!freeC || stepDistance > stepDistanceTarget)
pclary 6:0163f2737cc6 339 {
pclary 6:0163f2737cc6 340 legC.reset(1.0f);
pclary 6:0163f2737cc6 341 legState = D;
pclary 9:a6d1502f0f20 342 stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
pclary 9:a6d1502f0f20 343 stepDistance = 0;
pclary 6:0163f2737cc6 344 }
pclary 6:0163f2737cc6 345 break;
pclary 6:0163f2737cc6 346
pclary 6:0163f2737cc6 347 case D:
pclary 6:0163f2737cc6 348 if (!freeA || !freeB || !freeC) resetLegs();
pclary 9:a6d1502f0f20 349 else if (!freeD || stepDistance > stepDistanceTarget)
pclary 6:0163f2737cc6 350 {
pclary 6:0163f2737cc6 351 legD.reset(1.0f);
pclary 6:0163f2737cc6 352 legState = A;
pclary 9:a6d1502f0f20 353 stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
pclary 9:a6d1502f0f20 354 stepDistance = 0;
pclary 6:0163f2737cc6 355 }
pclary 6:0163f2737cc6 356 break;
pclary 6:0163f2737cc6 357 }
pclary 6:0163f2737cc6 358
pclary 9:a6d1502f0f20 359 dataLog.push(stepDistanceTarget);
pclary 9:a6d1502f0f20 360
pclary 9:a6d1502f0f20 361
pclary 9:a6d1502f0f20 362
pclary 9:a6d1502f0f20 363 break; // case walk:
pclary 9:a6d1502f0f20 364
pclary 9:a6d1502f0f20 365
pclary 6:0163f2737cc6 366
pclary 6:0163f2737cc6 367 case reset:
pclary 9:a6d1502f0f20 368 // Debug stuff
pclary 9:a6d1502f0f20 369 switch (legState)
pclary 9:a6d1502f0f20 370 {
pclary 9:a6d1502f0f20 371 case A: debug1 = 0; debug2 = 1; debug3 = 1; debug4 = 1; break;
pclary 9:a6d1502f0f20 372 case B: debug1 = 1; debug2 = 0; debug3 = 1; debug4 = 1; break;
pclary 9:a6d1502f0f20 373 case C: debug1 = 1; debug2 = 1; debug3 = 0; debug4 = 1; break;
pclary 9:a6d1502f0f20 374 case D: debug1 = 1; debug2 = 1; debug3 = 1; debug4 = 0; break;
pclary 9:a6d1502f0f20 375 }
pclary 9:a6d1502f0f20 376
pclary 9:a6d1502f0f20 377 cycleTime = stepTimer.read();
pclary 8:db453051f3f4 378
pclary 8:db453051f3f4 379 if ((cycleTime <= RESET_STEP_TIME) && legState != A)
pclary 6:0163f2737cc6 380 {
pclary 6:0163f2737cc6 381 legA.reset(-0.5f);
pclary 6:0163f2737cc6 382 legState = A;
pclary 6:0163f2737cc6 383 }
pclary 8:db453051f3f4 384 else if ((cycleTime > RESET_STEP_TIME) && legState == A)
pclary 6:0163f2737cc6 385 {
pclary 6:0163f2737cc6 386 legB.reset(0.0f);
pclary 6:0163f2737cc6 387 legState = B;
pclary 6:0163f2737cc6 388 }
pclary 8:db453051f3f4 389 else if ((cycleTime > RESET_STEP_TIME * 2) && legState == B)
pclary 6:0163f2737cc6 390 {
pclary 6:0163f2737cc6 391 legC.reset(0.5f);
pclary 6:0163f2737cc6 392 legState = C;
pclary 6:0163f2737cc6 393 }
pclary 8:db453051f3f4 394 else if ((cycleTime > RESET_STEP_TIME * 3) && legState == C)
pclary 6:0163f2737cc6 395 {
pclary 6:0163f2737cc6 396 legD.reset(1.0f);
pclary 6:0163f2737cc6 397 legState = D;
pclary 6:0163f2737cc6 398 }
pclary 8:db453051f3f4 399 else if ((cycleTime > RESET_STEP_TIME * 4) && legState == D)
pclary 6:0163f2737cc6 400 {
pclary 8:db453051f3f4 401 walkLegs();
pclary 6:0163f2737cc6 402 }
pclary 8:db453051f3f4 403
pclary 8:db453051f3f4 404 T.identity();
pclary 8:db453051f3f4 405
pclary 8:db453051f3f4 406 freeA = legA.update(T);
pclary 8:db453051f3f4 407 freeB = legB.update(T);
pclary 8:db453051f3f4 408 freeC = legC.update(T);
pclary 8:db453051f3f4 409 freeD = legD.update(T);
pclary 6:0163f2737cc6 410
pclary 7:aac5f901bd76 411 break; // case reset:
pclary 7:aac5f901bd76 412 } // switch (state)
pclary 7:aac5f901bd76 413 } // while (true)
pclary 7:aac5f901bd76 414 } // main()