Revision 7:6723f6887d00, committed 2015-12-29

Comitter:

henryrawas

Date:

Tue Dec 29 23:31:28 2015 +0000

Parent:

6:feb0a6311594

Child:

8:d98e2dec0f40

Commit message:

motion block alerts, more commands

Changed in this revision

--- a/AX-12A.lib	Mon Dec 28 17:29:12 2015 +0000
+++ b/AX-12A.lib	Tue Dec 29 23:31:28 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/robot-arm-demo-team/code/AX-12A/#bae6dc62dfb4
+http://developer.mbed.org/teams/robot-arm-demo-team/code/AX-12A/#a702043b1420

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Alert.cpp	Tue Dec 29 23:31:28 2015 +0000
@@ -0,0 +1,48 @@
+#include "mbed.h"
+#include "crt_abstractions.h"
+
+#include "Alert.h"
+
+
+
+SafeCircBuf<Alert, AlertBufSize, uint32_t> AlertBuf;
+
+void Alert::SetAlert(int severity, time_t created, char* msg, char* atype)
+{
+    Sev = severity;
+    Created = created;
+    
+    if (strlen(atype) >= AlertTypeMaxLen)
+    {
+        strncpy(AlertType, atype, AlertTypeMaxLen);
+        AlertType[AlertTypeMaxLen - 1] = 0;
+    }
+    else
+    {
+        strcpy(AlertType, atype);
+    }
+    if (strlen(msg) >= AlertMsgMaxLen)
+    {
+        strncpy(Msg, msg, AlertMsgMaxLen);
+        Msg[AlertMsgMaxLen - 1] = 0;
+    }
+    else
+    {
+        strcpy(Msg, msg);
+    }
+}
+
+
+void Alert::SetPositionAlert(int severity, time_t created, int partIx, float diff)
+{
+    char* msg = "Arm could not be moved to desired position. Part %d is off by %f";
+    int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, diff);
+    Sev = severity;
+    Created = created;
+    
+    strcpy(AlertType, "Position");
+    if (slen < 0)
+    {
+        strcpy(Msg, "Arm could not be moved to desired position.");
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Alert.h	Tue Dec 29 23:31:28 2015 +0000
@@ -0,0 +1,41 @@
+/* 
+Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license.
+*/
+
+#ifndef __ALERT_H__
+
+#include "mbed.h"
+#include "SafeCircBuf.h"
+
+#define AlertBufSize  20
+#define AlertMsgMaxLen  200
+#define AlertTypeMaxLen  16
+
+enum AlertSeverity
+{
+    AS_Info     = 0,
+    AS_Warn     = 1,
+    AS_Error    = 2
+};
+
+class Alert
+{
+public:
+    Alert() {};
+    
+    void SetAlert(int severity, time_t created, char* msg, char* atype);
+    
+    void SetPositionAlert(int severity, time_t created, int partIx, float diff);
+    
+    int Sev;
+    
+    char AlertType[AlertTypeMaxLen];
+    
+    char Msg[AlertMsgMaxLen];
+    
+    time_t Created;
+};
+
+extern SafeCircBuf<Alert, AlertBufSize, uint32_t> AlertBuf;
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ArmController.cpp	Tue Dec 29 23:31:28 2015 +0000
@@ -0,0 +1,410 @@
+#include "mbed.h"
+#include "rtos.h"
+
+#include "EthernetInterface.h"
+#include "mbed/logging.h"
+#include "mbed/mbedtime.h"
+#include <NTPClient.h>
+#include <DynamixelBus.h>
+#include <AX12.h>
+#include <Terminal.h>
+#include <vector>
+#include <RobotArm.h>
+#include <MeasureBuf.h>
+#include <IothubRobotArm.h>
+#include <ActionBuf.h>
+#include <Sequences.h>
+
+using namespace std;
+
+
+void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
+{
+    vector<float> lastVals;
+    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");
+    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");
+    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");
+    measureGroup.SetMeasure(NM_Degrees, seconds, lastVals);
+    MeasureBuf.push(measureGroup);
+}
+
+void PushAlert(int severity, char* msg, char* atype)
+{
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    alert.SetAlert(severity, seconds, msg, atype);
+    AlertBuf.push(alert);
+}
+
+void PushPositionAlert(int severity, int partIx, float pos)
+{
+    Alert alert;
+    time_t seconds = time(NULL);
+    
+    alert.SetPositionAlert(severity, seconds, partIx, pos);
+    AlertBuf.push(alert);
+}
+
+
+
+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
+};
+
+Queue<vector<ActionSequence>, 16> SequenceQ;
+volatile MainStates MainState;
+bool RunInSequence;
+bool RunInMoveStep;
+bool RunInDelay;
+void* CancelToMatch;
+
+osThreadId mainTid;
+
+void ControlArm(const char* cmd)
+{
+    if (strncmp(cmd, "pause", 5) == 0)
+    {
+        printf( "Pause cmd\r\n"); 
+        MainState = MS_Paused;
+        osSignalSet(mainTid, AS_Action);
+    }
+    else if (strncmp(cmd, "resume", 6) == 0)
+    {
+        printf( "Resume cmd\r\n"); 
+        MainState = MS_Running;
+        osSignalSet(mainTid, AS_Action);
+    }
+    else if (strncmp(cmd, "runupdown", 9) == 0)
+    {
+        printf( "Runupdown cmd\r\n"); 
+        MainState = MS_CancelToM;
+        CancelToMatch = &UpDownSeq;
+        SequenceQ.put(&UpDownSeq);
+        osSignalSet(mainTid, AS_Action);
+    }
+    else if (strncmp(cmd, "runuptwist", 10) == 0)
+    {
+        printf( "Runuptwist cmd\r\n"); 
+        MainState = MS_CancelToM;
+        CancelToMatch = &UpTwistSeq;
+        SequenceQ.put(&UpTwistSeq);
+        osSignalSet(mainTid, AS_Action);
+    }
+    else if (strncmp(cmd, "runhome", 10) == 0)
+    {
+        printf( "Runhome cmd\r\n"); 
+        MainState = MS_CancelToM;
+        CancelToMatch = &StartSeq;
+        SequenceQ.put(&StartSeq);
+        osSignalSet(mainTid, AS_Action);
+    }
+    else if (strncmp(cmd, "runwave", 10) == 0)
+    {
+        printf( "Runwave cmd\r\n"); 
+        MainState = MS_CancelToM;
+        CancelToMatch = &WaveSeq;
+        SequenceQ.put(&WaveSeq);
+        osSignalSet(mainTid, AS_Action);
+    }
+    else if (strncmp(cmd, "cancelone", 9) == 0)
+    {
+        printf( "CancelOne cmd\r\n"); 
+        MainState = MS_CancelOne;
+        osSignalSet(mainTid, AS_Action);
+    }
+}
+
+// run sequence thread timer routine
+void NextSeq(void const * tid)
+{
+    RunInDelay = false;
+    osSignalSet((osThreadId)tid, AS_Action);
+}
+
+void RunController()
+{
+    // init robot arm
+    RobotArm robotArm;
+    
+    int partSize = robotArm.GetNumParts();
+
+   
+    // set running state
+    MainState = MS_Idle;
+    RunInSequence = false;
+    RunInMoveStep = false;
+    RunInDelay = false;
+     
+    // get initial positions
+    MeasureGroup measureGroup;
+    robotArm.GetArmPositions(initPositions);
+    
+    vector<float> lastVals;
+    
+    // do inital status report
+    PushMeasurements(measureGroup, partSize, robotArm);
+    SendIothubData();
+ 
+    // Prepare main thread
+    mainTid = osThreadGetId();
+    int32_t signals = AS_Action;
+    
+    // create sequences
+    MakeSequences(partSize, initPositions);
+  
+    // add to queue
+    SequenceQ.put(&StartSeq);
+    
+    // state for sequence
+    vector<ActionSequence> *curseq = NULL;
+    int curseqIx = 0;
+
+    // signal to run the default action for demo
+    osSignalSet(mainTid, AS_Action);
+    
+    RtosTimer delayTimer(NextSeq, osTimerOnce, (void *)osThreadGetId());    
+    
+    MainState = MS_Running;
+    
+    while( true )
+    {
+        osEvent mainEvent = osSignalWait(signals, osWaitForever);
+        
+        switch (MainState)
+        {
+            case MS_Idle:
+                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();
+                }
+                MainState = MS_Running;
+                osSignalSet(mainTid, AS_Action);
+                break;
+                
+            case MS_CancelAll:
+                RunInSequence = false;
+                RunInMoveStep = false;
+                if (RunInDelay)
+                {
+                    RunInDelay = false;
+                    delayTimer.stop();
+                }
+                while (1) {
+                    osEvent evt = SequenceQ.get(0);
+                    if (evt.status != osEventMessage)
+                        break;
+                }
+                MainState = MS_Running;
+                break;
+                
+            case MS_CancelToM:
+                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
+                    break;
+                }
+                if (!RunInSequence)
+                {
+                    osEvent evt = SequenceQ.get(0);
+                    if (evt.status == osEventMessage)
+                    {
+                        printf( "New Seq \r\n"); 
+                        curseq = (vector<ActionSequence> *)evt.value.p;
+                        curseqIx = 0;
+                        RunInSequence = true;
+                        RunInMoveStep = false;
+                    }
+                }
+                
+                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;
+                            SendIothubData();
+                            break;
+                        }
+                        if (robotArm.MoveArmPositionsHasNext())
+                        {
+                            //printf( "Next Step\r\n");
+                            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
+                            {
+                                // report HW error
+                                // int partix = robotArm.GetLastErrorPart();
+                                // int errCode = robotArm.GetLastError();
+                                // send alert
+                                MainState = MS_Error;
+                            }
+                        }
+                        else
+                        {
+                            printf( "No more Step\r\n");
+                            RunInMoveStep = false;
+                        }
+                    }
+                    if (!RunInMoveStep)
+                    {
+                        printf( "Next Seq %d\r\n", curseqIx); 
+                        
+                        if (curseq != NULL)
+                        {
+                            if (curseqIx >= curseq->size())
+                            {
+                                RunInSequence = false;
+                                break;
+                            }
+
+                            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
+                                    }
+                                    break;
+                                case SA_Delay:
+                                    printf( " - Delay\r\n");
+                                    RunInDelay = true;
+                                    delayTimer.start(aseq.Ms);
+                                    break;
+
+                                case SA_Status:
+                                    printf( " - Status\r\n"); 
+                                    robotArm.GetArmPositions(lastVals);
+                                    
+                                    PushMeasurements(measureGroup, partSize, robotArm);
+                             
+                                    SendIothubData();
+            
+                                    osSignalSet(mainTid, AS_Action);
+                                    break;
+                                case SA_Repeat:
+                                    printf( " - Repeat\r\n"); 
+                                    curseqIx = 0;
+                                    osSignalSet(mainTid, AS_Action);
+                                    break;
+                                    
+                            }
+                        }
+                    }
+ 
+                }
+                break;
+        }
+
+    }
+}
+

