File content as of revision 7:6723f6887d00:

#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;
        }

    }
}