File content as of revision 33:8b9dcbf6d8ec:

// Copyright (c) Microsoft. All rights reserved.
// Licensed under the MIT license. See LICENSE file in the project root for full license information.

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

#include "threadapi.h"
#include <vector>

#include "RobotArm.h"
#include "Sequences.h"
#include "ControllerIo.h"
#include "ControllerUtil.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 this many ms
#define SEND_STATUS_TO          500


// 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 NeedHwReset;


// handle commands received from IoT Hub
// Add sequence to queue, change state, etc.
void ControlArmCommands(const char* cmd)
{
    if (strncmp(cmd, "pause", 5) == 0)
    {
        ShowLedBlue();
        MainState = MS_Paused;
    }
    else if (strncmp(cmd, "alert", 6) == 0)
    {
        ShowLedRed();
        MainState = MS_Error;
    }
    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();
        if (MainState == MS_Idle || MainState == MS_Paused)
            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();
        if (MainState == MS_Idle || MainState == MS_Paused)
            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;
    
    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;
    
    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 and not in error state, reset error
        if (NeedHwReset && MainState != MS_Error)
        {
            robotArm.ClearErrorState();
            NeedHwReset = false;
        }
        
        switch (MainState)
        {
            case MS_Idle:
                break;
                
            case MS_Paused:
                break;
                
            case MS_Error:
                break;
                
            case MS_CancelOne:
                curseq = NULL;
                robotArm.MoveArmPositionsEnd();
                MainState = MS_Running;
                break;
                
            case MS_CancelAll:
                curseq = NULL;
                robotArm.MoveArmPositionsEnd();
                while (1) {
                    osEvent evt = SequenceQ.get(0);
                    if (evt.status != osEventMessage)
                        break;
                }
                MainState = MS_Running;
                break;
                
            case MS_Resuming:
                robotArm.MoveArmPositionsResume();
                MainState = MS_Running;
                break;
                
            case MS_Running:
                if (curseq == NULL)
                {
                    // 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;
                    }
                    else
                    {
                        MainState = MS_Idle;
                    }
                }
                
                if (curseq != NULL)
                {
                    // 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);
                            NeedHwReset = true;
                            MainState = MS_Error;
                            break;
                        }
                    }
                    else
                    {
                        if (curseqIx >= curseq->size())
                        {
                            printf("sequence completed. Stopping\r\n");
                            curseq = NULL;
                            break;
                        }

                        ActionSequence* aseq = (*curseq)[curseqIx];
                        curseqIx++;
                        
                        switch (aseq->ActionType)
                        {
                            case SA_SetGoal:
                                printf(" - Move arm start\r\n"); 
                                robotArm.MoveArmPositionsStart(aseq->GoalVals, aseq->Param);
                                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
        int rc = 0;
        // call start to clear cached values
        robotArm.ArmMeasuresTestStart();
        
        rc = robotArm.ArmMeasuresTest(NM_Load);
        if (rc == -1 && sendAlert)
        {
            PushLoadAlert(robotArm);
            MainState = MS_Error;
        }
        
        rc = robotArm.ArmMeasuresTest(NM_Temperature);
        if (rc == -1 && sendAlert)
        {
            PushTemperatureAlert(robotArm);
            MainState = MS_Error;
        }

        rc = robotArm.ArmMeasuresTest(NM_Voltage);
        if (rc == -1 && sendAlert)
        {
            PushVoltageAlert(robotArm);
            MainState = MS_Error;
        }

        rc = robotArm.ArmMeasuresTest(NM_Degrees);
        if (rc == -1 && sendAlert)
        {
            PushPositionAlert(robotArm);
            MainState = MS_Error;
        }

        if (rc == -2 && sendAlert)
        {
            int partix = robotArm.GetLastErrorPart();
            int errCode = robotArm.GetLastError();
            printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); 
            PushHardwareAlert(partix, errCode);
            NeedHwReset = true;
            MainState = MS_Error;
        }
        
        // check if buzzer needs to be turned off
        BuzzerPoll(now);
        
        // check if time to send status
        if (now >= NextSendMs)
        {
            NextSendMs = now + SEND_STATUS_TO;
            if (!PushMeasurements(partSize, robotArm))
            {
                if (sendAlert)
                {
                    int partix = robotArm.GetLastErrorPart();
                    int errCode = robotArm.GetLastError();
                    printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); 
                    PushHardwareAlert(partix, errCode);
                    NeedHwReset = true;
                    MainState = MS_Error;
                }
            }
        }
    }
}