--- a/IothubRobotArm.cpp	Mon Dec 28 17:29:12 2015 +0000
+++ b/IothubRobotArm.cpp	Tue Dec 29 23:31:28 2015 +0000
@@ -31,7 +31,8 @@
 // message buffers to use
 #define MESSAGE_LEN 1024
 static char msgText[MESSAGE_LEN];
-static char propText[MESSAGE_LEN];
+//static char propText[MESSAGE_LEN];
+
 #define MESSAGE_COUNT 10
 EVENT_INSTANCE messages[MESSAGE_COUNT];
 
@@ -40,6 +41,7 @@
 static int callbackCounter;
 static int msgNumber;
 
+
 static  IOTHUB_CLIENT_HANDLE iotHubClientHandle;
 
 
@@ -93,16 +95,18 @@
 {
     EVENT_INSTANCE* eventInstance = (EVENT_INSTANCE*)userContextCallback;
     (void)printf("Confirmation[%d] received for message tracking id = %d with result = %s\r\n", callbackCounter, eventInstance->messageTrackingId, ENUM_TO_STRING(IOTHUB_CLIENT_CONFIRMATION_RESULT, result));
-    /* Some device specific action code goes here... */
+
     callbackCounter++;
     IoTHubMessage_Destroy(eventInstance->messageHandle);
     
     if (callbackCounter == msgNumber)
     {
         // call SendMeasurements again in case more to send
+        SendIothubData();
     }
 }
 
+
 // IoT Hub thread
 static Thread* IotThread = NULL;
 static bool IotThreadClose;
@@ -130,7 +134,6 @@
         {
             if ((ev.value.signals & IS_SendStatus) == IS_SendStatus)
             {
-                (void)printf("Iothub thread send status signal\r\n");
                 iotRobot.SendMeasurements();
             }
             else
@@ -148,9 +151,8 @@
     return true;
 }
 
-bool SendIothubMeasurements()
+bool SendIothubData()
 {
-    (void)printf("Iothub thread signal to send data\r\n");
     IotThread->signal_set(IS_SendStatus);
     return true;
 }
