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:
- 13:ffeff9b5e513
- Parent:
- 11:3a2e6eb9fbb8
- Child:
- 14:570c8071f577
diff -r ac6c9d7f8c40 -r ffeff9b5e513 ArmController.cpp --- a/ArmController.cpp Thu Jan 07 17:31:23 2016 +0000 +++ b/ArmController.cpp Fri Jan 15 22:02:46 2016 +0000 @@ -1,9 +1,7 @@ #include "mbed.h" #include "rtos.h" -#include "EthernetInterface.h" -#include "mbed/logging.h" -#include "mbed/mbedtime.h" +#include "threadapi.h" #include <NTPClient.h> #include <DynamixelBus.h> #include <AX12.h> @@ -19,155 +17,213 @@ 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 + MS_Error = 0x3, + MS_CancelOne = 0x4, + MS_CancelAll = 0x5, + MS_Resuming = 0x6 }; -// try to send some status every minute -#define IDLESTATUSTO 60000 +// 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 LastSendMs = 0; +int NextSendMs = 0; +int NextStepMs = 0; +int NextPollMs = 0; -Queue<vector<ActionSequence>, 16> SequenceQ; +// action sequences +Queue<vector<ActionSequence*>, 8> SequenceQ; + +// controller state MainStates MainState; -bool ErrorReset; bool RunInSequence; bool RunInMoveStep; -bool RunInDelay; -void* CancelToMatch; + +bool NeedHwReset; -osThreadId mainTid; - - -void DispMeasure(char* label, int partSize, vector<float>& vals) +// 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"); - +// 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) +// send alert message +void PushAlert(char* msg, char* atype) { Alert alert; time_t seconds = time(NULL); - alert.SetAlert(severity, seconds, msg, atype); + alert.SetAlert(seconds, msg, atype); AlertBuf.push(alert); - SendIothubData(); } -void PushPositionAlert(int severity, int partIx, float pos) +// 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(severity, seconds, partIx, pos); + alert.SetPositionAlert(seconds, ix, diff); AlertBuf.push(alert); - SendIothubData(); + + BuzzerStartMs((int)IdleTimer.read_ms(), 500); } -void PushHardwareAlert(int severity, int partIx, int code) +// 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(severity, seconds, partIx, code); + alert.SetHardwareAlert(seconds, partIx, code); AlertBuf.push(alert); - SendIothubData(); + NeedHwReset = true; + + BuzzerStartMs((int)IdleTimer.read_ms(), 500); } -void PushTemperatureAlert(int severity, int partIx, float temp) +// 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(severity, seconds, partIx, temp); + alert.SetTemperatureAlert(seconds, ix, lastVals[ix]); AlertBuf.push(alert); - SendIothubData(); + + BuzzerStartMs((int)IdleTimer.read_ms(), 500); } -// send alert if temperature is above 69C and return true -#define MaxTemp 69 -bool TestTemperature(vector<float>& vals) +// send temperature alert +void PushVoltageAlert(RobotArm& robotArm) { - bool err = false; + 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]); - 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; + 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(MeasureGroup measureGroup, int partSize, RobotArm robotArm) +void PushMeasurements(int partSize, RobotArm& robotArm) { - vector<float> lastVals; - time_t seconds = time(NULL); - LastSendMs = (int)IdleTimer.read_ms(); + // space for getting measurements from arm + MeasureGroup measureGroup; + float lastVals[NUMJOINTS]; + time_t seconds = time(NULL); + bool ok = true; - - ok = robotArm.GetArmMeasure(NM_Temperature, lastVals); + + ok = robotArm.GetArmLastMeasure(NM_Temperature, lastVals); DispMeasure("Temperatures", partSize, lastVals); - measureGroup.SetMeasure(NM_Temperature, seconds, lastVals); + measureGroup.SetMeasure(NM_Temperature, seconds, partSize, lastVals); MeasureBuf.push(measureGroup); if (ok) { - ok = robotArm.GetArmMeasure(NM_Voltage, lastVals); + ok = robotArm.GetArmLastMeasure(NM_Voltage, lastVals); DispMeasure("Voltage", partSize, lastVals); - measureGroup.SetMeasure(NM_Voltage, seconds, lastVals); + measureGroup.SetMeasure(NM_Voltage, seconds, partSize, lastVals); MeasureBuf.push(measureGroup); } if (ok) { - ok = robotArm.GetArmMeasure(NM_Degrees, lastVals); + ok = robotArm.GetArmLastMeasure(NM_Degrees, lastVals); DispMeasure("Rotation", partSize, lastVals); - measureGroup.SetMeasure(NM_Degrees, seconds, lastVals); + measureGroup.SetMeasure(NM_Degrees, seconds, partSize, 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) + { + 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 part %d, code %d \r\n", partix, errCode); - PushHardwareAlert(AS_Error, partix, errCode); + printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); + PushHardwareAlert(partix, errCode); MainState = MS_Error; } } @@ -176,146 +232,136 @@ 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); + MainState = MS_Resuming; } 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; + if (MainState == MS_Idle || MainState == MS_Paused) + MainState = MS_Running; SequenceQ.put(&UpDownSeq); - osSignalSet(mainTid, AS_Action); } else if (strncmp(cmd, "runuptwist", 10) == 0) { ShowLedGreen(); - MainState = MS_CancelOne; + if (MainState == MS_Idle || MainState == MS_Paused) + MainState = MS_Running; SequenceQ.put(&UpTwistSeq); - osSignalSet(mainTid, AS_Action); } else if (strncmp(cmd, "runhome", 10) == 0) { ShowLedGreen(); - MainState = MS_CancelOne; + if (MainState == MS_Idle || MainState == MS_Paused) + MainState = MS_Running; SequenceQ.put(&StartSeq); - osSignalSet(mainTid, AS_Action); } else if (strncmp(cmd, "runwave", 10) == 0) { ShowLedGreen(); - MainState = MS_CancelOne; + MainState = MS_Running; SequenceQ.put(&WaveSeq); - osSignalSet(mainTid, AS_Action); } else if (strncmp(cmd, "runtaps1", 10) == 0) { ShowLedGreen(); - MainState = MS_CancelOne; + if (MainState == MS_Idle || MainState == MS_Paused) + MainState = MS_Running; 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); + MainState = MS_Running; + SequenceQ.put(&BigWaveSeq); + } + else if (strncmp(cmd, "beep", 9) == 0) + { + BuzzerStartMs((int)IdleTimer.read_ms(), 500); } } -// run sequence thread timer routine -void NextSeq(void const * tid) + +// things to initialize early +void PrepareController() { - RunInDelay = false; - osSignalSet((osThreadId)tid, AS_Action); + MakeSequences(NUMJOINTS); } -// periodic timer routine -void PeriodicStatus(void const * tid) -{ - // make sure we run controller even if idle - osSignalSet((osThreadId)tid, AS_Action); -} - +// 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(); - LastSendMs = (int)IdleTimer.read_ms(); - // set running state + // 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; + NeedHwReset = false; - // create sequences - MakeSequences(partSize, initPositions); - - // add to queue + // set first sequence to run SequenceQ.put(&StartSeq); // state for sequence - vector<ActionSequence> *curseq = NULL; + 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); + // 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 ) { - osEvent mainEvent = osSignalWait(signals, osWaitForever); + int now = (int)IdleTimer.read_ms(); - if (ErrorReset) + // Sleep until next controller poll time + if (NextPollMs > now) + { + ThreadAPI_Sleep(NextPollMs - now); + now = (int)IdleTimer.read_ms(); + } + else { - ErrorReset = false; - robotArm.ClearErrorState(); + 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) @@ -324,129 +370,78 @@ 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(); - } + robotArm.MoveArmPositionsEnd(); MainState = MS_Running; - osSignalSet(mainTid, AS_Action); + // avoid next poll delay + NextPollMs = now; break; case MS_CancelAll: - printf("Cancel all\r\n"); RunInSequence = false; RunInMoveStep = false; - if (RunInDelay) - { - RunInDelay = false; - delayTimer.stop(); - } + 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_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) { + // 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; + 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) { - 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; + // check if delaying next move + if (NextStepMs > now) 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 + 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); + printf("Hardware error detected joint %d, code %d \r\n", partix, errCode); + PushHardwareAlert(partix, errCode); MainState = MS_Error; break; } @@ -459,8 +454,6 @@ } if (!RunInMoveStep) { - printf("Next Seq %d\r\n", curseqIx); - if (curseq != NULL) { if (curseqIx >= curseq->size()) @@ -470,44 +463,30 @@ break; } - ActionSequence aseq = (*curseq)[curseqIx]; + ActionSequence* aseq = (*curseq)[curseqIx]; curseqIx++; - switch (aseq.ActionType) + switch (aseq->ActionType) { case SA_SetGoal: printf(" - Move arm start\r\n"); - robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Param); + 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); + NextStepMs = aseq->Param + now; break; case SA_LoopBegin: printf(" - LoopBegin\r\n"); loopSeqIx = curseqIx; - loopCounter = aseq.Param; - osSignalSet(mainTid, AS_Action); + loopCounter = aseq->Param; break; case SA_LoopEnd: printf(" - LoopEnd\r\n"); loopCounter--; if (loopCounter > 0 && loopSeqIx > 0) curseqIx = loopSeqIx; - osSignalSet(mainTid, AS_Action); break; } } @@ -516,11 +495,79 @@ } 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; - int now = (int)IdleTimer.read_ms(); - if (now - LastSendMs > (IDLESTATUSTO - 1000)) + default: + nextMetric = NM_Temperature; + break; + } + if (rc == -2) { - PushMeasurements(measureGroup, partSize, robotArm); + 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); + } } } }