Alex Chepilev / Mbed 2 deprecated WalkingRobot

1

Dependencies:   CircularBuffer Radio Servo Terminal mbed

Fork of WalkingRobot by Patrick Clary

main.cpp@7:aac5f901bd76, 2013-01-13 (annotated)

Committer:
pclary
Date:
Sun Jan 13 20:43:28 2013 +0000
Revision:
7:aac5f901bd76
Parent:
6:0163f2737cc6
Child:
8:db453051f3f4
Fixed calibration values

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