File content as of revision 11:3a2e6eb9fbb8:

#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>
#include <ControllerIo.h>

using namespace std;


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

// try to send some status every minute
#define IDLESTATUSTO        60000
Timer IdleTimer;
int LastSendMs = 0;

Queue<vector<ActionSequence>, 16> SequenceQ;
MainStates MainState;
bool ErrorReset;
bool RunInSequence;
bool RunInMoveStep;
bool RunInDelay;
void* CancelToMatch;

osThreadId mainTid;


void DispMeasure(char* label, int partSize, vector<float>& vals)
{
    printf("%s: ", label);
    for (int ix = 0; ix < partSize; ix++)
    {
        printf("%d:%f ", ix, vals[ix]); 
    }
    printf("\r\n");

}


void PushAlert(int severity, char* msg, char* atype)
{
    Alert alert;
    time_t seconds = time(NULL);
    
    alert.SetAlert(severity, seconds, msg, atype);
    AlertBuf.push(alert);
    SendIothubData();
}

void PushPositionAlert(int severity, int partIx, float pos)
{
    Alert alert;
    time_t seconds = time(NULL);
    
    ShowLedRed();
    alert.SetPositionAlert(severity, seconds, partIx, pos);
    AlertBuf.push(alert);
    SendIothubData();
}

void PushHardwareAlert(int severity, int partIx, int code)
{
    Alert alert;
    time_t seconds = time(NULL);
    
    ShowLedRed();
    alert.SetHardwareAlert(severity, seconds, partIx, code);
    AlertBuf.push(alert);
    SendIothubData();
}

void PushTemperatureAlert(int severity, int partIx, float temp)
{
    Alert alert;
    time_t seconds = time(NULL);
    
    ShowLedRed();
    alert.SetTemperatureAlert(severity, seconds, partIx, temp);
    AlertBuf.push(alert);
    SendIothubData();
}

// send alert if temperature is above 69C and return true
#define MaxTemp     69
bool TestTemperature(vector<float>& vals)
{
    bool err = false;

    for (int ix = 0; ix < vals.size(); ix++)
    {
        if (vals[ix] > MaxTemp)
        {
            printf("Temperature threshold exceeded for part %d, temp %f \r\n", ix, vals[ix]); 
            PushTemperatureAlert(AS_Error, ix, vals[ix]);
            MainState = MS_Error;
            err = true;
        }
    }
    return err;
}


void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
{
    vector<float> lastVals;
    time_t seconds = time(NULL);
    LastSendMs = (int)IdleTimer.read_ms();
    
    bool ok = true;
    
    ok = robotArm.GetArmMeasure(NM_Temperature, lastVals);
    DispMeasure("Temperatures", partSize, lastVals);
    measureGroup.SetMeasure(NM_Temperature, seconds, lastVals);
    MeasureBuf.push(measureGroup);
    
    if (ok)
    {
        ok = robotArm.GetArmMeasure(NM_Voltage, lastVals);
        DispMeasure("Voltage", partSize, lastVals);
        measureGroup.SetMeasure(NM_Voltage, seconds, lastVals);
        MeasureBuf.push(measureGroup);
    }
    
    if (ok)
    {
        ok = robotArm.GetArmMeasure(NM_Degrees, lastVals);
        DispMeasure("Rotation", partSize, lastVals);
        measureGroup.SetMeasure(NM_Degrees, seconds, lastVals);
        MeasureBuf.push(measureGroup);
    }
    
//    robotArm.GetArmMeasure(NM_Load, lastVals);
//    DispMeasure("Load", partSize, lastVals);
//    measureGroup.SetMeasure(NM_Load, seconds, lastVals);
//    MeasureBuf.push(measureGroup);

    SendIothubData();
    
    if (!ok)
    {
        // report HW error
        int partix = robotArm.GetLastErrorPart();
        int errCode = robotArm.GetLastError();
        printf("Hardware error detected part %d, code %d \r\n", partix, errCode); 
        PushHardwareAlert(AS_Error, partix, errCode);
        MainState = MS_Error;
    }
}



