Diff: ArmController.cpp

Revision:

10:9b21566a5ddb

Parent:

9:a0fb6c370dbb

Child:

11:3a2e6eb9fbb8

--- a/ArmController.cpp	Thu Dec 31 20:02:58 2015 +0000
+++ b/ArmController.cpp	Wed Jan 06 00:58:41 2016 +0000
@@ -14,9 +14,11 @@
 #include <IothubRobotArm.h>
 #include <ActionBuf.h>
 #include <Sequences.h>
+#include <ControllerIo.h>
 
 using namespace std;
 
+
 vector<float> initPositions;
 
 
@@ -32,6 +34,11 @@
     MS_CancelToM      = 0x7
 };
 
+// try to send some status every minute
+#define IDLESTATUSTO        60000
+Timer IdleTimer;
+int LastSendMs = 0;
+
 Queue<vector<ActionSequence>, 16> SequenceQ;
 volatile MainStates MainState;
 bool RunInSequence;
@@ -69,6 +76,7 @@
     Alert alert;
     time_t seconds = time(NULL);
     
+    ShowLedRed();
     alert.SetPositionAlert(severity, seconds, partIx, pos);
     AlertBuf.push(alert);
     SendIothubData();
@@ -79,6 +87,7 @@
     Alert alert;
     time_t seconds = time(NULL);
     
+    ShowLedRed();
     alert.SetHardwareAlert(severity, seconds, partIx, code);
     AlertBuf.push(alert);
     SendIothubData();
@@ -89,6 +98,7 @@
     Alert alert;
     time_t seconds = time(NULL);
     
+    ShowLedRed();
     alert.SetTemperatureAlert(severity, seconds, partIx, temp);
     AlertBuf.push(alert);
     SendIothubData();
@@ -113,10 +123,13 @@
     return err;
 }
 
+
 void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
 {
     vector<float> lastVals;
     time_t seconds = time(NULL);
+    LastSendMs = (int)IdleTimer.read_ms();
+    
     bool ok = true;
     
     ok = robotArm.GetArmMeasure(NM_Temperature, lastVals);
@@ -158,64 +171,68 @@
     }
 }
 
+
+
 void ControlArm(const char* cmd)
 {
     if (strncmp(cmd, "pause", 5) == 0)
     {
-        printf("Pause cmd\r\n"); 
+        ShowLedBlue();
         MainState = MS_Paused;
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "resume", 6) == 0)
     {
-        printf("Resume cmd\r\n"); 
+        ShowLedGreen();
         MainState = MS_Running;
         osSignalSet(mainTid, AS_Action);
     }
