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:
- 7:6723f6887d00
- Parent:
- 5:36916b1c5a06
- Child:
- 8:d98e2dec0f40
diff -r feb0a6311594 -r 6723f6887d00 main.cpp --- a/main.cpp Mon Dec 28 17:29:12 2015 +0000 +++ b/main.cpp Tue Dec 29 23:31:28 2015 +0000 @@ -5,62 +5,17 @@ #include "mbed/logging.h" #include "mbed/mbedtime.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> + using namespace std; DigitalOut myled(LED_GREEN); Terminal pc(USBTX, USBRX); -extern Terminal* RobotArmPc; -extern Terminal* AX12Pc; -const float UpPos = 180.0f; - - -void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm) -{ - vector<float> lastVals; - - robotArm.GetArmMeasure(NM_Temperature, lastVals); - pc.printf( "Temperatures: "); - for( int servoIndex = 0; servoIndex < partSize; servoIndex++ ) - { - pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]); - } - pc.printf( "\r\n"); - measureGroup.SetMeasure(NM_Temperature, lastVals); - MeasureBuf.push(measureGroup); - - robotArm.GetArmMeasure(NM_Voltage, lastVals); - pc.printf( "Voltage: "); - for( int servoIndex = 0; servoIndex < partSize; servoIndex++ ) - { - pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]); - } - pc.printf( "\r\n"); - measureGroup.SetMeasure(NM_Voltage, lastVals); - MeasureBuf.push(measureGroup); - - robotArm.GetArmMeasure(NM_Degrees, lastVals); - pc.printf( "Rotation: "); - for( int servoIndex = 0; servoIndex < partSize; servoIndex++ ) - { - pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]); - } - pc.printf( "\r\n"); - measureGroup.SetMeasure(NM_Degrees, lastVals); - MeasureBuf.push(measureGroup); -} - +extern void RunController(); +extern bool StartIothubThread(); int setupRealTime(void) { @@ -124,326 +79,6 @@ } -vector<float> upPositions; -vector<float> homePositions; - -void MakeDemoSeq(vector<ActionSequence>& actions) -{ - // define actions - ActionSequence moveUp(SA_SetGoal, upPositions, 2500); - ActionSequence report(SA_Status); - ActionSequence pause(SA_Delay); - pause.SetDelay(2000); - ActionSequence moveDown(SA_SetGoal, homePositions, 2500); - ActionSequence rep(SA_Repeat); - - // add actions into a sequence - actions.push_back(moveUp); - actions.push_back(report); - actions.push_back(pause); - actions.push_back(moveDown); - actions.push_back(report); - actions.push_back(pause); - actions.push_back(rep); -} - - -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 -}; - -Queue<vector<ActionSequence>, 16> SequenceQ; -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) -{ - RunInDelay = false; - osSignalSet((osThreadId)tid, AS_Action); -} - -void run() -{ - // init robot arm - RobotArm robotArm; - - int partSize = robotArm.GetNumParts(); - - for( int servoIndex = 0; servoIndex < partSize; servoIndex++) - { - upPositions.push_back(UpPos); - } - - // set running state - MainState = MS_Idle; - RunInSequence = false; - RunInMoveStep = false; - RunInDelay = false; - - // get initial positions - MeasureGroup measureGroup; - robotArm.GetArmPositions(homePositions); - - vector<float> lastVals; - - // do inital status report - PushMeasurements(measureGroup, partSize, robotArm); - SendIothubMeasurements(); - - // Prepare main thread - mainTid = osThreadGetId(); - int32_t signals = AS_Action; - - // create sequences - MakeSequences(partSize, homePositions); - - // add to queue - SequenceQ.put(&UpDownSeq); - - // state for sequence - vector<ActionSequence> *curseq = NULL; - int curseqIx = 0; - - // signal to run the default action for demo - osSignalSet(mainTid, AS_Action); - - RtosTimer delayTimer(NextSeq, osTimerOnce, (void *)osThreadGetId()); - - MainState = MS_Running; - - while( true ) - { - osEvent mainEvent = osSignalWait(signals, osWaitForever); - - switch (MainState) - { - case MS_Idle: - break; - - case MS_Paused: - pc.printf( "Paused\r\n"); - break; - - case MS_Stopping: - pc.printf( "Stopping\r\n"); - break; - - case MS_Error: - pc.printf( "Error\r\n"); - break; - - case MS_CancelOne: - pc.printf( "Cancel sequence\r\n"); - RunInSequence = false; - RunInMoveStep = false; - if (RunInDelay) - { - RunInDelay = false; - delayTimer.stop(); - } - MainState = MS_Running; - osSignalSet(mainTid, AS_Action); - break; - - case MS_CancelAll: - RunInSequence = false; - RunInMoveStep = false; - if (RunInDelay) - { - RunInDelay = false; - delayTimer.stop(); - } - while (1) { - osEvent evt = SequenceQ.get(0); - if (evt.status != osEventMessage) - break; - } - MainState = MS_Running; - break; - - case MS_CancelToM: - 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 - break; - } - if (!RunInSequence) - { - osEvent evt = SequenceQ.get(0); - if (evt.status == osEventMessage) - { - pc.printf( "New Seq \r\n"); - curseq = (vector<ActionSequence> *)evt.value.p; - curseqIx = 0; - RunInSequence = true; - RunInMoveStep = false; - } - } - - if (RunInSequence) - { - if (RunInMoveStep) - { - if (robotArm.MoveArmPositionsHasNext()) - { - //pc.printf( "Next Step\r\n"); - int delaystep = 0; - bool ok = robotArm.MoveArmPositionsNext(delaystep); - if (ok) - { - if (delaystep > 0) - { - RunInDelay = true; - delayTimer.start(delaystep); - } - else - osSignalSet(mainTid, AS_Action); - } - } - else - { - pc.printf( "No more Step\r\n"); - RunInMoveStep = false; - } - } - 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++; - - bool ok; - switch (aseq.ActionType) - { - case SA_SetGoal: - pc.printf( " - Move arm start\r\n"); - ok = robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Ms); - if (ok) - { - RunInMoveStep = true; - osSignalSet(mainTid, AS_Action); - } - else - { - // TODO: send alert - } - break; - case SA_Delay: - pc.printf( " - Delay\r\n"); - RunInDelay = true; - delayTimer.start(aseq.Ms); - break; - - case SA_Status: - pc.printf( " - Status\r\n"); - robotArm.GetArmPositions(lastVals); - - PushMeasurements(measureGroup, partSize, robotArm); - - SendIothubMeasurements(); - - osSignalSet(mainTid, AS_Action); - break; - case SA_Repeat: - pc.printf( " - Repeat\r\n"); - curseqIx = 0; - osSignalSet(mainTid, AS_Action); - break; - - } - } - } - - } - break; - } - - } -} int main() { @@ -473,9 +108,7 @@ pc.printf( "Initialization done. Ready to run. \r\n"); - RobotArmPc = &pc; - AX12Pc = &pc; - - run(); + + RunController(); }