demo project
Dependencies: AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL
Diff: main.cpp
- Revision:
- 5:36916b1c5a06
- Parent:
- 4:36a4eceb1b7f
- Child:
- 7:6723f6887d00
--- a/main.cpp Wed Dec 23 18:34:06 2015 +0000 +++ b/main.cpp Mon Dec 28 17:19:37 2015 +0000 @@ -13,6 +13,7 @@ #include <MeasureBuf.h> #include <IothubRobotArm.h> #include <ActionBuf.h> +#include <Sequences.h> using namespace std; @@ -22,7 +23,7 @@ extern Terminal* RobotArmPc; extern Terminal* AX12Pc; -const float UpPos = 150.0f; +const float UpPos = 180.0f; void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm) @@ -129,11 +130,11 @@ void MakeDemoSeq(vector<ActionSequence>& actions) { // define actions - ActionSequence moveUp(SA_SetGoal, upPositions, 3000); + ActionSequence moveUp(SA_SetGoal, upPositions, 2500); ActionSequence report(SA_Status); ActionSequence pause(SA_Delay); - pause.SetDelay(5000); - ActionSequence moveDown(SA_SetGoal, homePositions, 3000); + pause.SetDelay(2000); + ActionSequence moveDown(SA_SetGoal, homePositions, 2500); ActionSequence rep(SA_Repeat); // add actions into a sequence @@ -160,12 +161,51 @@ }; Queue<vector<ActionSequence>, 16> SequenceQ; -MainStates MainState; +volatile MainStates MainState; bool RunInSequence; bool RunInMoveStep; bool RunInDelay; void* CancelToMatch; +osThreadId mainTid; + +void ControlArm(const char* cmd) +{ + if (strncmp(cmd, "pause", 5) == 0) + { + pc.printf( "Pause cmd\r\n"); + MainState = MS_Paused; + osSignalSet(mainTid, AS_Action); + } + else if (strncmp(cmd, "resume", 6) == 0) + { + pc.printf( "Resume cmd\r\n"); + MainState = MS_Running; + osSignalSet(mainTid, AS_Action); + } + else if (strncmp(cmd, "runupdown", 9) == 0) + { + pc.printf( "runupdown cmd\r\n"); + MainState = MS_CancelToM; + CancelToMatch = &UpDownSeq; + SequenceQ.put(&UpDownSeq); + osSignalSet(mainTid, AS_Action); + } + else if (strncmp(cmd, "runuptwist", 10) == 0) + { + pc.printf( "runuptwist cmd\r\n"); + MainState = MS_CancelOne; + CancelToMatch = &UpTwistSeq; + SequenceQ.put(&UpTwistSeq); + osSignalSet(mainTid, AS_Action); + } + else if (strncmp(cmd, "cancelone", 9) == 0) + { + pc.printf( "CancelOne cmd\r\n"); + MainState = MS_CancelOne; + osSignalSet(mainTid, AS_Action); + } +} // run sequence thread timer routine void NextSeq(void const * tid) @@ -203,15 +243,14 @@ SendIothubMeasurements(); // Prepare main thread - osThreadId mainTid = osThreadGetId(); + mainTid = osThreadGetId(); int32_t signals = AS_Action; - // create a sequence for demo - vector<ActionSequence> actions; - MakeDemoSeq(actions); - + // create sequences + MakeSequences(partSize, homePositions); + // add to queue - SequenceQ.put(&actions); + SequenceQ.put(&UpDownSeq); // state for sequence vector<ActionSequence> *curseq = NULL; @@ -345,9 +384,15 @@ if (!RunInMoveStep) { pc.printf( "Next Seq %d\r\n", curseqIx); - + if (curseq != NULL) { + if (curseqIx >= curseq->size()) + { + RunInSequence = false; + break; + } + ActionSequence aseq = (*curseq)[curseqIx]; curseqIx++; @@ -391,11 +436,6 @@ } } - - if (curseqIx >= curseq->size()) - { - RunInSequence = false; - } } }