Patrick Clary / Mbed 2 deprecated WalkingRobot

Control program for a four-legged 12 axis robot.

Dependencies:   CircularBuffer Servo Terminal mbed Radio

main.cpp@1:a5447cf54fba, 2012-10-22 (annotated)

Committer:
pclary
Date:
Mon Oct 22 06:04:06 2012 +0000
Revision:
1:a5447cf54fba
Parent:
0:449568595ed9
Child:
2:caf73a1d7827
Added step command

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 0:449568595ed9 4 #include <cstring>
pclary 0:449568595ed9 5
pclary 0:449568595ed9 6
pclary 0:449568595ed9 7
pclary 0:449568595ed9 8 RobotLeg leg(p17, p15, p13);
pclary 0:449568595ed9 9
pclary 0:449568595ed9 10
pclary 0:449568595ed9 11
pclary 0:449568595ed9 12 CmdHandler* move(Terminal* terminal, const char* input)
pclary 0:449568595ed9 13 {
pclary 0:449568595ed9 14 float x, y, z;
pclary 0:449568595ed9 15 bool success = false;
pclary 0:449568595ed9 16 char output[256];
pclary 0:449568595ed9 17
pclary 0:449568595ed9 18 if (sscanf(input, "move %f %f %f", &x, &y, &z) == 3)
pclary 0:449568595ed9 19 {
pclary 0:449568595ed9 20 success = leg.move(x, y, z);
pclary 0:449568595ed9 21 }
pclary 0:449568595ed9 22
pclary 0:449568595ed9 23 if (success)
pclary 0:449568595ed9 24 sprintf(output, "new angles: %7.2f %7.2f %7.2f", leg.thetaAngle, leg.phiAngle, leg.psiAngle);
pclary 0:449568595ed9 25 else
pclary 0:449568595ed9 26 sprintf(output, "invalid position: %7.2f %7.2f %7.2f", leg.thetaAngle, leg.phiAngle, leg.psiAngle);
pclary 0:449568595ed9 27 terminal->write(output);
pclary 0:449568595ed9 28
pclary 0:449568595ed9 29 return NULL;
pclary 0:449568595ed9 30 }
pclary 0:449568595ed9 31
pclary 0:449568595ed9 32
pclary 0:449568595ed9 33
pclary 1:a5447cf54fba 34 CmdHandler* step(Terminal* terminal, const char* input)
pclary 0:449568595ed9 35 {
pclary 1:a5447cf54fba 36 float x, y, z;
pclary 0:449568595ed9 37 char output[256];
pclary 0:449568595ed9 38
pclary 1:a5447cf54fba 39 if (sscanf(input, "step %f %f %f", &x, &y, &z) == 3)
pclary 0:449568595ed9 40 {
pclary 1:a5447cf54fba 41 leg.step(x, y, z);
pclary 1:a5447cf54fba 42 sprintf(output, "stepping to new position");
pclary 0:449568595ed9 43 }
pclary 0:449568595ed9 44 else
pclary 0:449568595ed9 45 {
pclary 0:449568595ed9 46 sprintf(output, "error reading input parameters");
pclary 0:449568595ed9 47 }
pclary 1:a5447cf54fba 48
pclary 0:449568595ed9 49 terminal->write(output);
pclary 0:449568595ed9 50
pclary 0:449568595ed9 51 return NULL;
pclary 0:449568595ed9 52 }
pclary 0:449568595ed9 53
pclary 0:449568595ed9 54
pclary 0:449568595ed9 55
pclary 0:449568595ed9 56 int main()
pclary 0:449568595ed9 57 {
pclary 0:449568595ed9 58 Terminal terminal;
pclary 0:449568595ed9 59
pclary 0:449568595ed9 60 // Set leg parameters
pclary 0:449568595ed9 61 leg.setDimensions(0.094f, 0.104f, -0.006f, 0.020f);
pclary 0:449568595ed9 62 leg.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
pclary 0:449568595ed9 63 leg.theta.calibrate(950, 2200, 60.0f, -60.0f);
pclary 0:449568595ed9 64 leg.phi.calibrate(975, 2250, 80.0f, -60.0f);
pclary 0:449568595ed9 65 leg.psi.calibrate(2400, 1125, 60.0f, -60.0f);
pclary 0:449568595ed9 66
pclary 0:449568595ed9 67 terminal.addCommand("move", &move);
pclary 1:a5447cf54fba 68 terminal.addCommand("step", &step);
pclary 0:449568595ed9 69
pclary 1:a5447cf54fba 70 while(true)
pclary 1:a5447cf54fba 71 {
pclary 1:a5447cf54fba 72 leg.update();
pclary 1:a5447cf54fba 73 }
pclary 0:449568595ed9 74 }