demo project

Dependencies:   AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL

Revision:
13:ffeff9b5e513
Parent:
11:3a2e6eb9fbb8
Child:
14:570c8071f577
--- a/ArmController.cpp	Thu Jan 07 17:31:23 2016 +0000
+++ b/ArmController.cpp	Fri Jan 15 22:02:46 2016 +0000
@@ -1,9 +1,7 @@
 #include "mbed.h"
 #include "rtos.h"
 
-#include "EthernetInterface.h"
-#include "mbed/logging.h"
-#include "mbed/mbedtime.h"
+#include "threadapi.h"
 #include <NTPClient.h>
 #include <DynamixelBus.h>
 #include <AX12.h>
@@ -19,155 +17,213 @@
 using namespace std;
 
 
-vector<float> initPositions;
-
-
 enum MainStates
 {
     MS_Idle           = 0x0,
     MS_Running        = 0x1,
     MS_Paused         = 0x2,
-    MS_Stopping       = 0x3,
-    MS_Error          = 0x4,
-    MS_CancelOne      = 0x5,
-    MS_CancelAll      = 0x6,
-    MS_CancelToM      = 0x7
+    MS_Error          = 0x3,
+    MS_CancelOne      = 0x4,
+    MS_CancelAll      = 0x5,
+    MS_Resuming       = 0x6
 };
 
-// try to send some status every minute
-#define IDLESTATUSTO        60000
+// controller thread runs every 25ms
+#define CONTROLLER_POLL         25
+
+// try to send some status every few seconds
+#define SEND_STATUS_TO          500
+// use slower send rate if paused
+#define SEND_STATUS_PAUSED_TO   20000
+
+// controller polling timer
 Timer IdleTimer;
-int LastSendMs = 0;
+int NextSendMs = 0;
+int NextStepMs = 0;
+int NextPollMs = 0;
 
-Queue<vector<ActionSequence>, 16> SequenceQ;
+// action sequences
+Queue<vector<ActionSequence*>, 8> SequenceQ;
+
+// controller state
 MainStates MainState;
-bool ErrorReset;
 bool RunInSequence;
 bool RunInMoveStep;
-bool RunInDelay;
-void* CancelToMatch;
+
+bool NeedHwReset;
 
-osThreadId mainTid;
-
-
-void DispMeasure(char* label, int partSize, vector<float>& vals)
+// utility method
+void DispMeasure(char* label, int partSize, float vals[])
 {
-    printf("%s: ", label);
-    for (int ix = 0; ix < partSize; ix++)
-    {
-        printf("%d:%f ", ix, vals[ix]); 
-    }
-    printf("\r\n");
-
+//    printf("%s: ", label);
+//    for (int ix = 0; ix < partSize; ix++)
+//    {
+//        printf("%d:%f ", ix, vals[ix]); 
+//    }
+//    printf("\r\n");
 }
 
-
-void PushAlert(int severity, char* msg, char* atype)
+// send alert message
+void PushAlert(char* msg, char* atype)
 {
     Alert alert;
     time_t seconds = time(NULL);
     
-    alert.SetAlert(severity, seconds, msg, atype);
+    alert.SetAlert(seconds, msg, atype);
     AlertBuf.push(alert);
-    SendIothubData();
 }
 
-void PushPositionAlert(int severity, int partIx, float pos)
+// send position alert
+void PushPositionAlert(RobotArm& robotArm)
 {
+    // stop trying to move
+    float diff = robotArm.GetLastPosDiff();
+    int ix = robotArm.GetLastErrorPart();
+    robotArm.MoveArmPositionsStop();
+    
+    // report load error
+    printf("Position error detected joint %d, value diff %f\r\n", ix, diff);
+
     Alert alert;
     time_t seconds = time(NULL);
     
     ShowLedRed();
-    alert.SetPositionAlert(severity, seconds, partIx, pos);
+    alert.SetPositionAlert(seconds, ix, diff);
     AlertBuf.push(alert);
-    SendIothubData();
+    
+    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
 }
 
