Diff: ArmController.cpp

Revision:

8:d98e2dec0f40

Parent:

7:6723f6887d00

Child:

9:a0fb6c370dbb

--- a/ArmController.cpp	Tue Dec 29 23:31:28 2015 +0000
+++ b/ArmController.cpp	Thu Dec 31 17:47:55 2015 +0000
@@ -17,6 +17,16 @@
 
 using namespace std;
 
+void DispMeasure(char* label, int partSize, vector<float>& vals)
+{
+    printf("%s: ", label);
+    for (int ix = 0; ix < partSize; ix++)
+    {
+        printf("%d:%f ", ix, vals[ix]); 
+    }
+    printf("\r\n");
+
+}
 
 void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
 {
@@ -24,34 +34,26 @@
     time_t seconds = time(NULL);
     
     robotArm.GetArmMeasure(NM_Temperature, lastVals);
-    printf( "Temperatures: ");
-    for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
-    {
-        printf( "%d:%f ", servoIndex, lastVals[servoIndex]); 
-    }
-    printf( "\r\n");
+    DispMeasure("Temperatures", partSize, lastVals);
     measureGroup.SetMeasure(NM_Temperature, seconds, lastVals);
     MeasureBuf.push(measureGroup);
-    
+        
     robotArm.GetArmMeasure(NM_Voltage, lastVals);
-    printf( "Voltage: ");
-    for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
-    {
-        printf( "%d:%f ", servoIndex, lastVals[servoIndex]); 
-    }
-    printf( "\r\n");
+    DispMeasure("Voltage", partSize, lastVals);
     measureGroup.SetMeasure(NM_Voltage, seconds, lastVals);
     MeasureBuf.push(measureGroup);
     
     robotArm.GetArmMeasure(NM_Degrees, lastVals);
-    printf( "Rotation: ");
-    for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
-    {
-        printf( "%d:%f ", servoIndex, lastVals[servoIndex]); 
-    }
-    printf( "\r\n");
+    DispMeasure("Rotation", partSize, lastVals);
     measureGroup.SetMeasure(NM_Degrees, seconds, lastVals);
     MeasureBuf.push(measureGroup);
+    
+//    robotArm.GetArmMeasure(NM_Load, lastVals);
+//    DispMeasure("Load", partSize, lastVals);
+//    measureGroup.SetMeasure(NM_Load, seconds, lastVals);
+//    MeasureBuf.push(measureGroup);
+
+    SendIothubData();
 }
 
 void PushAlert(int severity, char* msg, char* atype)
@@ -61,6 +63,7 @@
     
     alert.SetAlert(severity, seconds, msg, atype);
     AlertBuf.push(alert);
+    SendIothubData();
 }
 
 void PushPositionAlert(int severity, int partIx, float pos)
@@ -70,9 +73,28 @@
     
     alert.SetPositionAlert(severity, seconds, partIx, pos);
     AlertBuf.push(alert);
+    SendIothubData();
 }
 
+void PushHardwareAlert(int severity, int partIx, int code)
+{
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    alert.SetHardwareAlert(severity, seconds, partIx, code);
+    AlertBuf.push(alert);
+    SendIothubData();
+}
 
+void PushTemperatureAlert(int severity, int partIx, float temp)
+{
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    alert.SetTemperatureAlert(severity, seconds, partIx, temp);
+    AlertBuf.push(alert);
+    SendIothubData();
+}
 
 vector<float> initPositions;
 
@@ -98,23 +120,43 @@
 
 osThreadId mainTid;
 
