robot arm demo team
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
--- 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;
+ }
+ }
}
}
}