-void PushHardwareAlert(int severity, int partIx, int code)
+// send load alert
+void PushLoadAlert(RobotArm& robotArm)
+{
+    float lastVals[NUMJOINTS];
+    
+    // stop trying to move
+    robotArm.GetArmLastMeasure(NM_Load, lastVals);
+    int ix = robotArm.GetLastErrorPart();
+    robotArm.MoveArmPositionsStop();
+    
+    // report load error
+    printf("Load error detected joint %d, value %f\r\n", ix, lastVals[ix]);
+
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    ShowLedRed();
+    alert.SetLoadAlert(seconds, ix, lastVals[ix]);
+    AlertBuf.push(alert);
+    
+    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
+}
+
+// send hardware error alert
+void PushHardwareAlert(int partIx, int code)
 {
     Alert alert;
     time_t seconds = time(NULL);
     
     ShowLedRed();
-    alert.SetHardwareAlert(severity, seconds, partIx, code);
+    alert.SetHardwareAlert(seconds, partIx, code);
     AlertBuf.push(alert);
-    SendIothubData();
+    NeedHwReset = true;
+        
+    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
 }
 
-void PushTemperatureAlert(int severity, int partIx, float temp)
+// send temperature alert
+void PushTemperatureAlert(RobotArm& robotArm)
 {
+    float lastVals[NUMJOINTS];
+    
+    // stop trying to move
+    robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
+    int ix = robotArm.GetLastErrorPart();
+    robotArm.MoveArmPositionsStop();
+    
+    // report load error
+    printf("Temperature error detected joint %d, value %f\r\n", ix, lastVals[ix]);
+
     Alert alert;
     time_t seconds = time(NULL);
     
     ShowLedRed();
-    alert.SetTemperatureAlert(severity, seconds, partIx, temp);
+    alert.SetTemperatureAlert(seconds, ix, lastVals[ix]);
     AlertBuf.push(alert);
-    SendIothubData();
+    
+    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
 }
 
-// send alert if temperature is above 69C and return true
-#define MaxTemp     69
-bool TestTemperature(vector<float>& vals)
+// send temperature alert
+void PushVoltageAlert(RobotArm& robotArm)
 {
-    bool err = false;
+    float lastVals[NUMJOINTS];
+    
+    // stop trying to move
+    robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
+    int ix = robotArm.GetLastErrorPart();
+    robotArm.MoveArmPositionsStop();
+    
+    // report load error
+    printf("Voltage error detected joint %d, value %f\r\n", ix, lastVals[ix]);
 
-    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;
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    ShowLedRed();
+    alert.SetVoltageAlert(seconds, ix, lastVals[ix]);
+    AlertBuf.push(alert);
+    
+    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
 }
 
 
