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:
- 11:3a2e6eb9fbb8
- Parent:
- 10:9b21566a5ddb
- Child:
- 13:ffeff9b5e513
--- a/ArmController.cpp Wed Jan 06 00:58:41 2016 +0000 +++ b/ArmController.cpp Wed Jan 06 22:25:51 2016 +0000 @@ -40,7 +40,8 @@ int LastSendMs = 0; Queue<vector<ActionSequence>, 16> SequenceQ; -volatile MainStates MainState; +MainStates MainState; +bool ErrorReset; bool RunInSequence; bool RunInMoveStep; bool RunInDelay; @@ -175,6 +176,11 @@ void ControlArm(const char* cmd) { + if (MainState == MS_Error) + { + ErrorReset = true; + } + if (strncmp(cmd, "pause", 5) == 0) { ShowLedBlue(); @@ -196,42 +202,42 @@ else if (strncmp(cmd, "runupdown", 9) == 0) { ShowLedGreen(); - MainState = MS_Running; + MainState = MS_CancelOne; SequenceQ.put(&UpDownSeq); osSignalSet(mainTid, AS_Action); } else if (strncmp(cmd, "runuptwist", 10) == 0) { ShowLedGreen(); - MainState = MS_Running; + MainState = MS_CancelOne; SequenceQ.put(&UpTwistSeq); osSignalSet(mainTid, AS_Action); } else if (strncmp(cmd, "runhome", 10) == 0) { ShowLedGreen(); - MainState = MS_Running; + MainState = MS_CancelOne; SequenceQ.put(&StartSeq); osSignalSet(mainTid, AS_Action); } else if (strncmp(cmd, "runwave", 10) == 0) { ShowLedGreen(); - MainState = MS_Running; + MainState = MS_CancelOne; SequenceQ.put(&WaveSeq); osSignalSet(mainTid, AS_Action); } else if (strncmp(cmd, "runtaps1", 10) == 0) { ShowLedGreen(); - MainState = MS_Running; + MainState = MS_CancelOne; SequenceQ.put(&TapsSeq); osSignalSet(mainTid, AS_Action); } else if (strncmp(cmd, "fastwave", 8) == 0) { ShowLedGreen(); - MainState = MS_Running; + MainState = MS_CancelOne; SequenceQ.put(&FastWaveSeq); osSignalSet(mainTid, AS_Action); } @@ -266,7 +272,8 @@ RunInSequence = false; RunInMoveStep = false; RunInDelay = false; - + ErrorReset = false; + // get initial positions MeasureGroup measureGroup; robotArm.GetArmPositions(initPositions); @@ -305,6 +312,12 @@ { osEvent mainEvent = osSignalWait(signals, osWaitForever); + if (ErrorReset) + { + ErrorReset = false; + robotArm.ClearErrorState(); + } + switch (MainState) { case MS_Idle: