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@10:1b6daba32452, 2018-08-07 (annotated)

Committer:
WD40andTape
Date:
Tue Aug 07 15:31:59 2018 +0000
Revision:
10:1b6daba32452
Parent:
9:cd3607ba5643
Child:
11:7029367a1840
Moved final bits of low level communication code into the class. Tested and working. However stack overflow occurs on the ML during joystick steering.

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