-void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
+void PushMeasurements(int partSize, RobotArm& robotArm)
 {
-    vector<float> lastVals;
-    time_t seconds = time(NULL);
-    LastSendMs = (int)IdleTimer.read_ms();
+    // space for getting measurements from arm
+    MeasureGroup measureGroup;
     
+    float lastVals[NUMJOINTS];
+    time_t seconds = time(NULL);
+   
     bool ok = true;
-    
-    ok = robotArm.GetArmMeasure(NM_Temperature, lastVals);
+
+    ok = robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
     DispMeasure("Temperatures", partSize, lastVals);
-    measureGroup.SetMeasure(NM_Temperature, seconds, lastVals);
+    measureGroup.SetMeasure(NM_Temperature, seconds, partSize, lastVals);
     MeasureBuf.push(measureGroup);
     
     if (ok)
     {
-        ok = robotArm.GetArmMeasure(NM_Voltage, lastVals);
+        ok = robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
         DispMeasure("Voltage", partSize, lastVals);
-        measureGroup.SetMeasure(NM_Voltage, seconds, lastVals);
+        measureGroup.SetMeasure(NM_Voltage, seconds, partSize, lastVals);
         MeasureBuf.push(measureGroup);
     }
     
     if (ok)
     {
-        ok = robotArm.GetArmMeasure(NM_Degrees, lastVals);
+        ok = robotArm.GetArmLastMeasure(NM_Degrees, lastVals);
         DispMeasure("Rotation", partSize, lastVals);
-        measureGroup.SetMeasure(NM_Degrees, seconds, lastVals);
+        measureGroup.SetMeasure(NM_Degrees, seconds, partSize, lastVals);
         MeasureBuf.push(measureGroup);
     }
     
-//    robotArm.GetArmMeasure(NM_Load, lastVals);
-//    DispMeasure("Load", partSize, lastVals);
-//    measureGroup.SetMeasure(NM_Load, seconds, lastVals);
-//    MeasureBuf.push(measureGroup);
-
-    SendIothubData();
+    if (ok)
+    {
+        robotArm.GetArmLastMeasure(NM_Load, lastVals);
+        DispMeasure("Load", partSize, lastVals);
+        measureGroup.SetMeasure(NM_Load, seconds, partSize, lastVals);
+        MeasureBuf.push(measureGroup);
+    }
     
     if (!ok)
     {
         // report HW error
         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);
+        printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); 
+        PushHardwareAlert(partix, errCode);
         MainState = MS_Error;
     }
 }
@@ -176,146 +232,136 @@
 
 void ControlArm(const char* cmd)
 {
-    if (MainState == MS_Error)
-    {
-        ErrorReset = true;
-    }
-    
     if (strncmp(cmd, "pause", 5) == 0)
     {
         ShowLedBlue();
         MainState = MS_Paused;
-        osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "resume", 6) == 0)
     {
         ShowLedGreen();
-        MainState = MS_Running;
-        osSignalSet(mainTid, AS_Action);
+        MainState = MS_Resuming;
     }
     else if (strncmp(cmd, "cancel", 6) == 0)
     {
         ShowLedGreen();
         MainState = MS_CancelOne;
-        osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runupdown", 9) == 0)
     {
         ShowLedGreen();
-        MainState = MS_CancelOne;
+        if (MainState == MS_Idle || MainState == MS_Paused)
+            MainState = MS_Running;
         SequenceQ.put(&UpDownSeq);
-        osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runuptwist", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_CancelOne;
+        if (MainState == MS_Idle || MainState == MS_Paused)
+            MainState = MS_Running;
         SequenceQ.put(&UpTwistSeq);
-        osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runhome", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_CancelOne;
+        if (MainState == MS_Idle || MainState == MS_Paused)
+            MainState = MS_Running;
         SequenceQ.put(&StartSeq);
-        osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runwave", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_CancelOne;
+        MainState = MS_Running;
         SequenceQ.put(&WaveSeq);
-        osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "runtaps1", 10) == 0)
     {
         ShowLedGreen();
-        MainState = MS_CancelOne;
+        if (MainState == MS_Idle || MainState == MS_Paused)
+            MainState = MS_Running;
         SequenceQ.put(&TapsSeq);
-        osSignalSet(mainTid, AS_Action);
     }
     else if (strncmp(cmd, "fastwave", 8) == 0)
     {
         ShowLedGreen();
-        MainState = MS_CancelOne;
-        SequenceQ.put(&FastWaveSeq);
-        osSignalSet(mainTid, AS_Action);
+        MainState = MS_Running;
+        SequenceQ.put(&BigWaveSeq);
+    }
+    else if (strncmp(cmd, "beep", 9) == 0)
+    {
+        BuzzerStartMs((int)IdleTimer.read_ms(), 500);
     }
 }
 
-// run sequence thread timer routine
-void NextSeq(void const * tid)
+
+// things to initialize early 
+void PrepareController()
 {
-    RunInDelay = false;
-    osSignalSet((osThreadId)tid, AS_Action);
+    MakeSequences(NUMJOINTS);
 }
 
