1
Dependencies: CircularBuffer Radio Servo Terminal mbed
Fork of WalkingRobot by
main.cpp@5:475f67175510, 2013-01-13 (annotated)
- Committer:
- pclary
- Date:
- Sun Jan 13 02:59:48 2013 +0000
- Revision:
- 5:475f67175510
- Parent:
- 3:6fa07ceb897f
- Child:
- 6:0163f2737cc6
Changed things, terminal isn't working properly
Who changed what in which revision?
User | Revision | Line number | New 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 | 5:475f67175510 | 6 | #include "commands.h" |
pclary | 0:449568595ed9 | 7 | #include <cstring> |
pclary | 0:449568595ed9 | 8 | |
pclary | 3:6fa07ceb897f | 9 | |
pclary | 0:449568595ed9 | 10 | |
pclary | 3:6fa07ceb897f | 11 | CircularBuffer<float,16> dataLog; |
pclary | 2:caf73a1d7827 | 12 | float radius, speed; |
pclary | 5:475f67175510 | 13 | RobotLeg legA(p26, p29, p30); |
pclary | 5:475f67175510 | 14 | RobotLeg legB(p13, p14, p15); |
pclary | 5:475f67175510 | 15 | RobotLeg legC(p12, p11, p8); |
pclary | 5:475f67175510 | 16 | RobotLeg legD(p23, p24, p25); |
pclary | 5:475f67175510 | 17 | Radio radio(p5, p6, p7, p17, p19, p20); |
pclary | 0:449568595ed9 | 18 | |
pclary | 0:449568595ed9 | 19 | |
pclary | 0:449568595ed9 | 20 | |
pclary | 0:449568595ed9 | 21 | int main() |
pclary | 0:449568595ed9 | 22 | { |
pclary | 5:475f67175510 | 23 | Timer deltaTimer; |
pclary | 5:475f67175510 | 24 | Timer cycleTimer; |
pclary | 5:475f67175510 | 25 | float angle, arc; |
pclary | 2:caf73a1d7827 | 26 | vector3 v; |
pclary | 5:475f67175510 | 27 | matrix4 T; |
pclary | 5:475f67175510 | 28 | matrix4 PA, QA; |
pclary | 5:475f67175510 | 29 | matrix4 PB, QB; |
pclary | 5:475f67175510 | 30 | matrix4 PC, QC; |
pclary | 5:475f67175510 | 31 | matrix4 PD, QD; |
pclary | 0:449568595ed9 | 32 | Terminal terminal; |
pclary | 2:caf73a1d7827 | 33 | |
pclary | 2:caf73a1d7827 | 34 | terminal.addCommand("log", &log); |
pclary | 2:caf73a1d7827 | 35 | terminal.addCommand("speed", &setspeed); |
pclary | 2:caf73a1d7827 | 36 | terminal.addCommand("radius", &setradius); |
pclary | 5:475f67175510 | 37 | terminal.addCommand("read", &read); |
pclary | 2:caf73a1d7827 | 38 | |
pclary | 0:449568595ed9 | 39 | // Set leg parameters |
pclary | 5:475f67175510 | 40 | legA.setDimensions(0.11f, 0.125f, -0.006f, 0.020f); |
pclary | 5:475f67175510 | 41 | legB.setDimensions(0.11f, 0.125f, -0.006f, 0.020f); |
pclary | 5:475f67175510 | 42 | legC.setDimensions(0.11f, 0.125f, -0.006f, 0.020f); |
pclary | 5:475f67175510 | 43 | legD.setDimensions(0.11f, 0.125f, -0.006f, 0.020f); |
pclary | 5:475f67175510 | 44 | legA.setAngleOffsets(0.7853982f, 0.0f, 0.0f); |
pclary | 5:475f67175510 | 45 | legB.setAngleOffsets(0.7853982f, 0.0f, 0.0f); |
pclary | 5:475f67175510 | 46 | legC.setAngleOffsets(0.7853982f, 0.0f, 0.0f); |
pclary | 5:475f67175510 | 47 | legD.setAngleOffsets(0.7853982f, 0.0f, 0.0f); |
pclary | 5:475f67175510 | 48 | legA.setStepCircle(0.12, 0.11, -0.15, 0.11); |
pclary | 5:475f67175510 | 49 | legB.setStepCircle(0.12, 0.11, -0.15, 0.11); |
pclary | 5:475f67175510 | 50 | legC.setStepCircle(0.12, 0.11, -0.15, 0.11); |
pclary | 5:475f67175510 | 51 | legD.setStepCircle(0.12, 0.11, -0.15, 0.11); |
pclary | 5:475f67175510 | 52 | legA.theta.calibrate(1000, 2000, 45.0f, -45.0f); |
pclary | 5:475f67175510 | 53 | legA.phi.calibrate(1000, 2000, 70.0f, -45.0f); |
pclary | 5:475f67175510 | 54 | legA.psi.calibrate(2000, 1000, 80.0f, -45.0f); |
pclary | 5:475f67175510 | 55 | legB.theta.calibrate(1000, 2000, 45.0f, -45.0f); |
pclary | 5:475f67175510 | 56 | legB.phi.calibrate(1000, 2000, 70.0f, -45.0f); |
pclary | 5:475f67175510 | 57 | legB.psi.calibrate(2000, 1000, 80.0f, -45.0f); |
pclary | 5:475f67175510 | 58 | legC.theta.calibrate(2000, 1000, 45.0f, -45.0f); |
pclary | 5:475f67175510 | 59 | legC.phi.calibrate(2000, 1000, 70.0f, -45.0f); |
pclary | 5:475f67175510 | 60 | legC.psi.calibrate(1000, 2000, 80.0f, -45.0f); |
pclary | 5:475f67175510 | 61 | legD.theta.calibrate(2000, 1000, 45.0f, -45.0f); |
pclary | 5:475f67175510 | 62 | legD.phi.calibrate(2000, 1000, 70.0f, -45.0f); |
pclary | 5:475f67175510 | 63 | legD.psi.calibrate(1000, 2000, 80.0f, -45.0f); |
pclary | 5:475f67175510 | 64 | |
pclary | 2:caf73a1d7827 | 65 | // Create matrices to change base from robot coordinates to leg coordinates |
pclary | 5:475f67175510 | 66 | QA.translate(vector3(0.1f, 0.1f, 0.0f)); |
pclary | 5:475f67175510 | 67 | PA = QA.inverse(); |
pclary | 5:475f67175510 | 68 | QB.translate(vector3(-0.1f, -0.1f, 0.0f)); |
pclary | 5:475f67175510 | 69 | QB.a11 = -1.0f; QB.a22 = -1.0f; |
pclary | 5:475f67175510 | 70 | PB = QB.inverse(); |
pclary | 5:475f67175510 | 71 | QC.translate(vector3(0.1f, -0.1f, 0.0f)); |
pclary | 5:475f67175510 | 72 | QC.a11 = -1.0f; |
pclary | 5:475f67175510 | 73 | PC = QC.inverse(); |
pclary | 5:475f67175510 | 74 | QD.translate(vector3(-0.1f, 0.1f, 0.0f)); |
pclary | 5:475f67175510 | 75 | QD.a22 = -1.0f; |
pclary | 5:475f67175510 | 76 | PD = QD.inverse(); |
pclary | 2:caf73a1d7827 | 77 | |
pclary | 2:caf73a1d7827 | 78 | // Go to initial position |
pclary | 5:475f67175510 | 79 | legA.reset(-0.5f); |
pclary | 5:475f67175510 | 80 | legB.reset(0.0f); |
pclary | 5:475f67175510 | 81 | legC.reset(0.5f); |
pclary | 5:475f67175510 | 82 | legD.reset(1.0f); |
pclary | 2:caf73a1d7827 | 83 | |
pclary | 2:caf73a1d7827 | 84 | // Set radius and speed to go straight forward |
pclary | 2:caf73a1d7827 | 85 | radius = 0.0f; // meters |
pclary | 2:caf73a1d7827 | 86 | speed = 0.0f; // m/s |
pclary | 2:caf73a1d7827 | 87 | |
pclary | 5:475f67175510 | 88 | deltaTimer.start(); |
pclary | 5:475f67175510 | 89 | cycleTimer.start(); |
pclary | 2:caf73a1d7827 | 90 | |
pclary | 2:caf73a1d7827 | 91 | /* |
pclary | 2:caf73a1d7827 | 92 | // Dump debug info |
pclary | 2:caf73a1d7827 | 93 | 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 | 94 | T.a11, T.a12, T.a13, T.a14, |
pclary | 2:caf73a1d7827 | 95 | T.a21, T.a22, T.a23, T.a24, |
pclary | 2:caf73a1d7827 | 96 | T.a31, T.a32, T.a33, T.a34); |
pclary | 2:caf73a1d7827 | 97 | terminal.write(output); */ |
pclary | 2:caf73a1d7827 | 98 | |
pclary | 0:449568595ed9 | 99 | |
pclary | 1:a5447cf54fba | 100 | while(true) |
pclary | 1:a5447cf54fba | 101 | { |
pclary | 5:475f67175510 | 102 | // Get arc length of movement delta |
pclary | 5:475f67175510 | 103 | arc = speed*deltaTimer.read(); |
pclary | 5:475f67175510 | 104 | dataLog.push(deltaTimer.read()); |
pclary | 5:475f67175510 | 105 | deltaTimer.reset(); |
pclary | 2:caf73a1d7827 | 106 | |
pclary | 2:caf73a1d7827 | 107 | if (radius == 0) |
pclary | 2:caf73a1d7827 | 108 | { |
pclary | 2:caf73a1d7827 | 109 | angle = 0.0f; |
pclary | 2:caf73a1d7827 | 110 | v.x = 0.0f; |
pclary | 2:caf73a1d7827 | 111 | v.y = arc; |
pclary | 2:caf73a1d7827 | 112 | v.z = 0.0f; |
pclary | 2:caf73a1d7827 | 113 | } |
pclary | 2:caf73a1d7827 | 114 | else |
pclary | 2:caf73a1d7827 | 115 | { |
pclary | 2:caf73a1d7827 | 116 | angle = -arc/radius; |
pclary | 2:caf73a1d7827 | 117 | v.x = radius*(1 - cos(-angle)); |
pclary | 2:caf73a1d7827 | 118 | v.y = radius*sin(-angle); |
pclary | 2:caf73a1d7827 | 119 | v.z = 0.0f; |
pclary | 2:caf73a1d7827 | 120 | } |
pclary | 2:caf73a1d7827 | 121 | |
pclary | 5:475f67175510 | 122 | // Compute movement transformation in robot coordinates |
pclary | 5:475f67175510 | 123 | T.identity().rotateZ(angle).translate(v).inverse(); |
pclary | 2:caf73a1d7827 | 124 | |
pclary | 5:475f67175510 | 125 | legA.update(PA*T*QA); |
pclary | 5:475f67175510 | 126 | legB.update(PB*T*QB); |
pclary | 5:475f67175510 | 127 | legC.update(PC*T*QC); |
pclary | 5:475f67175510 | 128 | legD.update(PD*T*QD); |
pclary | 1:a5447cf54fba | 129 | } |
pclary | 0:449568595ed9 | 130 | } |