@@ -228,8 +230,16 @@
         // get buffer from array
         int i = msgNumber % MESSAGE_COUNT;
     
-        int msglen = msgSerialize.MeasureBufToString(msgText, MESSAGE_LEN);
-
+        int msglen = 0;
+        bool ismeasure = false;
+        // get alert if any, otherwise get measure data
+        msglen = msgSerialize.AlertBufToString(msgText, MESSAGE_LEN);
+        if (msglen == 0)
+        {
+            ismeasure = true;
+            msglen = msgSerialize.MeasureBufToString(msgText, MESSAGE_LEN);
+        }
+        
         if (msglen > 0)
         {
             if ((messages[i].messageHandle = IoTHubMessage_CreateFromByteArray((const unsigned char*)msgText, msglen)) == NULL)
@@ -240,12 +250,12 @@
             {
                 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");
-                }
+                //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");
+                //}
 
                 if (IoTHubClient_SendEventAsync(iotHubClientHandle, messages[i].messageHandle, SendConfirmationCallback, &messages[i]) != IOTHUB_CLIENT_OK)
                 {
@@ -253,7 +263,10 @@
                 }
                 else
                 {
-                    (void)printf("IoTHubClient_SendEventAsync accepted transmission to IoT Hub. tracking id: %d,  bytes: %d\r\n", msgNumber, msglen);
+                    if (ismeasure)
+                        (void)printf("IoTHubClient_SendEventAsync sending data to IoT Hub. tracking id: %d,  bytes: %d\r\n", msgNumber, msglen);
+                    else
+                        (void)printf("IoTHubClient_SendEventAsync sending alert to IoT Hub. tracking id: %d,  bytes: %d\r\n", msgNumber, msglen);
                 }
                 msgNumber++;
             }
@@ -270,3 +283,4 @@
     } // while
 
 }
+

--- a/IothubRobotArm.h	Mon Dec 28 17:29:12 2015 +0000
+++ b/IothubRobotArm.h	Tue Dec 29 23:31:28 2015 +0000
@@ -26,7 +26,7 @@
 
 extern bool StartIothubThread();
 
-extern bool SendIothubMeasurements();
+extern bool SendIothubData();
 
 extern void EndIothubThread();
 

--- a/IothubSerial.cpp	Mon Dec 28 17:29:12 2015 +0000
+++ b/IothubSerial.cpp	Tue Dec 29 23:31:28 2015 +0000
@@ -90,16 +90,36 @@
     return startlen - bufsize;
 }
 
+int AddTime(time_t seconds, char* buf, int bufsize)
+{
+    if (bufsize > 32)
+    {
+        int slen = strftime(buf, 32, "\"time\": \"%FT%T\",", localtime(&seconds));
+        if (slen > 0)
+        {
+            return slen;
+        }
+        else
+            return -1;
+    }        
+    else
+        return -1;
+}
+
 // try to serialize one or more measurements into the buffer
 // return bytes used or -1 if buffer too small or other error. 0 if no data
 // current serialization is a json object with entry per measure
-// eg: { temp: [1, 2], volt: [12.1, 12.2] }
+// eg: { "temp": [1, 2], "volt": [12.1, 12.2] }
 int IothubSerial::MeasureBufToString(char* buf, int bufsize)
 {
     int slen;
     bool hasdata = false;
     bool copydata = false;
-    int startlen = bufsize;
+    char* startbuf = buf;
+    bool settime = false;
+    char* lastcomma = NULL;
+    
+    time_t secs = 0;
     
     if (bufsize > 1)
     {
@@ -118,11 +138,23 @@
     if (_hasPending)
     {
         hasdata = true;
+        secs = _pending.Created;
+        slen = AddTime(secs, buf, bufsize);
+        if (slen > 0)
+        {
+            bufsize -= slen;
+            buf += slen;
+            settime = true;
+        }
+        else
+            return -1;          // no room for pending record
+
         slen = MeasureGroupToString(_pending, buf, bufsize);
         if (slen > 0)
         {
             bufsize -= slen;
             buf += slen;
+            lastcomma = buf;
             _hasPending = false;
             copydata = true;
         }
@@ -138,11 +170,37 @@
         }
         hasdata = true;
         _hasPending = true;
+        
+        if (secs != _pending.Created)
+        {
+            if (settime && lastcomma != NULL)
+            {
+                *(lastcomma - 1) = '}';
+                if (bufsize > 2)
+                {
+                    strcpy(buf, ",{");
+                    buf += 2;
+                    bufsize -= 2;
+                }
+            }
+            secs = _pending.Created;
+            slen = AddTime(secs, buf, bufsize);
+            if (slen > 0)
+            {
+                bufsize -= slen;
+                buf += slen;
+                settime = true;
+            }
+            else
+                break;
+        }
+        
         slen = MeasureGroupToString(_pending, buf, bufsize);
         if (slen > 0)
         {
             bufsize -= slen;
             buf += slen;
+            lastcomma = buf;
             _hasPending = false;
             copydata = true;
         }
@@ -157,7 +215,41 @@
         return -1;              // have data but buffer too small
 
     // replace final ',' with '}'
-    *(buf - 1) = '}';
+    *(lastcomma - 1) = '}';
+
+    return lastcomma - startbuf;
+}
+
+
+// try to serialize one or more alerts into the buffer
+// return bytes used or -1 if buffer too small or other error. 0 if no data
+// current serialization is a json object
+// eg: { "alert": "msg", "sev": n }
+int IothubSerial::AlertBufToString(char* buf, int bufsize)
+{
+    int slen;
+    bool hasdata = false;
+    bool copydata = false;
+    int startlen = bufsize;
+ 
+    if (AlertBuf.pop(_pendAlert))
+    {
+        hasdata = true;
+        slen = sprintf_s(buf, bufsize, "{ \"alerttype\": \"%s\", \"alert\": \"%s\", \"sev\": %d }", _pendAlert.AlertType, _pendAlert.Msg, _pendAlert.Sev);
+        if (slen > 0)
+        {
+            bufsize -= slen;
+            buf += slen;
+            copydata = true;
+        }
+    }
+    
+    if (!hasdata)
+        return 0;               // no data
+        
+    if (!copydata)
+        return -1;              // have data but buffer too small
+
 
     return startlen - bufsize;
 }

--- a/IothubSerial.h	Mon Dec 28 17:29:12 2015 +0000
+++ b/IothubSerial.h	Tue Dec 29 23:31:28 2015 +0000
@@ -8,6 +8,7 @@
 #include "mbed.h"
 #include "RobotNode.h"
 #include "MeasureBuf.h"
+#include "Alert.h"
 
 class IothubSerial
 {
@@ -17,13 +18,18 @@
     int MeasureGroupToString(MeasureGroup& mg, char* buf, int bufsize);
     
     int MeasureBufToString(char* buf, int bufsize);
-    
+
+    int AlertBufToString(char* buf, int bufsize);
+
     bool HasMeasureGroup();
     
 private:
     MeasureGroup _pending;
     
     bool _hasPending;
+    
+    Alert _pendAlert;
+
 };
 
 #endif

--- a/MeasureBuf.cpp	Mon Dec 28 17:29:12 2015 +0000
+++ b/MeasureBuf.cpp	Tue Dec 29 23:31:28 2015 +0000
@@ -1,5 +1,4 @@
 #include "mbed.h"
-#include "rtos.h"
 
 #include "MeasureBuf.h"
 
@@ -7,9 +6,11 @@
 
 SafeCircBuf<MeasureGroup, MeasureBufSize, uint32_t> MeasureBuf;
 
-void MeasureGroup::SetMeasure(NodeMeasure mId, vector<float>& vals)
+void MeasureGroup::SetMeasure(NodeMeasure mId, time_t created, vector<float>& vals)
 {
     MeasId = mId;
+    Created = created;
+    
     NumVals = MAX_PARTS < vals.size() ? MAX_PARTS : vals.size();
     
     for (int ix = 0; ix < NumVals; ix++)

--- a/MeasureBuf.h	Mon Dec 28 17:29:12 2015 +0000
+++ b/MeasureBuf.h	Tue Dec 29 23:31:28 2015 +0000
@@ -10,7 +10,7 @@
 #include "RobotArm.h"
 
 
-#define MeasureBufSize  100
+#define MeasureBufSize  20
 
 
 class MeasureGroup
@@ -18,13 +18,15 @@
 public:
     MeasureGroup() {};
     
-    void SetMeasure(NodeMeasure mId, vector<float>& vals);
+    void SetMeasure(NodeMeasure mId, time_t created, vector<float>& vals);
     
     NodeMeasure MeasId;
     
     int NumVals;
     
     float MeasVals[MAX_PARTS];
+    
+    time_t Created;
 };
 
 extern SafeCircBuf<MeasureGroup, MeasureBufSize, uint32_t> MeasureBuf;

--- a/RobotArm.cpp	Mon Dec 28 17:29:12 2015 +0000
+++ b/RobotArm.cpp	Tue Dec 29 23:31:28 2015 +0000
@@ -10,41 +10,42 @@
 using namespace std;
 
 DynamixelBus dynbus( PTC17, PTC16, D7, D6, 1000000 );
-Terminal* RobotArmPc = NULL;
 
 
 // set the id for each part in the chain, in order
 int PartIds[] = { 2, 3, 4, 6, 1 };
 
+#define FailMsLimit  200
+
 RobotArm::RobotArm()
 {
     // build arm
     int i = 0;
-    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]); 
+    printf( "id[%d] = %d\r\n", i, PartIds[i]); 
     AX12* servo1 = new AX12( &dynbus, PartIds[i] );
     servo1->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo1);
     
     i++;
