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@10:dc1ba352667e, 2013-04-09 (annotated)

Committer:
pclary
Date:
Tue Apr 09 01:36:50 2013 +0000
Revision:
10:dc1ba352667e
Parent:
9:a6d1502f0f20
Child:
11:9ee0214bd410
Probably reverted some things to a version that actually works

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