Diff: LegInterface.cpp

Revision:

6:a52ded837184

Parent:

5:86f1cd6657de

Child:

7:c164b4869f74

--- a/LegInterface.cpp	Fri Jun 20 06:10:34 2014 +0000
+++ b/LegInterface.cpp	Tue Aug 05 21:53:59 2014 +0000
@@ -145,6 +145,11 @@
     return true;
 }
 
+/**
+    Check if the query of leg state has been completed (successful or not)
+
+    @return true if completed
+*/
 
 bool LegInterface::queryStateCompleted()
 {
@@ -156,6 +161,54 @@
     return true;
 }
 
+/**
+    Check if the query of leg state has been successfully completed
+
+    @return true if successfully completed
+*/
+
+bool LegInterface::queryStateSuccess()
+{
+    for(int i = 0; i < NUM_LEGS; ++i) {
+        if(!transactionQueryState[i]->success()) {
+            return false;
+        }
+    }
+    return true;
+}
+
+/**
+    Check if the query of estop has been completed (successful or not)
+
+    @return true if completed
+*/
+
+bool LegInterface::queryEstopCompleted()
+{
+    for(int i = 0; i < NUM_LEGS; ++i) {
+        if(!transactionEstop[i]->completed()) {
+            return false;
+        }
+    }
+    return true;
+}
+
+/**
+    Check if the query of estop has been successfully completed
+
+    @return true if successfully completed
+*/
+
+bool LegInterface::queryEstopSuccess()
+{
+    for(int i = 0; i < NUM_LEGS; ++i) {
+        if(!transactionEstop[i]->success()) {
+            return false;
+        }
+    }
+    return true;
+}
+
 void LegInterface::queryStateTest(Serial *pc)
 {
     for(int i = 0; i < NUM_LEGS; ++i) {
@@ -166,34 +219,68 @@
 
 bool LegInterface::queryStateCopy(LegState_t legState[NUM_LEGS])
 {
-    // Has the prior attempt at sending the estop completed?
+    // Has the prior attempt at querying the state completed?
     if(!queryStateCompleted()) {
         // No: do not attempt
         return false;
     }
 
-    // Transaction complete: copy the state of the legs
-    legState[LEG_W] = this->legState[LEG_W];
-    legState[LEG_E] = this->legState[LEG_E];
-    legState[LEG_S] = this->legState[LEG_S];
+    // Transaction complete: copy the state of the legs only if the magic number is correct
+    if(this->legState[LEG_W].magic == I2C_MAGIC_NUMBER)
+        legState[LEG_W] = this->legState[LEG_W];
+    if(this->legState[LEG_E].magic == I2C_MAGIC_NUMBER)
+        legState[LEG_E] = this->legState[LEG_E];
+    if(this->legState[LEG_S].magic == I2C_MAGIC_NUMBER)
+        legState[LEG_S] = this->legState[LEG_S];
 
     return true;
 }
 
-bool LegInterface::queryStateCopy(float liftPosition[NUM_LEGS], float liftVelocity[NUM_LEGS], float wheelPosition[NUM_LEGS], float wheelVelocity[NUM_LEGS])
+/**
+    Copy the state of the legs from the LegInterface structure to usable structures.  In order for the data to be copied, the
+    transaction must be successful, and the leg packet must have a valid magic number (the latter is evalulated on a leg-by-leg 
+    basis).
+    
+    The input parameters are destructively modified only if the data are valid.  The exception is the "magic" parameter.  This is
+    only changed if the set of transactions was successful.
+    
+    @param liftPosition  Lift position of each of the legs (m above the ground)
+    @param liftVelocity  Velocity of the lifts (m/s)
+    @param wheelPosition  Position of each of the wheels (m)
+    @param wheelVelocity Velocity of the wheels (m/s)
+    @param cliff Cliff sensors
+    @param limit Limit sensors (lifts)
+    @param estop Estop state of each of the legs
+    @param magic Magic number from each leg
+    
+    @return true if Some data were updated; false if no change
+    
+*/
+
+bool LegInterface::queryStateCopy(float liftPosition[NUM_LEGS], float liftVelocity[NUM_LEGS], float wheelPosition[NUM_LEGS], float wheelVelocity[NUM_LEGS], 
+        bool cliff[NUM_LEGS], bool limit[NUM_LEGS], bool estop[NUM_LEGS], uint8_t magic[NUM_LEGS])
 {
-    // Has the prior attempt at sending the estop completed?
-    if(!queryStateCompleted()) {
-        // No: do not attempt
+    // Has the prior attempt at getting state completed?
+    if(!queryStateSuccess()) {
+        // No: do not attempt the copy
         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 - LIFT_OFFSET;
-        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;
+        if(this->legState[i].magic == I2C_MAGIC_NUMBER) {
+            // Only copy if the magic number matches
+            liftPosition[i] = this->legState[i].lift.pos / LIFT_TICKS_PER_METER - LIFT_OFFSET;
+            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;
+            cliff[i] = this->legState[i].cliff;
+            limit[i] = this->legState[i].limit;
+            estop[i] = this->legState[i].estop;
+
+        }
+        // Also copy the magic number
+        magic[i] = this->legState[i].magic;
     }
 
     return true;
@@ -311,6 +398,33 @@
     return setLegGoal(lift, wheel);
 }
 
+/**
+    Check the completion status of the Set Leg Goal transaction.
+
+    @return true if the last leg goal set was completed.
+*/
+
+bool LegInterface::getLegGoalCompleted()
+{
+    return(transactionSetGoal[LEG_W]->completed() &&
+           transactionSetGoal[LEG_E]->completed() &&
+           transactionSetGoal[LEG_S]->completed());
+}
+
+
+/**
+    Check the completion status of the Set Leg Goal transaction.
+
+    @return true if the last leg goal set was completed successfully.
+*/
+
+bool LegInterface::getLegGoalSuccess()
+{
+    return(transactionSetGoal[LEG_W]->success() &&
+           transactionSetGoal[LEG_E]->success() &&
+           transactionSetGoal[LEG_S]->success());
+}
+
 void LegInterface::displayStatus()
 {
     pc->printf("Transaction status:\n\r");
@@ -358,5 +472,32 @@
 void LegInterface::cycleBus()
 {
     I2CTransaction::cycleBus();
-    }
-    
\ No newline at end of file
+}
+
+void LegInterface::clearQueryTransactions()
+{
+    transactionQueryState[LEG_W]->clearStatus();
+    transactionQueryState[LEG_E]->clearStatus();
+    transactionQueryState[LEG_S]->clearStatus();
+}
+
+void LegInterface::clearSetGoalTransactions()
+{
+    transactionSetGoal[LEG_W]->clearStatus();
+    transactionSetGoal[LEG_E]->clearStatus();
+    transactionSetGoal[LEG_S]->clearStatus();
+}
+
+
+/**
+    Initialize the I2C pins and other hardware.  If it has already been initialized, then
+    we will re-initialize.
+    
+    @param sda Name of the pin that is used for data
+    @param scl Name of the pin that is used for the clock
+*/
+
+void LegInterface::initI2C(PinName sda, PinName scl)
+{
+    I2CTransaction::initI2C(sda, scl);
+}
\ No newline at end of file