-    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
+    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
     AX12* servo2 = new AX12( &dynbus, PartIds[i] );
     servo2->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo2);
     
     i++;
-    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
+    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
     AX12* servo3 = new AX12( &dynbus, PartIds[i] );
     servo3->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo3);
     
     i++;
-    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
+    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
     AX12* servo4 = new AX12( &dynbus, PartIds[i] );
     servo4->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo4);
     
     i++;
-    RobotArmPc->printf( "id[%d] = %d\r\n", i, PartIds[i]);  
+    printf( "id[%d] = %d\r\n", i, PartIds[i]);  
     AX12* servo6 = new AX12( &dynbus, PartIds[i] );
     servo6->TorqueEnable(true);
     _armParts[i] = dynamic_cast<RobotNode*>(servo6);
@@ -54,65 +55,10 @@
 
     numsteps = 0;
     _stepms = 20; // move every 20 ms
+    allowance = 2.0f;       // allow 2 degree fudge factor
+    failms = 0;
 }
 
-// move all parts to specified postions in ms time
-bool RobotArm::MoveArmPositions(vector<float> positions, int ms, int steps)
-{
-    _lastErrorPart = 0;
-    
-    GetArmLastPositions(lastpos);
-
-    for( int ix = 0; ix < _numParts; ix++)
-    {
-        float difference = positions[ix]- lastpos[ix];
-        differentials.push_back( difference );
-    }
-
-    if (steps > 1000) steps = 1000;
-    if (ms <= 0) ms = 1;
-    
-    delayms = ms / steps;
-    expDelay = delayms;
-    
-    elapseTimer.start();
-    bool ok = true;
-    
-    // move arm to new position in steps
-    for( int step = 1; step <= steps; step++)
-    {
-        //pc.foreground(Red);
-        //pc.background(Black);
-        //pc.locate( 0 , 24 );
-        float incr = (float)step / (float)steps;
-        for( int ix = 0; ix < _numParts; ix++ )
-        {
-            //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal); 
-            if (_armParts[ix]->HasAction(NA_Rotate))
-            {
-                float goal = lastpos[ix] + (differentials[ix] * incr);
-                bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
-                if (!ok)
-                {
-                    _lastErrorPart = ix;
-                    break;
-                }
-            }
-        }
-        if (!ok)
-            break;
-        // adjust delay
-        int elapsed = (int)elapseTimer.read_ms();
-        expDelay += delayms;
-        if (elapsed > expDelay && elapsed - expDelay < delayms)
-            wait_ms(elapsed - expDelay);
-        else
-            wait_ms(delayms);
-    }
-    
-    elapseTimer.stop();
-    return ok;
-}
 
 // move all parts to specified postions in ms time
 bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms)
@@ -121,24 +67,33 @@
 
     MoveArmPositionsEnd();
     GetArmPositions(lastpos);
+    lastgoals.clear();
 
+    numsteps = totms / _stepms;
+    if (numsteps == 0) numsteps = 1;
+    
     differentials.clear();
     for( int ix = 0; ix < _numParts; ix++)
     {
-        float difference = positions[ix] - lastpos[ix];
-        differentials.push_back( difference );
-        RobotArmPc->printf( "diff %d = %f\r\n", ix, difference);
+        if (positions[ix] > 0.0f)
+        {
+            float difference = (positions[ix] - lastpos[ix]) / (float)numsteps;
+            differentials.push_back( difference );
+        }
+        else
+        {
+            // negative goal. Treat as don't move
+            differentials.push_back(0.0f);
+        }
     }
 