void ControlArm(const char* cmd)
{
    if (MainState == MS_Error)
    {
        ErrorReset = true;
    }
    
    if (strncmp(cmd, "pause", 5) == 0)
    {
        ShowLedBlue();
        MainState = MS_Paused;
        osSignalSet(mainTid, AS_Action);
    }
    else if (strncmp(cmd, "resume", 6) == 0)
    {
        ShowLedGreen();
        MainState = MS_Running;
        osSignalSet(mainTid, AS_Action);
    }
    else if (strncmp(cmd, "cancel", 6) == 0)
    {
        ShowLedGreen();
        MainState = MS_CancelOne;
        osSignalSet(mainTid, AS_Action);
    }
    else if (strncmp(cmd, "runupdown", 9) == 0)
    {
        ShowLedGreen();
        MainState = MS_CancelOne;
        SequenceQ.put(&UpDownSeq);
        osSignalSet(mainTid, AS_Action);
    }
    else if (strncmp(cmd, "runuptwist", 10) == 0)
    {
        ShowLedGreen();
        MainState = MS_CancelOne;
        SequenceQ.put(&UpTwistSeq);
        osSignalSet(mainTid, AS_Action);
    }
    else if (strncmp(cmd, "runhome", 10) == 0)
    {
        ShowLedGreen();
        MainState = MS_CancelOne;
        SequenceQ.put(&StartSeq);
        osSignalSet(mainTid, AS_Action);
    }
    else if (strncmp(cmd, "runwave", 10) == 0)
    {
        ShowLedGreen();
        MainState = MS_CancelOne;
        SequenceQ.put(&WaveSeq);
        osSignalSet(mainTid, AS_Action);
    }
    else if (strncmp(cmd, "runtaps1", 10) == 0)
    {
        ShowLedGreen();
        MainState = MS_CancelOne;
        SequenceQ.put(&TapsSeq);
        osSignalSet(mainTid, AS_Action);
    }
    else if (strncmp(cmd, "fastwave", 8) == 0)
    {
        ShowLedGreen();
        MainState = MS_CancelOne;
        SequenceQ.put(&FastWaveSeq);
        osSignalSet(mainTid, AS_Action);
    }
}

// run sequence thread timer routine
void NextSeq(void const * tid)
{
    RunInDelay = false;
    osSignalSet((osThreadId)tid, AS_Action);
}

// periodic timer routine
void PeriodicStatus(void const * tid)
{
    // make sure we run controller even if idle
    osSignalSet((osThreadId)tid, AS_Action);
}

void RunController()
{
    // init robot arm
    RobotArm robotArm;
    
    int partSize = robotArm.GetNumParts();

    IdleTimer.start();
    LastSendMs = (int)IdleTimer.read_ms();
    
    // set running state
    MainState = MS_Idle;
    RunInSequence = false;
    RunInMoveStep = false;
    RunInDelay = false;
    ErrorReset = false;
    
    // get initial positions
    MeasureGroup measureGroup;
    robotArm.GetArmPositions(initPositions);
    
    vector<float> lastVals;
    
    // do inital status report
    PushMeasurements(measureGroup, partSize, robotArm);
 
    // 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;
    int loopSeqIx = 0;
    int loopCounter = 0;
    
    // signal to run the default action for demo
    osSignalSet(mainTid, AS_Action);
    
    RtosTimer delayTimer(NextSeq, osTimerOnce, (void *)osThreadGetId());    
    RtosTimer statusTimer(PeriodicStatus, osTimerPeriodic, (void *)osThreadGetId());    
    statusTimer.start(IDLESTATUSTO);
    
    MainState = MS_Running;
    
    while( true )
    {
        osEvent mainEvent = osSignalWait(signals, osWaitForever);
        
        if (ErrorReset)
        {
            ErrorReset = false;
            robotArm.ClearErrorState();
        }
        
        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:
                printf("Cancel all\r\n"); 
                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:
                printf("Cancel until latest\r\n"); 
                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
                    printf("Should be delaying. Skip action\r\n");
                    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;
                            break;
                        }
                        if (robotArm.MoveArmPositionsHasNext())
                        {
                            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();
                                printf("Hardware error detected part %d, code %d \r\n", partix, errCode); 
                                PushHardwareAlert(AS_Error, partix, errCode);
                                MainState = MS_Error;
                                break;
                            }
                        }
                        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())
                            {
                                printf("sequence completed. Stopping\r\n");
                                RunInSequence = false;
                                break;
                            }

                            ActionSequence aseq = (*curseq)[curseqIx];
                            curseqIx++;
                            
                            switch (aseq.ActionType)
                            {
                                case SA_SetGoal:
                                    printf(" - Move arm start\r\n"); 
                                    robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Param);
                                    RunInMoveStep = true;
                                    osSignalSet(mainTid, AS_Action);
                                    break;
                                case SA_Delay:
                                    printf(" - Delay\r\n");
                                    RunInDelay = true;
                                    delayTimer.start(aseq.Param);
                                    break;
                                case SA_Status:
                                    printf(" - Status\r\n"); 

                                    PushMeasurements(measureGroup, partSize, robotArm);

                                    robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
                                    TestTemperature(lastVals);
                                    
                                    osSignalSet(mainTid, AS_Action);
                                    break;
                                case SA_LoopBegin:
                                    printf(" - LoopBegin\r\n");
                                    loopSeqIx = curseqIx;
                                    loopCounter = aseq.Param;
                                    osSignalSet(mainTid, AS_Action);
                                    break;
                                case SA_LoopEnd:
                                    printf(" - LoopEnd\r\n");
                                    loopCounter--;
                                    if (loopCounter > 0 && loopSeqIx > 0)
                                        curseqIx = loopSeqIx;
                                    osSignalSet(mainTid, AS_Action);
                                    break;
                            }
                        }
                    }
 
                }
                break;
        }

        int now = (int)IdleTimer.read_ms();
        if (now - LastSendMs > (IDLESTATUSTO - 1000))
        {
            PushMeasurements(measureGroup, partSize, robotArm);
        }
    }
}