MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

main.cpp@27:6853ee8ffefd, 2019-01-29 (annotated)

Committer:
WD40andTape
Date:
Tue Jan 29 14:58:45 2019 +0000
Revision:
27:6853ee8ffefd
Parent:
26:7c59002c9cd7
Child:
28:8e0c502c1a50
Fixed sensor messages. Working.

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 21:0b10d8e615d1 11 // DEMAND VARIABLES
WD40andTape 26:7c59002c9cd7 12 double dblDemandSpeed_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
WD40andTape 26:7c59002c9cd7 13 double dblDemandPosition_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
dofydoink 11:7029367a1840 14
WD40andTape 7:5b6a2cefbf3b 15 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
dofydoink 12:595ed862e52f 16 LLComms llcomms;
WD40andTape 16:1e2804a4e5bd 17 HLComms hlcomms(SERVER_PORT);
WD40andTape 3:c83291bf9fd2 18
WD40andTape 10:1b6daba32452 19 Thread threadLowLevelSPI(osPriorityRealtime);
WD40andTape 21:0b10d8e615d1 20 Thread threadSetDemands(osPriorityNormal);
WD40andTape 4:303584310071 21 Thread threadReceiveAndReplan(osPriorityBelowNormal);
WD40andTape 17:bbaf3e8440ad 22 Thread threadSensorFeedback(osPriorityBelowNormal);
dofydoink 0:607bc887b6e0 23
WD40andTape 4:303584310071 24 Mutex mutPathIn;
WD40andTape 21:0b10d8e615d1 25 Semaphore semLLcomms(1);
WD40andTape 17:bbaf3e8440ad 26 Semaphore semSensorData(1);
dofydoink 0:607bc887b6e0 27
WD40andTape 21:0b10d8e615d1 28 Ticker setDemandsTicker;
WD40andTape 17:bbaf3e8440ad 29 Ticker SendSensorDataTicker;
WD40andTape 17:bbaf3e8440ad 30
WD40andTape 17:bbaf3e8440ad 31
WD40andTape 17:bbaf3e8440ad 32 void sendSensorData() {
WD40andTape 17:bbaf3e8440ad 33 while( true ) {
WD40andTape 17:bbaf3e8440ad 34 semSensorData.wait();
WD40andTape 26:7c59002c9cd7 35 int error_code = hlcomms.send_sensor_message(llcomms.positionSensor_mm,llcomms.pressureSensor_bar);
WD40andTape 17:bbaf3e8440ad 36 /*if( error_code < 0 ) {
WD40andTape 17:bbaf3e8440ad 37 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 17:bbaf3e8440ad 38 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 17:bbaf3e8440ad 39 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 17:bbaf3e8440ad 40 hlcomms.close_server();
WD40andTape 17:bbaf3e8440ad 41 return;
WD40andTape 17:bbaf3e8440ad 42 }*/
WD40andTape 17:bbaf3e8440ad 43 }
WD40andTape 17:bbaf3e8440ad 44 }
WD40andTape 17:bbaf3e8440ad 45
WD40andTape 17:bbaf3e8440ad 46 void signalSendSensorData() {
WD40andTape 17:bbaf3e8440ad 47 semSensorData.release();
WD40andTape 17:bbaf3e8440ad 48 }
dofydoink 0:607bc887b6e0 49
WD40andTape 7:5b6a2cefbf3b 50 // This function will be called when a new transmission is received from high level
WD40andTape 7:5b6a2cefbf3b 51 void ReceiveAndReplan() {
WD40andTape 6:f0a18e28a322 52
WD40andTape 6:f0a18e28a322 53 int error_code;
WD40andTape 7:5b6a2cefbf3b 54 error_code = hlcomms.setup_server();
WD40andTape 6:f0a18e28a322 55 if( error_code == -1 ) return;
WD40andTape 7:5b6a2cefbf3b 56 error_code = hlcomms.accept_connection();
WD40andTape 7:5b6a2cefbf3b 57 if( error_code == -1 ) {
WD40andTape 7:5b6a2cefbf3b 58 hlcomms.close_server();
WD40andTape 7:5b6a2cefbf3b 59 return;
WD40andTape 7:5b6a2cefbf3b 60 }
WD40andTape 6:f0a18e28a322 61
WD40andTape 22:82871f00f89d 62 SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals
WD40andTape 17:bbaf3e8440ad 63
WD40andTape 7:5b6a2cefbf3b 64 struct msg_format input; //hlcomms.msg_format
WD40andTape 4:303584310071 65
WD40andTape 6:f0a18e28a322 66 while( true ) {
WD40andTape 6:f0a18e28a322 67 // RECEIVE MESSAGE
WD40andTape 9:cd3607ba5643 68 error_code = hlcomms.receive_message();
WD40andTape 6:f0a18e28a322 69 if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
dofydoink 12:595ed862e52f 70 if(IS_PRINT_OUTPUT) printf("Client disconnected.\n\r");
WD40andTape 7:5b6a2cefbf3b 71 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 72 return;
WD40andTape 6:f0a18e28a322 73 } else if( error_code < 0 ) {
dofydoink 12:595ed862e52f 74 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 75 "Perhaps the server socket is not connected to a remote host? "
WD40andTape 6:f0a18e28a322 76 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 77 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 78 return;
WD40andTape 6:f0a18e28a322 79 }
WD40andTape 17:bbaf3e8440ad 80 // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! RECV MUTEX
WD40andTape 9:cd3607ba5643 81 input = hlcomms.process_message();
WD40andTape 1:2a43cf183a62 82
WD40andTape 7:5b6a2cefbf3b 83 // PROCESS INPUT
WD40andTape 19:e5acb2183d4e 84 if(IS_PRINT_OUTPUT) printf("REPLAN, %f\r\n",input.speed);
WD40andTape 14:54c3759e76ed 85
WD40andTape 19:e5acb2183d4e 86 double dblTarget_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
WD40andTape 19:e5acb2183d4e 87 // Update rear segment
WD40andTape 19:e5acb2183d4e 88 dblTarget_mm[3] = input.psi[0][0]*1000;
WD40andTape 19:e5acb2183d4e 89 dblTarget_mm[5] = 0.0; //input.psi[0][1]*1000;
WD40andTape 19:e5acb2183d4e 90 dblTarget_mm[6] = 0.0; //input.psi[0][2]*1000;
WD40andTape 19:e5acb2183d4e 91 // Update mid segment
WD40andTape 19:e5acb2183d4e 92 dblTarget_mm[4] = input.psi[1][0]*1000;
WD40andTape 19:e5acb2183d4e 93 dblTarget_mm[7] = dblTarget_mm[4]; // Same because two pumps are used
WD40andTape 14:54c3759e76ed 94 // Update front segment
WD40andTape 19:e5acb2183d4e 95 dblTarget_mm[0] = input.psi[2][0]*1000;
WD40andTape 19:e5acb2183d4e 96 dblTarget_mm[1] = input.psi[2][1]*1000;
WD40andTape 19:e5acb2183d4e 97 dblTarget_mm[2] = input.psi[2][2]*1000;
dofydoink 11:7029367a1840 98
WD40andTape 21:0b10d8e615d1 99 // Lock mutex, preventing setDemandsForLL from running
WD40andTape 19:e5acb2183d4e 100 mutPathIn.lock();
WD40andTape 19:e5acb2183d4e 101 // Limit requested speed
WD40andTape 19:e5acb2183d4e 102 double limitedSpeed_mmps = min( max( 0.0 , input.speed ) , (double)MAX_SPEED_MMPS );
WD40andTape 19:e5acb2183d4e 103 // For each actuator, limit the input position, calculate the position change, and select the absolute max
WD40andTape 19:e5acb2183d4e 104 double dblDisplacementToTarget_mm[N_CHANNELS];
WD40andTape 19:e5acb2183d4e 105 double maxDistanceToTarget_mm = 0.0;
WD40andTape 19:e5acb2183d4e 106 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 26:7c59002c9cd7 107 double dblCurrentPosition_mm = llcomms.positionSensor_mm[i];
WD40andTape 26:7c59002c9cd7 108 if(dblTarget_mm[i]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
WD40andTape 19:e5acb2183d4e 109 // Set actuator position change to 0
WD40andTape 19:e5acb2183d4e 110 dblDisplacementToTarget_mm[i] = 0.0;
WD40andTape 19:e5acb2183d4e 111 } else { // Requested position is positive
WD40andTape 19:e5acb2183d4e 112 // ? Limit requested chamber lengths
WD40andTape 19:e5acb2183d4e 113 // ? Convert from chamber length to actuator space
WD40andTape 19:e5acb2183d4e 114 // Limit actuator position
WD40andTape 26:7c59002c9cd7 115 dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , (double)MAX_ACTUATOR_LIMIT_MM );
WD40andTape 19:e5acb2183d4e 116 // Calculate actuator position change
WD40andTape 26:7c59002c9cd7 117 dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition_mm;
WD40andTape 19:e5acb2183d4e 118 // Select the max absolute actuator position change
WD40andTape 19:e5acb2183d4e 119 if(fabs(dblDisplacementToTarget_mm[i])>maxDistanceToTarget_mm) {
WD40andTape 19:e5acb2183d4e 120 maxDistanceToTarget_mm = fabs(dblDisplacementToTarget_mm[i]);
WD40andTape 19:e5acb2183d4e 121 }
WD40andTape 19:e5acb2183d4e 122 }
dofydoink 11:7029367a1840 123 }
WD40andTape 19:e5acb2183d4e 124 // For max actuator position change, calculate the time to destination at the limited speed
WD40andTape 19:e5acb2183d4e 125 double maxTimeToTarget_s = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
WD40andTape 19:e5acb2183d4e 126 // For each actuator, replan target position and velocity as required
WD40andTape 19:e5acb2183d4e 127 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 19:e5acb2183d4e 128 // If requested actuator position change is already within tolerance, do NOT replan that actuator
WD40andTape 26:7c59002c9cd7 129 //printf("%d\t%0.5f\t%0.5f\r\n",i,dblDisplacementToTarget_mm[i],FLT_PATH_TOLERANCE_MM);
WD40andTape 26:7c59002c9cd7 130 //printf("%d\t%0.5f\t%0.5f\t%0.5f\r\n",i,dblTarget_mm[i],fabs(dblDisplacementToTarget_mm[i]),maxTimeToTarget_s);
WD40andTape 26:7c59002c9cd7 131 if( fabs(dblDisplacementToTarget_mm[i]) < FLT_PATH_TOLERANCE_MM ) continue;
WD40andTape 19:e5acb2183d4e 132 // Calculate velocity for each motor to synchronise movements to complete in max time
WD40andTape 26:7c59002c9cd7 133 // Set dblDemandPosition_mm and dblDemandSpeed_mmps
WD40andTape 26:7c59002c9cd7 134 //printf("%d\t%0.5f\t%0.5f\t%0.5f\r\n",i,dblTarget_mm[i],fabs(dblDisplacementToTarget_mm[i]),maxTimeToTarget_s);
WD40andTape 26:7c59002c9cd7 135 dblDemandPosition_mm[i] = dblTarget_mm[i];
WD40andTape 26:7c59002c9cd7 136 dblDemandSpeed_mmps[i] = fabs(dblDisplacementToTarget_mm[i]) / maxTimeToTarget_s;
WD40andTape 19:e5acb2183d4e 137 }
WD40andTape 21:0b10d8e615d1 138 // Unlock mutex, allowing setDemandsForLL to run again
WD40andTape 19:e5acb2183d4e 139 mutPathIn.unlock();
dofydoink 12:595ed862e52f 140
WD40andTape 6:f0a18e28a322 141 // SEND MESSAGE
WD40andTape 19:e5acb2183d4e 142 error_code = hlcomms.send_duration_message(&maxTimeToTarget_s);
WD40andTape 6:f0a18e28a322 143 if( error_code < 0 ) {
dofydoink 12:595ed862e52f 144 if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
WD40andTape 6:f0a18e28a322 145 "Perhaps the server socket is not bound or not set to listen for connections? "
WD40andTape 6:f0a18e28a322 146 "Or the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 147 hlcomms.close_server();
WD40andTape 6:f0a18e28a322 148 return;
WD40andTape 6:f0a18e28a322 149 }
WD40andTape 6:f0a18e28a322 150 }
WD40andTape 6:f0a18e28a322 151
dofydoink 0:607bc887b6e0 152 }
dofydoink 0:607bc887b6e0 153
WD40andTape 24:bc852aa89e7a 154 void startLLcomms() { // Send new demands to LL after receiving new target data
WD40andTape 24:bc852aa89e7a 155 semLLcomms.release(); // Uses threadSetDemands which is normal priority
WD40andTape 24:bc852aa89e7a 156 }
WD40andTape 24:bc852aa89e7a 157
WD40andTape 26:7c59002c9cd7 158 Timer timerA;
WD40andTape 21:0b10d8e615d1 159 void setDemandsForLL() {
WD40andTape 21:0b10d8e615d1 160
WD40andTape 4:303584310071 161 while(1) {
WD40andTape 21:0b10d8e615d1 162 semLLcomms.wait();
WD40andTape 21:0b10d8e615d1 163 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 21:0b10d8e615d1 164 for(short int i=0; i<N_CHANNELS; i++) { // For each LL
WD40andTape 21:0b10d8e615d1 165 llcomms.mutChannel[i].lock(); // MUTEX LOCK
WD40andTape 26:7c59002c9cd7 166 //llcomms.demandPosition_mm[i] = 2*sin(timerA.read())+4;
WD40andTape 26:7c59002c9cd7 167 //llcomms.demandSpeed_mmps[i] = 4.0;
WD40andTape 26:7c59002c9cd7 168 //printf("%d\t%0.5f\t%0.5f\r\n",i,dblDemandPosition_mm[i],dblDemandSpeed_mmps[i]);
WD40andTape 26:7c59002c9cd7 169 llcomms.demandPosition_mm[i] = dblDemandPosition_mm[i];
WD40andTape 26:7c59002c9cd7 170 llcomms.demandSpeed_mmps[i] = dblDemandSpeed_mmps[i];
WD40andTape 21:0b10d8e615d1 171 llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
WD40andTape 22:82871f00f89d 172 llcomms.isDataReady[i] = 1; // Signal that data ready
WD40andTape 21:0b10d8e615d1 173 } // end for
WD40andTape 21:0b10d8e615d1 174 mutPathIn.unlock(); // Unlock relevant mutex
WD40andTape 24:bc852aa89e7a 175 } // end while(1)
WD40andTape 26:7c59002c9cd7 176
WD40andTape 3:c83291bf9fd2 177 }
dofydoink 0:607bc887b6e0 178
WD40andTape 13:a373dfc57b89 179 int main() {
WD40andTape 7:5b6a2cefbf3b 180 pc.baud(BAUD_RATE);
WD40andTape 17:bbaf3e8440ad 181 printf("ML engage. Compiled at %s\r\n.",__TIME__);
dofydoink 5:712e7634c779 182 wait(3);
WD40andTape 9:cd3607ba5643 183
WD40andTape 26:7c59002c9cd7 184 timerA.start();
WD40andTape 26:7c59002c9cd7 185
WD40andTape 10:1b6daba32452 186 threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
WD40andTape 7:5b6a2cefbf3b 187 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 21:0b10d8e615d1 188 threadSetDemands.start(setDemandsForLL); // Start planning thread
WD40andTape 17:bbaf3e8440ad 189 threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
WD40andTape 7:5b6a2cefbf3b 190
WD40andTape 24:bc852aa89e7a 191 setDemandsTicker.attach(&startLLcomms, 1/(float)LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 192
WD40andTape 7:5b6a2cefbf3b 193 Thread::wait(1);
WD40andTape 1:2a43cf183a62 194 while(1) {
WD40andTape 1:2a43cf183a62 195 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 196 }
WD40andTape 7:5b6a2cefbf3b 197 }