Revision 11:3a2e6eb9fbb8, committed 2016-01-06

Comitter:

henryrawas

Date:

Wed Jan 06 22:25:51 2016 +0000

Parent:

10:9b21566a5ddb

Child:

12:ac6c9d7f8c40

Commit message:

error reset. Better sequences;

Changed in this revision

--- a/AX-12A.lib	Wed Jan 06 00:58:41 2016 +0000
+++ b/AX-12A.lib	Wed Jan 06 22:25:51 2016 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/robot-arm-demo-team/code/AX-12A/#b6979be5a0a7
+http://developer.mbed.org/teams/robot-arm-demo-team/code/AX-12A/#705f671a7498

--- a/ArmController.cpp	Wed Jan 06 00:58:41 2016 +0000
+++ b/ArmController.cpp	Wed Jan 06 22:25:51 2016 +0000
@@ -40,7 +40,8 @@
 int LastSendMs = 0;
 
 Queue<vector<ActionSequence>, 16> SequenceQ;
-volatile MainStates MainState;
+MainStates MainState;
+bool ErrorReset;
 bool RunInSequence;
 bool RunInMoveStep;
 bool RunInDelay;
@@ -175,6 +176,11 @@
 
 void ControlArm(const char* cmd)
 {
+    if (MainState == MS_Error)
+    {
+        ErrorReset = true;
+    }
+    
     if (strncmp(cmd, "pause", 5) == 0)
     {
         ShowLedBlue();
@@ -196,42 +202,42 @@
     else if (strncmp(cmd, "runupdown", 9) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&UpDownSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runuptwist", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&UpTwistSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runhome", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&StartSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runwave", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&WaveSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runtaps1", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&TapsSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "fastwave", 8) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
+        MainState = MS_CancelOne;
         SequenceQ.put(&FastWaveSeq);
         osSignalSet(mainTid, AS_Action);
     }
@@ -266,7 +272,8 @@
     RunInSequence = false;
     RunInMoveStep = false;
     RunInDelay = false;
-     
+    ErrorReset = false;
+    
     // get initial positions
     MeasureGroup measureGroup;
     robotArm.GetArmPositions(initPositions);
@@ -305,6 +312,12 @@
     {
         osEvent mainEvent = osSignalWait(signals, osWaitForever);
         
+        if (ErrorReset)
+        {
+            ErrorReset = false;
+            robotArm.ClearErrorState();
+        }
+        
         switch (MainState)
         {
             case MS_Idle:

--- a/IothubRobotArm.cpp	Wed Jan 06 00:58:41 2016 +0000
+++ b/IothubRobotArm.cpp	Wed Jan 06 22:25:51 2016 +0000
@@ -145,8 +145,13 @@
 static void SendConfirmationCallback(IOTHUB_CLIENT_CONFIRMATION_RESULT result, void* userContextCallback)
 {
     EVENT_INSTANCE* eventInstance = (EVENT_INSTANCE*)userContextCallback;
-    (void)printf("Confirmation[%d] received for message  handle %x tracking id = %d with result = %d\r\n", 
-                eventInstance->messageHandle, callbackCounter, eventInstance->messageTrackingId, result);
+    if (eventInstance == NULL)
+    {
+        printf("SendConfirmation with NULL context\r\n");
+        return;
+    }
+    (void)printf("Confirmation[%d] received for tracking id = %d with result = %d\r\n", 
+                eventInstance->messageHandle, eventInstance->messageTrackingId, result);
 
     confirmTimer->stop();
     callbackCounter++;
@@ -197,14 +202,7 @@
         }
         else
         {
-            if ((ev.value.signals & IS_SendStatus) == IS_SendStatus)
-            {
-                iotRobot.SendMeasurements();
-            }
-            else
-            {
-                (void)printf("Iothub thread unknown signal\r\n");
-            }
+            iotRobot.SendMeasurements();
         }
     }
 }
@@ -257,7 +255,7 @@
         // For mbed add the certificate information
         if (IoTHubClient_SetOption(iotHubClientHandle, "TrustedCerts", certificates) != IOTHUB_CLIENT_OK)
         {
-            //printf("failure to set option \"TrustedCerts\"\r\n");
+            printf("failure to set option \"TrustedCerts\"\r\n");
             return false;
         }
 #endif // MBED_BUILD_TIMESTAMP
@@ -316,12 +314,6 @@
             {
                 messages[i].messageTrackingId = msgNumber;
                 
-                //MAP_HANDLE propMap = IoTHubMessage_Properties(messages[i].messageHandle);
-                //sprintf_s(propText, sizeof(propText), "PropMsg_%d", msgNumber);
-                //if (Map_AddOrUpdate(propMap, "PropName", propText) != MAP_OK)
-                //{
-                //    (void)printf("ERROR: Map_AddOrUpdate Failed!\r\n");
-                //}
                 confirmTimer->stop();
                 
                 if (IoTHubClient_SendEventAsync(iotHubClientHandle, messages[i].messageHandle, SendConfirmationCallback, &messages[i]) != IOTHUB_CLIENT_OK)

--- a/RobotArm.cpp	Wed Jan 06 00:58:41 2016 +0000
+++ b/RobotArm.cpp	Wed Jan 06 22:25:51 2016 +0000
@@ -9,7 +9,7 @@
 
 using namespace std;
 
-DynamixelBus dynbus( PTC17, PTC16, D7, D6, 1000000 );
+DynamixelBus dynbus( PTC17, PTC16, D7, D6, 500000 );
 
 
 // set the id for each part in the chain, in order
@@ -36,6 +36,13 @@
     failms = 0;
 }
 
+void RobotArm::ClearErrorState()
+{
+    for (int i = 0; i < _numParts; i++)
+    {
+        _armParts[i]->DoAction(NA_ClearError, 0.0f);
+    }
+}
 
 // move all parts to specified postions in ms time
 bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms)