+// send alert if temperature is above 69C and return true
+#define MaxTemp     69
+bool TestTemperature(vector<float>& vals)
+{
+    bool err = false;
+
+    for (int ix = 0; ix < vals.size(); ix++)
+    {
+        if (vals[ix] > MaxTemp)
+        {
+            printf("Temperature threshold exceeded for part %d, temp %f \r\n", ix, vals[ix]); 
+            PushTemperatureAlert(AS_Error, ix, vals[ix]);
+            MainState = MS_Error;
+            err = true;
+        }
+    }
+    return err;
+}
+
+
 void ControlArm(const char* cmd)
 {
     if (strncmp(cmd, "pause", 5) == 0)
     {
-        printf( "Pause cmd\r\n"); 
+        printf("Pause cmd\r\n"); 
         MainState = MS_Paused;
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "resume", 6) == 0)
     {
-        printf( "Resume cmd\r\n"); 
+        printf("Resume cmd\r\n"); 
         MainState = MS_Running;
         osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runupdown", 9) == 0)
     {
-        printf( "Runupdown cmd\r\n"); 
+        printf("Runupdown cmd\r\n"); 
         MainState = MS_CancelToM;
         CancelToMatch = &UpDownSeq;
         SequenceQ.put(&UpDownSeq);
@@ -122,7 +164,7 @@
     }
     else if (strncmp(cmd, "runuptwist", 10) == 0)
     {
-        printf( "Runuptwist cmd\r\n"); 
+        printf("Runuptwist cmd\r\n"); 
         MainState = MS_CancelToM;
         CancelToMatch = &UpTwistSeq;
         SequenceQ.put(&UpTwistSeq);
@@ -130,7 +172,7 @@
     }
     else if (strncmp(cmd, "runhome", 10) == 0)
     {
-        printf( "Runhome cmd\r\n"); 
+        printf("Runhome cmd\r\n"); 
         MainState = MS_CancelToM;
         CancelToMatch = &StartSeq;
         SequenceQ.put(&StartSeq);
@@ -138,15 +180,23 @@
     }
     else if (strncmp(cmd, "runwave", 10) == 0)
     {
-        printf( "Runwave cmd\r\n"); 
+        printf("Runwave cmd\r\n"); 
         MainState = MS_CancelToM;
         CancelToMatch = &WaveSeq;
         SequenceQ.put(&WaveSeq);
         osSignalSet(mainTid, AS_Action);
     }
+    else if (strncmp(cmd, "runtaps1", 10) == 0)
+    {
+        printf("Runtaps1cmd\r\n"); 
+        MainState = MS_CancelToM;
+        CancelToMatch = &TapsSeq;
+        SequenceQ.put(&TapsSeq);
+        osSignalSet(mainTid, AS_Action);
+    }
     else if (strncmp(cmd, "cancelone", 9) == 0)
     {
-        printf( "CancelOne cmd\r\n"); 
+        printf("CancelOne cmd\r\n"); 
         MainState = MS_CancelOne;
         osSignalSet(mainTid, AS_Action);
     }
@@ -181,7 +231,6 @@
     
     // do inital status report
     PushMeasurements(measureGroup, partSize, robotArm);
-    SendIothubData();
  
     // Prepare main thread
     mainTid = osThreadGetId();
@@ -214,19 +263,19 @@
                 break;
                 
             case MS_Paused:
-                printf( "Paused\r\n"); 
+                printf("Paused\r\n"); 
                 break;
                 
             case MS_Stopping:
-                printf( "Stopping\r\n"); 
+                printf("Stopping\r\n"); 
                 break;
                 
             case MS_Error:
-                printf( "Error\r\n"); 
+                printf("Error\r\n"); 
                 break;
                 
             case MS_CancelOne:
-                printf( "Cancel sequence\r\n"); 
+                printf("Cancel sequence\r\n"); 
                 RunInSequence = false;
                 RunInMoveStep = false;
                 if (RunInDelay)
@@ -288,7 +337,7 @@
                     osEvent evt = SequenceQ.get(0);
                     if (evt.status == osEventMessage)
                     {
-                        printf( "New Seq \r\n"); 
+                        printf("New Seq \r\n"); 
                         curseq = (vector<ActionSequence> *)evt.value.p;
                         curseqIx = 0;
                         RunInSequence = true;
@@ -305,15 +354,13 @@
                             // report position error
                             int ix = robotArm.GetLastErrorPart();
                             float diff = robotArm.GetLastPosDiff();
-                            printf( "Position error detected part %d, diff %f \r\n", ix, diff); 
+                            printf("Position error detected part %d, diff %f \r\n", ix, diff); 
                             PushPositionAlert(AS_Error, ix, diff);
                             MainState = MS_Error;
-                            SendIothubData();
                             break;
                         }
                         if (robotArm.MoveArmPositionsHasNext())
                         {
-                            //printf( "Next Step\r\n");
                             int delaystep = 0;
                             bool ok = robotArm.MoveArmPositionsNext();
                             if (ok)
@@ -332,21 +379,23 @@
                             else
                             {
                                 // report HW error
-                                // int partix = robotArm.GetLastErrorPart();
-                                // int errCode = robotArm.GetLastError();
-                                // send alert
+                                int partix = robotArm.GetLastErrorPart();
+                                int errCode = robotArm.GetLastError();
+                                printf("Hardware error detected part %d, code %d \r\n", partix, errCode); 
+                                PushHardwareAlert(AS_Error, partix, errCode);
                                 MainState = MS_Error;
+                                break;
                             }
                         }
                         else
                         {
-                            printf( "No more Step\r\n");
+                            printf("No more Step\r\n");
                             RunInMoveStep = false;
                         }
                     }
                     if (!RunInMoveStep)
                     {
-                        printf( "Next Seq %d\r\n", curseqIx); 
+                        printf("Next Seq %d\r\n", curseqIx); 
                         
                         if (curseq != NULL)
                         {
@@ -359,41 +408,33 @@
                             ActionSequence aseq = (*curseq)[curseqIx];
                             curseqIx++;
                             
-                            bool ok;
                             switch (aseq.ActionType)
                             {
                                 case SA_SetGoal:
-                                    printf( " - Move arm start\r\n"); 
-                                    ok = robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Ms);
-                                    if (ok)
-                                    {
-                                        RunInMoveStep = true;
-                                        osSignalSet(mainTid, AS_Action);
-                                    }
-                                    else
-                                    {
-                                        // TODO: send alert
-                                    }
+                                    printf(" - Move arm start\r\n"); 
+                                    robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Param);
+                                    RunInMoveStep = true;
+                                    osSignalSet(mainTid, AS_Action);
                                     break;
                                 case SA_Delay:
-                                    printf( " - Delay\r\n");
+                                    printf(" - Delay\r\n");
                                     RunInDelay = true;
-                                    delayTimer.start(aseq.Ms);
+                                    delayTimer.start(aseq.Param);
                                     break;
 
                                 case SA_Status:
-                                    printf( " - Status\r\n"); 
-                                    robotArm.GetArmPositions(lastVals);
-                                    
+                                    printf(" - Status\r\n"); 
+
                                     PushMeasurements(measureGroup, partSize, robotArm);
-                             
-                                    SendIothubData();
-            
+
+                                    robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
+                                    TestTemperature(lastVals);
+                                    
                                     osSignalSet(mainTid, AS_Action);
                                     break;
                                 case SA_Repeat:
-                                    printf( " - Repeat\r\n"); 
-                                    curseqIx = 0;
+                                    printf(" - Repeat\r\n"); 
+                                    curseqIx = aseq.Param;
                                     osSignalSet(mainTid, AS_Action);
                                     break;