Patrick Clary / Mbed 2 deprecated WalkingRobot

Control program for a four-legged 12 axis robot.

Dependencies:   CircularBuffer Servo Terminal mbed Radio

main.cpp@18:8806d24809c2, 2013-05-27 (annotated)

Committer:
pclary
Date:
Mon May 27 20:08:03 2013 +0000
Revision:
18:8806d24809c2
Parent:
16:cc1ae2a289ee
Child:
19:efba54b23912
Added logic to keep balance, currently doesn't work

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