1
Dependencies: CircularBuffer Radio Servo Terminal mbed
Fork of WalkingRobot by
main.cpp
- Committer:
- pclary
- Date:
- 2013-01-17
- Revision:
- 8:db453051f3f4
- Parent:
- 7:aac5f901bd76
- Child:
- 9:a6d1502f0f20
File content as of revision 8:db453051f3f4:
#include "mbed.h" #include "RobotLeg.h" #include "Matrix.h" #include "CircularBuffer.h" #include "Radio.h" #include "Terminal.h" #include <cstring> #include <cmath> #define MAXSPEED 0.1f #define MAXTURN 1.0f #define RESET_STEP_TIME 0.4f #define DIM_A 0.125f #define DIM_B 0.11f #define DIM_C 0.0025f #define DIM_D 0.025f #define CIRCLE_X 0.095f #define CIRCLE_Y 0.095f #define CIRCLE_Z -0.12f #define CIRCLE_R 0.09f enum state_t { walk, step, reset }; enum legstate_t { A, B, C, D }; CircularBuffer<float,16> dataLog; Radio radio(p5, p6, p7, p19, p20, p17); Timer deltaTimer; Timer RESET_STEP_TIMEr; 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; CmdHandler* log(Terminal* terminal, const char* input) { int start = 0; int end = 15; char output[256]; if (sscanf(input, "log %d %d", &start, &end) == 1) { // Print only one item sprintf(output, "%4d: %f\n", start, dataLog[start]); terminal->write(output); } else { // Print a range of items for (int i = start; i <= end; i++) { sprintf(output, "%4d: %f\n", i, dataLog[i]); terminal->write(output); } } return NULL; } // log() CmdHandler* read(Terminal* terminal, const char* input) { char output[256]; uint32_t data; data = radio.rx_controller; sprintf(output, "%d%d%d%d%d%d%d%d %d%d%d%d%d%d%d%d %d%d%d%d%d%d%d%d %d%d%d%d%d%d%d%d : %4d %4d %4d %4d", (data>>31)&1, (data>>30)&1, (data>>29)&1, (data>>28)&1, (data>>27)&1, (data>>26)&1, (data>>25)&1, (data>>24)&1, (data>>23)&1, (data>>22)&1, (data>>21)&1, (data>>20)&1, (data>>19)&1, (data>>18)&1, (data>>17)&1, (data>>16)&1, (data>>15)&1, (data>>14)&1, (data>>13)&1, (data>>12)&1, (data>>11)&1, (data>>10)&1, (data>>9)&1, (data>>8)&1, (data>>7)&1, (data>>6)&1, (data>>5)&1, (data>>4)&1, (data>>3)&1, (data>>2)&1, (data>>1)&1, (data>>0)&1, (int8_t)((data>>24)&0xff), (int8_t)((data>>16)&0xff), (int8_t)((data>>8)&0xff), (int8_t)((data)&0xff)); terminal->write(output); return NULL; } // read() int deadzone(int input, int zone) { if (input > zone) return input; else if (input < -zone) return input; else return 0; } // deadzone() void resetLegs() { legA.reset(-0.5f); legB.reset(0.0f); legC.reset(0.5f); legD.reset(1.0f); RESET_STEP_TIMEr.start(); state = reset; legState = D; } // resetLegs() 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; } int main() { float xaxis, yaxis, turnaxis, speed, angle; float deltaTime, cycleTime; vector3 v; matrix4 T; matrix4 PA, QA; matrix4 PB, QB; matrix4 PC, QC; matrix4 PD, QD; Terminal terminal; bool freeA, freeB, freeC, freeD; terminal.addCommand("log", &log); terminal.addCommand("read", &read); radio.reset(); // Set leg parameters legA.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D); legB.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D); legC.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D); legD.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D); legA.setAngleOffsets(0.7853982f, 0.0f, 0.0f); legB.setAngleOffsets(0.7853982f, 0.0f, 0.0f); legC.setAngleOffsets(0.7853982f, 0.0f, 0.0f); legD.setAngleOffsets(0.7853982f, 0.0f, 0.0f); legA.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R); legB.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R); legC.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R); legD.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R); legA.theta.calibrate(1000, 2000, 45.0f, -45.0f); legA.phi.calibrate(1000, 2000, 70.0f, -45.0f); legA.psi.calibrate(2000, 1000, 70.0f, -60.0f); legB.theta.calibrate(1000, 2000, 45.0f, -45.0f); legB.phi.calibrate(1000, 2000, 70.0f, -45.0f); legB.psi.calibrate(2000, 1000, 70.0f, -60.0f); legC.theta.calibrate(2000, 1000, 45.0f, -45.0f); legC.phi.calibrate(2000, 1000, 70.0f, -45.0f); legC.psi.calibrate(1000, 2000, 70.0f, -60.0f); legD.theta.calibrate(2000, 1000, 45.0f, -45.0f); legD.phi.calibrate(2000, 1000, 70.0f, -45.0f); legD.psi.calibrate(1000, 2000, 70.0f, -60.0f); // Initialize leg position deltas legA.nDeltaPosition = vector3(0.0f, 0.01f, 0.0f); legB.nDeltaPosition = vector3(0.0f, -0.01f, 0.0f); legC.nDeltaPosition = vector3(0.0f, 0.01f, 0.0f); legD.nDeltaPosition = vector3(0.0f, -0.01f, 0.0f); // Create matrices to change base from robot coordinates to leg coordinates QA.translate(vector3(0.1f, 0.1f, 0.0f)); PA = QA.inverse(); QB.translate(vector3(-0.1f, -0.1f, 0.0f)); QB.a11 = -1.0f; QB.a22 = -1.0f; PB = QB.inverse(); QC.translate(vector3(0.1f, -0.1f, 0.0f)); QC.a11 = -1.0f; PC = QC.inverse(); QD.translate(vector3(-0.1f, 0.1f, 0.0f)); QD.a22 = -1.0f; PD = QD.inverse(); // Start timers deltaTimer.start(); RESET_STEP_TIMEr.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.theta.enable(); wait(0.1f); legB.theta.enable(); wait(0.1f); legC.theta.enable(); wait(0.1f); legD.theta.enable(); wait(0.1f); legA.phi.enable(); wait(0.1f); legB.phi.enable(); wait(0.1f); legC.phi.enable(); wait(0.1f); legD.phi.enable(); wait(0.1f); legA.psi.enable(); wait(0.1f); legB.psi.enable(); wait(0.1f); legC.psi.enable(); wait(0.1f); legD.psi.enable(); wait(0.1f); resetLegs(); /* // Dump debug info 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", T.a11, T.a12, T.a13, T.a14, T.a21, T.a22, T.a23, T.a24, T.a31, T.a32, T.a33, T.a34); terminal.write(output); */ while(true) { dataLog.push((float)state * 10 + legState); switch (state) { case walk: case step: deltaTime = deltaTimer.read(); deltaTimer.reset(); // Read controller input xaxis = 0.0078125f * deadzone((int8_t)((radio.rx_controller>>0)&0xff), 8); // Convert to +/-1.0f range yaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>8)&0xff), 8); turnaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>16)&0xff), 8); // Compute delta movement vector and delta angle 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; // Compute movement transformation in robot coordinates T.identity().rotateZ(angle).translate(v).inverse(); // Update legs freeA = legA.update(PA*T*QA); freeB = legB.update(PB*T*QB); freeC = legC.update(PC*T*QC); freeD = legD.update(PD*T*QD); // Update state switch (legState) { case A: if (!freeB || !freeC || !freeD) resetLegs(); else if (!freeA || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold) { legA.reset(1.0f); legState = B; RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read(); RESET_STEP_TIMEr.reset(); } break; case B: if (!freeA || !freeC || !freeD) resetLegs(); else if (!freeB || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold) { legB.reset(1.0f); legState = C; RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read(); RESET_STEP_TIMEr.reset(); } break; case C: if (!freeA || !freeB || !freeD) resetLegs(); else if (!freeC || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold) { legC.reset(1.0f); legState = D; RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read(); RESET_STEP_TIMEr.reset(); } break; case D: if (!freeA || !freeB || !freeC) resetLegs(); else if (!freeD || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold) { legD.reset(1.0f); legState = A; RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read(); RESET_STEP_TIMEr.reset(); } break; } break; // case walk:, case step: case reset: cycleTime = RESET_STEP_TIMEr.read(); if ((cycleTime <= RESET_STEP_TIME) && legState != A) { legA.reset(-0.5f); legState = A; } else if ((cycleTime > RESET_STEP_TIME) && legState == A) { legB.reset(0.0f); legState = B; } else if ((cycleTime > RESET_STEP_TIME * 2) && legState == B) { legC.reset(0.5f); legState = C; } else if ((cycleTime > RESET_STEP_TIME * 3) && legState == C) { legD.reset(1.0f); legState = D; } else if ((cycleTime > RESET_STEP_TIME * 4) && legState == D) { walkLegs(); } T.identity(); freeA = legA.update(T); freeB = legB.update(T); freeC = legC.update(T); freeD = legD.update(T); break; // case reset: } // switch (state) } // while (true) } // main()