Patrick Clary / Mbed 2 deprecated WalkingRobot

Control program for a four-legged 12 axis robot.

Dependencies:   CircularBuffer Servo Terminal mbed Radio

main.cpp@20:bf46c0400b10, 2013-05-28 (annotated)

Committer:
pclary
Date:
Tue May 28 04:11:37 2013 +0000
Revision:
20:bf46c0400b10
Parent:
19:efba54b23912
Child:
21:c00567cbe6cc
Currently stays still if a requested move will put it out of balance

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 14:21f932d6069d 17 #define DIM_D 0.0275f
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 20:bf46c0400b10 22 #define PERIOD 0.005f
pclary 6:0163f2737cc6 23
pclary 11:9ee0214bd410 24
pclary 0:449568595ed9 25
pclary 3:6fa07ceb897f 26 CircularBuffer<float,16> dataLog;
pclary 9:a6d1502f0f20 27 Radio radio(p5, p6, p7, p16, p17, p18);
pclary 11:9ee0214bd410 28 RobotLeg legA(p26, p29, p30, false);
pclary 8:db453051f3f4 29 RobotLeg legB(p13, p14, p15, false);
pclary 11:9ee0214bd410 30 RobotLeg legC(p19, p11, p8, false);
pclary 11:9ee0214bd410 31 RobotLeg legD(p25, p24, p23, false);
pclary 20:bf46c0400b10 32 RobotLeg* leg[4] = { &legA, &legB, &legC, &legD };
pclary 20:bf46c0400b10 33 matrix4 QMat[4];
pclary 20:bf46c0400b10 34 matrix4 PMat[4];
pclary 6:0163f2737cc6 35
pclary 16:cc1ae2a289ee 36 DigitalOut led1(LED1);
pclary 16:cc1ae2a289ee 37 DigitalOut led2(LED2);
pclary 18:8806d24809c2 38 DigitalOut led3(LED3);
pclary 18:8806d24809c2 39 DigitalOut led4(LED4);
pclary 16:cc1ae2a289ee 40
pclary 13:1c5d255835ce 41
pclary 13:1c5d255835ce 42
pclary 12:a952bd74d363 43 CmdHandler* legpos(Terminal* terminal, const char*)
pclary 12:a952bd74d363 44 {
pclary 12:a952bd74d363 45 char output[256];
pclary 12:a952bd74d363 46 char abuf[64];
pclary 12:a952bd74d363 47 char bbuf[64];
pclary 12:a952bd74d363 48 char cbuf[64];
pclary 12:a952bd74d363 49 char dbuf[64];
pclary 12:a952bd74d363 50 legA.getPosition().print(abuf, 64);
pclary 12:a952bd74d363 51 legB.getPosition().print(bbuf, 64);
pclary 12:a952bd74d363 52 legC.getPosition().print(cbuf, 64);
pclary 12:a952bd74d363 53 legD.getPosition().print(dbuf, 64);
pclary 12:a952bd74d363 54 snprintf(output, 256, "A = [%s]\nB = [%s]\nC = [%s]\nD = [%s]", abuf, bbuf, cbuf, dbuf);
pclary 12:a952bd74d363 55 terminal->write(output);
pclary 12:a952bd74d363 56 return NULL;
pclary 12:a952bd74d363 57 }
pclary 12:a952bd74d363 58
pclary 6:0163f2737cc6 59
pclary 6:0163f2737cc6 60
pclary 6:0163f2737cc6 61 CmdHandler* log(Terminal* terminal, const char* input)
pclary 6:0163f2737cc6 62 {
pclary 6:0163f2737cc6 63 int start = 0;
pclary 6:0163f2737cc6 64 int end = 15;
pclary 6:0163f2737cc6 65 char output[256];
pclary 6:0163f2737cc6 66
pclary 6:0163f2737cc6 67 if (sscanf(input, "log %d %d", &start, &end) == 1)
pclary 6:0163f2737cc6 68 {
pclary 6:0163f2737cc6 69 // Print only one item
pclary 12:a952bd74d363 70 snprintf(output, 256, "%4d: %f\n", start, dataLog[start]);
pclary 6:0163f2737cc6 71 terminal->write(output);
pclary 6:0163f2737cc6 72 }
pclary 6:0163f2737cc6 73 else
pclary 6:0163f2737cc6 74 {
pclary 6:0163f2737cc6 75 // Print a range of items
pclary 6:0163f2737cc6 76 for (int i = start; i <= end; i++)
pclary 6:0163f2737cc6 77 {
pclary 12:a952bd74d363 78 snprintf(output, 256, "%4d: %f\n", i, dataLog[i]);
pclary 6:0163f2737cc6 79 terminal->write(output);
pclary 6:0163f2737cc6 80 }
pclary 6:0163f2737cc6 81 }
pclary 6:0163f2737cc6 82
pclary 6:0163f2737cc6 83 return NULL;
pclary 8:db453051f3f4 84 } // log()
pclary 6:0163f2737cc6 85
pclary 6:0163f2737cc6 86
pclary 6:0163f2737cc6 87
pclary 20:bf46c0400b10 88 bool processMovement(matrix4& TMat);
pclary 18:8806d24809c2 89 void setupLegs();
pclary 19:efba54b23912 90 void resetLegs();
pclary 18:8806d24809c2 91 float calcStability(vector3 p1, vector3 p2);
pclary 6:0163f2737cc6 92
pclary 19:efba54b23912 93
pclary 0:449568595ed9 94
pclary 0:449568595ed9 95 int main()
pclary 0:449568595ed9 96 {
pclary 9:a6d1502f0f20 97 Timer deltaTimer;
pclary 0:449568595ed9 98 Terminal terminal;
pclary 2:caf73a1d7827 99
pclary 2:caf73a1d7827 100 terminal.addCommand("log", &log);
pclary 12:a952bd74d363 101 terminal.addCommand("leg", &legpos);
pclary 2:caf73a1d7827 102
pclary 6:0163f2737cc6 103 radio.reset();
pclary 19:efba54b23912 104 setupLegs();
pclary 6:0163f2737cc6 105
pclary 20:bf46c0400b10 106 // Initialize matrices to change base from robot coordinates to leg coordinates
pclary 19:efba54b23912 107 QMat[0].translate(vector3(0.0508f, 0.0508f, 0.0f));
pclary 19:efba54b23912 108 QMat[1].translate(vector3(-0.0508f, -0.0508f, 0.0f));
pclary 19:efba54b23912 109 QMat[1].a11 = -1.0f; QMat[1].a22 = -1.0f;
pclary 19:efba54b23912 110 QMat[2].translate(vector3(-0.0508f, 0.0508f, 0.0f));
pclary 19:efba54b23912 111 QMat[2].a11 = -1.0f;
pclary 19:efba54b23912 112 QMat[3].translate(vector3(0.0508f, -0.0508f, 0.0f));
pclary 19:efba54b23912 113 QMat[3].a22 = -1.0f;
pclary 19:efba54b23912 114
pclary 19:efba54b23912 115 PMat[0] = QMat[0].inverse();
pclary 19:efba54b23912 116 PMat[1] = QMat[1].inverse();
pclary 19:efba54b23912 117 PMat[2] = QMat[2].inverse();
pclary 19:efba54b23912 118 PMat[3] = QMat[3].inverse();
pclary 19:efba54b23912 119
pclary 19:efba54b23912 120 matrix4 TMat;
pclary 11:9ee0214bd410 121
pclary 11:9ee0214bd410 122 // Start timer
pclary 11:9ee0214bd410 123 deltaTimer.start();
pclary 11:9ee0214bd410 124
pclary 20:bf46c0400b10 125 while (true)
pclary 11:9ee0214bd410 126 {
pclary 11:9ee0214bd410 127 while (deltaTimer.read() < PERIOD);
pclary 11:9ee0214bd410 128
pclary 11:9ee0214bd410 129 // Read controller input
pclary 19:efba54b23912 130 float xaxis = 0.0078125f * deadzone((int8_t)((radio.rx_controller>>0)&0xff), 8); // Convert to +/-1.0f range
pclary 19:efba54b23912 131 float yaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>8)&0xff), 8);
pclary 19:efba54b23912 132 float turnaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>16)&0xff), 8);
pclary 11:9ee0214bd410 133
pclary 19:efba54b23912 134 // Reset legs to sane positions when 'A' button is pressed
pclary 19:efba54b23912 135 if ((radio.rx_controller>>25)&0x1) resetLegs();
pclary 16:cc1ae2a289ee 136
pclary 11:9ee0214bd410 137 deltaTimer.reset();
pclary 20:bf46c0400b10 138 dataLog.push(deltaTimer.read());
pclary 11:9ee0214bd410 139
pclary 11:9ee0214bd410 140 // Compute delta movement vector and delta angle
pclary 19:efba54b23912 141 vector3 v(-xaxis, -yaxis, 0.0f);
pclary 20:bf46c0400b10 142 v = v * MAXSPEED * PERIOD;
pclary 20:bf46c0400b10 143 float angle = -turnaxis * MAXTURN * PERIOD;
pclary 11:9ee0214bd410 144
pclary 11:9ee0214bd410 145 // Compute movement transformation in robot coordinates
pclary 19:efba54b23912 146 TMat.identity().rotateZ(angle).translate(v).inverse();
pclary 18:8806d24809c2 147
pclary 20:bf46c0400b10 148 processMovement(TMat);
pclary 18:8806d24809c2 149
pclary 20:bf46c0400b10 150 } // while (true)
pclary 20:bf46c0400b10 151 } // main()
pclary 20:bf46c0400b10 152
pclary 20:bf46c0400b10 153
pclary 20:bf46c0400b10 154
pclary 20:bf46c0400b10 155 bool processMovement(matrix4& TMat)
pclary 20:bf46c0400b10 156 {
pclary 20:bf46c0400b10 157 // Get points used to calculate stability
pclary 20:bf46c0400b10 158 vector3 point1[4];
pclary 20:bf46c0400b10 159 vector3 point2[4];
pclary 20:bf46c0400b10 160 point1[0] = QMat[2]*leg[2]->getPosition();
pclary 20:bf46c0400b10 161 point1[1] = QMat[3]*leg[3]->getPosition();
pclary 20:bf46c0400b10 162 point1[2] = QMat[1]*leg[1]->getPosition();
pclary 20:bf46c0400b10 163 point1[3] = QMat[0]*leg[0]->getPosition();
pclary 20:bf46c0400b10 164 point2[0] = QMat[3]*leg[3]->getPosition();
pclary 20:bf46c0400b10 165 point2[1] = QMat[2]*leg[2]->getPosition();
pclary 20:bf46c0400b10 166 point2[2] = QMat[0]*leg[0]->getPosition();
pclary 20:bf46c0400b10 167 point2[3] = QMat[1]*leg[1]->getPosition();
pclary 20:bf46c0400b10 168
pclary 20:bf46c0400b10 169 // Check if each leg can perform this motion, find the next leg to step, and calculate stability of each leg
pclary 20:bf46c0400b10 170 bool legFree[4];
pclary 20:bf46c0400b10 171 float stepDist[4];
pclary 20:bf46c0400b10 172 float stability[4];
pclary 20:bf46c0400b10 173 for (int i = 0; i < 4; ++i)
pclary 20:bf46c0400b10 174 {
pclary 20:bf46c0400b10 175 legFree[i] = leg[i]->update(PMat[i]*TMat*QMat[i]);
pclary 20:bf46c0400b10 176 stepDist[i] = leg[i]->getStepDistance();
pclary 20:bf46c0400b10 177 stability[i] = calcStability(point1[i], point2[i]);
pclary 20:bf46c0400b10 178 }
pclary 20:bf46c0400b10 179
pclary 20:bf46c0400b10 180 // Check if each leg needs to step, and then check if it's stable before stepping
pclary 20:bf46c0400b10 181 bool stepping = leg[0]->getStepping() || leg[1]->getStepping() || leg[2]->getStepping() || leg[3]->getStepping();
pclary 20:bf46c0400b10 182 const float borderMax = 0.015f; // radius of support base in meters
pclary 20:bf46c0400b10 183 const float borderMin = 0.007f;
pclary 20:bf46c0400b10 184
pclary 20:bf46c0400b10 185 for (int i = 0; i < 4; ++i)
pclary 20:bf46c0400b10 186 {
pclary 20:bf46c0400b10 187 if (!legFree[i])
pclary 18:8806d24809c2 188 {
pclary 20:bf46c0400b10 189 if (stepping)
pclary 19:efba54b23912 190 {
pclary 20:bf46c0400b10 191 TMat.identity();
pclary 20:bf46c0400b10 192 return false;
pclary 20:bf46c0400b10 193 }
pclary 20:bf46c0400b10 194 else
pclary 20:bf46c0400b10 195 {
pclary 20:bf46c0400b10 196 if (stability[i] > borderMin)
pclary 19:efba54b23912 197 {
pclary 20:bf46c0400b10 198 // If stable, step
pclary 20:bf46c0400b10 199 leg[i]->reset(0.8);
pclary 20:bf46c0400b10 200 stepping = true;
pclary 20:bf46c0400b10 201 }
pclary 20:bf46c0400b10 202 else
pclary 20:bf46c0400b10 203 {
pclary 20:bf46c0400b10 204 // If unstable, move towards a stable position
pclary 20:bf46c0400b10 205 vector3 n;
pclary 20:bf46c0400b10 206 n.x = point2[i].y - point1[i].y;
pclary 20:bf46c0400b10 207 n.y = point1[i].x - point2[i].x;
pclary 20:bf46c0400b10 208 n = n.unit() * MAXSPEED * PERIOD;
pclary 20:bf46c0400b10 209 TMat.identity().translate(n).inverse();
pclary 20:bf46c0400b10 210 return false;
pclary 19:efba54b23912 211 }
pclary 19:efba54b23912 212 }
pclary 18:8806d24809c2 213 }
pclary 20:bf46c0400b10 214 }
pclary 20:bf46c0400b10 215
pclary 20:bf46c0400b10 216 // Check if the next leg to step is stable
pclary 20:bf46c0400b10 217 int next = least(stepDist[0], stepDist[1], stepDist[2], stepDist[3]);
pclary 20:bf46c0400b10 218 if (stability[next] > borderMax)
pclary 20:bf46c0400b10 219 {
pclary 20:bf46c0400b10 220 // Continue to carry out step as normal
pclary 20:bf46c0400b10 221 }
pclary 20:bf46c0400b10 222 else if (stability[next] > borderMin)
pclary 20:bf46c0400b10 223 {
pclary 20:bf46c0400b10 224 if (stepping)
pclary 20:bf46c0400b10 225 {
pclary 20:bf46c0400b10 226 TMat.identity();
pclary 20:bf46c0400b10 227 return false;
pclary 20:bf46c0400b10 228 }
pclary 20:bf46c0400b10 229 else
pclary 18:8806d24809c2 230 {
pclary 20:bf46c0400b10 231 leg[next]->reset(0.8);
pclary 20:bf46c0400b10 232 stepping = true;
pclary 19:efba54b23912 233 }
pclary 20:bf46c0400b10 234 }
pclary 20:bf46c0400b10 235 else
pclary 20:bf46c0400b10 236 {
pclary 20:bf46c0400b10 237 // If unstable, move towards a stable position
pclary 20:bf46c0400b10 238 vector3 n;
pclary 20:bf46c0400b10 239 n.x = point2[next].y - point1[next].y;
pclary 20:bf46c0400b10 240 n.y = point1[next].x - point2[next].x;
pclary 20:bf46c0400b10 241 n = n.unit() * MAXSPEED * PERIOD;
pclary 20:bf46c0400b10 242 TMat.identity().translate(n).inverse();
pclary 20:bf46c0400b10 243 return false;
pclary 20:bf46c0400b10 244 }
pclary 20:bf46c0400b10 245
pclary 20:bf46c0400b10 246 for (int i = 0; i < 4; ++i)
pclary 20:bf46c0400b10 247 {
pclary 20:bf46c0400b10 248 leg[i]->apply();
pclary 20:bf46c0400b10 249 }
pclary 20:bf46c0400b10 250
pclary 20:bf46c0400b10 251 // Debug info
pclary 20:bf46c0400b10 252 led1 = stability[0] > borderMin;
pclary 20:bf46c0400b10 253 led2 = stability[1] > borderMin;
pclary 20:bf46c0400b10 254 led3 = stability[2] > borderMin;
pclary 20:bf46c0400b10 255 led4 = stability[3] > borderMin;
pclary 20:bf46c0400b10 256
pclary 20:bf46c0400b10 257 return true;
pclary 20:bf46c0400b10 258 }
pclary 11:9ee0214bd410 259
pclary 11:9ee0214bd410 260
pclary 11:9ee0214bd410 261
pclary 19:efba54b23912 262 void resetLegs()
pclary 19:efba54b23912 263 {
pclary 19:efba54b23912 264 matrix4 T;
pclary 19:efba54b23912 265 legA.reset(-0.6f);
pclary 20:bf46c0400b10 266 while (legA.getStepping())
pclary 20:bf46c0400b10 267 {
pclary 20:bf46c0400b10 268 legA.update(T);
pclary 20:bf46c0400b10 269 legA.apply();
pclary 20:bf46c0400b10 270 }
pclary 19:efba54b23912 271 legB.reset(-0.1f);
pclary 20:bf46c0400b10 272 while (legB.getStepping())
pclary 20:bf46c0400b10 273 {
pclary 20:bf46c0400b10 274 legB.update(T);
pclary 20:bf46c0400b10 275 legB.apply();
pclary 20:bf46c0400b10 276 }
pclary 19:efba54b23912 277 legC.reset(0.4f);
pclary 20:bf46c0400b10 278 while (legC.getStepping())
pclary 20:bf46c0400b10 279 {
pclary 20:bf46c0400b10 280 legC.update(T);
pclary 20:bf46c0400b10 281 legC.apply();
pclary 20:bf46c0400b10 282 }
pclary 19:efba54b23912 283 legD.reset(0.9f);
pclary 20:bf46c0400b10 284 while (legD.getStepping())
pclary 20:bf46c0400b10 285 {
pclary 20:bf46c0400b10 286 legD.update(T);
pclary 20:bf46c0400b10 287 legD.apply();
pclary 20:bf46c0400b10 288 }
pclary 19:efba54b23912 289 }
pclary 19:efba54b23912 290
pclary 19:efba54b23912 291
pclary 19:efba54b23912 292
pclary 11:9ee0214bd410 293 void setupLegs()
pclary 11:9ee0214bd410 294 {
pclary 0:449568595ed9 295 // Set leg parameters
pclary 8:db453051f3f4 296 legA.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
pclary 8:db453051f3f4 297 legB.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
pclary 8:db453051f3f4 298 legC.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
pclary 8:db453051f3f4 299 legD.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
pclary 5:475f67175510 300 legA.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
pclary 5:475f67175510 301 legB.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
pclary 5:475f67175510 302 legC.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
pclary 5:475f67175510 303 legD.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
pclary 8:db453051f3f4 304 legA.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
pclary 8:db453051f3f4 305 legB.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
pclary 8:db453051f3f4 306 legC.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
pclary 8:db453051f3f4 307 legD.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
pclary 14:21f932d6069d 308 legA.theta.calibrate(1130, 2080, 45.0f, -45.0f);
pclary 14:21f932d6069d 309 legA.phi.calibrate(1150, 2080, 70.0f, -45.0f);
pclary 14:21f932d6069d 310 legA.psi.calibrate(1985, 1055, 70.0f, -60.0f);
pclary 14:21f932d6069d 311 legB.theta.calibrate(990, 1940, 45.0f, -45.0f);
pclary 14:21f932d6069d 312 legB.phi.calibrate(1105, 2055, 70.0f, -45.0f);
pclary 14:21f932d6069d 313 legB.psi.calibrate(2090, 1150, 70.0f, -60.0f);
pclary 14:21f932d6069d 314 legC.theta.calibrate(1930, 860, 45.0f, -45.0f);
pclary 14:21f932d6069d 315 legC.phi.calibrate(1945, 1000, 70.0f, -45.0f);
pclary 14:21f932d6069d 316 legC.psi.calibrate(1085, 2005, 70.0f, -60.0f);
pclary 14:21f932d6069d 317 legD.theta.calibrate(2020, 1080, 45.0f, -45.0f);
pclary 14:21f932d6069d 318 legD.phi.calibrate(2085, 1145, 70.0f, -45.0f);
pclary 14:21f932d6069d 319 legD.psi.calibrate(1070, 2010, 70.0f, -60.0f);
pclary 6:0163f2737cc6 320
pclary 6:0163f2737cc6 321 // Initialize leg position deltas
pclary 6:0163f2737cc6 322 legA.nDeltaPosition = vector3(0.0f, 0.01f, 0.0f);
pclary 6:0163f2737cc6 323 legB.nDeltaPosition = vector3(0.0f, -0.01f, 0.0f);
pclary 6:0163f2737cc6 324 legC.nDeltaPosition = vector3(0.0f, 0.01f, 0.0f);
pclary 6:0163f2737cc6 325 legD.nDeltaPosition = vector3(0.0f, -0.01f, 0.0f);
pclary 8:db453051f3f4 326
pclary 2:caf73a1d7827 327 // Go to initial position
pclary 9:a6d1502f0f20 328 legA.move(vector3(0.15f, 0.15f, 0.05f));
pclary 9:a6d1502f0f20 329 legB.move(vector3(0.15f, 0.15f, 0.05f));
pclary 9:a6d1502f0f20 330 legC.move(vector3(0.15f, 0.15f, 0.05f));
pclary 9:a6d1502f0f20 331 legD.move(vector3(0.15f, 0.15f, 0.05f));
pclary 8:db453051f3f4 332 legA.theta.enable(); wait(0.1f);
pclary 8:db453051f3f4 333 legB.theta.enable(); wait(0.1f);
pclary 8:db453051f3f4 334 legC.theta.enable(); wait(0.1f);
pclary 8:db453051f3f4 335 legD.theta.enable(); wait(0.1f);
pclary 8:db453051f3f4 336 legA.phi.enable(); wait(0.1f);
pclary 8:db453051f3f4 337 legB.phi.enable(); wait(0.1f);
pclary 8:db453051f3f4 338 legC.phi.enable(); wait(0.1f);
pclary 8:db453051f3f4 339 legD.phi.enable(); wait(0.1f);
pclary 8:db453051f3f4 340 legA.psi.enable(); wait(0.1f);
pclary 8:db453051f3f4 341 legB.psi.enable(); wait(0.1f);
pclary 8:db453051f3f4 342 legC.psi.enable(); wait(0.1f);
pclary 8:db453051f3f4 343 legD.psi.enable(); wait(0.1f);
pclary 9:a6d1502f0f20 344 wait(0.4f);
pclary 20:bf46c0400b10 345 legA.reset(-0.6f);
pclary 20:bf46c0400b10 346 legB.reset(-0.1f);
pclary 20:bf46c0400b10 347 legC.reset(0.4f);
pclary 20:bf46c0400b10 348 legD.reset(0.9f);
pclary 20:bf46c0400b10 349 matrix4 T;
pclary 20:bf46c0400b10 350 while (legA.getStepping())
pclary 20:bf46c0400b10 351 {
pclary 20:bf46c0400b10 352 legA.update(T);
pclary 20:bf46c0400b10 353 legA.apply();
pclary 20:bf46c0400b10 354 legB.update(T);
pclary 20:bf46c0400b10 355 legB.apply();
pclary 20:bf46c0400b10 356 legC.update(T);
pclary 20:bf46c0400b10 357 legC.apply();
pclary 20:bf46c0400b10 358 legD.update(T);
pclary 20:bf46c0400b10 359 legD.apply();
pclary 20:bf46c0400b10 360 }
pclary 11:9ee0214bd410 361 }
pclary 18:8806d24809c2 362
pclary 18:8806d24809c2 363
pclary 18:8806d24809c2 364
pclary 18:8806d24809c2 365 float calcStability(vector3 p1, vector3 p2)
pclary 18:8806d24809c2 366 {
pclary 18:8806d24809c2 367 float lx, ly, vx, vy;
pclary 18:8806d24809c2 368 lx = p2.x - p1.x;
pclary 18:8806d24809c2 369 ly = p2.y - p1.y;
pclary 18:8806d24809c2 370 vx = -p1.x;
pclary 18:8806d24809c2 371 vy = -p1.y;
pclary 18:8806d24809c2 372
pclary 18:8806d24809c2 373 return (ly*vx - lx*vy)/sqrt(lx*lx + ly*ly);
pclary 18:8806d24809c2 374 }
pclary 18:8806d24809c2 375