MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

main.cpp@12:595ed862e52f, 2018-08-13 (annotated)

Committer:
dofydoink
Date:
Mon Aug 13 09:16:29 2018 +0000
Revision:
12:595ed862e52f
Parent:
11:7029367a1840
Child:
13:a373dfc57b89
Tested and working with 3 channels.

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