-    numsteps = totms / _stepms;
-    if (numsteps == 0) numsteps = 1;
-
     curstep = 1;
     
     delayms = _stepms;
     
     elapseTimer.start();
     expDelay = (int)elapseTimer.read_ms() + delayms;
+    failms = 0;
     
     return true;
 }
@@ -148,7 +103,7 @@
     return (curstep <= numsteps);
 }
 
-bool RobotArm::MoveArmPositionsNext(int& nextdelay)
+bool RobotArm::MoveArmPositionsNext()
 {
     _lastErrorPart = 0;
 
@@ -160,19 +115,24 @@
     }
     
     bool ok = true;
-    float incr = (float)curstep / (float)numsteps;
+
     lastgoals.clear();
     for( int ix = 0; ix < _numParts; ix++ )
     {
         if (_armParts[ix]->HasAction(NA_Rotate))
         {
-            float goal = lastpos[ix] + (differentials[ix] * incr);
+            float goal = lastpos[ix] + (differentials[ix] * (float)curstep);
             lastgoals.push_back(goal);
-            bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
-            if (!ok)
+            if (differentials[ix] != 0.0f)
             {
-                _lastErrorPart = ix;
-                break;
+                bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
+                if (!ok)
+                {
+                    _lastErrorPart = ix;
+                    _lastError = _armParts[_lastErrorPart]->GetLastError();
+                    _lastPosDiff = 0;
+                    break;
+                }
             }
         }
     }
@@ -187,43 +147,83 @@
     {
         MoveArmPositionsEnd();
     }
-    else
-    {    
-        // adjust delay
+    
+    return true;
+}
+
+// calculate actual delay until expDelay
+bool RobotArm::MoveArmPositionsDelay(int& nextdelay)
+{
+    if (curstep <= numsteps)
+    {
         int elapsed = (int)elapseTimer.read_ms();
     
-        if (elapsed < expDelay)
+        if (elapsed <= expDelay)
         {
             if (expDelay - elapsed < delayms)
                 nextdelay = expDelay - elapsed;
             else
                 nextdelay = delayms;
+            // set next expected time by adding step delay
+            expDelay += delayms;
         }
         else
         {
-            // no delay before next step
-            nextdelay = 0;
+            nextdelay = delayms;
+           // set next expected time to now plus step delay
+            expDelay = elapsed + delayms;
         }
-        expDelay += delayms;
+    }
+    else
+    {
+        nextdelay = 0;
     }
     
     return true;
 }
 
-bool RobotArm::TestArmPosition(int& partix, float& diffpos)
+bool RobotArm::MoveArmPositionTest()
 {
     vector<float> curpos;
     GetArmPositions(curpos);
     for( int ix = 0; ix < _numParts; ix++ )
     {
-        float diff = abs(curpos[ix]  - lastgoals[ix]);
-        if (diff > abs(differentials[ix]) + allowance)
+        if (curpos[ix] > 0 && lastgoals.size() > ix)
         {
-            diffpos = diff;
-            partix = ix;
-            return false;
+            float diff = fabs(curpos[ix]  - lastgoals[ix]);
+            if (diff > (fabs(differentials[ix] * 2.0f) + allowance))
+            {
+                printf("Bad position for %d. Expect %f - got %f\r\n", ix, lastgoals[ix], curpos[ix]);
+                _lastPosDiff = diff;
+                _lastErrorPart = ix;
+                _lastError = 0;
+
+                int elapsed = (int)elapseTimer.read_ms();
+                if (failms > 0)
+                {
+                    if (elapsed - failms > FailMsLimit)
+                    {
+                        // continuous failure for time period
+                        // return false
+                        lastgoals.clear();
+                        failms = 0;
+                        return false;
+                    }
+                    // within time period. Do not report failure
+                    return true;
+                }
+                else
+                {
+                    // first failure after success
+                    // remember time. Do not report failure
+                    failms = elapsed;
+                    return true;
+                }
+            }
         }
     }
+    // success
+    failms = 0;
     return true;
 }
     
@@ -237,45 +237,6 @@
     return true;
 }
 
-// move one part to specified postion in ms time
-bool RobotArm::MovePartPosition(int partIx, float position, int ms, int steps)
-{
-    _lastErrorPart = 0;
-    if (!_armParts[partIx]->HasAction(NA_Rotate))
-        return false;
-        
-    float lastpos = _armParts[partIx]->GetLastMeasure(NM_Degrees);
-
-    float difference = position- lastpos;
-
-    if (steps > 1000) steps = 1000;
-    if (ms <= 0) ms = 1;
-    
-    int delayms = ms / steps;
-    
-    bool ok = true;
-    
-    // move arm to new position in steps
-    for( int step = 1; step <= steps; step++)
-    {
-        //pc.foreground(Red);
-        //pc.background(Black);
-        //pc.locate( 0 , 24 );
-        float incr = (float)step / (float)steps;
-
-        //pc.printf( "goal[%d] = %f\r\n", servoIndex, goal); 
-        float goal = lastpos + (difference * incr);
-        bool ok = _armParts[partIx]->DoAction(NA_Rotate, goal);
-        if (!ok)
-        {
-            _lastErrorPart = partIx;
-            break;
-        }
-            
-        wait_ms(delayms);
-    }
-    return ok;
-}
 
 // get all parts positions
 bool RobotArm::GetArmPositions(vector<float>& outPos)
@@ -289,7 +250,7 @@
     return true;
 }
 
-// get all parts positions
+// get all parts last measured positions
 bool RobotArm::GetArmLastPositions(vector<float>& outPos)
 {
     outPos.clear();
@@ -301,14 +262,7 @@
     return true;
 }
 
-// get one part position
-float RobotArm::GetPartPosition(int partIx)
-{
-    return _armParts[partIx]->GetMeasure(NM_Degrees);
-}
-
-
-// get all parts positions
+// get all parts measurements
 bool RobotArm::GetArmMeasure(int measureId, vector<float>& outPos)
 {
     outPos.clear();
@@ -320,7 +274,7 @@
     return true;
 }
 
-// get all parts positions
+// get all parts last measurements
 bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outPos)
 {
     outPos.clear();
@@ -332,12 +286,6 @@
     return true;
 }
 
-// get one part position
-float RobotArm::GetPartMeasure(int measureId, int partIx)
-{
-    return _armParts[partIx]->GetMeasure(measureId);
-}
-
 int RobotArm::GetNumParts()
 {
     return _numParts;
@@ -360,12 +308,22 @@
     return _armParts[partIx];
 }
 
