MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

main.cpp@20:b2139294c547, 2018-11-27 (annotated)

Committer:
WD40andTape
Date:
Tue Nov 27 16:53:06 2018 +0000
Revision:
20:b2139294c547
Parent:
19:e5acb2183d4e
Child:
21:0b10d8e615d1
Deleted old replan code.

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
WD40andTape 13:a373dfc57b89 11 //DigitalOut pinTesty(PB_8);
WD40andTape 13:a373dfc57b89 12
dofydoink 12:595ed862e52f 13 // Maximum achievable mm path tolerance plus additional % tolerance
dofydoink 12:595ed862e52f 14 const float FLT_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * (1.0f+FLT_PERCENT_PATH_TOLERANCE);
dofydoink 0:607bc887b6e0 15
WD40andTape 7:5b6a2cefbf3b 16 // PATH VARIABLES
WD40andTape 7:5b6a2cefbf3b 17 double dblVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
WD40andTape 16:1e2804a4e5bd 18 double dblLinearPathCurrentPos_mm[N_CHANNELS]= { 0.0 }; // The current position of the linear path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 19 double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
dofydoink 0:607bc887b6e0 20
dofydoink 12:595ed862e52f 21 // These have to be declared in the global scope or we get a stack overflow. No idea why.
dofydoink 11:7029367a1840 22 double _dblVelocity_mmps[N_CHANNELS];
dofydoink 12:595ed862e52f 23 //double _dblLinearPathCurrentPos_mm[N_CHANNELS];
dofydoink 11:7029367a1840 24
WD40andTape 7:5b6a2cefbf3b 25 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
dofydoink 12:595ed862e52f 26 LLComms llcomms;
WD40andTape 16:1e2804a4e5bd 27 HLComms hlcomms(SERVER_PORT);
WD40andTape 3:c83291bf9fd2 28
WD40andTape 10:1b6daba32452 29 Thread threadLowLevelSPI(osPriorityRealtime);
WD40andTape 13:a373dfc57b89 30 Thread threadSmoothPathPlan(osPriorityNormal);
WD40andTape 4:303584310071 31 Thread threadReceiveAndReplan(osPriorityBelowNormal);
WD40andTape 17:bbaf3e8440ad 32 Thread threadSensorFeedback(osPriorityBelowNormal);
dofydoink 0:607bc887b6e0 33
WD40andTape 4:303584310071 34 Mutex mutPathIn;
WD40andTape 7:5b6a2cefbf3b 35 Semaphore semPathPlan(1);
WD40andTape 17:bbaf3e8440ad 36 Semaphore semSensorData(1);
dofydoink 0:607bc887b6e0 37
WD40andTape 7:5b6a2cefbf3b 38 Timer timer;
WD40andTape 7:5b6a2cefbf3b 39 Ticker PathCalculationTicker;
WD40andTape 17:bbaf3e8440ad 40 Ticker SendSensorDataTicker;
WD40andTape 17:bbaf3e8440ad 41
WD40andTape 17:bbaf3e8440ad 42
WD40andTape 17:bbaf3e8440ad 43 void sendSensorData() {
WD40andTape 17:bbaf3e8440ad 44 while( true ) {
WD40andTape 17:bbaf3e8440ad 45 semSensorData.wait();
WD40andTape 17:bbaf3e8440ad 46 double pressures[N_CHANNELS];
WD40andTape 17:bbaf3e8440ad 47 // !!! READ ADC PRESSURE ASSUMES THAT LL ARE ON FIRST
WD40andTape 17:bbaf3e8440ad 48 for(short int i=0; i<N_CHANNELS; i++) {
WD40andTape 17:bbaf3e8440ad 49 pressures[i] = llcomms.ReadADCPressure_bar(i);
WD40andTape 17:bbaf3e8440ad 50 }
WD40andTape 17:bbaf3e8440ad 51 int error_code = hlcomms.send_sensor_message(dblLinearPathCurrentPos_mm,pressures);
WD40andTape 17:bbaf3e8440ad 52 /*if( error_code < 0 ) {
WD40andTape 17:bbaf3e8440ad 53 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 17:bbaf3e8440ad 54 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 17:bbaf3e8440ad 55 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 17:bbaf3e8440ad 56 hlcomms.close_server();
WD40andTape 17:bbaf3e8440ad 57 return;
WD40andTape 17:bbaf3e8440ad 58 }*/
WD40andTape 17:bbaf3e8440ad 59 }
WD40andTape 17:bbaf3e8440ad 60 }
WD40andTape 17:bbaf3e8440ad 61
WD40andTape 17:bbaf3e8440ad 62 void signalSendSensorData() {
WD40andTape 17:bbaf3e8440ad 63 semSensorData.release();
WD40andTape 17:bbaf3e8440ad 64 }
dofydoink 0:607bc887b6e0 65
WD40andTape 7:5b6a2cefbf3b 66 void startPathPlan() { // Plan a new linear path after receiving new target data
WD40andTape 4:303584310071 67 semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
dofydoink 0:607bc887b6e0 68 }
dofydoink 0:607bc887b6e0 69
WD40andTape 7:5b6a2cefbf3b 70 // This function will be called when a new transmission is received from high level
WD40andTape 7:5b6a2cefbf3b 71 void ReceiveAndReplan() {
WD40andTape 6:f0a18e28a322 72
WD40andTape 6:f0a18e28a322 73 int error_code;
WD40andTape 7:5b6a2cefbf3b 74 error_code = hlcomms.setup_server();
WD40andTape 6:f0a18e28a322 75 if( error_code == -1 ) return;
WD40andTape 7:5b6a2cefbf3b 76 error_code = hlcomms.accept_connection();
WD40andTape 7:5b6a2cefbf3b 77 if( error_code == -1 ) {
WD40andTape 7:5b6a2cefbf3b 78 hlcomms.close_server();
WD40andTape 7:5b6a2cefbf3b 79 return;
WD40andTape 7:5b6a2cefbf3b 80 }
WD40andTape 6:f0a18e28a322 81
WD40andTape 18:6533fb7f5a91 82 SendSensorDataTicker.attach(&signalSendSensorData, 0.1); // Set up planning thread to recur at fixed intervals
WD40andTape 17:bbaf3e8440ad 83
WD40andTape 7:5b6a2cefbf3b 84 struct msg_format input; //hlcomms.msg_format
WD40andTape 4:303584310071 85
WD40andTape 6:f0a18e28a322 86 while( true ) {
WD40andTape 6:f0a18e28a322 87 // RECEIVE MESSAGE
WD40andTape 9:cd3607ba5643 88 error_code = hlcomms.receive_message();
WD40andTape 6:f0a18e28a322 89 if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
dofydoink 12:595ed862e52f 90 if(IS_PRINT_OUTPUT) printf("Client disconnected.\n\r");
WD40andTape 7:5b6a2cefbf3b 91 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 92 return;
WD40andTape 6:f0a18e28a322 93 } else if( error_code < 0 ) {
dofydoink 12:595ed862e52f 94 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 95 "Perhaps the server socket is not connected to a remote host? "
WD40andTape 6:f0a18e28a322 96 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 97 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 98 return;
WD40andTape 6:f0a18e28a322 99 }
WD40andTape 17:bbaf3e8440ad 100 // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! RECV MUTEX
WD40andTape 9:cd3607ba5643 101 input = hlcomms.process_message();
WD40andTape 1:2a43cf183a62 102
WD40andTape 7:5b6a2cefbf3b 103 // PROCESS INPUT
WD40andTape 19:e5acb2183d4e 104 if(IS_PRINT_OUTPUT) printf("REPLAN, %f\r\n",input.speed);
WD40andTape 14:54c3759e76ed 105
WD40andTape 19:e5acb2183d4e 106 double dblTarget_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
WD40andTape 19:e5acb2183d4e 107 // Update rear segment
WD40andTape 19:e5acb2183d4e 108 dblTarget_mm[3] = input.psi[0][0]*1000;
WD40andTape 19:e5acb2183d4e 109 dblTarget_mm[5] = 0.0; //input.psi[0][1]*1000;
WD40andTape 19:e5acb2183d4e 110 dblTarget_mm[6] = 0.0; //input.psi[0][2]*1000;
WD40andTape 19:e5acb2183d4e 111 // Update mid segment
WD40andTape 19:e5acb2183d4e 112 dblTarget_mm[4] = input.psi[1][0]*1000;
WD40andTape 19:e5acb2183d4e 113 dblTarget_mm[7] = dblTarget_mm[4]; // Same because two pumps are used
WD40andTape 14:54c3759e76ed 114 // Update front segment
WD40andTape 19:e5acb2183d4e 115 dblTarget_mm[0] = input.psi[2][0]*1000;
WD40andTape 19:e5acb2183d4e 116 dblTarget_mm[1] = input.psi[2][1]*1000;
WD40andTape 19:e5acb2183d4e 117 dblTarget_mm[2] = input.psi[2][2]*1000;
dofydoink 11:7029367a1840 118
WD40andTape 19:e5acb2183d4e 119 // Lock mutex, preventing CalculateSmoothPath from running
WD40andTape 19:e5acb2183d4e 120 mutPathIn.lock();
WD40andTape 19:e5acb2183d4e 121 // Limit requested speed
WD40andTape 19:e5acb2183d4e 122 double limitedSpeed_mmps = min( max( 0.0 , input.speed ) , (double)MAX_SPEED_MMPS );
WD40andTape 19:e5acb2183d4e 123 // For each actuator, limit the input position, calculate the position change, and select the absolute max
WD40andTape 19:e5acb2183d4e 124 double dblDisplacementToTarget_mm[N_CHANNELS];
WD40andTape 19:e5acb2183d4e 125 double maxDistanceToTarget_mm = 0.0;
WD40andTape 19:e5acb2183d4e 126 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 19:e5acb2183d4e 127 // If requested position is negative
WD40andTape 19:e5acb2183d4e 128 if(dblTarget_mm[i]<0) {
WD40andTape 19:e5acb2183d4e 129 // Set actuator position change to 0
WD40andTape 19:e5acb2183d4e 130 dblDisplacementToTarget_mm[i] = 0.0;
WD40andTape 19:e5acb2183d4e 131 } else { // Requested position is positive
WD40andTape 19:e5acb2183d4e 132 // ? Limit requested chamber lengths
WD40andTape 19:e5acb2183d4e 133 // ? Convert from chamber length to actuator space
WD40andTape 19:e5acb2183d4e 134 // Limit actuator position
WD40andTape 19:e5acb2183d4e 135 dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , 40.0 ); // !!! USE A CONSTANT FOR THIS
WD40andTape 19:e5acb2183d4e 136 // Calculate actuator position change
WD40andTape 19:e5acb2183d4e 137 double dblCurrentPosition = dblLinearPathCurrentPos_mm[i];
WD40andTape 19:e5acb2183d4e 138 //dblCurrentPosition[i] = llcomms.ReadADCPosition_mtrs(i); // Read position from channel
WD40andTape 19:e5acb2183d4e 139 dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition;
WD40andTape 19:e5acb2183d4e 140 // Select the max absolute actuator position change
WD40andTape 19:e5acb2183d4e 141 if(fabs(dblDisplacementToTarget_mm[i])>maxDistanceToTarget_mm) {
WD40andTape 19:e5acb2183d4e 142 maxDistanceToTarget_mm = fabs(dblDisplacementToTarget_mm[i]);
WD40andTape 19:e5acb2183d4e 143 }
WD40andTape 19:e5acb2183d4e 144 }
dofydoink 11:7029367a1840 145 }
WD40andTape 19:e5acb2183d4e 146 // For max actuator position change, calculate the time to destination at the limited speed
WD40andTape 19:e5acb2183d4e 147 double maxTimeToTarget_s = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
WD40andTape 19:e5acb2183d4e 148 // For each actuator, replan target position and velocity as required
WD40andTape 19:e5acb2183d4e 149 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 19:e5acb2183d4e 150 // If requested actuator position change is already within tolerance, do NOT replan that actuator
WD40andTape 19:e5acb2183d4e 151 if( fabs(dblDisplacementToTarget_mm[i]) < FLT_PATH_TOLERANCE ) continue;
WD40andTape 19:e5acb2183d4e 152 // Calculate velocity for each motor to synchronise movements to complete in max time
WD40andTape 19:e5acb2183d4e 153 // Set dblTargetActPos_mm and dblVelocity_mmps
WD40andTape 19:e5acb2183d4e 154 dblTargetActPos_mm[i] = dblTarget_mm[i];
WD40andTape 19:e5acb2183d4e 155 dblVelocity_mmps[i] = dblDisplacementToTarget_mm[i] / maxTimeToTarget_s;
WD40andTape 19:e5acb2183d4e 156 }
WD40andTape 19:e5acb2183d4e 157 // Unlock mutex, allowing CalculateSmoothPath to run again
WD40andTape 19:e5acb2183d4e 158 mutPathIn.unlock();
dofydoink 12:595ed862e52f 159
WD40andTape 6:f0a18e28a322 160 // SEND MESSAGE
WD40andTape 19:e5acb2183d4e 161 error_code = hlcomms.send_duration_message(&maxTimeToTarget_s);
WD40andTape 6:f0a18e28a322 162 if( error_code < 0 ) {
dofydoink 12:595ed862e52f 163 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 164 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 6:f0a18e28a322 165 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 166 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 167 return;
WD40andTape 6:f0a18e28a322 168 }
WD40andTape 6:f0a18e28a322 169 }
WD40andTape 6:f0a18e28a322 170
dofydoink 0:607bc887b6e0 171 }
dofydoink 0:607bc887b6e0 172
WD40andTape 4:303584310071 173 void CalculateSmoothPath() {
dofydoink 5:712e7634c779 174 int jj;
WD40andTape 1:2a43cf183a62 175 double dblMeasuredSampleTime;
WD40andTape 7:5b6a2cefbf3b 176 double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; // The current position of the smooth path (not sent to actuator)
WD40andTape 7:5b6a2cefbf3b 177 //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 178 //double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar = 100,000 Pa)
WD40andTape 4:303584310071 179 while(1) {
dofydoink 0:607bc887b6e0 180 semPathPlan.wait();
WD40andTape 13:a373dfc57b89 181 //pinTesty = 1;
WD40andTape 1:2a43cf183a62 182 // If run time is more than 50 us from expected, calculate from measured time step
WD40andTape 4:303584310071 183 if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
dofydoink 5:712e7634c779 184 dblMeasuredSampleTime = timer.read();
WD40andTape 4:303584310071 185 } else {
WD40andTape 1:2a43cf183a62 186 dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
WD40andTape 1:2a43cf183a62 187 }
WD40andTape 4:303584310071 188 timer.reset();
WD40andTape 1:2a43cf183a62 189
dofydoink 5:712e7634c779 190 for(jj = 0; jj < N_CHANNELS; jj++) {
WD40andTape 7:5b6a2cefbf3b 191 //dblPressure_bar[jj] = ReadADCPressure_bar(jj); // Read pressure from channel
WD40andTape 7:5b6a2cefbf3b 192 //dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); // Read position from channel
WD40andTape 1:2a43cf183a62 193
WD40andTape 7:5b6a2cefbf3b 194 // Calculate next step in linear path
WD40andTape 7:5b6a2cefbf3b 195 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 7:5b6a2cefbf3b 196 // Check tolerance
dofydoink 12:595ed862e52f 197 if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= FLT_PATH_TOLERANCE) {
WD40andTape 7:5b6a2cefbf3b 198 dblVelocity_mmps[jj] = 0.0; // Stop linear path generation when linear path is within tolerance of target position
WD40andTape 7:5b6a2cefbf3b 199 }
dofydoink 11:7029367a1840 200 dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
WD40andTape 15:59471daef4cb 201 dblLinearPathCurrentPos_mm[jj] = min( max( 0.0 , dblLinearPathCurrentPos_mm[jj] ) , 40.0 );
WD40andTape 7:5b6a2cefbf3b 202 mutPathIn.unlock(); // Unlock relevant mutex
dofydoink 0:607bc887b6e0 203
WD40andTape 7:5b6a2cefbf3b 204 // Calculate next step in smooth path
dofydoink 12:595ed862e52f 205 dblSmoothPathCurrentPos_mm[jj] = FLT_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0f-FLT_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
dofydoink 11:7029367a1840 206 dblSmoothPathCurrentPos_mm[jj] = max( 0.0 , dblSmoothPathCurrentPos_mm[jj] );
dofydoink 11:7029367a1840 207 llcomms.mutChannel[jj].lock(); // MUTEX LOCK
WD40andTape 10:1b6daba32452 208 llcomms.demandPosition[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
WD40andTape 10:1b6daba32452 209 llcomms.demandPosition[jj] = llcomms.demandPosition[jj] & 0x1FFF; // Ensure number is 13-bit
WD40andTape 8:d6657767a182 210 llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
dofydoink 0:607bc887b6e0 211
WD40andTape 10:1b6daba32452 212 llcomms.isDataReady[jj] = 1; // Signal that data ready
WD40andTape 4:303584310071 213 } // end for
WD40andTape 7:5b6a2cefbf3b 214
dofydoink 12:595ed862e52f 215 //if(IS_PRINT_OUTPUT) printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
dofydoink 12:595ed862e52f 216 //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 217 // dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);
dofydoink 12:595ed862e52f 218 //if(IS_PRINT_OUTPUT) printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
WD40andTape 13:a373dfc57b89 219 //pinTesty = 0;
WD40andTape 4:303584310071 220 } // end while
WD40andTape 3:c83291bf9fd2 221 }
dofydoink 0:607bc887b6e0 222
WD40andTape 13:a373dfc57b89 223 int main() {
WD40andTape 7:5b6a2cefbf3b 224 pc.baud(BAUD_RATE);
WD40andTape 17:bbaf3e8440ad 225 printf("ML engage. Compiled at %s\r\n.",__TIME__);
dofydoink 5:712e7634c779 226 wait(3);
WD40andTape 9:cd3607ba5643 227
dofydoink 0:607bc887b6e0 228 timer.start();
WD40andTape 7:5b6a2cefbf3b 229
WD40andTape 10:1b6daba32452 230 threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
WD40andTape 7:5b6a2cefbf3b 231 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 7:5b6a2cefbf3b 232 threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning thread
WD40andTape 17:bbaf3e8440ad 233 threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
WD40andTape 7:5b6a2cefbf3b 234
WD40andTape 7:5b6a2cefbf3b 235 PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); // Set up planning thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 236
WD40andTape 7:5b6a2cefbf3b 237 Thread::wait(1);
WD40andTape 1:2a43cf183a62 238 while(1) {
WD40andTape 1:2a43cf183a62 239 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 240 }
WD40andTape 7:5b6a2cefbf3b 241 }