1
Dependencies: CircularBuffer Radio Servo Terminal mbed
Fork of WalkingRobot by
main.cpp@2:caf73a1d7827, 2012-10-22 (annotated)
- Committer:
- pclary
- Date:
- Mon Oct 22 22:06:11 2012 +0000
- Revision:
- 2:caf73a1d7827
- Parent:
- 1:a5447cf54fba
- Child:
- 3:6fa07ceb897f
Added reference frame motion and data logging
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 | 0:449568595ed9 | 3 | #include "Terminal.h" |
pclary | 2:caf73a1d7827 | 4 | #include "CircularBuffer.h" |
pclary | 0:449568595ed9 | 5 | #include <cstring> |
pclary | 0:449568595ed9 | 6 | |
pclary | 2:caf73a1d7827 | 7 | CircularBuffer<float,16> dataLog; |
pclary | 0:449568595ed9 | 8 | |
pclary | 0:449568595ed9 | 9 | RobotLeg leg(p17, p15, p13); |
pclary | 0:449568595ed9 | 10 | |
pclary | 2:caf73a1d7827 | 11 | float radius, speed; |
pclary | 2:caf73a1d7827 | 12 | |
pclary | 0:449568595ed9 | 13 | |
pclary | 0:449568595ed9 | 14 | |
pclary | 2:caf73a1d7827 | 15 | CmdHandler* log(Terminal* terminal, const char* input) |
pclary | 0:449568595ed9 | 16 | { |
pclary | 2:caf73a1d7827 | 17 | int start = 0; |
pclary | 2:caf73a1d7827 | 18 | int end = 15; |
pclary | 0:449568595ed9 | 19 | char output[256]; |
pclary | 0:449568595ed9 | 20 | |
pclary | 2:caf73a1d7827 | 21 | if (sscanf(input, "log %d %d", &start, &end) == 1) |
pclary | 2:caf73a1d7827 | 22 | { |
pclary | 2:caf73a1d7827 | 23 | // Print only one item |
pclary | 2:caf73a1d7827 | 24 | sprintf(output, "%4d: %f\n", start, dataLog[start]); |
pclary | 2:caf73a1d7827 | 25 | terminal->write(output); |
pclary | 2:caf73a1d7827 | 26 | } |
pclary | 2:caf73a1d7827 | 27 | else |
pclary | 0:449568595ed9 | 28 | { |
pclary | 2:caf73a1d7827 | 29 | // Print a range of items |
pclary | 2:caf73a1d7827 | 30 | for (int i = start; i <= end; i++) |
pclary | 2:caf73a1d7827 | 31 | { |
pclary | 2:caf73a1d7827 | 32 | sprintf(output, "%4d: %f\n", i, dataLog[i]); |
pclary | 2:caf73a1d7827 | 33 | terminal->write(output); |
pclary | 2:caf73a1d7827 | 34 | } |
pclary | 2:caf73a1d7827 | 35 | } |
pclary | 0:449568595ed9 | 36 | |
pclary | 2:caf73a1d7827 | 37 | return NULL; |
pclary | 2:caf73a1d7827 | 38 | } |
pclary | 2:caf73a1d7827 | 39 | |
pclary | 2:caf73a1d7827 | 40 | |
pclary | 2:caf73a1d7827 | 41 | |
pclary | 2:caf73a1d7827 | 42 | CmdHandler* setspeed(Terminal* terminal, const char* input) |
pclary | 2:caf73a1d7827 | 43 | { |
pclary | 2:caf73a1d7827 | 44 | const float maxSpeed = 0.5f; |
pclary | 2:caf73a1d7827 | 45 | const float minSpeed = -0.5f; |
pclary | 2:caf73a1d7827 | 46 | |
pclary | 2:caf73a1d7827 | 47 | float newSpeed; |
pclary | 2:caf73a1d7827 | 48 | char output[256]; |
pclary | 2:caf73a1d7827 | 49 | |
pclary | 2:caf73a1d7827 | 50 | if (sscanf(input, "speed %f", &newSpeed) == 1) |
pclary | 2:caf73a1d7827 | 51 | { |
pclary | 2:caf73a1d7827 | 52 | newSpeed = (newSpeed > maxSpeed ? maxSpeed : (newSpeed < minSpeed ? minSpeed : newSpeed)); |
pclary | 2:caf73a1d7827 | 53 | sprintf(output, "new speed: %f m/s", newSpeed); |
pclary | 2:caf73a1d7827 | 54 | speed = newSpeed; |
pclary | 2:caf73a1d7827 | 55 | } |
pclary | 0:449568595ed9 | 56 | else |
pclary | 2:caf73a1d7827 | 57 | { |
pclary | 2:caf73a1d7827 | 58 | sprintf(output, "error reading input parameters"); |
pclary | 2:caf73a1d7827 | 59 | } |
pclary | 2:caf73a1d7827 | 60 | |
pclary | 0:449568595ed9 | 61 | terminal->write(output); |
pclary | 0:449568595ed9 | 62 | |
pclary | 0:449568595ed9 | 63 | return NULL; |
pclary | 0:449568595ed9 | 64 | } |
pclary | 0:449568595ed9 | 65 | |
pclary | 0:449568595ed9 | 66 | |
pclary | 0:449568595ed9 | 67 | |
pclary | 2:caf73a1d7827 | 68 | CmdHandler* setradius(Terminal* terminal, const char* input) |
pclary | 0:449568595ed9 | 69 | { |
pclary | 2:caf73a1d7827 | 70 | float newRadius; |
pclary | 0:449568595ed9 | 71 | char output[256]; |
pclary | 0:449568595ed9 | 72 | |
pclary | 2:caf73a1d7827 | 73 | if (sscanf(input, "radius %f", &newRadius) == 1) |
pclary | 0:449568595ed9 | 74 | { |
pclary | 2:caf73a1d7827 | 75 | sprintf(output, "new turning radius: %f m", newRadius); |
pclary | 2:caf73a1d7827 | 76 | radius = newRadius; |
pclary | 0:449568595ed9 | 77 | } |
pclary | 0:449568595ed9 | 78 | else |
pclary | 0:449568595ed9 | 79 | { |
pclary | 0:449568595ed9 | 80 | sprintf(output, "error reading input parameters"); |
pclary | 0:449568595ed9 | 81 | } |
pclary | 1:a5447cf54fba | 82 | |
pclary | 0:449568595ed9 | 83 | terminal->write(output); |
pclary | 0:449568595ed9 | 84 | |
pclary | 0:449568595ed9 | 85 | return NULL; |
pclary | 0:449568595ed9 | 86 | } |
pclary | 0:449568595ed9 | 87 | |
pclary | 0:449568595ed9 | 88 | |
pclary | 0:449568595ed9 | 89 | |
pclary | 0:449568595ed9 | 90 | int main() |
pclary | 0:449568595ed9 | 91 | { |
pclary | 2:caf73a1d7827 | 92 | Timer timer; |
pclary | 2:caf73a1d7827 | 93 | float angle, arc; // radius, speed are globals now |
pclary | 2:caf73a1d7827 | 94 | vector3 v; |
pclary | 2:caf73a1d7827 | 95 | matrix4 P, Q, T; |
pclary | 0:449568595ed9 | 96 | Terminal terminal; |
pclary | 2:caf73a1d7827 | 97 | |
pclary | 2:caf73a1d7827 | 98 | terminal.addCommand("log", &log); |
pclary | 2:caf73a1d7827 | 99 | terminal.addCommand("speed", &setspeed); |
pclary | 2:caf73a1d7827 | 100 | terminal.addCommand("radius", &setradius); |
pclary | 2:caf73a1d7827 | 101 | |
pclary | 0:449568595ed9 | 102 | // Set leg parameters |
pclary | 0:449568595ed9 | 103 | leg.setDimensions(0.094f, 0.104f, -0.006f, 0.020f); |
pclary | 0:449568595ed9 | 104 | leg.setAngleOffsets(0.7853982f, 0.0f, 0.0f); |
pclary | 0:449568595ed9 | 105 | leg.theta.calibrate(950, 2200, 60.0f, -60.0f); |
pclary | 0:449568595ed9 | 106 | leg.phi.calibrate(975, 2250, 80.0f, -60.0f); |
pclary | 0:449568595ed9 | 107 | leg.psi.calibrate(2400, 1125, 60.0f, -60.0f); |
pclary | 0:449568595ed9 | 108 | |
pclary | 2:caf73a1d7827 | 109 | // Create matrices to change base from robot coordinates to leg coordinates |
pclary | 2:caf73a1d7827 | 110 | Q.translate(vector3(0.08f, 0.08f, 0.0f)); |
pclary | 2:caf73a1d7827 | 111 | P = Q.inverse(); |
pclary | 2:caf73a1d7827 | 112 | |
pclary | 2:caf73a1d7827 | 113 | // Go to initial position |
pclary | 2:caf73a1d7827 | 114 | leg.step(vector3(0.1f, 0.1f, -0.05f)); |
pclary | 2:caf73a1d7827 | 115 | |
pclary | 2:caf73a1d7827 | 116 | // Set radius and speed to go straight forward |
pclary | 2:caf73a1d7827 | 117 | radius = 0.0f; // meters |
pclary | 2:caf73a1d7827 | 118 | speed = 0.0f; // m/s |
pclary | 2:caf73a1d7827 | 119 | |
pclary | 2:caf73a1d7827 | 120 | timer.start(); |
pclary | 2:caf73a1d7827 | 121 | |
pclary | 2:caf73a1d7827 | 122 | /* |
pclary | 2:caf73a1d7827 | 123 | // Dump debug info |
pclary | 2:caf73a1d7827 | 124 | 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 | 125 | T.a11, T.a12, T.a13, T.a14, |
pclary | 2:caf73a1d7827 | 126 | T.a21, T.a22, T.a23, T.a24, |
pclary | 2:caf73a1d7827 | 127 | T.a31, T.a32, T.a33, T.a34); |
pclary | 2:caf73a1d7827 | 128 | terminal.write(output); */ |
pclary | 2:caf73a1d7827 | 129 | |
pclary | 0:449568595ed9 | 130 | |
pclary | 1:a5447cf54fba | 131 | while(true) |
pclary | 1:a5447cf54fba | 132 | { |
pclary | 2:caf73a1d7827 | 133 | arc = speed*timer.read(); |
pclary | 2:caf73a1d7827 | 134 | dataLog.push(timer.read()); |
pclary | 2:caf73a1d7827 | 135 | timer.reset(); |
pclary | 2:caf73a1d7827 | 136 | |
pclary | 2:caf73a1d7827 | 137 | if (radius == 0) |
pclary | 2:caf73a1d7827 | 138 | { |
pclary | 2:caf73a1d7827 | 139 | angle = 0.0f; |
pclary | 2:caf73a1d7827 | 140 | v.x = 0.0f; |
pclary | 2:caf73a1d7827 | 141 | v.y = arc; |
pclary | 2:caf73a1d7827 | 142 | v.z = 0.0f; |
pclary | 2:caf73a1d7827 | 143 | } |
pclary | 2:caf73a1d7827 | 144 | else |
pclary | 2:caf73a1d7827 | 145 | { |
pclary | 2:caf73a1d7827 | 146 | angle = -arc/radius; |
pclary | 2:caf73a1d7827 | 147 | v.x = radius*(1 - cos(-angle)); |
pclary | 2:caf73a1d7827 | 148 | v.y = radius*sin(-angle); |
pclary | 2:caf73a1d7827 | 149 | v.z = 0.0f; |
pclary | 2:caf73a1d7827 | 150 | } |
pclary | 2:caf73a1d7827 | 151 | |
pclary | 2:caf73a1d7827 | 152 | T.identity().rotateZ(angle).translate(v); |
pclary | 2:caf73a1d7827 | 153 | |
pclary | 2:caf73a1d7827 | 154 | leg.update(P*T.inverse()*Q); |
pclary | 1:a5447cf54fba | 155 | } |
pclary | 0:449568595ed9 | 156 | } |