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
Revision 13:ffeff9b5e513, committed 2016-01-15
- Comitter:
- henryrawas
- Date:
- Fri Jan 15 22:02:46 2016 +0000
- Parent:
- 12:ac6c9d7f8c40
- Child:
- 14:570c8071f577
- Commit message:
- Always test status and send data
Changed in this revision
--- a/ActionBuf.cpp Thu Jan 07 17:31:23 2016 +0000
+++ b/ActionBuf.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -4,9 +4,6 @@
#include "ActionBuf.h"
-
-SafeCircBuf<ActionGroup, ActionBufSize, uint32_t> ActionBuf;
-
bool ActionGroup::SetAction(ArmAction aId, char* args)
{
if (strlen(args) < ActionArgSize)
@@ -29,27 +26,35 @@
{
ActionType = aType;
Param = 0;
+ NumParts = 0;
};
-ActionSequence::ActionSequence(SequenceAction aType, vector<float>& vals, int param)
+ActionSequence::ActionSequence(SequenceAction aType, int parts, const float vals[], int param)
{
ActionType = aType;
- GoalVals = vals;
+
+ if (parts > NUMJOINTS) parts = NUMJOINTS;
+
+ if (vals != NULL)
+ {
+ for (int i = 0; i < parts; i++)
+ GoalVals[i] = vals[i];
+ }
+ NumParts = parts;
+
Param = param;
}
-ActionSequence::ActionSequence(SequenceAction aType, int parts, float vals[], int param)
+void ActionSequence::SetGoal(int parts, const float vals[])
{
- ActionType = aType;
- GoalVals.clear();
- for (int i = 0; i < parts; i++)
- GoalVals.push_back(vals[i]);
- Param = param;
-}
-
-void ActionSequence::SetGoal(vector<float>& vals)
-{
- GoalVals = vals;
+ if (parts > NUMJOINTS) parts = NUMJOINTS;
+
+ if (vals != NULL)
+ {
+ for (int i = 0; i < parts; i++)
+ GoalVals[i] = vals[i];
+ }
+ NumParts = parts;
}
void ActionSequence::SetParam(int param)
--- a/ActionBuf.h Thu Jan 07 17:31:23 2016 +0000
+++ b/ActionBuf.h Fri Jan 15 22:02:46 2016 +0000
@@ -10,7 +10,6 @@
#include "RobotArm.h"
-#define ActionBufSize 100
#define ActionArgSize 40
class ActionGroup
@@ -25,16 +24,14 @@
char ActionArg[ActionArgSize];
};
-extern SafeCircBuf<ActionGroup, ActionBufSize, uint32_t> ActionBuf;
enum SequenceAction
{
SA_SetGoal = 0x1,
SA_Delay = 0x2,
- SA_Status = 0x3,
- SA_LoopBegin = 0x4,
- SA_LoopEnd = 0x5
+ SA_LoopBegin = 0x3,
+ SA_LoopEnd = 0x4
};
class ActionSequence
@@ -44,11 +41,10 @@
ActionSequence(SequenceAction aType);
- ActionSequence(SequenceAction aType, vector<float>& vals, int param);
+
+ ActionSequence(SequenceAction aType, int parts, const float vals[], int param);
- ActionSequence(SequenceAction aType, int parts, float vals[], int param);
-
- void SetGoal(vector<float>& vals);
+ void SetGoal(int parts, const float vals[]);
void SetParam(int param);
@@ -56,7 +52,9 @@
SequenceAction ActionType;
- vector<float> GoalVals;
+ float GoalVals[NUMJOINTS];
+
+ int NumParts;
int Param;
--- a/Alert.cpp Thu Jan 07 17:31:23 2016 +0000
+++ b/Alert.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -7,9 +7,8 @@
SafeCircBuf<Alert, AlertBufSize, uint32_t> AlertBuf;
-void Alert::SetAlert(int severity, time_t created, char* msg, char* atype)
+void Alert::SetAlert(time_t created, char* msg, char* atype)
{
- Sev = severity;
Created = created;
if (strlen(atype) >= AlertTypeMaxLen)
@@ -33,32 +32,67 @@
}
-void Alert::SetPositionAlert(int severity, time_t created, int partIx, float diff)
+void Alert::SetPositionAlert(time_t created, int partIx, float diff)
{
- char* msg = "Arm part failed to move to desired position. Part %d is off by %f";
+ char* msg = "Arm joint failed to move to desired position. Joint %d is off by %f";
int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, diff);
- Sev = severity;
+
Created = created;
+ strcpy(MeasureName, "rot");
+ Index = partIx;
+ Value = diff;
strcpy(AlertType, "Position");
}
-void Alert::SetHardwareAlert(int severity, time_t created, int partIx, int code)
+void Alert::SetLoadAlert(time_t created, int partIx, float val)
{
- char* msg = "Arm part reported an error. Part %d error code %d";
+ char* msg = "Arm joint reported a high load. Joint %d, load %f";
+ int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, val);
+
+ Created = created;
+ strcpy(MeasureName, "load");
+ Index = partIx;
+ Value = val;
+
+ strcpy(AlertType, "Load");
+}
+
+void Alert::SetHardwareAlert(time_t created, int partIx, int code)
+{
+ char* msg = "Arm joint reported an error. Joint %d error code %d";
int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, code);
- Sev = severity;
+
Created = created;
+ strcpy(MeasureName, "error");
+ Index = partIx;
+ Value = (float)code;
strcpy(AlertType, "Hardware");
}
-void Alert::SetTemperatureAlert(int severity, time_t created, int partIx, float temp)
+void Alert::SetTemperatureAlert(time_t created, int partIx, float temp)
{
- char* msg = "Arm part reported a high temperature. Part %d temperature %f";
+ char* msg = "Arm joint reported a high temperature. Joint %d temperature %f";
int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, temp);
- Sev = severity;
+
Created = created;
+ strcpy(MeasureName, "temp");
+ Index = partIx;
+ Value = temp;
strcpy(AlertType, "Temperature");
}
+
+void Alert::SetVoltageAlert(time_t created, int partIx, float val)
+{
+ char* msg = "Arm joint reported an unexpected voltge. Joint %d volt %f";
+ int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, val);
+
+ Created = created;
+ strcpy(MeasureName, "volt");
+ Index = partIx;
+ Value = val;
+
+ strcpy(AlertType, "Voltage");
+}
--- a/Alert.h Thu Jan 07 17:31:23 2016 +0000
+++ b/Alert.h Fri Jan 15 22:02:46 2016 +0000
@@ -7,36 +7,39 @@
#include "mbed.h"
#include "SafeCircBuf.h"
-#define AlertBufSize 20
-#define AlertMsgMaxLen 200
+#define AlertBufSize 8
+#define AlertMsgMaxLen 120
#define AlertTypeMaxLen 16
+#define MeasureNameMaxLen 16
-enum AlertSeverity
-{
- AS_Info = 0,
- AS_Warn = 1,
- AS_Error = 2
-};
class Alert
{
public:
Alert() {};
- void SetAlert(int severity, time_t created, char* msg, char* atype);
+ void SetAlert(time_t created, char* msg, char* atype);
- void SetPositionAlert(int severity, time_t created, int partIx, float diff);
+ void SetPositionAlert(time_t created, int partIx, float diff);
+
+ void SetLoadAlert(time_t created, int partIx, float val);
- void SetHardwareAlert(int severity, time_t created, int partIx, int code);
+ void SetHardwareAlert(time_t created, int partIx, int code);
- void SetTemperatureAlert(int severity, time_t created, int partIx, float temp);
+ void SetTemperatureAlert(time_t created, int partIx, float temp);
- int Sev;
+ void SetVoltageAlert(time_t created, int partIx, float val);
char AlertType[AlertTypeMaxLen];
char Msg[AlertMsgMaxLen];
+ int Index;
+
+ char MeasureName[MeasureNameMaxLen];
+
+ float Value;
+
time_t Created;
};
--- 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);
+ }
}
}
}
--- a/ControllerIo.cpp Thu Jan 07 17:31:23 2016 +0000
+++ b/ControllerIo.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -5,6 +5,10 @@
DigitalOut redLed(LED_RED);
DigitalOut blueLed(LED_BLUE);
+DigitalOut buzzer(D2, 1);
+
+
+int BuzzStopMs = 0;
void ShowLedColor(int col)
{
@@ -29,7 +33,36 @@
{
ShowLedColor(2);
}
+
void ShowLedBlue()
{
ShowLedColor(3);
}
+
+void BuzzerStart()
+{
+ buzzer = 0;
+}
+
+void BuzzerStop()
+{
+ buzzer = 1;
+}
+
+void BuzzerStartMs(int nowMs, int durationMs)
+{
+ BuzzStopMs = nowMs + durationMs;
+ BuzzerStart();
+}
+
+void BuzzerPoll(int nowMs)
+{
+ if (BuzzStopMs != 0)
+ {
+ if (nowMs >= BuzzStopMs)
+ {
+ BuzzStopMs = 0;
+ BuzzerStop();
+ }
+ }
+}
--- a/ControllerIo.h Thu Jan 07 17:31:23 2016 +0000 +++ b/ControllerIo.h Fri Jan 15 22:02:46 2016 +0000 @@ -7,8 +7,15 @@ #include "mbed.h" +// LEDs extern void ShowLedGreen(); extern void ShowLedRed(); extern void ShowLedBlue(); +// buzzer +extern void BuzzerStart(); +extern void BuzzerStop(); +extern void BuzzerStartMs(int nowMs, int durationMs); +extern void BuzzerPoll(int nowMs); + #endif
--- a/IothubRobotArm.cpp Thu Jan 07 17:31:23 2016 +0000
+++ b/IothubRobotArm.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -11,6 +11,7 @@
#include "iothub_client.h"
#include "iothub_message.h"
+#include "threadapi.h"
#include "crt_abstractions.h"
#include "iothubtransportamqp.h"
#include "MeasureBuf.h"
@@ -96,13 +97,17 @@
int messageTrackingId; // For tracking the messages within the user callback.
} EVENT_INSTANCE;
+
// message buffers to use
-#define MESSAGE_LEN 1024
+#define MESSAGE_LEN 1024
static char msgText[MESSAGE_LEN];
-//static char propText[MESSAGE_LEN];
+
+#define MESSAGE_COUNT 2
+EVENT_INSTANCE messages[MESSAGE_COUNT];
-#define MESSAGE_COUNT 1
-EVENT_INSTANCE messages[MESSAGE_COUNT];
+// send thread poll ms
+#define SEND_POLL_MS 500
+
// context for send & receive
static int receiveContext;
@@ -129,12 +134,14 @@
if (IoTHubMessage_GetByteArray(message, (const unsigned char**)&buffer, &size) == IOTHUB_MESSAGE_OK)
{
- (void)printf("Received Message handle %x with Data: <%.*s> & Size=%d\r\n", message, (int)size, buffer, (int)size);
int slen = size;
+
if (size >= 20)
slen = 19;
strncpy(cmdbuf, buffer, slen);
cmdbuf[slen] = 0;
+ (void)printf(cmdbuf);
+ (void)printf("\r\n");
ControlArm((const char*)cmdbuf);
}
@@ -150,18 +157,12 @@
printf("SendConfirmation with NULL context\r\n");
return;
}
- (void)printf("Confirmation[%d] received for tracking id = %d with result = %d\r\n",
- eventInstance->messageHandle, eventInstance->messageTrackingId, result);
+
+ (void)printf("Confirmation received\r\n");
confirmTimer->stop();
callbackCounter++;
IoTHubMessage_Destroy(eventInstance->messageHandle);
-
- if (callbackCounter == msgNumber)
- {
- // call SendMeasurements again in case more to send
- SendIothubData();
- }
}
// communication timeout
@@ -173,57 +174,59 @@
// IoT Hub thread
-static Thread* IotThread = NULL;
+static THREAD_HANDLE IotThread;
static bool IotThreadClose;
// entry point for ITHub sending thread
-void IothubThread(void const *args)
+int IothubThread(void *args)
{
(void)printf("Iothub thread start\r\n");
IotThreadClose = false;
IothubRobotArm iotRobot;
confirmTimer = new RtosTimer(CommunicationTO, osTimerOnce, (void *)osThreadGetId());
-
- confirmTimer->start(SEND_CONFIRM_TO);
- InitEthernet();
- confirmTimer->stop();
-
+
iotRobot.Init();
+ // wait for connection establishment for SSL
+ ThreadAPI_Sleep(15000);
while (1)
{
- osEvent ev = Thread::signal_wait(IS_SendStatus);
if (IotThreadClose)
{
- (void)printf("Iothub thread close signal\r\n");
+ (void)printf("Iothub thread close\r\n");
iotRobot.Terminate();
break;
}
else
{
- iotRobot.SendMeasurements();
+ iotRobot.SendMessage();
}
+ ThreadAPI_Sleep(SEND_POLL_MS);
}
+
+ return 0;
}
bool StartIothubThread()
{
- IotThread = new Thread(IothubThread, NULL, osPriorityLow);
+ InitEthernet();
+
+ ThreadAPI_Create(&IotThread, IothubThread, NULL);
+
+ //IotThread = new Thread(IothubThread, NULL, osPriorityLow);
return true;
}
bool SendIothubData()
{
- IotThread->signal_set(IS_SendStatus);
return true;
}
void EndIothubThread()
{
IotThreadClose = true;
- IotThread->signal_set(IS_SendStatus);
}
@@ -238,9 +241,6 @@
callbackCounter = 0;
msgNumber = 0;
- // in case calling init twice without terminate
- Terminate();
-
(void)printf("Starting the IoTHub RobotArm sample AMQP...\r\n");
if ((iotHubClientHandle = IoTHubClient_CreateFromConnectionString(connectionString, AMQP_Protocol)) == NULL)
@@ -285,7 +285,7 @@
}
-void IothubRobotArm::SendMeasurements(void)
+void IothubRobotArm::SendMessage(void)
{
// send until circular buf empty or no sending buffers avail
// may drop message if confirmations are slow
@@ -295,12 +295,11 @@
int i = msgNumber % MESSAGE_COUNT;
int msglen = 0;
- bool ismeasure = false;
+
// get alert if any, otherwise get measure data
msglen = msgSerialize.AlertBufToString(msgText, MESSAGE_LEN);
if (msglen == 0)
{
- ismeasure = true;
msglen = msgSerialize.MeasureBufToString(msgText, MESSAGE_LEN);
}
@@ -315,7 +314,6 @@
messages[i].messageTrackingId = msgNumber;
confirmTimer->stop();
-
if (IoTHubClient_SendEventAsync(iotHubClientHandle, messages[i].messageHandle, SendConfirmationCallback, &messages[i]) != IOTHUB_CLIENT_OK)
{
(void)printf("ERROR: IoTHubClient_SendEventAsync..........FAILED!\r\n");
@@ -323,17 +321,14 @@
else
{
confirmTimer->start(SEND_CONFIRM_TO);
- if (ismeasure)
- (void)printf("IoTHubClient_SendEventAsync sending data to IoT Hub. tracking id: %d, bytes: %d\r\n", msgNumber, msglen);
- else
- (void)printf("IoTHubClient_SendEventAsync sending alert to IoT Hub. tracking id: %d, bytes: %d\r\n", msgNumber, msglen);
+ (void)printf("Send async completed\r\n");
}
msgNumber++;
}
}
else if (msglen == 0)
{
- break;
+ break;
}
else if (msglen < 0)
{
--- a/IothubRobotArm.h Thu Jan 07 17:31:23 2016 +0000
+++ b/IothubRobotArm.h Fri Jan 15 22:02:46 2016 +0000
@@ -17,7 +17,7 @@
void Terminate();
- void SendMeasurements(void);
+ void SendMessage(void);
private:
--- a/IothubSerial.cpp Thu Jan 07 17:31:23 2016 +0000
+++ b/IothubSerial.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -2,10 +2,10 @@
#include "IothubSerial.h"
#include "crt_abstractions.h"
-char* nametemp = "temp";
-char* namevolt = "volt";
-char* namedeg = "rot";
-char* nameload = "load";
+const char* nametemp = "temp";
+const char* namevolt = "volt";
+const char* namedeg = "rot";
+const char* nameload = "load";
IothubSerial::IothubSerial()
{
@@ -18,7 +18,7 @@
// eg: temp: [22.0, 23.1, 22.3],
int IothubSerial::MeasureGroupToString(MeasureGroup& mg, char* buf, int bufsize)
{
- char* name;
+ const char* name;
int slen;
int startlen = bufsize;
@@ -44,9 +44,21 @@
return -1;
}
- if (bufsize > strlen(name) + 5)
+ slen = sprintf_s(buf, bufsize, "\"%s\": [", name);
+ if (slen > 0)
{
- slen = sprintf_s(buf, bufsize, "\"%s\": [", name);
+ bufsize -= slen;
+ buf += slen;
+ }
+ else
+ return -1;
+
+ for (int i = 0; i < mg.NumVals; i++)
+ {
+ if (i < mg.NumVals - 1)
+ slen = sprintf_s(buf, bufsize, "%.2f, ", mg.MeasVals[i]);
+ else
+ slen = sprintf_s(buf, bufsize, "%.2f ", mg.MeasVals[i]);
if (slen > 0)
{
bufsize -= slen;
@@ -55,43 +67,16 @@
else
return -1;
}
- else
- return -1;
-
- for (int i = 0; i < mg.NumVals; i++)
+
+ slen = sprintf_s(buf, bufsize, "],");
+ if (slen > 0)
{
- if (bufsize > 9)
- {
- if (i < mg.NumVals - 1)
- slen = sprintf_s(buf, bufsize, "%7.2f, ", mg.MeasVals[i]);
- else
- slen = sprintf_s(buf, bufsize, "%7.2f ", mg.MeasVals[i]);
- if (slen > 0)
- {
- bufsize -= slen;
- buf += slen;
- }
- else
- return -1;
- }
- else
- return -1;
- }
-
- if (bufsize > 2)
- {
- slen = sprintf_s(buf, bufsize, "],");
- if (slen > 0)
- {
- bufsize -= slen;
- buf += slen;
- }
- else
- return -1;
+ bufsize -= slen;
+ buf += slen;
}
else
return -1;
-
+
return startlen - bufsize;
}
@@ -112,9 +97,9 @@
}
// try to serialize one or more measurements into the buffer
-// return bytes used or -1 if buffer too small or other error. 0 if no data
-// current serialization is a json object with entry per measure
-// eg: { "temp": [1, 2], "volt": [12.1, 12.2] }
+// return bytes used / -1 if buffer too small / 0 if no data
+// current serialization is a json array of objects with time and array per measure
+// eg: [{ "time": "2016-01-23T14:55:02", "temp": [1, 2], "volt": [12.1, 12.2] }]
int IothubSerial::MeasureBufToString(char* buf, int bufsize)
{
int slen;
@@ -125,17 +110,14 @@
char* lastcomma = NULL;
time_t secs = 0;
+ // reserve 1 space for end
+ bufsize--;
- if (bufsize > 1)
+ slen = sprintf_s(buf, bufsize, "[{");
+ if (slen > 0)
{
- slen = sprintf_s(buf, bufsize, "{");
- if (slen > 0)
- {
- bufsize -= slen;
- buf += slen;
- }
- else
- return -1;
+ bufsize -= slen;
+ buf += slen;
}
else
return -1;
@@ -210,7 +192,7 @@
copydata = true;
}
else
- break; // no room to serialize, leave pending for mext message
+ break; // no room to serialize, leave pending for next message
}
if (!hasdata)
@@ -221,15 +203,18 @@
// replace final ',' with '}'
*(lastcomma - 1) = '}';
-
+ // this is the extra space we reserved at the start.
+ *(lastcomma) = ']';
+ lastcomma++;
+
return lastcomma - startbuf;
}
// try to serialize one or more alerts into the buffer
// return bytes used or -1 if buffer too small or other error. 0 if no data
-// current serialization is a json object
-// eg: { "alert": "msg", "sev": n }
+// Serialize to a json object with time, measurename, value, message, and index of joint
+// eg: { "alerttype": "Temperature", "message": "too hot", "measurename", "temp", "index": 2, "value": 79.3, "time": "2016-01-23T14:55:02" }
int IothubSerial::AlertBufToString(char* buf, int bufsize)
{
int slen;
@@ -239,8 +224,11 @@
if (AlertBuf.pop(_pendAlert))
{
+ char tbuf[32];
+ (void)strftime(tbuf, 32, "%FT%T", localtime(&_pendAlert.Created));
hasdata = true;
- slen = sprintf_s(buf, bufsize, "{ \"alerttype\": \"%s\", \"alert\": \"%s\", \"sev\": %d }", _pendAlert.AlertType, _pendAlert.Msg, _pendAlert.Sev);
+ slen = sprintf_s(buf, bufsize, "{ \"alerttype\": \"%s\", \"message\": \"%s\", \"measurename\": \"%s\", \"index\": %d , \"value\": %f, \"time\": \"%s\" }",
+ _pendAlert.AlertType, _pendAlert.Msg, _pendAlert.MeasureName, _pendAlert.Index, _pendAlert.Value, tbuf);
if (slen > 0)
{
bufsize -= slen;
--- a/MeasureBuf.cpp Thu Jan 07 17:31:23 2016 +0000
+++ b/MeasureBuf.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -6,12 +6,12 @@
SafeCircBuf<MeasureGroup, MeasureBufSize, uint32_t> MeasureBuf;
-void MeasureGroup::SetMeasure(NodeMeasure mId, time_t created, vector<float>& vals)
+void MeasureGroup::SetMeasure(NodeMeasure mId, time_t created, int numParts, float vals[])
{
MeasId = mId;
Created = created;
- NumVals = MAX_PARTS < vals.size() ? MAX_PARTS : vals.size();
+ NumVals = NUMJOINTS < numParts ? NUMJOINTS : numParts;
for (int ix = 0; ix < NumVals; ix++)
{
@@ -19,3 +19,15 @@
}
}
+MeasureGroup& MeasureGroup::operator=(const MeasureGroup& rhs)
+{
+ MeasId = rhs.MeasId;
+ Created = rhs.Created;
+ NumVals = rhs.NumVals;
+ for (int ix = 0; ix < NUMJOINTS; ix++)
+ {
+ MeasVals[ix] = rhs.MeasVals[ix];
+ }
+ return *this;
+}
+
--- a/MeasureBuf.h Thu Jan 07 17:31:23 2016 +0000
+++ b/MeasureBuf.h Fri Jan 15 22:02:46 2016 +0000
@@ -10,7 +10,7 @@
#include "RobotArm.h"
-#define MeasureBufSize 20
+#define MeasureBufSize 10
class MeasureGroup
@@ -18,13 +18,15 @@
public:
MeasureGroup() {};
- void SetMeasure(NodeMeasure mId, time_t created, vector<float>& vals);
+ MeasureGroup& operator=(const MeasureGroup& rhs);
+
+ void SetMeasure(NodeMeasure mId, time_t created, int numParts, float vals[]);
NodeMeasure MeasId;
int NumVals;
- float MeasVals[MAX_PARTS];
+ float MeasVals[NUMJOINTS];
time_t Created;
};
--- a/RobotArm.cpp Thu Jan 07 17:31:23 2016 +0000
+++ b/RobotArm.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -3,6 +3,7 @@
#include <DynamixelBus.h>
#include <NodeAX12.h>
+//#include <NodeEmul.h>
#include <Terminal.h>
#include <vector>
#include <RobotArm.h>
@@ -13,64 +14,77 @@
// set the id for each part in the chain, in order
-#define NUMPARTS 5
int PartIds[] = { 2, 3, 4, 6, 1 };
-#define FailMsLimit 200
+// default to move every 25 ms
+#define StepPeriodMs 25
+
+// Thresholds
+// allow 3 degrees plus requested move diff for position error
+#define PositionErrorAllow 3.0f
+// time for continuous position error
+#define FailMsLimit 250
+// load level allowance
+#define MaxLoadLimit 950.0f
+// Temperature limit
+#define MaxTemp 69
+// Voltage limits
+#define MaxVoltLimit 13
+#define MinVoltLimit 10
+
RobotArm::RobotArm()
{
// build arm
- for (int i = 0; i < NUMPARTS; i++)
+ for (int ix = 0; ix < NUMJOINTS; ix++)
{
- NodeAX12* servo = new NodeAX12(&dynbus, PartIds[i]);
+ NodeAX12* servo = new NodeAX12(&dynbus, PartIds[ix]);
+ //NodeEmul* servo = new NodeEmul(PartIds[i]);
servo->DoAction(NA_Init, 0.0f);
- _armParts[i] = dynamic_cast<RobotNode*>(servo);
+ _armParts[ix] = dynamic_cast<RobotNode*>(servo);
}
- _numParts = NUMPARTS;
+ _numParts = NUMJOINTS;
numsteps = 0;
- _stepms = 20; // move every 20 ms
- allowance = 2.0f; // allow 2 degree diff for position error
- failms = 0;
+ _stepms = StepPeriodMs;
}
+
void RobotArm::ClearErrorState()
{
- for (int i = 0; i < _numParts; i++)
+ for (int ix = 0; ix < _numParts; ix++)
{
- _armParts[i]->DoAction(NA_ClearError, 0.0f);
+ _armParts[ix]->DoAction(NA_ClearError, 0.0f);
+ failms[ix] = 0;
}
}
// move all parts to specified postions in ms time
-bool RobotArm::MoveArmPositionsStart(vector<float> positions, int totms)
+bool RobotArm::MoveArmPositionsStart(float positions[], int totms)
{
_lastErrorPart = 0;
MoveArmPositionsEnd();
+
GetArmPositions(lastpos);
- lastgoals.clear();
- endgoals.clear();
numsteps = totms / _stepms;
if (numsteps == 0) numsteps = 1;
- differentials.clear();
for (int ix = 0; ix < _numParts; ix++)
{
if (positions[ix] > 0.0f)
{
- endgoals.push_back(positions[ix]);
+ endgoals[ix] = positions[ix];
float difference = (positions[ix] - lastpos[ix]) / (float)numsteps;
- differentials.push_back(difference);
+ differentials[ix] = difference;
}
else
{
// negative goal. Treat as don't move
- endgoals.push_back(lastpos[ix]);
- differentials.push_back(0.0f);
+ differentials[ix] = 0.0f;
}
+ failms[ix] = 0;
}
curstep = 1;
@@ -79,7 +93,45 @@
elapseTimer.start();
expDelay = (int)elapseTimer.read_ms() + delayms;
- failms = 0;
+
+ return true;
+}
+
+// continue interrupted action
+bool RobotArm::MoveArmPositionsResume()
+{
+ if (curstep > numsteps)
+ {
+ // no more steps
+ MoveArmPositionsEnd();
+ return true;
+ }
+ GetArmPositions(lastpos);
+
+ // reset numsteps to be what was remaining
+ numsteps = numsteps - curstep + 1;
+
+ for (int ix = 0; ix < _numParts; ix++)
+ {
+ if (endgoals[ix] > 0.0f)
+ {
+ float difference = (endgoals[ix] - lastpos[ix]) / (float)numsteps;
+ differentials[ix] = difference;
+ }
+ else
+ {
+ // negative goal. Treat as don't move
+ differentials[ix] = 0.0f;
+ }
+ failms[ix] = 0;
+ }
+
+ curstep = 1;
+
+ delayms = _stepms;
+
+ elapseTimer.start();
+ expDelay = (int)elapseTimer.read_ms() + delayms;
return true;
}
@@ -102,15 +154,14 @@
bool ok = true;
- lastgoals.clear();
for (int ix = 0; ix < _numParts; ix++)
{
if (_armParts[ix]->HasAction(NA_Rotate))
{
- float goal = (curstep == numsteps) ?
+ float goal = (curstep == numsteps || differentials[ix] == 0.0f) ?
endgoals[ix] : // last step - use actual goal
(lastpos[ix] + (differentials[ix] * (float)curstep));
- lastgoals.push_back(goal);
+ lastgoals[ix] = goal;
if (differentials[ix] != 0.0f)
{
bool ok = _armParts[ix]->DoAction(NA_Rotate, goal);
@@ -170,51 +221,19 @@
return true;
}
-bool RobotArm::MoveArmPositionTest()
+// set goal to current position
+// prevents jump when obstruction is removed
+void RobotArm::MoveArmPositionsStop()
{
- vector<float> curpos;
+ float curpos[NUMJOINTS];
GetArmPositions(curpos);
+
for (int ix = 0; ix < _numParts; ix++)
{
- if (curpos[ix] > 0 && lastgoals.size() > ix)
- {
- float diff = fabs(curpos[ix] - lastgoals[ix]);
- if (diff > (fabs(differentials[ix] * 2.0f) + allowance))
- {
- printf("Bad position for %d. Expect %f - got %f\r\n", ix, lastgoals[ix], curpos[ix]);
- _lastPosDiff = diff;
- _lastErrorPart = ix;
- _lastError = 0;
+ (void)_armParts[ix]->DoAction(NA_Rotate, curpos[ix]);
+ }
+}
- int elapsed = (int)elapseTimer.read_ms();
- if (failms > 0)
- {
- if (elapsed - failms > FailMsLimit)
- {
- // continuous failure for time period
- // return false
- lastgoals.clear();
- failms = 0;
- return false;
- }
- // within time period. Do not report failure
- return true;
- }
- else
- {
- // first failure after success
- // remember time. Do not report failure
- failms = elapsed;
- return true;
- }
- }
- }
- }
- // success
- failms = 0;
- return true;
-}
-
bool RobotArm::MoveArmPositionsEnd()
{
if (numsteps > 0)
@@ -225,16 +244,114 @@
return true;
}
+extern Timer IdleTimer;
+int RobotArm::ArmMeasuresTest(int measureId)
+{
+ float curvals[NUMJOINTS];
+
+ if (!GetArmMeasure(measureId, curvals))
+ {
+ return -2;
+ }
+
+ int rc = 0;
+
+ switch (measureId)
+ {
+ case NM_Temperature:
+ for (int ix = 0; ix < _numParts; ix++)
+ {
+ float val = curvals[ix];
+ if (val > MaxTemp)
+ {
+ _lastErrorPart = ix;
+ rc = -1;
+ break;
+ }
+ }
+ break;
+
+ case NM_Degrees:
+ for (int ix = 0; ix < _numParts; ix++)
+ {
+ float val = curvals[ix];
+ if (val > 0.0f)
+ {
+ float diff = fabs(val - lastgoals[ix]);
+ if (diff > (fabs(differentials[ix] * 2.0f) + PositionErrorAllow))
+ {
+ int elapsed = (int)elapseTimer.read_ms();
+ if (failms[ix] > 0)
+ {
+ if (elapsed - failms[ix] > FailMsLimit)
+ {
+ // continuous failure for time period
+ // report failure
+ _lastPosDiff = diff;
+ _lastErrorPart = ix;
+
+ failms[ix] = 0;
+ rc = -1;
+ }
+ }
+ else
+ {
+ // first failure after success
+ // remember first fail time.
+ failms[ix] = elapsed;
+ }
+ }
+ else
+ {
+ // within allowable range - clear time
+ failms[ix] = 0;
+ }
+ }
+ }
+ break;
+
+ case NM_Voltage:
+ for (int ix = 0; ix < _numParts; ix++)
+ {
+ float val = curvals[ix];
+ if (val > MaxVoltLimit || val < MinVoltLimit)
+ {
+ _lastErrorPart = ix;
+ rc = -1;
+ break;
+ }
+ }
+ break;
+
+ case NM_Load:
+ for (int ix = 0; ix < _numParts; ix++)
+ {
+ float val = curvals[ix];
+ if (val > MaxLoadLimit)
+ {
+ _lastErrorPart = ix;
+ rc = -1;
+ break;
+ }
+ }
+ break;
+
+
+ default:
+ break;
+ }
+
+ return rc;
+}
// get all parts positions
-bool RobotArm::GetArmPositions(vector<float>& outPos)
+bool RobotArm::GetArmPositions(float outPos[])
{
bool ok = true;
- outPos.clear();
for (int ix = 0; ix < _numParts; ix++)
{
float pos = _armParts[ix]->GetMeasure(NM_Degrees);
- outPos.push_back(pos);
+ outPos[ix] = pos;
if (_armParts[ix]->HasError())
{
_lastErrorPart = ix;
@@ -246,14 +363,13 @@
}
// get all parts last measured positions
-bool RobotArm::GetArmLastPositions(vector<float>& outPos)
+bool RobotArm::GetArmLastPositions(float outPos[])
{
bool ok = true;
- outPos.clear();
for (int ix = 0; ix < _numParts; ix++)
{
float pos = _armParts[ix]->GetLastMeasure(NM_Degrees);
- outPos.push_back(pos);
+ outPos[ix] = pos;
if (_armParts[ix]->HasError())
{
_lastErrorPart = ix;
@@ -265,14 +381,13 @@
}
// get all parts measurements
-bool RobotArm::GetArmMeasure(int measureId, vector<float>& outVals)
+bool RobotArm::GetArmMeasure(int measureId, float outVals[])
{
bool ok = true;
- outVals.clear();
for (int ix = 0; ix < _numParts; ix++)
{
float val = _armParts[ix]->GetMeasure(measureId);
- outVals.push_back(val);
+ outVals[ix] = val;
if (_armParts[ix]->HasError())
{
_lastErrorPart = ix;
@@ -284,14 +399,13 @@
}
// get all parts last measurements
-bool RobotArm::GetArmLastMeasure(int measureId, vector<float>& outVals)
+bool RobotArm::GetArmLastMeasure(int measureId, float outVals[])
{
bool ok = true;
- outVals.clear();
for (int ix = 0; ix < _numParts; ix++)
{
float val = _armParts[ix]->GetLastMeasure(measureId);
- outVals.push_back(val);
+ outVals[ix] = val;
if (_armParts[ix]->HasError())
{
_lastErrorPart = ix;
@@ -339,7 +453,3 @@
return _lastErrorPart;
}
-void RobotArm::SetAllowance(float allow)
-{
- allowance = allow;
-}
--- a/RobotArm.h Thu Jan 07 17:31:23 2016 +0000
+++ b/RobotArm.h Fri Jan 15 22:02:46 2016 +0000
@@ -11,7 +11,8 @@
#include "DynamixelBus.h"
#include "NodeAX12.h"
-#define MAX_PARTS 8
+// define number of joints for this arm
+#define NUMJOINTS 5
enum ArmAction
@@ -43,38 +44,44 @@
RobotArm();
- // start - move all parts to specified postions in ms time
- bool MoveArmPositionsStart(vector<float> positions, int ms);
+ // move all parts to specified postions in ms time - start
+ bool MoveArmPositionsStart(float positions[], int ms);
+
+ // move all parts to specified postions - resume after pause
+ bool MoveArmPositionsResume();
- // start - move all parts to specified postions - test if done
+ // move all parts to specified postions - test if done
bool MoveArmPositionsHasNext();
- // start - move all parts to specified postions - next step
+ // move all parts to specified postions - next step
bool MoveArmPositionsNext();
- // start - move all parts to specified postions - next delay
+ // move all parts to specified postions - next step delay
bool MoveArmPositionsDelay(int& nextdelay);
- // start - move all parts to specified postions - finish
+ // move all parts to specified postions - finish
bool MoveArmPositionsEnd();
- // start - test if positions are close to expected
- bool MoveArmPositionTest();
+ // reset goal to the current position ie. stop trying to move
+ void MoveArmPositionsStop();
// clear part error state
void ClearErrorState();
// get all parts positions
- bool GetArmPositions(vector<float>& outPos);
+ bool GetArmPositions(float outVals[]);
// get all parts last positions
- bool GetArmLastPositions(vector<float>& outPos);
+ bool GetArmLastPositions(float outVals[]);
+
+ // test if measurements are in expected range
+ int ArmMeasuresTest(int measureId);
// get all parts for a measurement
- bool GetArmMeasure(int measureId, vector<float>& outPos);
+ bool GetArmMeasure(int measureId, float outVals[]);
// get all parts last measurement
- bool GetArmLastMeasure(int measureId, vector<float>& outPos);
+ bool GetArmLastMeasure(int measureId, float outVals[]);
int GetNumParts();
@@ -96,12 +103,9 @@
// get size of position diff (valid if error)
float GetLastPosDiff();
- // set allwable position diff
- void SetAllowance(float allow);
-
private:
// sensors and actuators
- RobotNode* _armParts[MAX_PARTS];
+ RobotNode* _armParts[NUMJOINTS];
int _numParts;
@@ -119,15 +123,15 @@
float _lastPosDiff;
// step-wise position moves
- vector<float> endgoals;
- vector<float> differentials;
- vector<float> lastpos;
- vector<float> lastgoals;
+ float endgoals[NUMJOINTS];
+ float differentials[NUMJOINTS];
+ float lastpos[NUMJOINTS];
+ float lastgoals[NUMJOINTS];
// allowance for difference between expected pos and actual pos
float allowance;
// keep track of time period when position is off
- int failms;
+ int failms[NUMJOINTS];
int numsteps;
int curstep;
--- a/RobotNode/NodeAX12.cpp Thu Jan 07 17:31:23 2016 +0000
+++ b/RobotNode/NodeAX12.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -6,7 +6,7 @@
NodeAX12::NodeAX12(DynamixelBus* pbus, ServoId ID) :
_Servo(pbus, ID)
{
- _LastPosition= 0.0f;
+ _LastPosition= 0;
_LastTemperature = 0;
_LastVoltage = 0;
_LastLoad = 0;
@@ -60,8 +60,11 @@
case NM_Load:
val = (float)_Servo.GetLoad();
if (val != 0.0f)
+ {
_LastLoad = val;
+ }
return val;
+
default:
return 0.0f;
@@ -83,8 +86,8 @@
case NM_Load:
return _LastLoad;
-
- default:
+
+ default:
return 0.0f;
}
}
--- a/RobotNode/NodeAX12.h Thu Jan 07 17:31:23 2016 +0000
+++ b/RobotNode/NodeAX12.h Fri Jan 15 22:02:46 2016 +0000
@@ -44,6 +44,7 @@
// last read load
float _LastLoad;
+
};
#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotNode/NodeEmul.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -0,0 +1,121 @@
+
+#include "mbed.h"
+#include "NodeEmul.h"
+#include "threadapi.h"
+
+// emulated device that does nothing
+
+NodeEmul::NodeEmul(int id)
+{
+ _LastPosition= 20.0f + id;
+ _LastTemperature = 30 + id;
+ _LastVoltage = 12 + ((float)id * 0.1f);
+ _LastLoad = 9 + id;
+}
+
+
+bool NodeEmul::HasMeasure(int measureId)
+{
+ switch (measureId)
+ {
+ case NM_Temperature:
+ return true;
+
+ case NM_Degrees:
+ return true;
+
+ case NM_Voltage:
+ return true;
+
+ case NM_Load:
+ return true;
+
+ default:
+ return false;
+ }
+}
+
+float NodeEmul::GetMeasure(int measureId)
+{
+ return GetLastMeasure(measureId);
+}
+
+float NodeEmul::GetLastMeasure(int measureId)
+{
+ switch (measureId)
+ {
+ case NM_Temperature:
+ return (float)_LastTemperature;
+
+ case NM_Degrees:
+ return _LastPosition;
+
+ case NM_Voltage:
+ return _LastVoltage;
+
+ case NM_Load:
+ return _LastLoad;
+
+ default:
+ return 0.0f;
+ }
+}
+
+bool NodeEmul::HasAction(int actionId)
+{
+ switch (actionId)
+ {
+ case NA_Ping:
+ return true;
+
+ case NA_Init:
+ return true;
+
+ case NA_Rotate:
+ return true;
+
+ case NA_ClearError:
+ return true;
+
+ default:
+ return false;
+ }
+}
+
+
+bool NodeEmul::DoAction(int actionId, float actionValue)
+{
+ switch (actionId)
+ {
+ case NA_Ping:
+ return true;
+
+ case NA_Init:
+ return true;
+
+ case NA_Rotate:
+ _LastPosition = actionValue;
+ return true;
+
+ case NA_ClearError:
+ return true;
+
+ default:
+ return false;
+ }
+}
+
+NodePartType NodeEmul::GetNodeType()
+{
+ return NT_Emul;
+}
+
+int NodeEmul::GetLastError()
+{
+ return 0;
+}
+
+bool NodeEmul::HasError()
+{
+ return false;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotNode/NodeEmul.h Fri Jan 15 22:02:46 2016 +0000
@@ -0,0 +1,48 @@
+/*
+Copyright (c) 2015 Jonathan Pickett & Microsoft. Some appropriate open source license.
+*/
+
+#ifndef __NODE_Emul_H__
+#define __NODE_Emul_H__
+
+#include "RobotNode.h"
+
+
+
+class NodeEmul : public RobotNode
+{
+public:
+ NodeEmul(int id);
+
+ virtual bool HasMeasure(int measureId);
+
+ virtual float GetMeasure(int measureId);
+
+ virtual float GetLastMeasure(int measureId);
+
+ virtual bool HasAction(int actionId);
+
+ virtual bool DoAction(int actionId, float actionValue);
+
+ virtual int GetLastError();
+
+ virtual bool HasError();
+
+ virtual NodePartType GetNodeType();
+
+private:
+
+ // last read temperature
+ int _LastTemperature;
+
+ // last read position
+ float _LastPosition;
+
+ // last read voltage
+ float _LastVoltage;
+
+ // last read load
+ float _LastLoad;
+};
+
+#endif
--- a/RobotNode/RobotNode.h Thu Jan 07 17:31:23 2016 +0000
+++ b/RobotNode/RobotNode.h Fri Jan 15 22:02:46 2016 +0000
@@ -23,7 +23,8 @@
enum NodePartType
{
- NT_AX12 = 0x1
+ NT_AX12 = 0x1,
+ NT_Emul = 0x7f
};
--- a/Sequences.cpp Thu Jan 07 17:31:23 2016 +0000
+++ b/Sequences.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -6,101 +6,85 @@
const float RightPos = 90.0f;
const float NoMove = -1.0f;
-// this must have same value as in RobotArm
-#define NUMPARTS 5
+
+vector<ActionSequence*> UpDownSeq;
+vector<ActionSequence*> UpTwistSeq;
+vector<ActionSequence*> StartSeq;
+vector<ActionSequence*> WaveSeq;
+vector<ActionSequence*> TapsSeq;
+vector<ActionSequence*> BigWaveSeq;
-vector<ActionSequence> UpDownSeq;
-vector<ActionSequence> UpTwistSeq;
-vector<ActionSequence> StartSeq;
-vector<ActionSequence> WaveSeq;
-vector<ActionSequence> TapsSeq;
-vector<ActionSequence> FastWaveSeq;
+const float upPositions[] = { UpPos, UpPos, UpPos, UpPos, UpPos };
+const float downPositions[] = { RightPos, 120.0f, 240.0f, 150.0f, 240.0f };
+const float homePositions[] = { RightPos, 225.0f, 270.0f, 135.0f, UpPos };
+const float waveUpPositions[] = { NoMove, NoMove, NoMove, NoMove, 135.0f };
+const float waveDownPositions[] = { NoMove, NoMove, NoMove, NoMove, 225.0f };
+const float waveMiddlePositions[] = { NoMove, NoMove, NoMove, NoMove, 180.0f };
+const float bigwaveUpPositions[] = { NoMove, 135.0f, NoMove, NoMove, NoMove };
+const float bigwaveDownPositions[] = { NoMove, 225.0f, NoMove, NoMove, NoMove };
+
+const float tapStartPositions[] = { RightPos, 180.0f, 270.0f, 180.0f, 180.0f };
+const float tap1Positions[] = { 60.2f, NoMove, NoMove, NoMove, NoMove };
+const float tap2Positions[] = { 68.7f, NoMove, NoMove, NoMove, NoMove };
+const float tap3Positions[] = { 77.2f, NoMove, NoMove, NoMove, NoMove };
+const float tap4Positions[] = { 85.7f, NoMove, NoMove, NoMove, NoMove };
+const float tap5Positions[] = { 94.2f, NoMove, NoMove, NoMove, NoMove };
+const float tap6Positions[] = { 102.7f, NoMove, NoMove, NoMove, NoMove };
+const float tap7Positions[] = { 111.2f, NoMove, NoMove, NoMove, NoMove };
+const float tap8Positions[] = { 119.7f, NoMove, NoMove, NoMove, NoMove };
+
-void MakeSequences(int partSize, vector<float>& startPositions)
+void MakeSequences(int partSize)
{
- float upPositions[] = { UpPos, UpPos, UpPos, UpPos, UpPos };
- float downPositions[] = { RightPos, 120.0f, 240.0f, 150.0f, 240.0f };
- float homePositions[] = { RightPos, 225.0f, 270.0f, 135.0f, UpPos };
- float rightPositions[] = { UpPos, UpPos, UpPos, UpPos, UpPos };
- float waveUpPositions[] = { NoMove, NoMove, NoMove, NoMove, 135.0f };
- float waveDownPositions[] = { NoMove, NoMove, NoMove, NoMove, 225.0f };
- float waveMiddlePositions[] = { NoMove, NoMove, NoMove, NoMove, 180.0f };
- float fastwaveUpPositions[] = { NoMove, 135.0f, NoMove, NoMove, NoMove };
- float fastwaveDownPositions[] = { NoMove, 225.0f, NoMove, NoMove, NoMove };
-
- float tapStartPositions[] = { RightPos, 180.0f, 270.0f, 180.0f, 180.0f };
- float tap1Positions[] = { 60.2f, NoMove, NoMove, NoMove, NoMove };
- float tap2Positions[] = { 68.7f, NoMove, NoMove, NoMove, NoMove };
- float tap3Positions[] = { 77.2f, NoMove, NoMove, NoMove, NoMove };
- float tap4Positions[] = { 85.7f, NoMove, NoMove, NoMove, NoMove };
- float tap5Positions[] = { 94.2f, NoMove, NoMove, NoMove, NoMove };
- float tap6Positions[] = { 102.7f, NoMove, NoMove, NoMove, NoMove };
- float tap7Positions[] = { 111.2f, NoMove, NoMove, NoMove, NoMove };
- float tap8Positions[] = { 119.7f, NoMove, NoMove, NoMove, NoMove };
-
// define actions
- ActionSequence moveStart(SA_SetGoal, partSize, homePositions, 1500);
- ActionSequence moveUp(SA_SetGoal, partSize, upPositions, 1000);
- ActionSequence moveDown(SA_SetGoal, partSize, downPositions, 1000);
- ActionSequence waveUp(SA_SetGoal, partSize, waveUpPositions, 1000);
- ActionSequence waveDown(SA_SetGoal, partSize, waveDownPositions, 1000);
- ActionSequence fastwaveUp(SA_SetGoal, partSize, fastwaveUpPositions, 400);
- ActionSequence fastwaveDown(SA_SetGoal, partSize, fastwaveDownPositions, 400);
- ActionSequence tapsStart(SA_SetGoal, partSize, tapStartPositions, 1000);
- ActionSequence taps1(SA_SetGoal, partSize, tap1Positions, 200);
- ActionSequence taps2(SA_SetGoal, partSize, tap2Positions, 200);
- ActionSequence taps3(SA_SetGoal, partSize, tap3Positions, 200);
- ActionSequence taps4(SA_SetGoal, partSize, tap4Positions, 200);
- ActionSequence taps5(SA_SetGoal, partSize, tap5Positions, 200);
- ActionSequence taps6(SA_SetGoal, partSize, tap6Positions, 200);
- ActionSequence taps7(SA_SetGoal, partSize, tap7Positions, 200);
- ActionSequence taps8(SA_SetGoal, partSize, tap8Positions, 200);
- ActionSequence tapDown(SA_SetGoal, partSize, waveDownPositions, 80);
- ActionSequence tapUp(SA_SetGoal, partSize, waveMiddlePositions, 80);
+ ActionSequence* moveStart = new ActionSequence(SA_SetGoal, partSize, homePositions, 1500);
+ ActionSequence* moveUp = new ActionSequence(SA_SetGoal, partSize, upPositions, 1500);
+ ActionSequence* moveDown = new ActionSequence(SA_SetGoal, partSize, downPositions, 1000);
+ ActionSequence* waveUp = new ActionSequence(SA_SetGoal, partSize, waveUpPositions, 1000);
+ ActionSequence* waveDown = new ActionSequence(SA_SetGoal, partSize, waveDownPositions, 1000);
+ ActionSequence* bigwaveUp = new ActionSequence(SA_SetGoal, partSize, bigwaveUpPositions, 1000);
+ ActionSequence* bigwaveDown = new ActionSequence(SA_SetGoal, partSize, bigwaveDownPositions, 1000);
+ ActionSequence* tapsStart = new ActionSequence(SA_SetGoal, partSize, tapStartPositions, 1000);
+ ActionSequence* taps1 = new ActionSequence(SA_SetGoal, partSize, tap1Positions, 300);
+ ActionSequence* taps2 = new ActionSequence(SA_SetGoal, partSize, tap2Positions, 300);
+ ActionSequence* taps3 = new ActionSequence(SA_SetGoal, partSize, tap3Positions, 300);
+ ActionSequence* taps4 = new ActionSequence(SA_SetGoal, partSize, tap4Positions, 300);
+ ActionSequence* taps5 = new ActionSequence(SA_SetGoal, partSize, tap5Positions, 300);
+ ActionSequence* taps6 = new ActionSequence(SA_SetGoal, partSize, tap6Positions, 300);
+ ActionSequence* taps7 = new ActionSequence(SA_SetGoal, partSize, tap7Positions, 300);
+ ActionSequence* taps8 = new ActionSequence(SA_SetGoal, partSize, tap8Positions, 300);
+ ActionSequence* tapDown = new ActionSequence(SA_SetGoal, partSize, waveDownPositions, 500);
+ ActionSequence* tapUp = new ActionSequence(SA_SetGoal, partSize, waveMiddlePositions, 500);
- ActionSequence report(SA_Status);
- ActionSequence pause2(SA_Delay);
- pause2.SetParam(2000);
- ActionSequence loop20(SA_LoopBegin);
- loop20.SetParam(20);
- ActionSequence loop2(SA_LoopBegin);
- loop2.SetParam(2);
- ActionSequence loopEnd(SA_LoopEnd);
- ActionSequence pause100ms(SA_Delay);
- pause100ms.SetParam(100);
- ActionSequence pause200ms(SA_Delay);
- pause200ms.SetParam(200);
- ActionSequence pause500ms(SA_Delay);
- pause500ms.SetParam(500);
- ActionSequence pause1(SA_Delay);
- pause1.SetParam(1000);
+ ActionSequence* pause2 = new ActionSequence(SA_Delay, 0 , NULL, 2000);
+ ActionSequence* loop20 = new ActionSequence(SA_LoopBegin, 0 , NULL, 20);
+ ActionSequence* loop2 = new ActionSequence(SA_LoopBegin, 0 , NULL, 2);
+ ActionSequence* loopEnd = new ActionSequence(SA_LoopEnd);
+ ActionSequence* pause100ms = new ActionSequence(SA_Delay, 0 , NULL, 100);
+ ActionSequence* pause200ms = new ActionSequence(SA_Delay, 0 , NULL, 200);
+ ActionSequence* pause500ms = new ActionSequence(SA_Delay, 0 , NULL, 500);
+ ActionSequence* pause1 = new ActionSequence(SA_Delay, 0 , NULL, 1000);
// add actions into StartSeq
StartSeq.clear();
StartSeq.push_back(moveUp);
- StartSeq.push_back(report);
// add actions into WaveSeq
WaveSeq.clear();
WaveSeq.push_back(moveStart);
WaveSeq.push_back(loop2);
WaveSeq.push_back(waveUp);
- WaveSeq.push_back(report);
WaveSeq.push_back(waveDown);
- WaveSeq.push_back(report);
WaveSeq.push_back(loopEnd);
WaveSeq.push_back(moveUp);
// add actions into UpDownSeq
UpDownSeq.clear();
UpDownSeq.push_back(moveUp);
- UpDownSeq.push_back(report);
UpDownSeq.push_back(pause1);
UpDownSeq.push_back(moveDown);
- UpDownSeq.push_back(report);
+ UpDownSeq.push_back(pause1);
UpDownSeq.push_back(moveUp);
- UpDownSeq.push_back(pause1);
- UpDownSeq.push_back(report);
// add actions into RightStepsSeq
// move to vertical, then move each part one at a time, then return to home
@@ -109,21 +93,20 @@
for (int partIx = 0; partIx < partSize; partIx++)
{
+ float pos[NUMJOINTS];
for (int partIy = 0; partIy < partSize; partIy++)
{
if (partIx == partIy)
- rightPositions[partIy] = RightPos;
+ pos[partIy] = RightPos;
else
- rightPositions[partIy] = UpPos;
+ pos[partIy] = UpPos;
}
- ActionSequence* seq = new ActionSequence(SA_SetGoal, partSize, rightPositions, 1000);
- UpTwistSeq.push_back(*seq);
+ ActionSequence* seq = new ActionSequence(SA_SetGoal, partSize, pos, 1000);
+ UpTwistSeq.push_back(seq);
UpTwistSeq.push_back(pause1);
- UpTwistSeq.push_back(report);
}
UpTwistSeq.push_back(moveUp);
- UpTwistSeq.push_back(report);
// add actions to tap sequence
TapsSeq.clear();
@@ -156,18 +139,16 @@
TapsSeq.push_back(taps7);
TapsSeq.push_back(tapDown);
TapsSeq.push_back(tapUp);
- TapsSeq.push_back(report);
TapsSeq.push_back(moveUp);
- // add actions into FastWaveSeq
- FastWaveSeq.clear();
- FastWaveSeq.push_back(moveUp);
- FastWaveSeq.push_back(loop20);
- FastWaveSeq.push_back(fastwaveUp);
- FastWaveSeq.push_back(pause100ms);
- FastWaveSeq.push_back(fastwaveDown);
- FastWaveSeq.push_back(pause100ms);
- FastWaveSeq.push_back(loopEnd);
- FastWaveSeq.push_back(report);
-
+ // add actions into BigWaveSeq
+ BigWaveSeq.clear();
+ BigWaveSeq.push_back(moveUp);
+ BigWaveSeq.push_back(loop20);
+ BigWaveSeq.push_back(bigwaveUp);
+ BigWaveSeq.push_back(pause100ms);
+ BigWaveSeq.push_back(bigwaveDown);
+ BigWaveSeq.push_back(pause100ms);
+ BigWaveSeq.push_back(loopEnd);
+
}
--- a/Sequences.h Thu Jan 07 17:31:23 2016 +0000 +++ b/Sequences.h Fri Jan 15 22:02:46 2016 +0000 @@ -5,13 +5,13 @@ #ifndef __SEQUENCES_H__ #define __SEQUENCES_H__ -extern vector<ActionSequence> UpDownSeq; -extern vector<ActionSequence> UpTwistSeq; -extern vector<ActionSequence> StartSeq; -extern vector<ActionSequence> WaveSeq; -extern vector<ActionSequence> TapsSeq; -extern vector<ActionSequence> FastWaveSeq; +extern vector<ActionSequence*> UpDownSeq; +extern vector<ActionSequence*> UpTwistSeq; +extern vector<ActionSequence*> StartSeq; +extern vector<ActionSequence*> WaveSeq; +extern vector<ActionSequence*> TapsSeq; +extern vector<ActionSequence*> BigWaveSeq; -void MakeSequences(int partSize, vector<float>& homePositions); +void MakeSequences(int partSize); #endif
--- a/iothub_client.lib Thu Jan 07 17:31:23 2016 +0000 +++ b/iothub_client.lib Fri Jan 15 22:02:46 2016 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/AzureIoTClient/code/iothub_client/#02a02ab6402f +http://mbed.org/users/AzureIoTClient/code/iothub_client/#8bc251cad5f6
--- a/main.cpp Thu Jan 07 17:31:23 2016 +0000
+++ b/main.cpp Fri Jan 15 22:02:46 2016 +0000
@@ -9,7 +9,7 @@
Terminal pc(USBTX, USBRX);
-
+extern void PrepareController();
extern void RunController();
extern bool StartIothubThread();
@@ -32,6 +32,9 @@
ShowLedGreen();
+ // prepare the motion sequences
+ PrepareController();
+
// start IotHub connection
StartIothubThread();
@@ -41,7 +44,9 @@
pc.printf("Initialization done. Ready to run. \r\n");
-
+ // try running this thread at a higher priority
+ osThreadId maintid = osThreadGetId();
+ osThreadSetPriority(maintid, osPriorityHigh);
+
RunController();
-
}