File content as of revision 13:ffeff9b5e513:

#include "mbed.h"
#include "rtos.h"

#include "threadapi.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;


enum MainStates
{
    MS_Idle           = 0x0,
    MS_Running        = 0x1,
    MS_Paused         = 0x2,
    MS_Error          = 0x3,
    MS_CancelOne      = 0x4,
    MS_CancelAll      = 0x5,
    MS_Resuming       = 0x6
};

// controller thread runs every 25ms
#define CONTROLLER_POLL         25

// try to send some status every few seconds
#define SEND_STATUS_TO          500
// use slower send rate if paused
#define SEND_STATUS_PAUSED_TO   20000

// controller polling timer
Timer IdleTimer;
int NextSendMs = 0;
int NextStepMs = 0;
int NextPollMs = 0;

// action sequences
Queue<vector<ActionSequence*>, 8> SequenceQ;

// controller state
MainStates MainState;
bool RunInSequence;
bool RunInMoveStep;

bool NeedHwReset;

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

// send alert message
void PushAlert(char* msg, char* atype)
{
    Alert alert;
    time_t seconds = time(NULL);
    
    alert.SetAlert(seconds, msg, atype);
    AlertBuf.push(alert);
}

// send position alert
void PushPositionAlert(RobotArm& robotArm)
{
    // stop trying to move
    float diff = robotArm.GetLastPosDiff();
    int ix = robotArm.GetLastErrorPart();
    robotArm.MoveArmPositionsStop();
    
    // report load error
    printf("Position error detected joint %d, value diff %f\r\n", ix, diff);

    Alert alert;
    time_t seconds = time(NULL);
    
    ShowLedRed();
    alert.SetPositionAlert(seconds, ix, diff);
    AlertBuf.push(alert);
    
    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
}

// send load alert
void PushLoadAlert(RobotArm& robotArm)
{
    float lastVals[NUMJOINTS];
    
    // stop trying to move
    robotArm.GetArmLastMeasure(NM_Load, lastVals);
    int ix = robotArm.GetLastErrorPart();
    robotArm.MoveArmPositionsStop();
    
    // report load error
    printf("Load error detected joint %d, value %f\r\n", ix, lastVals[ix]);

    Alert alert;
    time_t seconds = time(NULL);
    
    ShowLedRed();
    alert.SetLoadAlert(seconds, ix, lastVals[ix]);
    AlertBuf.push(alert);
    
    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
}

// send hardware error alert
void PushHardwareAlert(int partIx, int code)
{
    Alert alert;
    time_t seconds = time(NULL);
    
    ShowLedRed();
    alert.SetHardwareAlert(seconds, partIx, code);
    AlertBuf.push(alert);
    NeedHwReset = true;
        
    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
}

// send temperature alert
void PushTemperatureAlert(RobotArm& robotArm)
{
    float lastVals[NUMJOINTS];
    
    // stop trying to move
    robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
    int ix = robotArm.GetLastErrorPart();
    robotArm.MoveArmPositionsStop();
    
    // report load error
    printf("Temperature error detected joint %d, value %f\r\n", ix, lastVals[ix]);

    Alert alert;
    time_t seconds = time(NULL);
    
    ShowLedRed();
    alert.SetTemperatureAlert(seconds, ix, lastVals[ix]);
    AlertBuf.push(alert);
    
    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
}

// send temperature alert
void PushVoltageAlert(RobotArm& robotArm)
{
    float lastVals[NUMJOINTS];
    
    // stop trying to move
    robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
    int ix = robotArm.GetLastErrorPart();
    robotArm.MoveArmPositionsStop();
    
    // report load error
    printf("Voltage error detected joint %d, value %f\r\n", ix, lastVals[ix]);

    Alert alert;
    time_t seconds = time(NULL);
    
    ShowLedRed();
    alert.SetVoltageAlert(seconds, ix, lastVals[ix]);
    AlertBuf.push(alert);
    
    BuzzerStartMs((int)IdleTimer.read_ms(), 500);
}


