Diff: LegInterface.cpp

Revision:

8:402f4a307d08

Parent:

7:c164b4869f74

--- a/LegInterface.cpp	Tue Nov 11 23:01:02 2014 +0000
+++ b/LegInterface.cpp	Sat Dec 20 15:41:40 2014 +0000
@@ -382,6 +382,10 @@
     return false;
 }
 
+/**
+    Set the goals for all the legs
+*/
+
 bool LegInterface::setLegGoal(int32_t liftPos[NUM_LEGS], int32_t wheelVel[NUM_LEGS])
 {
     // Has the previous attempt completed?
@@ -389,6 +393,27 @@
             !transactionSetGoal[LEG_E]->completed() ||
             !transactionSetGoal[LEG_S]->completed()) {
         // No: refuse to send
+
+        // Debugging
+        pc->putc('{');
+        pc->putc('0' + transactionSetGoal[LEG_W]->completed());
+        pc->putc('0' + transactionSetGoal[LEG_E]->completed());
+        pc->putc('0' + transactionSetGoal[LEG_S]->completed());
+        pc->putc('-');
+
+        /*
+        // For offending legs: clear the I2C status (next attempt will be seen as completed)
+        for(int i = 0; i < NUM_LEGS; ++i) {
+            if(!transactionSetGoal[i]->completed()) {
+                transactionSetGoal[i]->clearStatus();
+            }
+        }
+        */
+
+        pc->putc('0' + transactionSetGoal[LEG_W]->completed());
+        pc->putc('0' + transactionSetGoal[LEG_E]->completed());
+        pc->putc('0' + transactionSetGoal[LEG_S]->completed());
+        pc->putc('}');
         return false;
     }
 
@@ -479,6 +504,32 @@
 
 }
 
+/**
+    Reset the transaction status for each of the legs (if necessary).
+    
+    This affects the internal status of the MODI2C transaction objects.  Hence, 
+    this method should only be called if it is clear that the ISR has "forgotten" 
+    about one of the leg transactions.  
+    
+    @TODO
+    It is unclear why we need this.  There is probably a bug in the MODI2C code that 
+    causes this to happen.  However, it is interesting that it is only this transaction type
+    that seems to need this...  
+    
+    This is somewhat redundant with clearSetGoalTransactions().   Should resolve this at some point
+*/
+
+void LegInterface::clearLegSetGoalStatus()
+{
+    for(int i = 0; i < NUM_LEGS; ++i) {
+        // Reset leg only if it has not completed
+        if(!transactionSetGoal[i]->completed()) {
+            // This causes the transaction to be seen as being completed
+            transactionSetGoal[i]->clearStatus();
+        }
+    }
+}
+
 void LegInterface::reset()
 {
     I2CTransaction::reset();