-// periodic timer routine
-void PeriodicStatus(void const * tid)
-{
-    // make sure we run controller even if idle
-    osSignalSet((osThreadId)tid, AS_Action);
-}
-
+// Run the controller thread loop
 void RunController()
 {
+    printf("RunController start");
+
     // init robot arm
     RobotArm robotArm;
+    robotArm.SetStepMs(CONTROLLER_POLL);
     
     int partSize = robotArm.GetNumParts();
 
     IdleTimer.start();
-    LastSendMs = (int)IdleTimer.read_ms();
     
-    // set running state
+     // set running state
     MainState = MS_Idle;
     RunInSequence = false;
     RunInMoveStep = false;
-    RunInDelay = false;
-    ErrorReset = false;
-    
-    // get initial positions
-    MeasureGroup measureGroup;
-    robotArm.GetArmPositions(initPositions);
-    
-    vector<float> lastVals;
     
-    // do inital status report
-    PushMeasurements(measureGroup, partSize, robotArm);
- 
-    // Prepare main thread
-    mainTid = osThreadGetId();
-    int32_t signals = AS_Action;
+    NeedHwReset = false;
     
-    // create sequences
-    MakeSequences(partSize, initPositions);
-  
-    // add to queue
+    // set first sequence to run
     SequenceQ.put(&StartSeq);
     
     // state for sequence
-    vector<ActionSequence> *curseq = NULL;
+    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);
+    // init time outs
+    NextStepMs = 0;
+    NextPollMs = (int)IdleTimer.read_ms();
+    NextSendMs = (int)IdleTimer.read_ms() + SEND_STATUS_TO;
     
     MainState = MS_Running;
