ArmController.cpp

00001 // Copyright (c) Microsoft. All rights reserved.
00002 // Licensed under the MIT license. See LICENSE file in the project root for full license information.
00003 
00004 #include "mbed.h"
00005 #include "rtos.h"
00006 
00007 #include "threadapi.h"
00008 #include <vector>
00009 
00010 #include "RobotArm.h"
00011 #include "Sequences.h"
00012 #include "ControllerIo.h"
00013 #include "ControllerUtil.h"
00014 
00015 using namespace std;
00016 
00017 
00018 enum MainStates
00019 {
00020     MS_Idle           = 0x0,
00021     MS_Running        = 0x1,
00022     MS_Paused         = 0x2,
00023     MS_Error          = 0x3,
00024     MS_CancelOne      = 0x4,
00025     MS_CancelAll      = 0x5,
00026     MS_Resuming       = 0x6
00027 };
00028 
00029 // controller thread runs every 25ms
00030 #define CONTROLLER_POLL         25
00031 
00032 // try to send some status this many ms
00033 #define SEND_STATUS_TO          500
00034 
00035 
00036 // controller polling timer
00037 Timer IdleTimer;
00038 int NextSendMs = 0;
00039 int NextStepMs = 0;
00040 int NextPollMs = 0;
00041 
00042 // action sequences
00043 Queue<vector<ActionSequence*>, 8> SequenceQ;
00044 
00045 // controller state
00046 MainStates MainState;
00047 
00048 bool NeedHwReset;
00049 
00050 
00051 // handle commands received from IoT Hub
00052 // Add sequence to queue, change state, etc.
00053 void ControlArmCommands(const char* cmd)
00054 {
00055     if (strncmp(cmd, "pause", 5) == 0)
00056     {
00057         ShowLedBlue();
00058         MainState = MS_Paused;
00059     }
00060     else if (strncmp(cmd, "alert", 6) == 0)
00061     {
00062         ShowLedRed();
00063         MainState = MS_Error;
00064     }
00065     else if (strncmp(cmd, "resume", 6) == 0)
00066     {
00067         ShowLedGreen();
00068         MainState = MS_Resuming;
00069     }
00070     else if (strncmp(cmd, "cancel", 6) == 0)
00071     {
00072         ShowLedGreen();
00073         MainState = MS_CancelOne;
00074     }
00075     else if (strncmp(cmd, "runupdown", 9) == 0)
00076     {
00077         ShowLedGreen();
00078         if (MainState == MS_Idle || MainState == MS_Paused)
00079             MainState = MS_Running;
00080         SequenceQ.put(&UpDownSeq);
00081     }
00082     else if (strncmp(cmd, "runuptwist", 10) == 0)
00083     {
00084         ShowLedGreen();
00085         if (MainState == MS_Idle || MainState == MS_Paused)
00086             MainState = MS_Running;
00087         SequenceQ.put(&UpTwistSeq);
00088     }
00089     else if (strncmp(cmd, "runhome", 10) == 0)
00090     {
00091         ShowLedGreen();
00092         if (MainState == MS_Idle || MainState == MS_Paused)
00093             MainState = MS_Running;
00094         SequenceQ.put(&StartSeq);
00095     }
00096     else if (strncmp(cmd, "runwave", 10) == 0)
00097     {
00098         ShowLedGreen();
00099         if (MainState == MS_Idle || MainState == MS_Paused)
00100             MainState = MS_Running;
00101         SequenceQ.put(&WaveSeq);
00102     }
00103     else if (strncmp(cmd, "runtaps1", 10) == 0)
00104     {
00105         ShowLedGreen();
00106         if (MainState == MS_Idle || MainState == MS_Paused)
00107             MainState = MS_Running;
00108         SequenceQ.put(&TapsSeq);
00109     }
00110     else if (strncmp(cmd, "fastwave", 8) == 0)
00111     {
00112         ShowLedGreen();
00113         if (MainState == MS_Idle || MainState == MS_Paused)
00114             MainState = MS_Running;
00115         SequenceQ.put(&BigWaveSeq);
00116     }
00117     else if (strncmp(cmd, "beep", 9) == 0)
00118     {
00119         BuzzerStartMs((int)IdleTimer.read_ms(), 500);
00120     }
00121 }
00122 
00123 
00124 // things to initialize early 
00125 void PrepareController()
00126 {
00127     MakeSequences(NUMJOINTS);
00128 }
00129 
00130 // Run the controller thread loop
00131 void RunController()
00132 {
00133     printf("RunController start");
00134 
00135     // init robot arm
00136     RobotArm robotArm;
00137     robotArm.SetStepMs(CONTROLLER_POLL);
00138     
00139     int partSize = robotArm.GetNumParts();
00140 
00141     IdleTimer.start();
00142     
00143      // set running state
00144     MainState = MS_Idle;
00145     
00146     NeedHwReset = false;
00147     
00148     // set first sequence to run
00149     SequenceQ.put(&StartSeq);
00150     
00151     // state for sequence
00152     vector<ActionSequence*> *curseq = NULL;
00153     int curseqIx = 0;
00154     int loopSeqIx = 0;
00155     int loopCounter = 0;
00156     
00157     // init time outs
00158     NextStepMs = 0;
00159     NextPollMs = (int)IdleTimer.read_ms();
00160     NextSendMs = (int)IdleTimer.read_ms() + SEND_STATUS_TO;
00161     
00162     MainState = MS_Running;
00163     
00164     while( true )
00165     {
00166         int now = (int)IdleTimer.read_ms();
00167         
00168         // Sleep until next controller poll time 
00169         if (NextPollMs > now)
00170         {
00171             ThreadAPI_Sleep(NextPollMs - now);
00172             now = (int)IdleTimer.read_ms();
00173         }
00174         else
00175         {
00176             printf("too slow %d\r\n", now - NextPollMs);
00177         }
00178         // set next poll time
00179         NextPollMs = now + CONTROLLER_POLL;
00180 
00181         // if had HW error and not in error state, reset error
00182         if (NeedHwReset && MainState != MS_Error)
00183         {
00184             robotArm.ClearErrorState();
00185             NeedHwReset = false;
00186         }
00187         
00188         switch (MainState)
00189         {
00190             case MS_Idle:
00191                 break;
00192                 
00193             case MS_Paused:
00194                 break;
00195                 
00196             case MS_Error:
00197                 break;
00198                 
00199             case MS_CancelOne:
00200                 curseq = NULL;
00201                 robotArm.MoveArmPositionsEnd();
00202                 MainState = MS_Running;
00203                 break;
00204                 
00205             case MS_CancelAll:
00206                 curseq = NULL;
00207                 robotArm.MoveArmPositionsEnd();
00208                 while (1) {
00209                     osEvent evt = SequenceQ.get(0);
00210                     if (evt.status != osEventMessage)
00211                         break;
00212                 }
00213                 MainState = MS_Running;
00214                 break;
00215                 
00216             case MS_Resuming:
00217                 robotArm.MoveArmPositionsResume();
00218                 MainState = MS_Running;
00219                 break;
00220                 
00221             case MS_Running:
00222                 if (curseq == NULL)
00223                 {
00224                     // start new sequence of actions
00225                     osEvent evt = SequenceQ.get(0);
00226                     if (evt.status == osEventMessage)
00227                     {
00228                         printf("New Seq\r\n"); 
00229                         curseq = (vector<ActionSequence*> *)evt.value.p;
00230                         curseqIx = 0;
00231                     }
00232                     else
00233                     {
00234                         MainState = MS_Idle;
00235                     }
00236                 }
00237                 
00238                 if (curseq != NULL)
00239                 {
00240                     // check if delaying next move
00241                     if (NextStepMs > now)
00242                         break;
00243                     
00244                     if (robotArm.MoveArmPositionsHasNext())
00245                     {
00246                         bool ok = robotArm.MoveArmPositionsNext();
00247                         if (!ok)
00248                         {
00249                             // report HW error
00250                             int partix = robotArm.GetLastErrorPart();
00251                             int errCode = robotArm.GetLastError();
00252                             printf("Hardware error detected joint %d, code %d \r\n", partix, errCode);
00253                             PushHardwareAlert(partix, errCode);
00254                             NeedHwReset = true;
00255                             MainState = MS_Error;
00256                             break;
00257                         }
00258                     }
00259                     else
00260                     {
00261                         if (curseqIx >= curseq->size())
00262                         {
00263                             printf("sequence completed. Stopping\r\n");
00264                             curseq = NULL;
00265                             break;
00266                         }
00267 
00268                         ActionSequence* aseq = (*curseq)[curseqIx];
00269                         curseqIx++;
00270                         
00271                         switch (aseq->ActionType)
00272                         {
00273                             case SA_SetGoal:
00274                                 printf(" - Move arm start\r\n"); 
00275                                 robotArm.MoveArmPositionsStart(aseq->GoalVals, aseq->Param);
00276                                 break;
00277                             case SA_Delay:
00278                                 printf(" - Delay\r\n");
00279                                 NextStepMs = aseq->Param + now;
00280                                 break;
00281                             case SA_LoopBegin:
00282                                 printf(" - LoopBegin\r\n");
00283                                 loopSeqIx = curseqIx;
00284                                 loopCounter = aseq->Param;
00285                                 break;
00286                             case SA_LoopEnd:
00287                                 printf(" - LoopEnd\r\n");
00288                                 loopCounter--;
00289                                 if (loopCounter > 0 && loopSeqIx > 0)
00290                                     curseqIx = loopSeqIx;
00291                                 break;
00292                         }
00293                     }
00294  
00295                 }
00296                 break;
00297         }
00298         
00299         bool sendAlert = MainState != MS_Error;
00300         
00301         // get measurements and test thresholds
00302         int rc = 0;
00303         // call start to clear cached values
00304         robotArm.ArmMeasuresTestStart();
00305         
00306         rc = robotArm.ArmMeasuresTest(NM_Load);
00307         if (rc == -1 && sendAlert)
00308         {
00309             PushLoadAlert(robotArm);
00310             MainState = MS_Error;
00311         }
00312         
00313         rc = robotArm.ArmMeasuresTest(NM_Temperature);
00314         if (rc == -1 && sendAlert)
00315         {
00316             PushTemperatureAlert(robotArm);
00317             MainState = MS_Error;
00318         }
00319 
00320         rc = robotArm.ArmMeasuresTest(NM_Voltage);
00321         if (rc == -1 && sendAlert)
00322         {
00323             PushVoltageAlert(robotArm);
00324             MainState = MS_Error;
00325         }
00326 
00327         rc = robotArm.ArmMeasuresTest(NM_Degrees);
00328         if (rc == -1 && sendAlert)
00329         {
00330             PushPositionAlert(robotArm);
00331             MainState = MS_Error;
00332         }
00333 
00334         if (rc == -2 && sendAlert)
00335         {
00336             int partix = robotArm.GetLastErrorPart();
00337             int errCode = robotArm.GetLastError();
00338             printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); 
00339             PushHardwareAlert(partix, errCode);
00340             NeedHwReset = true;
00341             MainState = MS_Error;
00342         }
00343         
00344         // check if buzzer needs to be turned off
00345         BuzzerPoll(now);
00346         
00347         // check if time to send status
00348         if (now >= NextSendMs)
00349         {
00350             NextSendMs = now + SEND_STATUS_TO;
00351             if (!PushMeasurements(partSize, robotArm))
00352             {
00353                 if (sendAlert)
00354                 {
00355                     int partix = robotArm.GetLastErrorPart();
00356                     int errCode = robotArm.GetLastError();
00357                     printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); 
00358                     PushHardwareAlert(partix, errCode);
00359                     NeedHwReset = true;
00360                     MainState = MS_Error;
00361                 }
00362             }
00363         }
00364     }
00365 }
00366