Patrick Clary / Mbed 2 deprecated WalkingRobot

Control program for a four-legged 12 axis robot.

Dependencies:   CircularBuffer Servo Terminal mbed Radio

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?

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 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 }