-unsigned char RobotArm::GetLastError()
+int RobotArm::GetLastError()
 {
-    return _armParts[_lastErrorPart]->GetLastError();
+    return _lastError;
+}
+
+float RobotArm::GetLastPosDiff()
+{
+    return _lastPosDiff;
 }
 
 int RobotArm::GetLastErrorPart()
 {
     return _lastErrorPart;
 }
+
+void RobotArm::SetAllowance(float allow)
+{
+    allowance = allow;
+}

--- a/RobotArm.h	Mon Dec 28 17:29:12 2015 +0000
+++ b/RobotArm.h	Tue Dec 29 23:31:28 2015 +0000
@@ -36,15 +36,13 @@
     IS_SendStatus       = 0x2
 };
 
+
 class RobotArm
 {
 public:
     
     RobotArm();
     
-    // move all parts to specified postions in ms time
-    bool MoveArmPositions(vector<float> positions, int ms, int steps);
-
     // start - move all parts to specified postions in ms time
     bool MoveArmPositionsStart(vector<float> positions, int ms);
 
@@ -52,16 +50,16 @@
     bool MoveArmPositionsHasNext();
     
     // start - move all parts to specified postions - next step
-    bool MoveArmPositionsNext(int& nextdelay);
+    bool MoveArmPositionsNext();
+
+    // start - move all parts to specified postions - next delay
+    bool MoveArmPositionsDelay(int& nextdelay);
 
     // start - move all parts to specified postions - finish
     bool MoveArmPositionsEnd();
     
     // start - test if positions are close to expected
-    bool TestArmPosition(int& partix, float& diffpos);
-    
-    // move one part to specified postion in ms time
-    bool MovePartPosition(int partIx, float position, int ms, int steps);
+    bool MoveArmPositionTest();
     
     // get all parts positions
     bool GetArmPositions(vector<float>& outPos);
@@ -69,17 +67,11 @@
     // get all parts last positions
     bool GetArmLastPositions(vector<float>& outPos);
     
-    // get one part position
-    float GetPartPosition(int partIx);
-    
     // get all parts for a measurement
     bool GetArmMeasure(int measureId, vector<float>& outPos);
     
     // get all parts last measurement
     bool GetArmLastMeasure(int measureId, vector<float>& outPos);
-    
-    // get one part measurement
-    float GetPartMeasure(int measureId, int partIx);
    
     int GetNumParts();
     
@@ -93,11 +85,17 @@
     RobotNode* GetArmPart(int partIx);
     
     // get last error code from action
-    unsigned char GetLastError();
+    int GetLastError();
     
     // get index of part with error
     int GetLastErrorPart();
     
+    // get size of position diff (valid if error)
+    float GetLastPosDiff();
+    
+    // set allwable position diff
+    void SetAllowance(float allow);
+    
 private:
     // sensors and actuators
     RobotNode* _armParts[MAX_PARTS];
@@ -112,6 +110,10 @@
     
     // part ix for last error
     int _lastErrorPart;
+    // last HW error
+    int _lastError;
+    // last position error
+    float _lastPosDiff;
     
     // for thread friendly moves
     vector<float> lastpos;
@@ -119,7 +121,9 @@
     vector<float> lastgoals;
     
     // allowance for difference between expected pos and actual pos
-    static const float allowance = 1.0f;
+    float allowance;
+    // keep track of time period when position is off
+    int failms;
     
     int numsteps;
     int curstep;

--- a/RobotNode/RobotNode.h	Mon Dec 28 17:29:12 2015 +0000
+++ b/RobotNode/RobotNode.h	Tue Dec 29 23:31:28 2015 +0000
@@ -14,12 +14,12 @@
 
 enum NodeAction
 {
-    NA_Rotate            = 0x1
+    NA_Rotate           = 0x1
 };
 
 enum NodePartType
 {
-    NT_AX12              = 0x1
+    NT_AX12             = 0x1
 };
 
 
@@ -36,7 +36,7 @@
     
     virtual bool DoAction(int actionId, float actionValue) = 0;
     
-    virtual unsigned char GetLastError() = 0;
+    virtual int GetLastError() = 0;
     
     virtual NodePartType GetNodeType() = 0;
 };

--- a/Sequences.cpp	Mon Dec 28 17:29:12 2015 +0000
+++ b/Sequences.cpp	Tue Dec 29 23:31:28 2015 +0000
@@ -3,30 +3,95 @@
 #include <ActionBuf.h>
 
 const float UpPos = 180.0f;
-const float rightPos = 90.0f;
+const float RightPos = 90.0f;
+const float NoMove = -1.0f;
 
 
 vector<ActionSequence> UpDownSeq;
 vector<ActionSequence> UpTwistSeq;