--- a/RobotArm.h	Wed Jan 06 00:58:41 2016 +0000
+++ b/RobotArm.h	Wed Jan 06 22:25:51 2016 +0000
@@ -61,6 +61,9 @@
     // start - test if positions are close to expected
     bool MoveArmPositionTest();
     
+    // clear part error state
+    void ClearErrorState();
+    
     // get all parts positions
     bool GetArmPositions(vector<float>& outPos);
     

--- a/RobotNode/RobotNode.h	Wed Jan 06 00:58:41 2016 +0000
+++ b/RobotNode/RobotNode.h	Wed Jan 06 22:25:51 2016 +0000
@@ -15,7 +15,8 @@
 
 enum NodeAction
 {
-    NA_Rotate           = 0x1
+    NA_Rotate           = 0x1,
+    NA_ClearError       = 0x2
 };
 
 enum NodePartType

--- a/Sequences.cpp	Wed Jan 06 00:58:41 2016 +0000
+++ b/Sequences.cpp	Wed Jan 06 22:25:51 2016 +0000
@@ -28,7 +28,7 @@
     float fastwaveUpPositions[] = { NoMove, 135.0f, NoMove, NoMove, NoMove };
     float fastwaveDownPositions[] = { NoMove, 225.0f, NoMove, NoMove, NoMove };
     
-    float tapStartPositions[] = { RightPos, 135.0f, 225.0f, 180.0f, 180.0f };
+    float tapStartPositions[] = { RightPos, 180.0f, 270.0f, 180.0f, 180.0f };
     float tap1Positions[] = { 60.2f, NoMove, NoMove, NoMove, NoMove };
     float tap2Positions[] = { 68.7f, NoMove, NoMove, NoMove, NoMove };
     float tap3Positions[] = { 77.2f, NoMove, NoMove, NoMove, NoMove };
@@ -44,8 +44,8 @@
     ActionSequence moveDown(SA_SetGoal, partSize, downPositions, 1000);
     ActionSequence waveUp(SA_SetGoal, partSize, waveUpPositions, 1000);
     ActionSequence waveDown(SA_SetGoal, partSize, waveDownPositions, 1000);
-    ActionSequence fastwaveUp(SA_SetGoal, partSize, fastwaveUpPositions, 200);
-    ActionSequence fastwaveDown(SA_SetGoal, partSize, fastwaveDownPositions, 200);
+    ActionSequence fastwaveUp(SA_SetGoal, partSize, fastwaveUpPositions, 400);
+    ActionSequence fastwaveDown(SA_SetGoal, partSize, fastwaveDownPositions, 400);
     ActionSequence tapsStart(SA_SetGoal, partSize, tapStartPositions, 1000);
     ActionSequence taps1(SA_SetGoal, partSize, tap1Positions, 200);
     ActionSequence taps2(SA_SetGoal, partSize, tap2Positions, 200);
@@ -77,7 +77,7 @@
     
     // add actions into StartSeq
     StartSeq.clear();
-    StartSeq.push_back(moveStart);
+    StartSeq.push_back(moveUp);
     StartSeq.push_back(report);
     
     // add actions into WaveSeq
@@ -89,6 +89,7 @@
     WaveSeq.push_back(waveDown);
     WaveSeq.push_back(report);
     WaveSeq.push_back(loopEnd);
+    WaveSeq.push_back(moveUp);
     
     // add actions into UpDownSeq
     UpDownSeq.clear();
@@ -97,6 +98,9 @@
     UpDownSeq.push_back(pause1);
     UpDownSeq.push_back(moveDown);
     UpDownSeq.push_back(report);
+    UpDownSeq.push_back(moveUp);
+    UpDownSeq.push_back(pause1);
+    UpDownSeq.push_back(report);
     
     // add actions into RightStepsSeq
     // move to vertical, then move each part one at a time, then return to home
@@ -153,6 +157,7 @@
     TapsSeq.push_back(tapDown);
     TapsSeq.push_back(tapUp);
     TapsSeq.push_back(report);
+    TapsSeq.push_back(moveUp);
 
     // add actions into FastWaveSeq
     FastWaveSeq.clear();