Control program for a four-legged 12 axis robot.
Dependencies: CircularBuffer Servo Terminal mbed Radio
main.cpp
- Committer:
- pclary
- Date:
- 2013-05-21
- Revision:
- 12:a952bd74d363
- Parent:
- 11:9ee0214bd410
- Child:
- 13:1c5d255835ce
File content as of revision 12:a952bd74d363:
#include "mbed.h" #include "RobotLeg.h" #include "Matrix.h" #include "CircularBuffer.h" #include "Radio.h" #include "Terminal.h" #include "utility.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 #define PERIOD 0.002f CircularBuffer<float,16> dataLog; Radio radio(p5, p6, p7, p16, p17, p18); RobotLeg legA(p26, p29, p30, false); RobotLeg legB(p13, p14, p15, false); RobotLeg legC(p19, p11, p8, false); RobotLeg legD(p25, p24, p23, false); CmdHandler* legpos(Terminal* terminal, const char*) { char output[256]; char abuf[64]; char bbuf[64]; char cbuf[64]; char dbuf[64]; legA.getPosition().print(abuf, 64); legB.getPosition().print(bbuf, 64); legC.getPosition().print(cbuf, 64); legD.getPosition().print(dbuf, 64); snprintf(output, 256, "A = [%s]\nB = [%s]\nC = [%s]\nD = [%s]", abuf, bbuf, cbuf, dbuf); terminal->write(output); return NULL; } 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 snprintf(output, 256, "%4d: %f\n", start, dataLog[start]); terminal->write(output); } else { // Print a range of items for (int i = start; i <= end; i++) { snprintf(output, 256, "%4d: %f\n", i, dataLog[i]); terminal->write(output); } } return NULL; } // log() 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.6f); legB.reset(-0.1f); legC.reset(0.4f); legD.reset(0.9f); } // resetLegs() void setupLegs(); int main() { Timer deltaTimer; float xaxis, yaxis, turnaxis, angle; float deltaTime; vector3 v; matrix4 T; matrix4 PA, QA; matrix4 PB, QB; matrix4 PC, QC; matrix4 PD, QD; Terminal terminal; terminal.addCommand("log", &log); terminal.addCommand("leg", &legpos); radio.reset(); setupLegs(); // Create matrices to change base from robot coordinates to leg coordinates QA.translate(vector3(0.508f, 0.508f, 0.0f)); PA = QA.inverse(); QB.translate(vector3(-0.508f, -0.508f, 0.0f)); QB.a11 = -1.0f; QB.a22 = -1.0f; PB = QB.inverse(); QC.translate(vector3(-0.508f, 0.508f, 0.0f)); QC.a11 = -1.0f; PC = QC.inverse(); QD.translate(vector3(0.508f, -0.508f, 0.0f)); QD.a22 = -1.0f; PD = QD.inverse(); // Start timer deltaTimer.start(); while(true) { while (deltaTimer.read() < PERIOD); // 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); turnaxis = 0.1f; // Get delta-time deltaTime = deltaTimer.read(); deltaTimer.reset(); dataLog.push(deltaTime); // Compute delta movement vector and delta angle v.x = -xaxis; v.y = -yaxis; v.z = 0.0f; v = v * MAXSPEED * deltaTime; angle = -turnaxis * MAXTURN * deltaTime; // Compute movement transformation in robot coordinates T.identity().rotateZ(angle).translate(v).inverse(); bool stepping = legA.getStepping() || legB.getStepping() || legC.getStepping() || legD.getStepping(); bool lockup = false; if (!legA.update(PA*T*QA)) { if (stepping) lockup = true; else { legA.reset(0.8); stepping = true; } } if (!legB.update(PB*T*QB)) { if (stepping) lockup = true; else { legB.reset(0.8f); stepping = true; } } if (!legC.update(PC*T*QC)) { if (stepping) lockup = true; else { legC.reset(0.8f); stepping = true; } } if (!legD.update(PD*T*QD)) { if (stepping) lockup = true; else { legD.reset(0.8f); stepping = true; } } if (!lockup) { legA.apply(); legB.apply(); legC.apply(); legD.apply(); } } // while (true) } // main() void setupLegs() { // 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(1130, 2060, 45.0f, -45.0f); legA.phi.calibrate(1200, 2050, 70.0f, -45.0f); legA.psi.calibrate(2020, 1050, 70.0f, -60.0f); legB.theta.calibrate(980, 1930, 45.0f, -45.0f); legB.phi.calibrate(1120, 2030, 70.0f, -45.0f); legB.psi.calibrate(2070, 1170, 70.0f, -60.0f); legC.theta.calibrate(1920, 860, 45.0f, -45.0f); legC.phi.calibrate(1930, 1050, 70.0f, -45.0f); legC.psi.calibrate(1100, 2000, 70.0f, -60.0f); legD.theta.calibrate(2000, 1070, 45.0f, -45.0f); legD.phi.calibrate(1930, 1050, 70.0f, -45.0f); legD.psi.calibrate(1020, 1900, 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); // Go to initial position 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); 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); wait(0.4f); resetLegs(); }