+vector<ActionSequence> StartSeq;
+vector<ActionSequence> WaveSeq;
 
 
-void MakeSequences(int partSize, vector<float>& homePositions)
+void MakeSequences(int partSize, vector<float>& startPositions)
 {
     vector<float> upPositions;
-
-    for( int partIx = 0; partIx < partSize; partIx++)
+    vector<float> downPositions;
+    vector<float> homePositions;
+    vector<float> waveUpPositions;
+    vector<float> waveDownPositions;
+    vector<float> rightPositions;
+    
+    for (int partIx = 0; partIx < partSize; partIx++)
     {
         upPositions.push_back(UpPos);
     }
+    
+    for (int partIx = 0; partIx < partSize; partIx++)
+    {
+        rightPositions.push_back(UpPos);
+    }
+    
+    // define normal start position
+    homePositions.push_back(RightPos);
+    if (partSize > 3)
+    {
+        homePositions.push_back(225.0f);
+        homePositions.push_back(270.0f);
+        homePositions.push_back(135.0f);
+        for (int partIx = 4; partIx < partSize; partIx++)
+        {
+            homePositions.push_back(180.0f);
+        }
+    }
+
+    // define down position
+    downPositions.push_back(RightPos);
+    if (partSize > 3)
+    {
+        downPositions.push_back(120.0f);
+        downPositions.push_back(240.0f);
+        downPositions.push_back(150.0f);
+        for (int partIx = 4; partIx < partSize; partIx++)
+        {
+            downPositions.push_back(240.0f);
+        }
+    }
+    
+    // define wave up and wave down to only move last part
+    for (int partIx = 0; partIx < partSize - 1; partIx++)
+    {
+        waveUpPositions.push_back(NoMove);
+        waveDownPositions.push_back(NoMove);
+    }
+    waveUpPositions.push_back(120.0f);
+    waveDownPositions.push_back(210.0f);
 
     // define actions
+    ActionSequence moveStart(SA_SetGoal, homePositions, 1500);
     ActionSequence moveUp(SA_SetGoal, upPositions, 2500);
+    ActionSequence moveDown(SA_SetGoal, downPositions, 2500);
+    ActionSequence waveUp(SA_SetGoal, waveUpPositions, 1500);
+    ActionSequence waveDown(SA_SetGoal, waveDownPositions, 1500);
+    
     ActionSequence report(SA_Status);
     ActionSequence pause2(SA_Delay);
     pause2.SetDelay(2000);
-    ActionSequence moveDown(SA_SetGoal, homePositions, 2500);
     ActionSequence rep(SA_Repeat);  
 
+    // add actions into StartSeq
+    StartSeq.clear();
+    StartSeq.push_back(moveStart);
+    StartSeq.push_back(report);
+    
+    // add actions into WaveSeq
+    WaveSeq.clear();
+    WaveSeq.push_back(waveUp);
+    WaveSeq.push_back(report);
+    WaveSeq.push_back(waveDown);
+    WaveSeq.push_back(report);
+    WaveSeq.push_back(rep);
+    
     // add actions into UpDownSeq
     UpDownSeq.clear();
     UpDownSeq.push_back(moveUp);
@@ -37,25 +102,19 @@
     UpDownSeq.push_back(pause2);
     UpDownSeq.push_back(rep);
     
-    vector<float> rightPositions;
-    for( int partIx = 0; partIx < partSize; partIx++)
-    {
-        rightPositions.push_back(UpPos);
-    }
-    
     // add actions into RightStepsSeq
     // move to vertical, then move each part one at a time, then return to home
     UpTwistSeq.clear();
     UpTwistSeq.push_back(moveUp);
     
-    for( int partIx = 0; partIx < partSize; partIx++)
+    for (int partIx = 0; partIx < partSize; partIx++)
     {
         vector<float>* rightPositions = new vector<float>();
-        for( int partIy = 0; partIy < partSize; partIy++)
+        for (int partIy = 0; partIy < partSize; partIy++)
         {
             float val = UpPos;
             if (partIx == partIy)
-                val = rightPos;
+                val = RightPos;
             rightPositions->push_back(val);
         }
 

--- a/Sequences.h	Mon Dec 28 17:29:12 2015 +0000
+++ b/Sequences.h	Tue Dec 29 23:31:28 2015 +0000
@@ -7,6 +7,9 @@
 
 extern vector<ActionSequence> UpDownSeq;
 extern vector<ActionSequence> UpTwistSeq;
+extern vector<ActionSequence> StartSeq;
+extern vector<ActionSequence> WaveSeq;
+
 
 void MakeSequences(int partSize, vector<float>& homePositions);
 

--- a/main.cpp	Mon Dec 28 17:29:12 2015 +0000
+++ b/main.cpp	Tue Dec 29 23:31:28 2015 +0000
@@ -5,62 +5,17 @@
 #include "mbed/logging.h"
 #include "mbed/mbedtime.h"
 #include <NTPClient.h>
-#include <DynamixelBus.h>
-#include <AX12.h>
 #include <Terminal.h>
-#include <vector>
-#include <RobotArm.h>
-#include <MeasureBuf.h>
-#include <IothubRobotArm.h>
-#include <ActionBuf.h>
-#include <Sequences.h>
+
 
 using namespace std;
 
 DigitalOut myled(LED_GREEN);
 Terminal pc(USBTX, USBRX);
 
-extern Terminal* RobotArmPc;
-extern Terminal* AX12Pc;
 
-const float UpPos = 180.0f;
-
-
-void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
-{
-    vector<float> lastVals;
-    
-    robotArm.GetArmMeasure(NM_Temperature, lastVals);
-    pc.printf( "Temperatures: ");
-    for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
-    {
-        pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]); 
-    }
-    pc.printf( "\r\n");
-    measureGroup.SetMeasure(NM_Temperature, lastVals);
-    MeasureBuf.push(measureGroup);
-    
-    robotArm.GetArmMeasure(NM_Voltage, lastVals);
-    pc.printf( "Voltage: ");
-    for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
-    {
-        pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]); 
-    }
-    pc.printf( "\r\n");
-    measureGroup.SetMeasure(NM_Voltage, lastVals);
-    MeasureBuf.push(measureGroup);
-    
-    robotArm.GetArmMeasure(NM_Degrees, lastVals);
-    pc.printf( "Rotation: ");
-    for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
-    {
-        pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]); 
-    }
-    pc.printf( "\r\n");
-    measureGroup.SetMeasure(NM_Degrees, lastVals);
-    MeasureBuf.push(measureGroup);
-}
-
+extern void RunController();
+extern bool StartIothubThread();
 
 int setupRealTime(void)
 {
@@ -124,326 +79,6 @@
 }
 
 