+    else if (strncmp(cmd, "cancel", 6) == 0)
+    {
+        ShowLedGreen();
+        MainState = MS_CancelOne;
+        osSignalSet(mainTid, AS_Action);
+    }
     else if (strncmp(cmd, "runupdown", 9) == 0)
     {
-        printf("Runupdown cmd\r\n"); 
-        MainState = MS_CancelToM;
-        CancelToMatch = &UpDownSeq;
+        ShowLedGreen();
+        MainState = MS_Running;
         SequenceQ.put(&UpDownSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runuptwist", 10) == 0)
     {
-        printf("Runuptwist cmd\r\n"); 
-        MainState = MS_CancelToM;
-        CancelToMatch = &UpTwistSeq;
+        ShowLedGreen();
+        MainState = MS_Running;
         SequenceQ.put(&UpTwistSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runhome", 10) == 0)
     {
-        printf("Runhome cmd\r\n"); 
-        MainState = MS_CancelToM;
-        CancelToMatch = &StartSeq;
+        ShowLedGreen();
+        MainState = MS_Running;
         SequenceQ.put(&StartSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runwave", 10) == 0)
     {
-        printf("Runwave cmd\r\n"); 
-        MainState = MS_CancelToM;
-        CancelToMatch = &WaveSeq;
+        ShowLedGreen();
+        MainState = MS_Running;
         SequenceQ.put(&WaveSeq);
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runtaps1", 10) == 0)
     {
-        printf("Runtaps1cmd\r\n"); 
-        MainState = MS_CancelToM;
-        CancelToMatch = &TapsSeq;
+        ShowLedGreen();
+        MainState = MS_Running;
         SequenceQ.put(&TapsSeq);
         osSignalSet(mainTid, AS_Action);
     }
-    else if (strncmp(cmd, "cancelone", 9) == 0)
+    else if (strncmp(cmd, "fastwave", 8) == 0)
     {
-        printf("CancelOne cmd\r\n"); 
-        MainState = MS_CancelOne;
+        ShowLedGreen();
+        MainState = MS_Running;
+        SequenceQ.put(&FastWaveSeq);
         osSignalSet(mainTid, AS_Action);
     }
 }
@@ -227,6 +244,13 @@
     osSignalSet((osThreadId)tid, AS_Action);
 }
 
+// periodic timer routine
+void PeriodicStatus(void const * tid)
+{
+    // make sure we run controller even if idle
+    osSignalSet((osThreadId)tid, AS_Action);
+}
+
 void RunController()
 {
     // init robot arm
@@ -234,7 +258,9 @@
     
     int partSize = robotArm.GetNumParts();
 
-   
+    IdleTimer.start();
+    LastSendMs = (int)IdleTimer.read_ms();
+    
     // set running state
     MainState = MS_Idle;
     RunInSequence = false;
@@ -263,11 +289,15 @@
     // state for sequence
     vector<ActionSequence> *curseq = NULL;
     int curseqIx = 0;
-
+    int loopSeqIx = 0;
+    int loopCounter = 0;
+    
     // signal to run the default action for demo
     osSignalSet(mainTid, AS_Action);
     
     RtosTimer delayTimer(NextSeq, osTimerOnce, (void *)osThreadGetId());    
+    RtosTimer statusTimer(PeriodicStatus, osTimerPeriodic, (void *)osThreadGetId());    
+    statusTimer.start(IDLESTATUSTO);
     
     MainState = MS_Running;
     
@@ -306,6 +336,7 @@
                 break;
                 
             case MS_CancelAll:
+                printf("Cancel all\r\n"); 
                 RunInSequence = false;
                 RunInMoveStep = false;
                 if (RunInDelay)
@@ -322,6 +353,7 @@
                 break;
                 
             case MS_CancelToM:
+                printf("Cancel until latest\r\n"); 
                 RunInSequence = false;
                 RunInMoveStep = false;
                 if (RunInDelay)
@@ -348,6 +380,7 @@
                 if (RunInDelay)
                 {
                     // waiting for timer to fire. do nothing
+                    printf("Should be delaying. Skip action\r\n");
                     break;
                 }
                 if (!RunInSequence)
@@ -419,6 +452,7 @@
                         {
                             if (curseqIx >= curseq->size())
                             {
+                                printf("sequence completed. Stopping\r\n");
                                 RunInSequence = false;
                                 break;
                             }
@@ -439,7 +473,6 @@
                                     RunInDelay = true;
                                     delayTimer.start(aseq.Param);
                                     break;
-
                                 case SA_Status:
                                     printf(" - Status\r\n"); 
 
@@ -450,12 +483,19 @@
                                     
                                     osSignalSet(mainTid, AS_Action);
                                     break;
-                                case SA_Repeat:
-                                    printf(" - Repeat\r\n"); 
-                                    curseqIx = aseq.Param;
+                                case SA_LoopBegin:
+                                    printf(" - LoopBegin\r\n");
+                                    loopSeqIx = curseqIx;
+                                    loopCounter = aseq.Param;
                                     osSignalSet(mainTid, AS_Action);
                                     break;
-                                    
+                                case SA_LoopEnd:
+                                    printf(" - LoopEnd\r\n");
+                                    loopCounter--;
+                                    if (loopCounter > 0 && loopSeqIx > 0)
+                                        curseqIx = loopSeqIx;
+                                    osSignalSet(mainTid, AS_Action);
+                                    break;
                             }
                         }
                     }
@@ -464,6 +504,11 @@
                 break;
         }
 
+        int now = (int)IdleTimer.read_ms();
+        if (now - LastSendMs > (IDLESTATUSTO - 1000))
+        {
+            PushMeasurements(measureGroup, partSize, robotArm);
+        }
     }
 }