+    NodeMeasure nextMetric = NM_Temperature;
     
     while( true )
     {
-        osEvent mainEvent = osSignalWait(signals, osWaitForever);
+        int now = (int)IdleTimer.read_ms();
         
-        if (ErrorReset)
+        // Sleep until next controller poll time 
+        if (NextPollMs > now)
+        {
+            ThreadAPI_Sleep(NextPollMs - now);
+            now = (int)IdleTimer.read_ms();
+        }
+        else
         {
-            ErrorReset = false;
-            robotArm.ClearErrorState();
+            printf("too slow %d\r\n", now - NextPollMs);
+        }
+        // set next poll time
+        NextPollMs = now + CONTROLLER_POLL;
+
+        // if had HW error, reset error state
+        if (MainState != MS_Error)
+        {
+            if (NeedHwReset)
+            {
+                robotArm.ClearErrorState();
+                NeedHwReset = false;
+            }
         }
         
         switch (MainState)
@@ -324,129 +370,78 @@
                 break;
                 
             case MS_Paused:
-                printf("Paused\r\n"); 
-                break;
-                
-            case MS_Stopping:
-                printf("Stopping\r\n"); 
                 break;
                 
             case MS_Error:
-                printf("Error\r\n"); 
                 break;
                 
             case MS_CancelOne:
-                printf("Cancel sequence\r\n"); 
                 RunInSequence = false;
                 RunInMoveStep = false;
-                if (RunInDelay)
-                {
-                    RunInDelay = false;
-                    delayTimer.stop();
-                }
+                robotArm.MoveArmPositionsEnd();
                 MainState = MS_Running;
-                osSignalSet(mainTid, AS_Action);
+                // avoid next poll delay
+                NextPollMs = now;
                 break;
                 
             case MS_CancelAll:
-                printf("Cancel all\r\n"); 
                 RunInSequence = false;
                 RunInMoveStep = false;
-                if (RunInDelay)
-                {
-                    RunInDelay = false;
-                    delayTimer.stop();
-                }
+                robotArm.MoveArmPositionsEnd();
                 while (1) {
                     osEvent evt = SequenceQ.get(0);
                     if (evt.status != osEventMessage)
                         break;
                 }
+                // avoid next poll delay
+                NextPollMs = now;
+                MainState = MS_Running;
+                break;
+                
+            case MS_Resuming:
+                robotArm.MoveArmPositionsResume();
+                // avoid next poll delay
+                NextPollMs = now;
                 MainState = MS_Running;
                 break;
                 
-            case MS_CancelToM:
-                printf("Cancel until latest\r\n"); 
-                RunInSequence = false;
-                RunInMoveStep = false;
-                if (RunInDelay)
-                {
-                    RunInDelay = false;
-                    delayTimer.stop();
-                }
-                while (1) {
-                    osEvent evt = SequenceQ.get(0);
-                    if (evt.status != osEventMessage)
-                        break;
-                    else if (evt.value.p == CancelToMatch)
-                    {
-                        curseq = (vector<ActionSequence> *)evt.value.p;
-                        curseqIx = 0;
-                        RunInSequence = true;
-                    }
-                }
-                MainState = MS_Running;
-                osSignalSet(mainTid, AS_Action);
-                break;
-                
             case MS_Running:
-                if (RunInDelay)
-                {
-                    // waiting for timer to fire. do nothing
-                    printf("Should be delaying. Skip action\r\n");
-                    break;
-                }
                 if (!RunInSequence)
                 {
+                    // start new sequence of actions
                     osEvent evt = SequenceQ.get(0);
                     if (evt.status == osEventMessage)
                     {
-                        printf("New Seq \r\n"); 
-                        curseq = (vector<ActionSequence> *)evt.value.p;
+                        printf("New Seq\r\n"); 
+                        curseq = (vector<ActionSequence*> *)evt.value.p;
                         curseqIx = 0;
                         RunInSequence = true;
                         RunInMoveStep = false;
                     }
+                    else
+                    {
+                        MainState = MS_Idle;
+                    }
                 }
                 
                 if (RunInSequence)
                 {
                     if (RunInMoveStep)
                     {
-                        if (!robotArm.MoveArmPositionTest())
-                        {
-                            // report position error
-                            int ix = robotArm.GetLastErrorPart();
-                            float diff = robotArm.GetLastPosDiff();
-                            printf("Position error detected part %d, diff %f \r\n", ix, diff); 
-                            PushPositionAlert(AS_Error, ix, diff);
-                            MainState = MS_Error;
+                        // check if delaying next move
+                        if (NextStepMs > now)
                             break;
-                        }
+                        
                         if (robotArm.MoveArmPositionsHasNext())
                         {
-                            int delaystep = 0;
                             bool ok = robotArm.MoveArmPositionsNext();
-                            if (ok)
-                            {
-                                robotArm.MoveArmPositionsDelay(delaystep);
-                                if (delaystep > 0)
-                                {
-                                    RunInDelay = true;
-                                    delayTimer.start(delaystep);
-                                }
-                                else
-                                {
-                                    osSignalSet(mainTid, AS_Action);
-                                }
-                            }
-                            else
+                            if (!ok)
                             {
                                 // report HW error
                                 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);
+                                printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
+                                PushHardwareAlert(partix, errCode);
                                 MainState = MS_Error;
                                 break;
                             }
@@ -459,8 +454,6 @@
                     }
                     if (!RunInMoveStep)
                     {
-                        printf("Next Seq %d\r\n", curseqIx); 
-                        
                         if (curseq != NULL)
                         {
                             if (curseqIx >= curseq->size())
@@ -470,44 +463,30 @@
                                 break;
                             }
 
-                            ActionSequence aseq = (*curseq)[curseqIx];
+                            ActionSequence* aseq = (*curseq)[curseqIx];
                             curseqIx++;
                             
-                            switch (aseq.ActionType)
+                            switch (aseq->ActionType)
                             {
                                 case SA_SetGoal:
                                     printf(" - Move arm start\r\n"); 
-                                    robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Param);
+                                    robotArm.MoveArmPositionsStart(aseq->GoalVals, aseq->Param);
                                     RunInMoveStep = true;
-                                    osSignalSet(mainTid, AS_Action);
                                     break;
                                 case SA_Delay:
                                     printf(" - Delay\r\n");
-                                    RunInDelay = true;
-                                    delayTimer.start(aseq.Param);
-                                    break;
-                                case SA_Status:
-                                    printf(" - Status\r\n"); 
-
-                                    PushMeasurements(measureGroup, partSize, robotArm);
-
-                                    robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
-                                    TestTemperature(lastVals);
-                                    
-                                    osSignalSet(mainTid, AS_Action);
+                                    NextStepMs = aseq->Param + now;
                                     break;
                                 case SA_LoopBegin:
                                     printf(" - LoopBegin\r\n");
                                     loopSeqIx = curseqIx;
-                                    loopCounter = aseq.Param;
-                                    osSignalSet(mainTid, AS_Action);
+                                    loopCounter = aseq->Param;
                                     break;
                                 case SA_LoopEnd:
                                     printf(" - LoopEnd\r\n");
                                     loopCounter--;
                                     if (loopCounter > 0 && loopSeqIx > 0)
                                         curseqIx = loopSeqIx;
-                                    osSignalSet(mainTid, AS_Action);
                                     break;
                             }
                         }
@@ -516,11 +495,79 @@
                 }
                 break;
         }