void PushMeasurements(int partSize, RobotArm& robotArm)
{
    // space for getting measurements from arm
    MeasureGroup measureGroup;
    
    float lastVals[NUMJOINTS];
    time_t seconds = time(NULL);
   
    bool ok = true;

    ok = robotArm.GetArmLastMeasure(NM_Temperature, lastVals);
    DispMeasure("Temperatures", partSize, lastVals);
    measureGroup.SetMeasure(NM_Temperature, seconds, partSize, lastVals);
    MeasureBuf.push(measureGroup);
    
    if (ok)
    {
        ok = robotArm.GetArmLastMeasure(NM_Voltage, lastVals);
        DispMeasure("Voltage", partSize, lastVals);
        measureGroup.SetMeasure(NM_Voltage, seconds, partSize, lastVals);
        MeasureBuf.push(measureGroup);
    }
    
    if (ok)
    {
        ok = robotArm.GetArmLastMeasure(NM_Degrees, lastVals);
        DispMeasure("Rotation", partSize, lastVals);
        measureGroup.SetMeasure(NM_Degrees, seconds, partSize, lastVals);
        MeasureBuf.push(measureGroup);
    }
    
    if (ok)
    {
        robotArm.GetArmLastMeasure(NM_Load, lastVals);
        DispMeasure("Load", partSize, lastVals);
        measureGroup.SetMeasure(NM_Load, seconds, partSize, lastVals);
        MeasureBuf.push(measureGroup);
    }
    
    if (!ok)
    {
        // report HW error
        int partix = robotArm.GetLastErrorPart();
        int errCode = robotArm.GetLastError();
        printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); 
        PushHardwareAlert(partix, errCode);
        MainState = MS_Error;
    }
}



void ControlArm(const char* cmd)
{
    if (strncmp(cmd, "pause", 5) == 0)
    {
        ShowLedBlue();
        MainState = MS_Paused;
    }
    else if (strncmp(cmd, "resume", 6) == 0)
    {
        ShowLedGreen();
        MainState = MS_Resuming;
    }
    else if (strncmp(cmd, "cancel", 6) == 0)
    {
        ShowLedGreen();
        MainState = MS_CancelOne;
    }
    else if (strncmp(cmd, "runupdown", 9) == 0)
    {
        ShowLedGreen();
        if (MainState == MS_Idle || MainState == MS_Paused)
            MainState = MS_Running;
        SequenceQ.put(&UpDownSeq);
    }
    else if (strncmp(cmd, "runuptwist", 10) == 0)
    {
        ShowLedGreen();
        if (MainState == MS_Idle || MainState == MS_Paused)
            MainState = MS_Running;
        SequenceQ.put(&UpTwistSeq);
    }
    else if (strncmp(cmd, "runhome", 10) == 0)
    {
        ShowLedGreen();
        if (MainState == MS_Idle || MainState == MS_Paused)
            MainState = MS_Running;
        SequenceQ.put(&StartSeq);
    }
    else if (strncmp(cmd, "runwave", 10) == 0)
    {
        ShowLedGreen();
        MainState = MS_Running;
        SequenceQ.put(&WaveSeq);
    }
    else if (strncmp(cmd, "runtaps1", 10) == 0)
    {
        ShowLedGreen();
        if (MainState == MS_Idle || MainState == MS_Paused)
            MainState = MS_Running;
        SequenceQ.put(&TapsSeq);
    }
    else if (strncmp(cmd, "fastwave", 8) == 0)
    {
        ShowLedGreen();
        MainState = MS_Running;
        SequenceQ.put(&BigWaveSeq);
    }
    else if (strncmp(cmd, "beep", 9) == 0)
    {
        BuzzerStartMs((int)IdleTimer.read_ms(), 500);
    }
}


// things to initialize early 
void PrepareController()
{
    MakeSequences(NUMJOINTS);
}

