demo project
Dependencies: AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL
Diff: ArmController.cpp
- Revision:
- 14:570c8071f577
- Parent:
- 13:ffeff9b5e513
- Child:
- 15:4bd10f531cdc
diff -r ffeff9b5e513 -r 570c8071f577 ArmController.cpp --- a/ArmController.cpp Fri Jan 15 22:02:46 2016 +0000 +++ b/ArmController.cpp Fri Jan 15 23:10:30 2016 +0000 @@ -2,15 +2,8 @@ #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> @@ -31,9 +24,9 @@ // controller thread runs every 25ms #define CONTROLLER_POLL 25 -// try to send some status every few seconds +// try to send some status this many ms #define SEND_STATUS_TO 500 -// use slower send rate if paused +// use slower status send rate if paused #define SEND_STATUS_PAUSED_TO 20000 // controller polling timer @@ -47,190 +40,19 @@ // 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); -} +extern void PushPositionAlert(RobotArm& robotArm); +extern void PushLoadAlert(RobotArm& robotArm); +extern void PushTemperatureAlert(RobotArm& robotArm); +extern void PushVoltageAlert(RobotArm& robotArm); +extern void PushHardwareAlert(int partIx, int code); +extern bool PushMeasurements(int partSize, RobotArm& robotArm); -// 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) +// handle commands received from IoT Hub +// Add sequence to queue, change state, etc. +void ControlArmCommands(const char* cmd) { if (strncmp(cmd, "pause", 5) == 0) { @@ -271,7 +93,8 @@ else if (strncmp(cmd, "runwave", 10) == 0) { ShowLedGreen(); - MainState = MS_Running; + if (MainState == MS_Idle || MainState == MS_Paused) + MainState = MS_Running; SequenceQ.put(&WaveSeq); } else if (strncmp(cmd, "runtaps1", 10) == 0) @@ -284,7 +107,8 @@ else if (strncmp(cmd, "fastwave", 8) == 0) { ShowLedGreen(); - MainState = MS_Running; + if (MainState == MS_Idle || MainState == MS_Paused) + MainState = MS_Running; SequenceQ.put(&BigWaveSeq); } else if (strncmp(cmd, "beep", 9) == 0) @@ -315,8 +139,6 @@ // set running state MainState = MS_Idle; - RunInSequence = false; - RunInMoveStep = false; NeedHwReset = false; @@ -354,14 +176,11 @@ // set next poll time NextPollMs = now + CONTROLLER_POLL; - // if had HW error, reset error state - if (MainState != MS_Error) + // if had HW error and not in error state, reset error + if (NeedHwReset && MainState != MS_Error) { - if (NeedHwReset) - { - robotArm.ClearErrorState(); - NeedHwReset = false; - } + robotArm.ClearErrorState(); + NeedHwReset = false; } switch (MainState) @@ -376,8 +195,7 @@ break; case MS_CancelOne: - RunInSequence = false; - RunInMoveStep = false; + curseq = NULL; robotArm.MoveArmPositionsEnd(); MainState = MS_Running; // avoid next poll delay @@ -385,8 +203,7 @@ break; case MS_CancelAll: - RunInSequence = false; - RunInMoveStep = false; + curseq = NULL; robotArm.MoveArmPositionsEnd(); while (1) { osEvent evt = SequenceQ.get(0); @@ -406,7 +223,7 @@ break; case MS_Running: - if (!RunInSequence) + if (curseq == NULL) { // start new sequence of actions osEvent evt = SequenceQ.get(0); @@ -415,8 +232,6 @@ printf("New Seq\r\n"); curseq = (vector<ActionSequence*> *)evt.value.p; curseqIx = 0; - RunInSequence = true; - RunInMoveStep = false; } else { @@ -424,71 +239,60 @@ } } - if (RunInSequence) + if (curseq != NULL) { - if (RunInMoveStep) + // check if delaying next move + if (NextStepMs > now) + break; + + if (robotArm.MoveArmPositionsHasNext()) { - // check if delaying next move - if (NextStepMs > now) - break; - - if (robotArm.MoveArmPositionsHasNext()) + bool ok = robotArm.MoveArmPositionsNext(); + if (!ok) { - 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; + // 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; } } - if (!RunInMoveStep) + else { - if (curseq != NULL) + if (curseqIx >= curseq->size()) { - if (curseqIx >= curseq->size()) - { - printf("sequence completed. Stopping\r\n"); - RunInSequence = false; - break; - } + 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); - 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; - } + 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; } } @@ -503,7 +307,7 @@ // so we read the load and 1 other value each time int rc = 0; rc = robotArm.ArmMeasuresTest(NM_Load); - if (rc < 0 && sendAlert) + if (rc == -1 && sendAlert) { PushLoadAlert(robotArm); MainState = MS_Error; @@ -513,7 +317,7 @@ { case NM_Temperature: rc = robotArm.ArmMeasuresTest(NM_Temperature); - if (rc < 0 && sendAlert) + if (rc == -1 && sendAlert) { PushTemperatureAlert(robotArm); MainState = MS_Error; @@ -523,7 +327,7 @@ case NM_Voltage: rc = robotArm.ArmMeasuresTest(NM_Voltage); - if (rc < 0 && sendAlert) + if (rc == -1 && sendAlert) { PushVoltageAlert(robotArm); MainState = MS_Error; @@ -534,7 +338,7 @@ case NM_Degrees: rc = robotArm.ArmMeasuresTest(NM_Degrees); - if (rc < 0 && sendAlert) + if (rc == -1 && sendAlert) { PushPositionAlert(robotArm); MainState = MS_Error; @@ -546,12 +350,13 @@ nextMetric = NM_Temperature; break; } - if (rc == -2) + 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; } @@ -566,7 +371,18 @@ now > NextSendMs + SEND_STATUS_PAUSED_TO) { NextSendMs = now + SEND_STATUS_TO; - PushMeasurements(partSize, robotArm); + 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; + } + } } } }