Diff: ArmController.cpp

Revision:

7:6723f6887d00

Child:

8:d98e2dec0f40

--- /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;
+        }
+
+    }
+}
+