MorphGI / Mbed OS MGI_Rebuild_MidLevel_9_0-Basic_PWM

Basic Mid-Level control for the rebuilt MorphGI control unit, using PWM to communicate with the low level controllers.

Dependencies:   ros_lib_kinetic

main.cpp@11:7029367a1840, 2018-08-10 (annotated)

Committer:
dofydoink
Date:
Fri Aug 10 10:40:18 2018 +0000
Revision:
11:7029367a1840
Parent:
10:1b6daba32452
Child:
12:595ed862e52f
Tested and working with 1 actuator.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WD40andTape 7:5b6a2cefbf3b 1 // STANDARD IMPORTS
WD40andTape 7:5b6a2cefbf3b 2 #include "math.h"
WD40andTape 7:5b6a2cefbf3b 3 // MBED IMPORTS
dofydoink 0:607bc887b6e0 4 #include "mbed.h"
WD40andTape 6:f0a18e28a322 5 #include "mbed_events.h"
WD40andTape 7:5b6a2cefbf3b 6 // CUSTOM IMPORTS
dofydoink 11:7029367a1840 7 #include "MLOptions.h"
WD40andTape 7:5b6a2cefbf3b 8 #include "HLComms.h"
WD40andTape 8:d6657767a182 9 #include "LLComms.h"
dofydoink 0:607bc887b6e0 10
dofydoink 11:7029367a1840 11 const double DBL_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * (1.0+DBL_PERCENT_PATH_TOLERANCE); // Path tolerance in mm with 5% tolerance
dofydoink 0:607bc887b6e0 12
WD40andTape 7:5b6a2cefbf3b 13 // PATH VARIABLES
WD40andTape 7:5b6a2cefbf3b 14 double dblVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 15 double dblLinearPathCurrentPos_mm[N_CHANNELS]={ 0.0 }; // The current position of the linear path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 16 double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
dofydoink 0:607bc887b6e0 17
dofydoink 11:7029367a1840 18 double _dblVelocity_mmps[N_CHANNELS];
dofydoink 11:7029367a1840 19 double _dblLinearPathCurrentPos_mm[N_CHANNELS];
dofydoink 11:7029367a1840 20
WD40andTape 7:5b6a2cefbf3b 21 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
WD40andTape 8:d6657767a182 22 LLComms llcomms(LOW_LEVEL_SPI_FREQUENCY);//(N_CHANNELS);
WD40andTape 3:c83291bf9fd2 23
WD40andTape 10:1b6daba32452 24 Thread threadLowLevelSPI(osPriorityRealtime);
WD40andTape 4:303584310071 25 Thread threadReceiveAndReplan(osPriorityBelowNormal);
WD40andTape 4:303584310071 26 Thread threadSmoothPathPlan(osPriorityNormal);
dofydoink 0:607bc887b6e0 27
WD40andTape 4:303584310071 28 Mutex mutPathIn;
WD40andTape 7:5b6a2cefbf3b 29 Semaphore semPathPlan(1);
dofydoink 0:607bc887b6e0 30
WD40andTape 7:5b6a2cefbf3b 31 Timer timer;
WD40andTape 7:5b6a2cefbf3b 32 Ticker PathCalculationTicker;
dofydoink 0:607bc887b6e0 33
WD40andTape 7:5b6a2cefbf3b 34 void startPathPlan() { // Plan a new linear path after receiving new target data
WD40andTape 4:303584310071 35 semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 36 }
dofydoink 0:607bc887b6e0 37
WD40andTape 7:5b6a2cefbf3b 38 // This function will be called when a new transmission is received from high level
WD40andTape 7:5b6a2cefbf3b 39 void ReceiveAndReplan() {
WD40andTape 7:5b6a2cefbf3b 40 HLComms hlcomms(SERVER_PORT);
WD40andTape 6:f0a18e28a322 41
WD40andTape 6:f0a18e28a322 42 int error_code;
WD40andTape 7:5b6a2cefbf3b 43 error_code = hlcomms.setup_server();
WD40andTape 6:f0a18e28a322 44 if( error_code == -1 ) return;
WD40andTape 7:5b6a2cefbf3b 45 error_code = hlcomms.accept_connection();
WD40andTape 7:5b6a2cefbf3b 46 if( error_code == -1 ) {
WD40andTape 7:5b6a2cefbf3b 47 hlcomms.close_server();
WD40andTape 7:5b6a2cefbf3b 48 return;
WD40andTape 7:5b6a2cefbf3b 49 }
WD40andTape 6:f0a18e28a322 50
WD40andTape 7:5b6a2cefbf3b 51 int ii;
WD40andTape 7:5b6a2cefbf3b 52 struct msg_format input; //hlcomms.msg_format
WD40andTape 4:303584310071 53
WD40andTape 6:f0a18e28a322 54 while( true ) {
WD40andTape 6:f0a18e28a322 55 // RECEIVE MESSAGE
WD40andTape 9:cd3607ba5643 56 error_code = hlcomms.receive_message();
WD40andTape 6:f0a18e28a322 57 if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
WD40andTape 6:f0a18e28a322 58 printf("Client disconnected.\n\r");
WD40andTape 7:5b6a2cefbf3b 59 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 60 return;
WD40andTape 6:f0a18e28a322 61 } else if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 62 printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 63 "Perhaps the server socket is not connected to a remote host? "
WD40andTape 6:f0a18e28a322 64 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 65 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 66 return;
WD40andTape 6:f0a18e28a322 67 }
WD40andTape 6:f0a18e28a322 68 //printf("Message received.\r\n");
WD40andTape 9:cd3607ba5643 69 input = hlcomms.process_message();
WD40andTape 1:2a43cf183a62 70
WD40andTape 7:5b6a2cefbf3b 71 // PROCESS INPUT
WD40andTape 7:5b6a2cefbf3b 72 double dblTargetChambLen_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
dofydoink 11:7029367a1840 73 //printf("REPLAN, %f\r\n",input.duration);
WD40andTape 7:5b6a2cefbf3b 74 // Update front segment
dofydoink 11:7029367a1840 75 /*dblTargetChambLen_mm[0] = input.psi[0][0]*1000;
WD40andTape 6:f0a18e28a322 76 dblTargetChambLen_mm[1] = input.psi[0][1]*1000;
WD40andTape 6:f0a18e28a322 77 dblTargetChambLen_mm[2] = input.psi[0][2]*1000;
WD40andTape 7:5b6a2cefbf3b 78 // Update mid segment
WD40andTape 6:f0a18e28a322 79 dblTargetChambLen_mm[6] = input.psi[1][0]*1000;
WD40andTape 7:5b6a2cefbf3b 80 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; // Same because two pumps are used
WD40andTape 7:5b6a2cefbf3b 81 // Update rear segment
WD40andTape 6:f0a18e28a322 82 dblTargetChambLen_mm[3] = input.psi[2][0]*1000;
WD40andTape 6:f0a18e28a322 83 dblTargetChambLen_mm[4] = input.psi[2][1]*1000;
dofydoink 11:7029367a1840 84 dblTargetChambLen_mm[5] = input.psi[2][2]*1000;*/
dofydoink 11:7029367a1840 85 dblTargetChambLen_mm[3] = input.psi[0][0]*1000;
dofydoink 11:7029367a1840 86 dblTargetChambLen_mm[4] = input.psi[0][1]*1000;
dofydoink 11:7029367a1840 87 dblTargetChambLen_mm[5] = input.psi[0][2]*1000;
dofydoink 11:7029367a1840 88 // Update mid segment
dofydoink 11:7029367a1840 89 dblTargetChambLen_mm[6] = input.psi[1][0]*1000;
dofydoink 11:7029367a1840 90 dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; // Same because two pumps are used
dofydoink 11:7029367a1840 91 // Update rear segment
dofydoink 11:7029367a1840 92 dblTargetChambLen_mm[0] = input.psi[2][0]*1000;
dofydoink 11:7029367a1840 93 dblTargetChambLen_mm[1] = input.psi[2][1]*1000;
dofydoink 11:7029367a1840 94 dblTargetChambLen_mm[2] = input.psi[2][2]*1000;
dofydoink 11:7029367a1840 95
dofydoink 11:7029367a1840 96 mutPathIn.lock(); // Lock variables to avoid race condition
dofydoink 11:7029367a1840 97 for(int j = 0; j<N_CHANNELS; j++) {
dofydoink 11:7029367a1840 98 //_dblVelocity_mmps[j] = dblVelocity_mmps[j];
dofydoink 11:7029367a1840 99 _dblLinearPathCurrentPos_mm[j] = dblLinearPathCurrentPos_mm[j];
dofydoink 11:7029367a1840 100 }
dofydoink 11:7029367a1840 101 mutPathIn.unlock(); // Unlock mutex
dofydoink 0:607bc887b6e0 102
WD40andTape 6:f0a18e28a322 103 bool isTimeChanged = 0;
WD40andTape 6:f0a18e28a322 104 double dblMaxRecalculatedTime = input.duration;
WD40andTape 4:303584310071 105 for (ii = 0; ii< N_CHANNELS; ii++) {
dofydoink 0:607bc887b6e0 106 //check to see if positions are achievable
WD40andTape 6:f0a18e28a322 107 dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
WD40andTape 6:f0a18e28a322 108 //!! LIMIT CHAMBER LENGTHS TOO
WD40andTape 6:f0a18e28a322 109 if( dblTargetActPos_mm[ii]<0.0 || dblTargetActPos_mm[ii]>25.0 ) {
WD40andTape 6:f0a18e28a322 110 dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 25.0 );
WD40andTape 6:f0a18e28a322 111 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 112 }
WD40andTape 6:f0a18e28a322 113 }
WD40andTape 6:f0a18e28a322 114 double dblActPosChange;
WD40andTape 6:f0a18e28a322 115 short sgn;
WD40andTape 7:5b6a2cefbf3b 116 for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
dofydoink 11:7029367a1840 117 /*dblActPosChange = 1.0; // or = 0.0;
dofydoink 11:7029367a1840 118 _dblVelocity_mmps[ii] = 0.0;*/ // DOESN'T CRASH
dofydoink 11:7029367a1840 119 /*dblActPosChange = dblTargetActPos_mm[ii];
dofydoink 11:7029367a1840 120 _dblVelocity_mmps[ii] = 0.0;*/ // DOESN'T CRASH
dofydoink 11:7029367a1840 121 /*dblActPosChange = _dblLinearPathCurrentPos_mm[ii];*/ // DOESN'T CRASH
dofydoink 11:7029367a1840 122 /*_dblVelocity_mmps[ii] = _dblLinearPathCurrentPos_mm[ii];*/ // DOESN'T CRASH
dofydoink 11:7029367a1840 123 /*dblActPosChange = 0.0;
dofydoink 11:7029367a1840 124 _dblVelocity_mmps[ii] = _dblLinearPathCurrentPos_mm[ii];*/ // DOESN'T CRASH
dofydoink 11:7029367a1840 125
dofydoink 11:7029367a1840 126 // DOES CRASH but not with a return at the end of while OR if _ variables are declared globally
dofydoink 11:7029367a1840 127 /*dblActPosChange = _dblLinearPathCurrentPos_mm[ii];
dofydoink 11:7029367a1840 128 _dblVelocity_mmps[ii] = 0.0;*/
dofydoink 11:7029367a1840 129
dofydoink 11:7029367a1840 130 dblActPosChange = dblTargetActPos_mm[ii] - _dblLinearPathCurrentPos_mm[ii];
WD40andTape 6:f0a18e28a322 131 if( fabs(dblActPosChange) < DBL_PATH_TOLERANCE ) {
WD40andTape 6:f0a18e28a322 132 dblActPosChange = 0.0;
WD40andTape 6:f0a18e28a322 133 isTimeChanged = 1;
dofydoink 0:607bc887b6e0 134 }
dofydoink 11:7029367a1840 135 //IS BELOW
WD40andTape 6:f0a18e28a322 136 if( input.duration < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 137 sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
dofydoink 11:7029367a1840 138 _dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 139 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 140 } else {
dofydoink 11:7029367a1840 141 _dblVelocity_mmps[ii] = dblActPosChange / input.duration;
WD40andTape 6:f0a18e28a322 142 }
dofydoink 11:7029367a1840 143 //IS ABOVE
WD40andTape 7:5b6a2cefbf3b 144 // Check to see if velocities are achievable
dofydoink 11:7029367a1840 145 if(abs(_dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
dofydoink 11:7029367a1840 146 if(_dblVelocity_mmps[ii]>0) {
dofydoink 11:7029367a1840 147 _dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 148 } else {
dofydoink 11:7029367a1840 149 _dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
WD40andTape 6:f0a18e28a322 150 }
WD40andTape 6:f0a18e28a322 151 isTimeChanged = 1;
WD40andTape 6:f0a18e28a322 152 }
WD40andTape 6:f0a18e28a322 153 double dblRecalculatedTime;
dofydoink 11:7029367a1840 154 if( fabs(_dblVelocity_mmps[ii]) < 0.000000001 ) {
WD40andTape 6:f0a18e28a322 155 dblRecalculatedTime = 0;
WD40andTape 6:f0a18e28a322 156 } else {
dofydoink 11:7029367a1840 157 dblRecalculatedTime = dblActPosChange / _dblVelocity_mmps[ii];
WD40andTape 6:f0a18e28a322 158 }
WD40andTape 6:f0a18e28a322 159 if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
WD40andTape 6:f0a18e28a322 160 dblMaxRecalculatedTime = dblRecalculatedTime;
WD40andTape 6:f0a18e28a322 161 }
WD40andTape 6:f0a18e28a322 162 }
WD40andTape 6:f0a18e28a322 163 if( isTimeChanged ) {
WD40andTape 6:f0a18e28a322 164 if( dblMaxRecalculatedTime < input.duration ) {
WD40andTape 6:f0a18e28a322 165 dblMaxRecalculatedTime = input.duration;
WD40andTape 6:f0a18e28a322 166 }
dofydoink 11:7029367a1840 167 if( dblMaxRecalculatedTime >= 0.000000001 ) {
dofydoink 11:7029367a1840 168 for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
dofydoink 11:7029367a1840 169 _dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - _dblLinearPathCurrentPos_mm[ii]) / dblMaxRecalculatedTime;
dofydoink 11:7029367a1840 170 dblVelocity_mmps[ii] = _dblVelocity_mmps[ii];
dofydoink 11:7029367a1840 171 }
WD40andTape 6:f0a18e28a322 172 }
WD40andTape 6:f0a18e28a322 173 }
WD40andTape 6:f0a18e28a322 174
dofydoink 11:7029367a1840 175 //printf("Sending message...\r\n");
WD40andTape 6:f0a18e28a322 176 // SEND MESSAGE
dofydoink 11:7029367a1840 177 //dblMaxRecalculatedTime = 1.0;
WD40andTape 9:cd3607ba5643 178 hlcomms.make_message(&dblMaxRecalculatedTime);
WD40andTape 9:cd3607ba5643 179 error_code = hlcomms.send_message();
WD40andTape 6:f0a18e28a322 180 if( error_code < 0 ) {
WD40andTape 6:f0a18e28a322 181 printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 182 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 6:f0a18e28a322 183 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 184 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 185 return;
WD40andTape 6:f0a18e28a322 186 }
dofydoink 11:7029367a1840 187 //printf("Message sent.\r\n");
WD40andTape 6:f0a18e28a322 188 }
WD40andTape 6:f0a18e28a322 189
dofydoink 0:607bc887b6e0 190 }
dofydoink 0:607bc887b6e0 191
WD40andTape 4:303584310071 192 void CalculateSmoothPath() {
dofydoink 5:712e7634c779 193 int jj;
WD40andTape 1:2a43cf183a62 194 double dblMeasuredSampleTime;
WD40andTape 7:5b6a2cefbf3b 195 double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; // The current position of the smooth path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 196 //double dblPosition_mtrs[N_CHANNELS]; // The actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
WD40andTape 7:5b6a2cefbf3b 197 //double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar = 100,000 Pa)
WD40andTape 4:303584310071 198 while(1) {
dofydoink 0:607bc887b6e0 199 semPathPlan.wait();
WD40andTape 1:2a43cf183a62 200
WD40andTape 1:2a43cf183a62 201 // If run time is more than 50 us from expected, calculate from measured time step
WD40andTape 4:303584310071 202 if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
dofydoink 5:712e7634c779 203 dblMeasuredSampleTime = timer.read();
WD40andTape 4:303584310071 204 } else {
WD40andTape 1:2a43cf183a62 205 dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
WD40andTape 1:2a43cf183a62 206 }
WD40andTape 4:303584310071 207 timer.reset();
WD40andTape 1:2a43cf183a62 208
dofydoink 5:712e7634c779 209 for(jj = 0; jj < N_CHANNELS; jj++) {
WD40andTape 7:5b6a2cefbf3b 210 //dblPressure_bar[jj] = ReadADCPressure_bar(jj); // Read pressure from channel
WD40andTape 7:5b6a2cefbf3b 211 //dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); // Read position from channel
WD40andTape 1:2a43cf183a62 212
WD40andTape 7:5b6a2cefbf3b 213 // Calculate next step in linear path
WD40andTape 7:5b6a2cefbf3b 214 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 7:5b6a2cefbf3b 215 // Check tolerance
WD40andTape 7:5b6a2cefbf3b 216 if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= DBL_PATH_TOLERANCE) {
WD40andTape 7:5b6a2cefbf3b 217 dblVelocity_mmps[jj] = 0.0; // Stop linear path generation when linear path is within tolerance of target position
WD40andTape 7:5b6a2cefbf3b 218 }
dofydoink 11:7029367a1840 219 dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
dofydoink 11:7029367a1840 220 dblLinearPathCurrentPos_mm[jj] = max( 0.0 , dblLinearPathCurrentPos_mm[jj] );
WD40andTape 7:5b6a2cefbf3b 221 mutPathIn.unlock(); // Unlock relevant mutex
dofydoink 0:607bc887b6e0 222
WD40andTape 7:5b6a2cefbf3b 223 // Calculate next step in smooth path
dofydoink 5:712e7634c779 224 dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
dofydoink 11:7029367a1840 225 dblSmoothPathCurrentPos_mm[jj] = max( 0.0 , dblSmoothPathCurrentPos_mm[jj] );
dofydoink 11:7029367a1840 226 llcomms.mutChannel[jj].lock(); // MUTEX LOCK
WD40andTape 10:1b6daba32452 227 llcomms.demandPosition[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
WD40andTape 10:1b6daba32452 228 llcomms.demandPosition[jj] = llcomms.demandPosition[jj] & 0x1FFF; // Ensure number is 13-bit
WD40andTape 8:d6657767a182 229 llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
dofydoink 0:607bc887b6e0 230
WD40andTape 10:1b6daba32452 231 llcomms.isDataReady[jj] = 1; // Signal that data ready
WD40andTape 4:303584310071 232 } // end for
WD40andTape 7:5b6a2cefbf3b 233
WD40andTape 6:f0a18e28a322 234 //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
dofydoink 11:7029367a1840 235 //printf("%f, %f, %f, %f, %f, %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblLinearPathCurrentPos_mm[1],dblLinearPathCurrentPos_mm[2],
dofydoink 11:7029367a1840 236 // dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);
WD40andTape 6:f0a18e28a322 237 //printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
WD40andTape 7:5b6a2cefbf3b 238
WD40andTape 4:303584310071 239 } // end while
WD40andTape 3:c83291bf9fd2 240 }
dofydoink 0:607bc887b6e0 241
WD40andTape 9:cd3607ba5643 242 int main() {
WD40andTape 7:5b6a2cefbf3b 243 pc.baud(BAUD_RATE);
dofydoink 0:607bc887b6e0 244 printf("Hi, there! I'll be your mid-level controller for today.\r\n");
dofydoink 5:712e7634c779 245 wait(3);
WD40andTape 9:cd3607ba5643 246
dofydoink 0:607bc887b6e0 247 timer.start();
WD40andTape 7:5b6a2cefbf3b 248
WD40andTape 10:1b6daba32452 249 threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
WD40andTape 7:5b6a2cefbf3b 250 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 7:5b6a2cefbf3b 251 threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning thread
WD40andTape 7:5b6a2cefbf3b 252
WD40andTape 7:5b6a2cefbf3b 253 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); // Set up planning thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 254
WD40andTape 7:5b6a2cefbf3b 255 Thread::wait(1);
WD40andTape 1:2a43cf183a62 256 while(1) {
WD40andTape 1:2a43cf183a62 257 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 258 }
WD40andTape 7:5b6a2cefbf3b 259 }