Diff: main.cpp

Revision:

7:6723f6887d00

Parent:

5:36916b1c5a06

Child:

8:d98e2dec0f40

--- 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(); 
 
 }