Control program for a four-legged 12 axis robot.
Dependencies: CircularBuffer Servo Terminal mbed Radio
Diff: main.cpp
- Revision:
- 9:a6d1502f0f20
- Parent:
- 8:db453051f3f4
- Child:
- 10:dc1ba352667e
--- a/main.cpp Thu Jan 17 18:34:09 2013 +0000 +++ b/main.cpp Thu Jan 31 23:51:15 2013 +0000 @@ -4,6 +4,7 @@ #include "CircularBuffer.h" #include "Radio.h" #include "Terminal.h" +#include "utility.h" #include <cstring> #include <cmath> @@ -22,7 +23,6 @@ enum state_t { walk, - step, reset }; @@ -35,16 +35,15 @@ }; CircularBuffer<float,16> dataLog; -Radio radio(p5, p6, p7, p19, p20, p17); -Timer deltaTimer; -Timer RESET_STEP_TIMEr; +Radio radio(p5, p6, p7, p16, p17, p18); +Timer stepTimer; RobotLeg legA(p26, p29, p30, false); // Start the legs disabled RobotLeg legB(p13, p14, p15, false); RobotLeg legC(p12, p11, p8, false); RobotLeg legD(p23, p24, p25, false); state_t state; legstate_t legState; -float RESET_STEP_TIMEThreshold; +float stepDistance, stepDistanceTarget; @@ -94,6 +93,41 @@ +CmdHandler* resetc(Terminal* terminal, const char* input) +{ + char output[256]; + float f; + vector3 v; + + if (sscanf(input, "reset %f", &f) == 1) + { + v = legA.reset(f); + sprintf(output, "reset: %f %f %f", v.x, v.y, v.z); + terminal->write(output); + } + else + { + sprintf(output, "syntax error"); + terminal->write(output); + } + + return NULL; +} // reset() + + + +CmdHandler* rndp(Terminal* terminal, const char* input) +{ + char output[256]; + + sprintf(output, "%f %f %f", legA.nDeltaPosition.x, legA.nDeltaPosition.y, legA.nDeltaPosition.z); + terminal->write(output); + + return NULL; +} // step() + + + int deadzone(int input, int zone) { if (input > zone) return input; @@ -109,7 +143,7 @@ legB.reset(0.0f); legC.reset(0.5f); legD.reset(1.0f); - RESET_STEP_TIMEr.start(); + stepTimer.reset(); state = reset; legState = D; } // resetLegs() @@ -118,21 +152,17 @@ void walkLegs() { - float xaxis, yaxis, speed; - state = walk; legState = A; - RESET_STEP_TIMEr.reset(); - xaxis = 0.0078125f * deadzone((int8_t)((radio.rx_controller>>0)&0xff), 8); - yaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>8)&0xff), 8); - speed = sqrt(xaxis*xaxis + yaxis*yaxis) * MAXSPEED * deltaTimer.read(); - RESET_STEP_TIMEThreshold = CIRCLE_R / 2.0f / speed; -} + stepDistanceTarget = CIRCLE_R / 2.0f; + stepDistance = 0; +} // walkLegs() int main() { + Timer deltaTimer; float xaxis, yaxis, turnaxis, speed, angle; float deltaTime, cycleTime; vector3 v; @@ -146,6 +176,15 @@ terminal.addCommand("log", &log); terminal.addCommand("read", &read); + terminal.addCommand("reset", &resetc); + terminal.addCommand("ndp", &rndp); + + + DigitalOut debug1(LED1); + DigitalOut debug2(LED2); + DigitalOut debug3(LED3); + DigitalOut debug4(LED4); + radio.reset(); @@ -196,13 +235,13 @@ // Start timers deltaTimer.start(); - RESET_STEP_TIMEr.start(); + stepTimer.start(); // Go to initial position - legA.move(vector3(0.2f, 0.2f, 0.05f)); - legB.move(vector3(0.2f, 0.2f, 0.05f)); - legC.move(vector3(0.2f, 0.2f, 0.05f)); - legD.move(vector3(0.2f, 0.2f, 0.05f)); + legA.move(vector3(0.15f, 0.15f, 0.05f)); + legB.move(vector3(0.15f, 0.15f, 0.05f)); + legC.move(vector3(0.15f, 0.15f, 0.05f)); + legD.move(vector3(0.15f, 0.15f, 0.05f)); legA.theta.enable(); wait(0.1f); legB.theta.enable(); wait(0.1f); legC.theta.enable(); wait(0.1f); @@ -215,6 +254,7 @@ legB.psi.enable(); wait(0.1f); legC.psi.enable(); wait(0.1f); legD.psi.enable(); wait(0.1f); + wait(0.4f); resetLegs(); /* @@ -229,12 +269,19 @@ while(true) { - dataLog.push((float)state * 10 + legState); - switch (state) { case walk: - case step: + // Debug stuff + switch (legState) + { + case A: debug1 = 1; debug2 = 0; debug3 = 0; debug4 = 0; break; + case B: debug1 = 0; debug2 = 1; debug3 = 0; debug4 = 0; break; + case C: debug1 = 0; debug2 = 0; debug3 = 1; debug4 = 0; break; + case D: debug1 = 0; debug2 = 0; debug3 = 0; debug4 = 1; break; + } + + // Get delta-time deltaTime = deltaTimer.read(); deltaTimer.reset(); @@ -244,12 +291,13 @@ turnaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>16)&0xff), 8); // Compute delta movement vector and delta angle + speed = sqrt(xaxis*xaxis + yaxis*yaxis) * MAXSPEED; v.x = -xaxis; v.y = -yaxis; v.z = 0; v = v * MAXSPEED * deltaTime; - speed = sqrt(v.x*v.x + v.y*v.y); angle = -turnaxis * MAXTURN * deltaTime; + stepDistance += deltaTime * (speed + fabs(angle * 0.141f)); // Compute movement transformation in robot coordinates T.identity().rotateZ(angle).translate(v).inverse(); @@ -265,53 +313,68 @@ { case A: if (!freeB || !freeC || !freeD) resetLegs(); - else if (!freeA || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold) + else if (!freeA || stepDistance > stepDistanceTarget) { legA.reset(1.0f); legState = B; - RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read(); - RESET_STEP_TIMEr.reset(); + stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance; + stepDistance = 0; } break; case B: if (!freeA || !freeC || !freeD) resetLegs(); - else if (!freeB || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold) + else if (!freeB || stepDistance > stepDistanceTarget) { legB.reset(1.0f); legState = C; - RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read(); - RESET_STEP_TIMEr.reset(); + stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance; + stepDistance = 0; } break; case C: if (!freeA || !freeB || !freeD) resetLegs(); - else if (!freeC || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold) + else if (!freeC || stepDistance > stepDistanceTarget) { legC.reset(1.0f); legState = D; - RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read(); - RESET_STEP_TIMEr.reset(); + stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance; + stepDistance = 0; } break; case D: if (!freeA || !freeB || !freeC) resetLegs(); - else if (!freeD || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold) + else if (!freeD || stepDistance > stepDistanceTarget) { legD.reset(1.0f); legState = A; - RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read(); - RESET_STEP_TIMEr.reset(); + stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance; + stepDistance = 0; } break; } - break; // case walk:, case step: + dataLog.push(stepDistanceTarget); + + + + break; // case walk: + + case reset: - cycleTime = RESET_STEP_TIMEr.read(); + // Debug stuff + switch (legState) + { + case A: debug1 = 0; debug2 = 1; debug3 = 1; debug4 = 1; break; + case B: debug1 = 1; debug2 = 0; debug3 = 1; debug4 = 1; break; + case C: debug1 = 1; debug2 = 1; debug3 = 0; debug4 = 1; break; + case D: debug1 = 1; debug2 = 1; debug3 = 1; debug4 = 0; break; + } + + cycleTime = stepTimer.read(); if ((cycleTime <= RESET_STEP_TIME) && legState != A) {