File content as of revision 4:36a4eceb1b7f:

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

using namespace std;

DigitalOut myled(LED_GREEN);
Terminal pc(USBTX, USBRX);

extern Terminal* RobotArmPc;
extern Terminal* AX12Pc;

const float UpPos = 150.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);
}


int setupRealTime(void)
{
    int result;

    (void)printf("setupRealTime begin\r\n");
    if (EthernetInterface::connect())
    {
        (void)printf("Error initializing EthernetInterface.\r\n");
        result = __LINE__;
    }
    else
    {
        (void)printf("setupRealTime NTP begin\r\n");
        NTPClient ntp;
        if (ntp.setTime("0.pool.ntp.org") != 0)
        {
            (void)printf("Failed setting time.\r\n");
            result = __LINE__;
        }
        else
        {
            (void)printf("set time correctly!\r\n");
            result = 0;
        }
        (void)printf("setupRealTime NTP end\r\n");
        EthernetInterface::disconnect();
    }
    (void)printf("setupRealTime end\r\n");

    return result;
}

int InitEthernet()
{
    (void)printf("Initializing mbed specific things...\r\n");

        /* These are needed in order to initialize the time provider for Proton-C */
    mbed_log_init();
    mbedtime_init();

    if (EthernetInterface::init())
    {
        (void)printf("Error initializing EthernetInterface.\r\n");
        return -1;
    }

    if (setupRealTime() != 0)
    {
        (void)printf("Failed setting up real time clock\r\n");
        return -1;
    }

    if (EthernetInterface::connect())
    {
        (void)printf("Error connecting EthernetInterface.\r\n");
        return -1;
    }
    
    return 0;
}


vector<float> upPositions;
vector<float> homePositions;

void MakeDemoSeq(vector<ActionSequence>& actions)
{
    // define actions
    ActionSequence moveUp(SA_SetGoal, upPositions, 3000);
    ActionSequence report(SA_Status);
    ActionSequence pause(SA_Delay);
    pause.SetDelay(5000);
    ActionSequence moveDown(SA_SetGoal, homePositions, 3000);
    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;
MainStates MainState;
bool RunInSequence;
bool RunInMoveStep;
bool RunInDelay;
void* CancelToMatch;


// 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
    osThreadId mainTid = osThreadGetId();
    int32_t signals = AS_Action;
    
    // create a sequence for demo
    vector<ActionSequence> actions;
    MakeDemoSeq(actions);
    
    // add to queue
    SequenceQ.put(&actions);
    
    // 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)
                        {
                            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;
                                    
                            }
                        }
                        
                        if (curseqIx >= curseq->size())
                        {
                            RunInSequence = false;
                        }
                    }
 
                }
                break;
        }

    }
}

int main()
{
    pc.baud(115200); 

    pc.cls();
    pc.foreground(Yellow);
    pc.background(Black);

    pc.locate( 0, 0 );
    pc.printf("**********************\r\n");
    pc.printf("RobotArmDemo start\r\n");
    pc.printf("**********************\r\n");

    pc.foreground(Teal);
    pc.background(Black);

    InitEthernet();
    
    // start IotHub connection
    StartIothubThread();
    
    // time delay is to allow the position encoders to come online after initial power supply event ~ 5 secs
    // and to allow IoTHub SSL connection established
    // TODO: test for connection established
    Thread::wait(15000);
    
    pc.printf( "Initialization done. Ready to run. \r\n");

    RobotArmPc = &pc;
    AX12Pc = &pc;
    
    run(); 

}