-vector<float> upPositions;
-vector<float> homePositions;
-
-void MakeDemoSeq(vector<ActionSequence>& actions)
-{
-    // define actions
-    ActionSequence moveUp(SA_SetGoal, upPositions, 2500);
-    ActionSequence report(SA_Status);
-    ActionSequence pause(SA_Delay);
-    pause.SetDelay(2000);
-    ActionSequence moveDown(SA_SetGoal, homePositions, 2500);
-    ActionSequence rep(SA_Repeat);  
-
-    // add actions into a sequence
-    actions.push_back(moveUp);
-    actions.push_back(report);
-    actions.push_back(pause);
-    actions.push_back(moveDown);
-    actions.push_back(report);
-    actions.push_back(pause);
-    actions.push_back(rep);
-}
-
-
-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
-};
-
-Queue<vector<ActionSequence>, 16> SequenceQ;
-volatile MainStates MainState;
-bool RunInSequence;
-bool RunInMoveStep;
-bool RunInDelay;
-void* CancelToMatch;
-
-osThreadId mainTid;
-
-void ControlArm(const char* cmd)
-{
-    if (strncmp(cmd, "pause", 5) == 0)
-    {
-        pc.printf( "Pause cmd\r\n"); 
-        MainState = MS_Paused;
-        osSignalSet(mainTid, AS_Action);
-    }
-    else if (strncmp(cmd, "resume", 6) == 0)
-    {
-        pc.printf( "Resume cmd\r\n"); 
-        MainState = MS_Running;
-        osSignalSet(mainTid, AS_Action);
-    }
-    else if (strncmp(cmd, "runupdown", 9) == 0)
-    {
-        pc.printf( "runupdown cmd\r\n"); 
-        MainState = MS_CancelToM;
-        CancelToMatch = &UpDownSeq;
-        SequenceQ.put(&UpDownSeq);
-        osSignalSet(mainTid, AS_Action);
-    }
-    else if (strncmp(cmd, "runuptwist", 10) == 0)
-    {
-        pc.printf( "runuptwist cmd\r\n"); 
-        MainState = MS_CancelOne;
-        CancelToMatch = &UpTwistSeq;
-        SequenceQ.put(&UpTwistSeq);
-        osSignalSet(mainTid, AS_Action);
-    }
-    else if (strncmp(cmd, "cancelone", 9) == 0)
-    {
-        pc.printf( "CancelOne cmd\r\n"); 
-        MainState = MS_CancelOne;
-        osSignalSet(mainTid, AS_Action);
-    }
-}
-
-// run sequence thread timer routine
-void NextSeq(void const * tid)
-{
-    RunInDelay = false;
-    osSignalSet((osThreadId)tid, AS_Action);
-}
-
-void run()
-{
-    // init robot arm
-    RobotArm robotArm;
-    
-    int partSize = robotArm.GetNumParts();
-
-    for( int servoIndex = 0; servoIndex < partSize; servoIndex++)
-    {
-        upPositions.push_back(UpPos);
-    }
-    
-    // set running state
-    MainState = MS_Idle;
-    RunInSequence = false;
-    RunInMoveStep = false;
-    RunInDelay = false;
-     
-    // get initial positions
-    MeasureGroup measureGroup;
-    robotArm.GetArmPositions(homePositions);
-    
-    vector<float> lastVals;
-    
-    // do inital status report
-    PushMeasurements(measureGroup, partSize, robotArm);
-    SendIothubMeasurements();
- 
-    // Prepare main thread
-    mainTid = osThreadGetId();
-    int32_t signals = AS_Action;
-    
-    // create sequences
-    MakeSequences(partSize, homePositions);
-  
-    // add to queue
-    SequenceQ.put(&UpDownSeq);
-    
-    // state for sequence
-    vector<ActionSequence> *curseq = NULL;
-    int curseqIx = 0;
-
-    // signal to run the default action for demo
-    osSignalSet(mainTid, AS_Action);
-    
-    RtosTimer delayTimer(NextSeq, osTimerOnce, (void *)osThreadGetId());    
-    
-    MainState = MS_Running;
-    
-    while( true )
-    {
-        osEvent mainEvent = osSignalWait(signals, osWaitForever);
-        
-        switch (MainState)
-        {
-            case MS_Idle:
-                break;
-                
-            case MS_Paused:
-                pc.printf( "Paused\r\n"); 
-                break;
-                
-            case MS_Stopping:
-                pc.printf( "Stopping\r\n"); 
-                break;
-                
-            case MS_Error:
-                pc.printf( "Error\r\n"); 
-                break;
-                
-            case MS_CancelOne:
-                pc.printf( "Cancel sequence\r\n"); 
-                RunInSequence = false;
-                RunInMoveStep = false;
-                if (RunInDelay)
-                {
-                    RunInDelay = false;
-                    delayTimer.stop();
-                }
-                MainState = MS_Running;
-                osSignalSet(mainTid, AS_Action);
-                break;
-                
-            case MS_CancelAll:
-                RunInSequence = false;
-                RunInMoveStep = false;
-                if (RunInDelay)
-                {
-                    RunInDelay = false;
-                    delayTimer.stop();
-                }
-                while (1) {
-                    osEvent evt = SequenceQ.get(0);
-                    if (evt.status != osEventMessage)
-                        break;
-                }
-                MainState = MS_Running;
-                break;
-                
-            case MS_CancelToM:
-                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
-                    break;
-                }
-                if (!RunInSequence)
-                {
-                    osEvent evt = SequenceQ.get(0);
-                    if (evt.status == osEventMessage)
-                    {
-                        pc.printf( "New Seq \r\n"); 
-                        curseq = (vector<ActionSequence> *)evt.value.p;
-                        curseqIx = 0;
-                        RunInSequence = true;
-                        RunInMoveStep = false;
-                    }
-                }
-                
-                if (RunInSequence)
-                {
-                    if (RunInMoveStep)
-                    {
-                        if (robotArm.MoveArmPositionsHasNext())
-                        {
-                            //pc.printf( "Next Step\r\n");
-                            int delaystep = 0;
-                            bool ok = robotArm.MoveArmPositionsNext(delaystep);
-                            if (ok)
-                            {
-                                if (delaystep > 0)
-                                {
-                                    RunInDelay = true;
-                                    delayTimer.start(delaystep);
-                                }
-                                else
-                                    osSignalSet(mainTid, AS_Action);
-                            }
-                        }
-                        else
-                        {
-                            pc.printf( "No more Step\r\n");
-                            RunInMoveStep = false;
-                        }
-                    }
-                    if (!RunInMoveStep)
-                    {
-                        pc.printf( "Next Seq %d\r\n", curseqIx); 
-                        
-                        if (curseq != NULL)
-                        {
-                            if (curseqIx >= curseq->size())
-                            {
-                                RunInSequence = false;
-                                break;
-                            }
-
-                            ActionSequence aseq = (*curseq)[curseqIx];
-                            curseqIx++;
-                            
-                            bool ok;
-                            switch (aseq.ActionType)
-                            {
-                                case SA_SetGoal:
-                                    pc.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
-                                    }
-                                    break;
-                                case SA_Delay:
-                                    pc.printf( " - Delay\r\n");
-                                    RunInDelay = true;
-                                    delayTimer.start(aseq.Ms);
-                                    break;
-
-                                case SA_Status:
-                                    pc.printf( " - Status\r\n"); 
-                                    robotArm.GetArmPositions(lastVals);
-                                    
-                                    PushMeasurements(measureGroup, partSize, robotArm);
-                             
-                                    SendIothubMeasurements();
-            
-                                    osSignalSet(mainTid, AS_Action);
-                                    break;
-                                case SA_Repeat:
-                                    pc.printf( " - Repeat\r\n"); 
-                                    curseqIx = 0;
-                                    osSignalSet(mainTid, AS_Action);
-                                    break;
-                                    
-                            }
-                        }
-                    }
- 
-                }
-                break;
-        }
-
-    }
-}
 
 int main()
 {
@@ -473,9 +108,7 @@
     
     pc.printf( "Initialization done. Ready to run. \r\n");
 
-    RobotArmPc = &pc;
-    AX12Pc = &pc;
-    
-    run(); 
+
+    RunController(); 
 
 }