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@8:db453051f3f4, 2013-01-17 (annotated)

Committer:
pclary
Date:
Thu Jan 17 18:34:09 2013 +0000
Revision:
8:db453051f3f4
Parent:
7:aac5f901bd76
Child:
9:a6d1502f0f20
Changed leg position initialization and resynchronization routines.

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