Diff: LegInterface.cpp

Revision:

2:17c7a02f8401

Parent:

1:d3abc0577ebc

Child:

3:8e7471af3453

--- a/LegInterface.cpp	Thu May 22 21:17:58 2014 +0000
+++ b/LegInterface.cpp	Fri May 23 19:27:12 2014 +0000
@@ -131,7 +131,7 @@
     return true;
 }
 
-bool LegInterface::queryStateCompleted(int timeout)
+bool LegInterface::queryStateWaitForCompletion(int timeout)
 {
     for(int i = 0; i < NUM_LEGS; ++i) {
         if(!transactionQueryState[i]->waitForCompletion(timeout)) {
@@ -141,6 +141,17 @@
     return true;
 }
 
+
+bool LegInterface::queryStateCompleted()
+{
+    for(int i = 0; i < NUM_LEGS; ++i) {
+        if(!transactionQueryState[i]->completed()) {
+            return false;
+        }
+    }
+    return true;
+}
+
 bool LegInterface::queryStateCopy(LegState_t legState[NUM_LEGS])
 {
     // Has the prior attempt at sending the estop completed?
@@ -159,6 +170,27 @@
     return true;
 }
 
+bool LegInterface::queryStateCopy(float liftPosition[NUM_LEGS], float liftVelocity[NUM_LEGS], float wheelPosition[NUM_LEGS], float wheelVelocity[NUM_LEGS])
+{
+    // Has the prior attempt at sending the estop completed?
+    if(!transactionQueryState[LEG_W]->completed() ||
+            !transactionQueryState[LEG_E]->completed() ||
+            !transactionQueryState[LEG_S]->completed()) {
+        // No: do not attempt
+        return false;
+    }
+
+    // Transaction complete: copy the state of the legs
+    for(int i = 0; i < NUM_LEGS; ++i) {
+        liftPosition[i] = this->legState[i].lift.pos / LIFT_TICKS_PER_METER;
+        liftVelocity[i] = this->legState[i].lift.vel / LIFT_TICKS_PER_METER;
+        wheelPosition[i] = this->legState[i].wheel.pos / TICKS_PER_METER;
+        wheelVelocity[i] = this->legState[i].wheel.vel / TICKS_PER_METER;
+    }
+
+    return true;
+}
+
 void LegInterface::displayLegState(LegState_t *legState)
 {
     pc->printf("Cliff = %d, Limit = %d\n\r", legState->cliff, legState->limit);
@@ -260,7 +292,15 @@
 
 bool LegInterface::setLegGoal(float liftPos[NUM_LEGS], float wheelVel[NUM_LEGS])
 {
+    int32_t lift[NUM_LEGS];
+    int32_t wheel[NUM_LEGS];
 
+    for(int i = 0; i < NUM_LEGS; ++i) {
+        wheel[i] = (int32_t) (wheelVel[i] * TICKS_PER_METER);
+        lift[i] = (int32_t) (liftPos[i] * LIFT_TICKS_PER_METER);
+    }
+
+    return setLegGoal(lift, wheel);
 }
 
 void LegInterface::displayStatus()