robot arm demo team / Mbed 2 deprecated RobotArmDemo Featured

demo project

Dependencies:   AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL

main.cpp@4:36a4eceb1b7f, 2015-12-23 (annotated)

Committer:
henryrawas
Date:
Wed Dec 23 18:34:06 2015 +0000
Revision:
4:36a4eceb1b7f
Parent:
2:37021fb3b45b
Child:
5:36916b1c5a06
RobotArm plus publish IoTHub status

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jepickett 0:547ac906c46b 1 #include "mbed.h"
henryrawas 4:36a4eceb1b7f 2 #include "rtos.h"
henryrawas 4:36a4eceb1b7f 3
henryrawas 4:36a4eceb1b7f 4 #include "EthernetInterface.h"
henryrawas 4:36a4eceb1b7f 5 #include "mbed/logging.h"
henryrawas 4:36a4eceb1b7f 6 #include "mbed/mbedtime.h"
henryrawas 4:36a4eceb1b7f 7 #include <NTPClient.h>
jepickett 2:37021fb3b45b 8 #include <DynamixelBus.h>
jepickett 2:37021fb3b45b 9 #include <AX12.h>
jepickett 2:37021fb3b45b 10 #include <Terminal.h>
jepickett 2:37021fb3b45b 11 #include <vector>
henryrawas 4:36a4eceb1b7f 12 #include <RobotArm.h>
henryrawas 4:36a4eceb1b7f 13 #include <MeasureBuf.h>
henryrawas 4:36a4eceb1b7f 14 #include <IothubRobotArm.h>
henryrawas 4:36a4eceb1b7f 15 #include <ActionBuf.h>
henryrawas 4:36a4eceb1b7f 16
jepickett 2:37021fb3b45b 17 using namespace std;
jepickett 0:547ac906c46b 18
jepickett 2:37021fb3b45b 19 DigitalOut myled(LED_GREEN);
jepickett 2:37021fb3b45b 20 Terminal pc(USBTX, USBRX);
henryrawas 4:36a4eceb1b7f 21
henryrawas 4:36a4eceb1b7f 22 extern Terminal* RobotArmPc;
henryrawas 4:36a4eceb1b7f 23 extern Terminal* AX12Pc;
henryrawas 4:36a4eceb1b7f 24
henryrawas 4:36a4eceb1b7f 25 const float UpPos = 150.0f;
henryrawas 4:36a4eceb1b7f 26
henryrawas 4:36a4eceb1b7f 27
henryrawas 4:36a4eceb1b7f 28 void PushMeasurements(MeasureGroup measureGroup, int partSize, RobotArm robotArm)
henryrawas 4:36a4eceb1b7f 29 {
henryrawas 4:36a4eceb1b7f 30 vector<float> lastVals;
henryrawas 4:36a4eceb1b7f 31
henryrawas 4:36a4eceb1b7f 32 robotArm.GetArmMeasure(NM_Temperature, lastVals);
henryrawas 4:36a4eceb1b7f 33 pc.printf( "Temperatures: ");
henryrawas 4:36a4eceb1b7f 34 for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
henryrawas 4:36a4eceb1b7f 35 {
henryrawas 4:36a4eceb1b7f 36 pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
henryrawas 4:36a4eceb1b7f 37 }
henryrawas 4:36a4eceb1b7f 38 pc.printf( "\r\n");
henryrawas 4:36a4eceb1b7f 39 measureGroup.SetMeasure(NM_Temperature, lastVals);
henryrawas 4:36a4eceb1b7f 40 MeasureBuf.push(measureGroup);
henryrawas 4:36a4eceb1b7f 41
henryrawas 4:36a4eceb1b7f 42 robotArm.GetArmMeasure(NM_Voltage, lastVals);
henryrawas 4:36a4eceb1b7f 43 pc.printf( "Voltage: ");
henryrawas 4:36a4eceb1b7f 44 for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
henryrawas 4:36a4eceb1b7f 45 {
henryrawas 4:36a4eceb1b7f 46 pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
henryrawas 4:36a4eceb1b7f 47 }
henryrawas 4:36a4eceb1b7f 48 pc.printf( "\r\n");
henryrawas 4:36a4eceb1b7f 49 measureGroup.SetMeasure(NM_Voltage, lastVals);
henryrawas 4:36a4eceb1b7f 50 MeasureBuf.push(measureGroup);
henryrawas 4:36a4eceb1b7f 51
henryrawas 4:36a4eceb1b7f 52 robotArm.GetArmMeasure(NM_Degrees, lastVals);
henryrawas 4:36a4eceb1b7f 53 pc.printf( "Rotation: ");
henryrawas 4:36a4eceb1b7f 54 for( int servoIndex = 0; servoIndex < partSize; servoIndex++ )
henryrawas 4:36a4eceb1b7f 55 {
henryrawas 4:36a4eceb1b7f 56 pc.printf( "%d:%f ", servoIndex, lastVals[servoIndex]);
henryrawas 4:36a4eceb1b7f 57 }
henryrawas 4:36a4eceb1b7f 58 pc.printf( "\r\n");
henryrawas 4:36a4eceb1b7f 59 measureGroup.SetMeasure(NM_Degrees, lastVals);
henryrawas 4:36a4eceb1b7f 60 MeasureBuf.push(measureGroup);
henryrawas 4:36a4eceb1b7f 61 }
henryrawas 4:36a4eceb1b7f 62
henryrawas 4:36a4eceb1b7f 63
henryrawas 4:36a4eceb1b7f 64 int setupRealTime(void)
henryrawas 4:36a4eceb1b7f 65 {
henryrawas 4:36a4eceb1b7f 66 int result;
henryrawas 4:36a4eceb1b7f 67
henryrawas 4:36a4eceb1b7f 68 (void)printf("setupRealTime begin\r\n");
henryrawas 4:36a4eceb1b7f 69 if (EthernetInterface::connect())
henryrawas 4:36a4eceb1b7f 70 {
henryrawas 4:36a4eceb1b7f 71 (void)printf("Error initializing EthernetInterface.\r\n");
henryrawas 4:36a4eceb1b7f 72 result = __LINE__;
henryrawas 4:36a4eceb1b7f 73 }
henryrawas 4:36a4eceb1b7f 74 else
henryrawas 4:36a4eceb1b7f 75 {
henryrawas 4:36a4eceb1b7f 76 (void)printf("setupRealTime NTP begin\r\n");
henryrawas 4:36a4eceb1b7f 77 NTPClient ntp;
henryrawas 4:36a4eceb1b7f 78 if (ntp.setTime("0.pool.ntp.org") != 0)
henryrawas 4:36a4eceb1b7f 79 {
henryrawas 4:36a4eceb1b7f 80 (void)printf("Failed setting time.\r\n");
henryrawas 4:36a4eceb1b7f 81 result = __LINE__;
henryrawas 4:36a4eceb1b7f 82 }
henryrawas 4:36a4eceb1b7f 83 else
henryrawas 4:36a4eceb1b7f 84 {
henryrawas 4:36a4eceb1b7f 85 (void)printf("set time correctly!\r\n");
henryrawas 4:36a4eceb1b7f 86 result = 0;
henryrawas 4:36a4eceb1b7f 87 }
henryrawas 4:36a4eceb1b7f 88 (void)printf("setupRealTime NTP end\r\n");
henryrawas 4:36a4eceb1b7f 89 EthernetInterface::disconnect();
henryrawas 4:36a4eceb1b7f 90 }
henryrawas 4:36a4eceb1b7f 91 (void)printf("setupRealTime end\r\n");
henryrawas 4:36a4eceb1b7f 92
henryrawas 4:36a4eceb1b7f 93 return result;
henryrawas 4:36a4eceb1b7f 94 }
henryrawas 4:36a4eceb1b7f 95
henryrawas 4:36a4eceb1b7f 96 int InitEthernet()
henryrawas 4:36a4eceb1b7f 97 {
henryrawas 4:36a4eceb1b7f 98 (void)printf("Initializing mbed specific things...\r\n");
henryrawas 4:36a4eceb1b7f 99
henryrawas 4:36a4eceb1b7f 100 /* These are needed in order to initialize the time provider for Proton-C */
henryrawas 4:36a4eceb1b7f 101 mbed_log_init();
henryrawas 4:36a4eceb1b7f 102 mbedtime_init();
henryrawas 4:36a4eceb1b7f 103
henryrawas 4:36a4eceb1b7f 104 if (EthernetInterface::init())
henryrawas 4:36a4eceb1b7f 105 {
henryrawas 4:36a4eceb1b7f 106 (void)printf("Error initializing EthernetInterface.\r\n");
henryrawas 4:36a4eceb1b7f 107 return -1;
henryrawas 4:36a4eceb1b7f 108 }
henryrawas 4:36a4eceb1b7f 109
henryrawas 4:36a4eceb1b7f 110 if (setupRealTime() != 0)
henryrawas 4:36a4eceb1b7f 111 {
henryrawas 4:36a4eceb1b7f 112 (void)printf("Failed setting up real time clock\r\n");
henryrawas 4:36a4eceb1b7f 113 return -1;
henryrawas 4:36a4eceb1b7f 114 }
henryrawas 4:36a4eceb1b7f 115
henryrawas 4:36a4eceb1b7f 116 if (EthernetInterface::connect())
henryrawas 4:36a4eceb1b7f 117 {
henryrawas 4:36a4eceb1b7f 118 (void)printf("Error connecting EthernetInterface.\r\n");
henryrawas 4:36a4eceb1b7f 119 return -1;
henryrawas 4:36a4eceb1b7f 120 }
henryrawas 4:36a4eceb1b7f 121
henryrawas 4:36a4eceb1b7f 122 return 0;
henryrawas 4:36a4eceb1b7f 123 }
henryrawas 4:36a4eceb1b7f 124
henryrawas 4:36a4eceb1b7f 125
henryrawas 4:36a4eceb1b7f 126 vector<float> upPositions;
henryrawas 4:36a4eceb1b7f 127 vector<float> homePositions;
henryrawas 4:36a4eceb1b7f 128
henryrawas 4:36a4eceb1b7f 129 void MakeDemoSeq(vector<ActionSequence>& actions)
henryrawas 4:36a4eceb1b7f 130 {
henryrawas 4:36a4eceb1b7f 131 // define actions
henryrawas 4:36a4eceb1b7f 132 ActionSequence moveUp(SA_SetGoal, upPositions, 3000);
henryrawas 4:36a4eceb1b7f 133 ActionSequence report(SA_Status);
henryrawas 4:36a4eceb1b7f 134 ActionSequence pause(SA_Delay);
henryrawas 4:36a4eceb1b7f 135 pause.SetDelay(5000);
henryrawas 4:36a4eceb1b7f 136 ActionSequence moveDown(SA_SetGoal, homePositions, 3000);
henryrawas 4:36a4eceb1b7f 137 ActionSequence rep(SA_Repeat);
henryrawas 4:36a4eceb1b7f 138
henryrawas 4:36a4eceb1b7f 139 // add actions into a sequence
henryrawas 4:36a4eceb1b7f 140 actions.push_back(moveUp);
henryrawas 4:36a4eceb1b7f 141 actions.push_back(report);
henryrawas 4:36a4eceb1b7f 142 actions.push_back(pause);
henryrawas 4:36a4eceb1b7f 143 actions.push_back(moveDown);
henryrawas 4:36a4eceb1b7f 144 actions.push_back(report);
henryrawas 4:36a4eceb1b7f 145 actions.push_back(pause);
henryrawas 4:36a4eceb1b7f 146 actions.push_back(rep);
henryrawas 4:36a4eceb1b7f 147 }
henryrawas 4:36a4eceb1b7f 148
henryrawas 4:36a4eceb1b7f 149
henryrawas 4:36a4eceb1b7f 150 enum MainStates
henryrawas 4:36a4eceb1b7f 151 {
henryrawas 4:36a4eceb1b7f 152 MS_Idle = 0x0,
henryrawas 4:36a4eceb1b7f 153 MS_Running = 0x1,
henryrawas 4:36a4eceb1b7f 154 MS_Paused = 0x2,
henryrawas 4:36a4eceb1b7f 155 MS_Stopping = 0x3,
henryrawas 4:36a4eceb1b7f 156 MS_Error = 0x4,
henryrawas 4:36a4eceb1b7f 157 MS_CancelOne = 0x5,
henryrawas 4:36a4eceb1b7f 158 MS_CancelAll = 0x6,
henryrawas 4:36a4eceb1b7f 159 MS_CancelToM = 0x7
henryrawas 4:36a4eceb1b7f 160 };
henryrawas 4:36a4eceb1b7f 161
henryrawas 4:36a4eceb1b7f 162 Queue<vector<ActionSequence>, 16> SequenceQ;
henryrawas 4:36a4eceb1b7f 163 MainStates MainState;
henryrawas 4:36a4eceb1b7f 164 bool RunInSequence;
henryrawas 4:36a4eceb1b7f 165 bool RunInMoveStep;
henryrawas 4:36a4eceb1b7f 166 bool RunInDelay;
henryrawas 4:36a4eceb1b7f 167 void* CancelToMatch;
henryrawas 4:36a4eceb1b7f 168
henryrawas 4:36a4eceb1b7f 169
henryrawas 4:36a4eceb1b7f 170 // run sequence thread timer routine
henryrawas 4:36a4eceb1b7f 171 void NextSeq(void const * tid)
henryrawas 4:36a4eceb1b7f 172 {
henryrawas 4:36a4eceb1b7f 173 RunInDelay = false;
henryrawas 4:36a4eceb1b7f 174 osSignalSet((osThreadId)tid, AS_Action);
henryrawas 4:36a4eceb1b7f 175 }
henryrawas 4:36a4eceb1b7f 176
henryrawas 4:36a4eceb1b7f 177 void run()
henryrawas 4:36a4eceb1b7f 178 {
henryrawas 4:36a4eceb1b7f 179 // init robot arm
henryrawas 4:36a4eceb1b7f 180 RobotArm robotArm;
henryrawas 4:36a4eceb1b7f 181
henryrawas 4:36a4eceb1b7f 182 int partSize = robotArm.GetNumParts();
jepickett 2:37021fb3b45b 183
henryrawas 4:36a4eceb1b7f 184 for( int servoIndex = 0; servoIndex < partSize; servoIndex++)
henryrawas 4:36a4eceb1b7f 185 {
henryrawas 4:36a4eceb1b7f 186 upPositions.push_back(UpPos);
henryrawas 4:36a4eceb1b7f 187 }
henryrawas 4:36a4eceb1b7f 188
henryrawas 4:36a4eceb1b7f 189 // set running state
henryrawas 4:36a4eceb1b7f 190 MainState = MS_Idle;
henryrawas 4:36a4eceb1b7f 191 RunInSequence = false;
henryrawas 4:36a4eceb1b7f 192 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 193 RunInDelay = false;
henryrawas 4:36a4eceb1b7f 194
henryrawas 4:36a4eceb1b7f 195 // get initial positions
henryrawas 4:36a4eceb1b7f 196 MeasureGroup measureGroup;
henryrawas 4:36a4eceb1b7f 197 robotArm.GetArmPositions(homePositions);
henryrawas 4:36a4eceb1b7f 198
henryrawas 4:36a4eceb1b7f 199 vector<float> lastVals;
henryrawas 4:36a4eceb1b7f 200
henryrawas 4:36a4eceb1b7f 201 // do inital status report
henryrawas 4:36a4eceb1b7f 202 PushMeasurements(measureGroup, partSize, robotArm);
henryrawas 4:36a4eceb1b7f 203 SendIothubMeasurements();
henryrawas 4:36a4eceb1b7f 204
henryrawas 4:36a4eceb1b7f 205 // Prepare main thread
henryrawas 4:36a4eceb1b7f 206 osThreadId mainTid = osThreadGetId();
henryrawas 4:36a4eceb1b7f 207 int32_t signals = AS_Action;
henryrawas 4:36a4eceb1b7f 208
henryrawas 4:36a4eceb1b7f 209 // create a sequence for demo
henryrawas 4:36a4eceb1b7f 210 vector<ActionSequence> actions;
henryrawas 4:36a4eceb1b7f 211 MakeDemoSeq(actions);
henryrawas 4:36a4eceb1b7f 212
henryrawas 4:36a4eceb1b7f 213 // add to queue
henryrawas 4:36a4eceb1b7f 214 SequenceQ.put(&actions);
henryrawas 4:36a4eceb1b7f 215
henryrawas 4:36a4eceb1b7f 216 // state for sequence
henryrawas 4:36a4eceb1b7f 217 vector<ActionSequence> *curseq = NULL;
henryrawas 4:36a4eceb1b7f 218 int curseqIx = 0;
henryrawas 4:36a4eceb1b7f 219
henryrawas 4:36a4eceb1b7f 220 // signal to run the default action for demo
henryrawas 4:36a4eceb1b7f 221 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 222
henryrawas 4:36a4eceb1b7f 223 RtosTimer delayTimer(NextSeq, osTimerOnce, (void *)osThreadGetId());
henryrawas 4:36a4eceb1b7f 224
henryrawas 4:36a4eceb1b7f 225 MainState = MS_Running;
henryrawas 4:36a4eceb1b7f 226
henryrawas 4:36a4eceb1b7f 227 while( true )
henryrawas 4:36a4eceb1b7f 228 {
henryrawas 4:36a4eceb1b7f 229 osEvent mainEvent = osSignalWait(signals, osWaitForever);
henryrawas 4:36a4eceb1b7f 230
henryrawas 4:36a4eceb1b7f 231 switch (MainState)
henryrawas 4:36a4eceb1b7f 232 {
henryrawas 4:36a4eceb1b7f 233 case MS_Idle:
henryrawas 4:36a4eceb1b7f 234 break;
henryrawas 4:36a4eceb1b7f 235
henryrawas 4:36a4eceb1b7f 236 case MS_Paused:
henryrawas 4:36a4eceb1b7f 237 pc.printf( "Paused\r\n");
henryrawas 4:36a4eceb1b7f 238 break;
henryrawas 4:36a4eceb1b7f 239
henryrawas 4:36a4eceb1b7f 240 case MS_Stopping:
henryrawas 4:36a4eceb1b7f 241 pc.printf( "Stopping\r\n");
henryrawas 4:36a4eceb1b7f 242 break;
henryrawas 4:36a4eceb1b7f 243
henryrawas 4:36a4eceb1b7f 244 case MS_Error:
henryrawas 4:36a4eceb1b7f 245 pc.printf( "Error\r\n");
henryrawas 4:36a4eceb1b7f 246 break;
henryrawas 4:36a4eceb1b7f 247
henryrawas 4:36a4eceb1b7f 248 case MS_CancelOne:
henryrawas 4:36a4eceb1b7f 249 pc.printf( "Cancel sequence\r\n");
henryrawas 4:36a4eceb1b7f 250 RunInSequence = false;
henryrawas 4:36a4eceb1b7f 251 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 252 if (RunInDelay)
henryrawas 4:36a4eceb1b7f 253 {
henryrawas 4:36a4eceb1b7f 254 RunInDelay = false;
henryrawas 4:36a4eceb1b7f 255 delayTimer.stop();
henryrawas 4:36a4eceb1b7f 256 }
henryrawas 4:36a4eceb1b7f 257 MainState = MS_Running;
henryrawas 4:36a4eceb1b7f 258 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 259 break;
henryrawas 4:36a4eceb1b7f 260
henryrawas 4:36a4eceb1b7f 261 case MS_CancelAll:
henryrawas 4:36a4eceb1b7f 262 RunInSequence = false;
henryrawas 4:36a4eceb1b7f 263 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 264 if (RunInDelay)
henryrawas 4:36a4eceb1b7f 265 {
henryrawas 4:36a4eceb1b7f 266 RunInDelay = false;
henryrawas 4:36a4eceb1b7f 267 delayTimer.stop();
henryrawas 4:36a4eceb1b7f 268 }
henryrawas 4:36a4eceb1b7f 269 while (1) {
henryrawas 4:36a4eceb1b7f 270 osEvent evt = SequenceQ.get(0);
henryrawas 4:36a4eceb1b7f 271 if (evt.status != osEventMessage)
henryrawas 4:36a4eceb1b7f 272 break;
henryrawas 4:36a4eceb1b7f 273 }
henryrawas 4:36a4eceb1b7f 274 MainState = MS_Running;
henryrawas 4:36a4eceb1b7f 275 break;
henryrawas 4:36a4eceb1b7f 276
henryrawas 4:36a4eceb1b7f 277 case MS_CancelToM:
henryrawas 4:36a4eceb1b7f 278 RunInSequence = false;
henryrawas 4:36a4eceb1b7f 279 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 280 if (RunInDelay)
henryrawas 4:36a4eceb1b7f 281 {
henryrawas 4:36a4eceb1b7f 282 RunInDelay = false;
henryrawas 4:36a4eceb1b7f 283 delayTimer.stop();
henryrawas 4:36a4eceb1b7f 284 }
henryrawas 4:36a4eceb1b7f 285 while (1) {
henryrawas 4:36a4eceb1b7f 286 osEvent evt = SequenceQ.get(0);
henryrawas 4:36a4eceb1b7f 287 if (evt.status != osEventMessage)
henryrawas 4:36a4eceb1b7f 288 break;
henryrawas 4:36a4eceb1b7f 289 else if (evt.value.p == CancelToMatch)
henryrawas 4:36a4eceb1b7f 290 {
henryrawas 4:36a4eceb1b7f 291 curseq = (vector<ActionSequence> *)evt.value.p;
henryrawas 4:36a4eceb1b7f 292 curseqIx = 0;
henryrawas 4:36a4eceb1b7f 293 RunInSequence = true;
henryrawas 4:36a4eceb1b7f 294 }
henryrawas 4:36a4eceb1b7f 295 }
henryrawas 4:36a4eceb1b7f 296 MainState = MS_Running;
henryrawas 4:36a4eceb1b7f 297 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 298 break;
henryrawas 4:36a4eceb1b7f 299
henryrawas 4:36a4eceb1b7f 300 case MS_Running:
henryrawas 4:36a4eceb1b7f 301 if (RunInDelay)
henryrawas 4:36a4eceb1b7f 302 {
henryrawas 4:36a4eceb1b7f 303 // waiting for timer to fire. do nothing
henryrawas 4:36a4eceb1b7f 304 break;
henryrawas 4:36a4eceb1b7f 305 }
henryrawas 4:36a4eceb1b7f 306 if (!RunInSequence)
henryrawas 4:36a4eceb1b7f 307 {
henryrawas 4:36a4eceb1b7f 308 osEvent evt = SequenceQ.get(0);
henryrawas 4:36a4eceb1b7f 309 if (evt.status == osEventMessage)
henryrawas 4:36a4eceb1b7f 310 {
henryrawas 4:36a4eceb1b7f 311 pc.printf( "New Seq \r\n");
henryrawas 4:36a4eceb1b7f 312 curseq = (vector<ActionSequence> *)evt.value.p;
henryrawas 4:36a4eceb1b7f 313 curseqIx = 0;
henryrawas 4:36a4eceb1b7f 314 RunInSequence = true;
henryrawas 4:36a4eceb1b7f 315 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 316 }
henryrawas 4:36a4eceb1b7f 317 }
henryrawas 4:36a4eceb1b7f 318
henryrawas 4:36a4eceb1b7f 319 if (RunInSequence)
henryrawas 4:36a4eceb1b7f 320 {
henryrawas 4:36a4eceb1b7f 321 if (RunInMoveStep)
henryrawas 4:36a4eceb1b7f 322 {
henryrawas 4:36a4eceb1b7f 323 if (robotArm.MoveArmPositionsHasNext())
henryrawas 4:36a4eceb1b7f 324 {
henryrawas 4:36a4eceb1b7f 325 //pc.printf( "Next Step\r\n");
henryrawas 4:36a4eceb1b7f 326 int delaystep = 0;
henryrawas 4:36a4eceb1b7f 327 bool ok = robotArm.MoveArmPositionsNext(delaystep);
henryrawas 4:36a4eceb1b7f 328 if (ok)
henryrawas 4:36a4eceb1b7f 329 {
henryrawas 4:36a4eceb1b7f 330 if (delaystep > 0)
henryrawas 4:36a4eceb1b7f 331 {
henryrawas 4:36a4eceb1b7f 332 RunInDelay = true;
henryrawas 4:36a4eceb1b7f 333 delayTimer.start(delaystep);
henryrawas 4:36a4eceb1b7f 334 }
henryrawas 4:36a4eceb1b7f 335 else
henryrawas 4:36a4eceb1b7f 336 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 337 }
henryrawas 4:36a4eceb1b7f 338 }
henryrawas 4:36a4eceb1b7f 339 else
henryrawas 4:36a4eceb1b7f 340 {
henryrawas 4:36a4eceb1b7f 341 pc.printf( "No more Step\r\n");
henryrawas 4:36a4eceb1b7f 342 RunInMoveStep = false;
henryrawas 4:36a4eceb1b7f 343 }
henryrawas 4:36a4eceb1b7f 344 }
henryrawas 4:36a4eceb1b7f 345 if (!RunInMoveStep)
henryrawas 4:36a4eceb1b7f 346 {
henryrawas 4:36a4eceb1b7f 347 pc.printf( "Next Seq %d\r\n", curseqIx);
henryrawas 4:36a4eceb1b7f 348
henryrawas 4:36a4eceb1b7f 349 if (curseq != NULL)
henryrawas 4:36a4eceb1b7f 350 {
henryrawas 4:36a4eceb1b7f 351 ActionSequence aseq = (*curseq)[curseqIx];
henryrawas 4:36a4eceb1b7f 352 curseqIx++;
henryrawas 4:36a4eceb1b7f 353
henryrawas 4:36a4eceb1b7f 354 bool ok;
henryrawas 4:36a4eceb1b7f 355 switch (aseq.ActionType)
henryrawas 4:36a4eceb1b7f 356 {
henryrawas 4:36a4eceb1b7f 357 case SA_SetGoal:
henryrawas 4:36a4eceb1b7f 358 pc.printf( " - Move arm start\r\n");
henryrawas 4:36a4eceb1b7f 359 ok = robotArm.MoveArmPositionsStart(aseq.GoalVals, aseq.Ms);
henryrawas 4:36a4eceb1b7f 360 if (ok)
henryrawas 4:36a4eceb1b7f 361 {
henryrawas 4:36a4eceb1b7f 362 RunInMoveStep = true;
henryrawas 4:36a4eceb1b7f 363 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 364 }
henryrawas 4:36a4eceb1b7f 365 else
henryrawas 4:36a4eceb1b7f 366 {
henryrawas 4:36a4eceb1b7f 367 // TODO: send alert
henryrawas 4:36a4eceb1b7f 368 }
henryrawas 4:36a4eceb1b7f 369 break;
henryrawas 4:36a4eceb1b7f 370 case SA_Delay:
henryrawas 4:36a4eceb1b7f 371 pc.printf( " - Delay\r\n");
henryrawas 4:36a4eceb1b7f 372 RunInDelay = true;
henryrawas 4:36a4eceb1b7f 373 delayTimer.start(aseq.Ms);
henryrawas 4:36a4eceb1b7f 374 break;
henryrawas 4:36a4eceb1b7f 375
henryrawas 4:36a4eceb1b7f 376 case SA_Status:
henryrawas 4:36a4eceb1b7f 377 pc.printf( " - Status\r\n");
henryrawas 4:36a4eceb1b7f 378 robotArm.GetArmPositions(lastVals);
henryrawas 4:36a4eceb1b7f 379
henryrawas 4:36a4eceb1b7f 380 PushMeasurements(measureGroup, partSize, robotArm);
henryrawas 4:36a4eceb1b7f 381
henryrawas 4:36a4eceb1b7f 382 SendIothubMeasurements();
henryrawas 4:36a4eceb1b7f 383
henryrawas 4:36a4eceb1b7f 384 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 385 break;
henryrawas 4:36a4eceb1b7f 386 case SA_Repeat:
henryrawas 4:36a4eceb1b7f 387 pc.printf( " - Repeat\r\n");
henryrawas 4:36a4eceb1b7f 388 curseqIx = 0;
henryrawas 4:36a4eceb1b7f 389 osSignalSet(mainTid, AS_Action);
henryrawas 4:36a4eceb1b7f 390 break;
henryrawas 4:36a4eceb1b7f 391
henryrawas 4:36a4eceb1b7f 392 }
henryrawas 4:36a4eceb1b7f 393 }
henryrawas 4:36a4eceb1b7f 394
henryrawas 4:36a4eceb1b7f 395 if (curseqIx >= curseq->size())
henryrawas 4:36a4eceb1b7f 396 {
henryrawas 4:36a4eceb1b7f 397 RunInSequence = false;
henryrawas 4:36a4eceb1b7f 398 }
henryrawas 4:36a4eceb1b7f 399 }
henryrawas 4:36a4eceb1b7f 400
henryrawas 4:36a4eceb1b7f 401 }
henryrawas 4:36a4eceb1b7f 402 break;
henryrawas 4:36a4eceb1b7f 403 }
henryrawas 4:36a4eceb1b7f 404
henryrawas 4:36a4eceb1b7f 405 }
henryrawas 4:36a4eceb1b7f 406 }
jepickett 0:547ac906c46b 407
jepickett 0:547ac906c46b 408 int main()
jepickett 0:547ac906c46b 409 {
jepickett 2:37021fb3b45b 410 pc.baud(115200);
jepickett 2:37021fb3b45b 411
jepickett 2:37021fb3b45b 412 pc.cls();
jepickett 2:37021fb3b45b 413 pc.foreground(Yellow);
jepickett 2:37021fb3b45b 414 pc.background(Black);
jepickett 2:37021fb3b45b 415
jepickett 2:37021fb3b45b 416 pc.locate( 0, 0 );
jepickett 2:37021fb3b45b 417 pc.printf("**********************\r\n");
jepickett 2:37021fb3b45b 418 pc.printf("RobotArmDemo start\r\n");
jepickett 2:37021fb3b45b 419 pc.printf("**********************\r\n");
jepickett 2:37021fb3b45b 420
jepickett 2:37021fb3b45b 421 pc.foreground(Teal);
jepickett 2:37021fb3b45b 422 pc.background(Black);
jepickett 2:37021fb3b45b 423
henryrawas 4:36a4eceb1b7f 424 InitEthernet();
henryrawas 4:36a4eceb1b7f 425
henryrawas 4:36a4eceb1b7f 426 // start IotHub connection
henryrawas 4:36a4eceb1b7f 427 StartIothubThread();
jepickett 2:37021fb3b45b 428
henryrawas 4:36a4eceb1b7f 429 // time delay is to allow the position encoders to come online after initial power supply event ~ 5 secs
henryrawas 4:36a4eceb1b7f 430 // and to allow IoTHub SSL connection established
henryrawas 4:36a4eceb1b7f 431 // TODO: test for connection established
henryrawas 4:36a4eceb1b7f 432 Thread::wait(15000);
henryrawas 4:36a4eceb1b7f 433
henryrawas 4:36a4eceb1b7f 434 pc.printf( "Initialization done. Ready to run. \r\n");
jepickett 2:37021fb3b45b 435
henryrawas 4:36a4eceb1b7f 436 RobotArmPc = &pc;
henryrawas 4:36a4eceb1b7f 437 AX12Pc = &pc;
jepickett 2:37021fb3b45b 438
henryrawas 4:36a4eceb1b7f 439 run();
jepickett 2:37021fb3b45b 440
henryrawas 4:36a4eceb1b7f 441 }