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:
- 13:ffeff9b5e513
- Parent:
- 11:3a2e6eb9fbb8
- Child:
- 14:570c8071f577
--- 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);
+ }
}
}
}