1
Dependencies: CircularBuffer Radio Servo Terminal mbed
Fork of WalkingRobot by
Diff: main.cpp
- Revision:
- 10:dc1ba352667e
- Parent:
- 9:a6d1502f0f20
--- a/main.cpp Thu Jan 31 23:51:15 2013 +0000 +++ b/main.cpp Tue Apr 09 01:36:50 2013 +0000 @@ -43,7 +43,6 @@ RobotLeg legD(p23, p24, p25, false); state_t state; legstate_t legState; -float stepDistance, stepDistanceTarget; @@ -154,8 +153,6 @@ { state = walk; legState = A; - stepDistanceTarget = CIRCLE_R / 2.0f; - stepDistance = 0; } // walkLegs() @@ -163,7 +160,7 @@ int main() { Timer deltaTimer; - float xaxis, yaxis, turnaxis, speed, angle; + float xaxis, yaxis, turnaxis, angle; float deltaTime, cycleTime; vector3 v; matrix4 T; @@ -172,7 +169,6 @@ matrix4 PC, QC; matrix4 PD, QD; Terminal terminal; - bool freeA, freeB, freeC, freeD; terminal.addCommand("log", &log); terminal.addCommand("read", &read); @@ -291,74 +287,39 @@ 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; angle = -turnaxis * MAXTURN * deltaTime; - stepDistance += deltaTime * (speed + fabs(angle * 0.141f)); // 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) + if (!legA.update(PA*T*QA)) { - case A: - if (!freeB || !freeC || !freeD) resetLegs(); - else if (!freeA || stepDistance > stepDistanceTarget) - { - legA.reset(1.0f); - legState = B; - stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance; - stepDistance = 0; - } - break; + legA.reset(1.0f); + legState = A; + } - case B: - if (!freeA || !freeC || !freeD) resetLegs(); - else if (!freeB || stepDistance > stepDistanceTarget) - { - legB.reset(1.0f); - legState = C; - stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance; - stepDistance = 0; - } - break; - - case C: - if (!freeA || !freeB || !freeD) resetLegs(); - else if (!freeC || stepDistance > stepDistanceTarget) - { - legC.reset(1.0f); - legState = D; - stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance; - stepDistance = 0; - } - break; - - case D: - if (!freeA || !freeB || !freeC) resetLegs(); - else if (!freeD || stepDistance > stepDistanceTarget) - { - legD.reset(1.0f); - legState = A; - stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance; - stepDistance = 0; - } - break; + if (!legB.update(PB*T*QB)) + { + legB.reset(1.0f); + legState = B; } - dataLog.push(stepDistanceTarget); + if (!legC.update(PC*T*QC)) + { + legC.reset(1.0f); + legState = C; + } - + if (!legD.update(PD*T*QD)) + { + legD.reset(1.0f); + legState = D; + } break; // case walk: @@ -403,10 +364,10 @@ T.identity(); - freeA = legA.update(T); - freeB = legB.update(T); - freeC = legC.update(T); - freeD = legD.update(T); + legA.update(T); + legB.update(T); + legC.update(T); + legD.update(T); break; // case reset: } // switch (state)