+        
+        bool sendAlert = MainState != MS_Error;
+        
+        // get measurements and test thresholds
+        // Reading all values takes too long, 
+        // so we read the load and 1 other value each time
+        int rc = 0;
+        rc = robotArm.ArmMeasuresTest(NM_Load);
+        if (rc < 0 && sendAlert)
+        {
+            PushLoadAlert(robotArm);
+            MainState = MS_Error;
+        }
+        
+        switch (nextMetric)
+        {
+            case NM_Temperature:
+                rc = robotArm.ArmMeasuresTest(NM_Temperature);
+                if (rc < 0 && sendAlert)
+                {
+                    PushTemperatureAlert(robotArm);
+                    MainState = MS_Error;
+                }
+                nextMetric = NM_Voltage;
+                break;
+                
+            case NM_Voltage:
+                rc = robotArm.ArmMeasuresTest(NM_Voltage);
+                if (rc < 0 && sendAlert)
+                {
+                    PushVoltageAlert(robotArm);
+                    MainState = MS_Error;
+                }
+                nextMetric = NM_Degrees;
+                break;
+                
+               
+            case NM_Degrees:
+                rc = robotArm.ArmMeasuresTest(NM_Degrees);
+                if (rc < 0 && sendAlert)
+                {
+                    PushPositionAlert(robotArm);
+                    MainState = MS_Error;
+                }
+                nextMetric = NM_Temperature;
+                break;
 
-        int now = (int)IdleTimer.read_ms();
-        if (now - LastSendMs > (IDLESTATUSTO - 1000))
+            default:
+                nextMetric = NM_Temperature;
+                break;
+        }
+        if (rc == -2)
         {
-            PushMeasurements(measureGroup, partSize, robotArm);
+            int partix = robotArm.GetLastErrorPart();
+            int errCode = robotArm.GetLastError();
+            printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); 
+            PushHardwareAlert(partix, errCode);
+            MainState = MS_Error;
+        }
+        
+        // check if buzzer needs to be turned off
+        BuzzerPoll(now);
+        
+        // check if time to send status
+        if (now >= NextSendMs)
+        {
+            // if paused, use longer time out for sending data
+            if (MainState != MS_Paused ||
+                now > NextSendMs + SEND_STATUS_PAUSED_TO)
+            {
+                NextSendMs = now + SEND_STATUS_TO;
+                PushMeasurements(partSize, robotArm);
+            }
         }
     }
 }