Alex Chepilev / Mbed 2 deprecated WalkingRobot

1

Dependencies:   CircularBuffer Radio Servo Terminal mbed

Fork of WalkingRobot by Patrick Clary

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?

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