// Run the controller thread loop
void RunController()
{
    printf("RunController start");

    // init robot arm
    RobotArm robotArm;
    robotArm.SetStepMs(CONTROLLER_POLL);
    
    int partSize = robotArm.GetNumParts();

    IdleTimer.start();
    
     // set running state
    MainState = MS_Idle;
    RunInSequence = false;
    RunInMoveStep = false;
    
    NeedHwReset = false;
    
    // set first sequence to run
    SequenceQ.put(&StartSeq);
    
    // state for sequence
    vector<ActionSequence*> *curseq = NULL;
    int curseqIx = 0;
    int loopSeqIx = 0;
    int loopCounter = 0;
    
    // init time outs
    NextStepMs = 0;
    NextPollMs = (int)IdleTimer.read_ms();
    NextSendMs = (int)IdleTimer.read_ms() + SEND_STATUS_TO;
    
    MainState = MS_Running;
    NodeMeasure nextMetric = NM_Temperature;
    
    while( true )
    {
        int now = (int)IdleTimer.read_ms();
        
        // Sleep until next controller poll time 
        if (NextPollMs > now)
        {
            ThreadAPI_Sleep(NextPollMs - now);
            now = (int)IdleTimer.read_ms();
        }
        else
        {
            printf("too slow %d\r\n", now - NextPollMs);
        }
        // set next poll time
        NextPollMs = now + CONTROLLER_POLL;

        // if had HW error, reset error state
        if (MainState != MS_Error)
        {
            if (NeedHwReset)
            {
                robotArm.ClearErrorState();
                NeedHwReset = false;
            }
        }
        
        switch (MainState)
        {
            case MS_Idle:
                break;
                
            case MS_Paused:
                break;
                
            case MS_Error:
                break;
                
            case MS_CancelOne:
                RunInSequence = false;
                RunInMoveStep = false;
                robotArm.MoveArmPositionsEnd();
                MainState = MS_Running;
                // avoid next poll delay
                NextPollMs = now;
                break;
                
            case MS_CancelAll:
                RunInSequence = false;
                RunInMoveStep = false;
                robotArm.MoveArmPositionsEnd();
                while (1) {
                    osEvent evt = SequenceQ.get(0);
                    if (evt.status != osEventMessage)
                        break;
                }
                // avoid next poll delay
                NextPollMs = now;
                MainState = MS_Running;
                break;
                
            case MS_Resuming:
                robotArm.MoveArmPositionsResume();
                // avoid next poll delay
                NextPollMs = now;
                MainState = MS_Running;
                break;
                
            case MS_Running:
                if (!RunInSequence)
                {
                    // start new sequence of actions
                    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;
                    }
                    else
                    {
                        MainState = MS_Idle;
                    }
                }
                
                if (RunInSequence)
                {
                    if (RunInMoveStep)
                    {
                        // check if delaying next move
                        if (NextStepMs > now)
                            break;
                        
                        if (robotArm.MoveArmPositionsHasNext())
                        {
                            bool ok = robotArm.MoveArmPositionsNext();
                            if (!ok)
                            {
                                // report HW error
                                int partix = robotArm.GetLastErrorPart();
                                int errCode = robotArm.GetLastError();
                                printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
                                PushHardwareAlert(partix, errCode);
                                MainState = MS_Error;
                                break;
                            }
                        }
                        else
                        {
                            printf("No more Step\r\n");
                            RunInMoveStep = false;
                        }
                    }
                    if (!RunInMoveStep)
                    {
                        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;
                                    break;
                                case SA_Delay:
                                    printf(" - Delay\r\n");
                                    NextStepMs = aseq->Param + now;
                                    break;
                                case SA_LoopBegin:
                                    printf(" - LoopBegin\r\n");
                                    loopSeqIx = curseqIx;
                                    loopCounter = aseq->Param;
                                    break;
                                case SA_LoopEnd:
                                    printf(" - LoopEnd\r\n");
                                    loopCounter--;
                                    if (loopCounter > 0 && loopSeqIx > 0)
                                        curseqIx = loopSeqIx;
                                    break;
                            }
                        }
                    }
 
                }
                break;
        }
        
        bool sendAlert = MainState != MS_Error;
        
        // get measurements and test thresholds
        // Reading all values takes too long, 
        // so we read the load and 1 other value each time
        int rc = 0;
        rc = robotArm.ArmMeasuresTest(NM_Load);
        if (rc < 0 && sendAlert)
        {
            PushLoadAlert(robotArm);
            MainState = MS_Error;
        }
        
        switch (nextMetric)
        {
            case NM_Temperature:
                rc = robotArm.ArmMeasuresTest(NM_Temperature);
                if (rc < 0 && sendAlert)
                {
                    PushTemperatureAlert(robotArm);
                    MainState = MS_Error;
                }
                nextMetric = NM_Voltage;
                break;
                
            case NM_Voltage:
                rc = robotArm.ArmMeasuresTest(NM_Voltage);
                if (rc < 0 && sendAlert)
                {
                    PushVoltageAlert(robotArm);
                    MainState = MS_Error;
                }
                nextMetric = NM_Degrees;
                break;
                
               
            case NM_Degrees:
                rc = robotArm.ArmMeasuresTest(NM_Degrees);
                if (rc < 0 && sendAlert)
                {
                    PushPositionAlert(robotArm);
                    MainState = MS_Error;
                }
                nextMetric = NM_Temperature;
                break;

            default:
                nextMetric = NM_Temperature;
                break;
        }
        if (rc == -2)
        {
            int partix = robotArm.GetLastErrorPart();
            int errCode = robotArm.GetLastError();
            printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); 
            PushHardwareAlert(partix, errCode);
            MainState = MS_Error;
        }
        
        // check if buzzer needs to be turned off
        BuzzerPoll(now);
        
        // check if time to send status
        if (now >= NextSendMs)
        {
            // if paused, use longer time out for sending data
            if (MainState != MS_Paused ||
                now > NextSendMs + SEND_STATUS_PAUSED_TO)
            {
                NextSendMs = now + SEND_STATUS_TO;
                PushMeasurements(partSize, robotArm);
            }
        }
    }
}