MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

main.cpp@41:0e3898b29230, 2020-09-03 (annotated)

Committer:
dofydoink
Date:
Thu Sep 03 16:30:23 2020 +0000
Revision:
41:0e3898b29230
Parent:
36:4459be8296e9 
Adjusted max pressure to 12bar to fit new sensors

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 28:8e0c502c1a50 17 HLComms hlcomms(HL_COMMS_FREQ_HZ);
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 void sendSensorData() {
WD40andTape 17:bbaf3e8440ad 32 while( true ) {
WD40andTape 17:bbaf3e8440ad 33 semSensorData.wait();
WD40andTape 29:10a5cf37a875 34 hlcomms.send_sensor_message(llcomms.positionSensor_uint,llcomms.pressureSensor_uint);
WD40andTape 17:bbaf3e8440ad 35 }
WD40andTape 17:bbaf3e8440ad 36 }
WD40andTape 17:bbaf3e8440ad 37
WD40andTape 17:bbaf3e8440ad 38 void signalSendSensorData() {
WD40andTape 17:bbaf3e8440ad 39 semSensorData.release();
WD40andTape 17:bbaf3e8440ad 40 }
dofydoink 0:607bc887b6e0 41
WD40andTape 7:5b6a2cefbf3b 42 // This function will be called when a new transmission is received from high level
WD40andTape 7:5b6a2cefbf3b 43 void ReceiveAndReplan() {
WD40andTape 6:f0a18e28a322 44
WD40andTape 22:82871f00f89d 45 SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals
WD40andTape 17:bbaf3e8440ad 46
WD40andTape 28:8e0c502c1a50 47 struct demands_struct input;
WD40andTape 33:9877ca32e43c 48 DigitalOut SupportPins[4] = {PE_4, PE_2, PE_3, PE_6};
WD40andTape 4:303584310071 49
WD40andTape 6:f0a18e28a322 50 while( true ) {
WD40andTape 28:8e0c502c1a50 51 hlcomms.newData.wait();
WD40andTape 28:8e0c502c1a50 52 input = hlcomms.get_demands();
WD40andTape 33:9877ca32e43c 53 // SUPPORT FUNCTIONS
WD40andTape 33:9877ca32e43c 54 // [isInsufflate,isSuction,isWashLens,isJet]
WD40andTape 33:9877ca32e43c 55 for(short int i=0; i<4; i++) {
WD40andTape 33:9877ca32e43c 56 if(i<2) { // Active low, i.e. 0 = Off
WD40andTape 33:9877ca32e43c 57 SupportPins[i].write((short int)input.utitilies_bool[i]);
WD40andTape 33:9877ca32e43c 58 } else { // Active high, i.e. 1 = Off
WD40andTape 33:9877ca32e43c 59 SupportPins[i].write((short int)(!input.utitilies_bool[i]));
WD40andTape 33:9877ca32e43c 60 }
WD40andTape 33:9877ca32e43c 61 }
WD40andTape 7:5b6a2cefbf3b 62 // PROCESS INPUT
WD40andTape 32:8c59c536a2a6 63 double maxTimesToTarget_s[3] = { -1.0 };
WD40andTape 32:8c59c536a2a6 64 //[8,7,6,4,3,-1,-1,0,-1]
WD40andTape 32:8c59c536a2a6 65 //FRONT = channels 0,1,2
WD40andTape 32:8c59c536a2a6 66 //MID = channels 3,4(,5)
WD40andTape 32:8c59c536a2a6 67 //REAR = channels (6,)7(,8)
WD40andTape 32:8c59c536a2a6 68 // Lock mutex, preventing setDemandsForLL from running
WD40andTape 32:8c59c536a2a6 69 mutPathIn.lock();
WD40andTape 31:08cb04eb75fc 70 for(short int segNum=0; segNum<3; segNum++) {
WD40andTape 31:08cb04eb75fc 71 // Limit requested speed
WD40andTape 32:8c59c536a2a6 72 if( input.speeds_mmps[segNum] > 0 ) {
WD40andTape 32:8c59c536a2a6 73 double limitedSpeed_mmps = min( max( 0.0 , input.speeds_mmps[segNum] ) , (double)MAX_SPEED_MMPS );
WD40andTape 31:08cb04eb75fc 74 // For each actuator, limit the input position, calculate the position change, and select the absolute max
WD40andTape 31:08cb04eb75fc 75 double dblDistanceToTarget_mm[3] = { -1.0 };
WD40andTape 31:08cb04eb75fc 76 double maxDistanceToTarget_mm = 0.0;
WD40andTape 32:8c59c536a2a6 77 for(short int i=0; i<3; i++) {
WD40andTape 32:8c59c536a2a6 78 short int channel = segNum*3+i;
WD40andTape 36:4459be8296e9 79 if(channel>=N_CHANNELS) {
WD40andTape 32:8c59c536a2a6 80 continue;
WD40andTape 32:8c59c536a2a6 81 }
WD40andTape 32:8c59c536a2a6 82 double dblCurrentPosition_mm = llcomms.positionSensor_mm[channel];
WD40andTape 32:8c59c536a2a6 83 if(input.psi_mm[channel]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
WD40andTape 31:08cb04eb75fc 84 continue;
WD40andTape 31:08cb04eb75fc 85 }
WD40andTape 31:08cb04eb75fc 86 // Limit actuator position
WD40andTape 32:8c59c536a2a6 87 input.psi_mm[channel] = min( max( 0.0 , input.psi_mm[channel] ) , (double)MAX_ACTUATOR_LIMIT_MM );
WD40andTape 31:08cb04eb75fc 88 // Calculate actuator position change
WD40andTape 32:8c59c536a2a6 89 dblDistanceToTarget_mm[i] = fabs(input.psi_mm[channel] - dblCurrentPosition_mm);
WD40andTape 31:08cb04eb75fc 90 // Select the max absolute actuator position change
WD40andTape 31:08cb04eb75fc 91 if(dblDistanceToTarget_mm[i]>maxDistanceToTarget_mm) {
WD40andTape 31:08cb04eb75fc 92 maxDistanceToTarget_mm = dblDistanceToTarget_mm[i];
WD40andTape 31:08cb04eb75fc 93 }
WD40andTape 29:10a5cf37a875 94 }
WD40andTape 31:08cb04eb75fc 95 // For max actuator position change, calculate the time to destination at the limited speed
WD40andTape 31:08cb04eb75fc 96 maxTimesToTarget_s[segNum] = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
WD40andTape 31:08cb04eb75fc 97 // For each actuator, replan target position and velocity as required
WD40andTape 32:8c59c536a2a6 98 for(short int i=0; i<3; i++) {
WD40andTape 32:8c59c536a2a6 99 short int channel = segNum*3+i;
WD40andTape 31:08cb04eb75fc 100 // If requested actuator position change is already within tolerance, do NOT replan that actuator
WD40andTape 31:08cb04eb75fc 101 if( dblDistanceToTarget_mm[i] <= 0.0 ) continue;
WD40andTape 31:08cb04eb75fc 102 // Calculate velocity for each motor to synchronise movements to complete in max time
WD40andTape 31:08cb04eb75fc 103 // Set dblDemandPosition_mm and dblDemandSpeed_mmps
WD40andTape 32:8c59c536a2a6 104 dblDemandPosition_mm[channel] = input.psi_mm[channel];
WD40andTape 32:8c59c536a2a6 105 dblDemandSpeed_mmps[channel] = dblDistanceToTarget_mm[i] / maxTimesToTarget_s[segNum];
WD40andTape 19:e5acb2183d4e 106 }
WD40andTape 31:08cb04eb75fc 107 } else {
WD40andTape 32:8c59c536a2a6 108 maxTimesToTarget_s[segNum] = -1.0;
WD40andTape 19:e5acb2183d4e 109 }
dofydoink 11:7029367a1840 110 }
WD40andTape 32:8c59c536a2a6 111 // Unlock mutex, allowing setDemandsForLL to run again
WD40andTape 32:8c59c536a2a6 112 mutPathIn.unlock();
dofydoink 12:595ed862e52f 113
WD40andTape 6:f0a18e28a322 114 // SEND MESSAGE
WD40andTape 31:08cb04eb75fc 115 hlcomms.send_durations_message(maxTimesToTarget_s);
WD40andTape 6:f0a18e28a322 116 }
WD40andTape 6:f0a18e28a322 117
dofydoink 0:607bc887b6e0 118 }
dofydoink 0:607bc887b6e0 119
WD40andTape 24:bc852aa89e7a 120 void startLLcomms() { // Send new demands to LL after receiving new target data
WD40andTape 24:bc852aa89e7a 121 semLLcomms.release(); // Uses threadSetDemands which is normal priority
WD40andTape 24:bc852aa89e7a 122 }
WD40andTape 24:bc852aa89e7a 123
WD40andTape 21:0b10d8e615d1 124 void setDemandsForLL() {
WD40andTape 21:0b10d8e615d1 125
WD40andTape 4:303584310071 126 while(1) {
WD40andTape 21:0b10d8e615d1 127 semLLcomms.wait();
WD40andTape 21:0b10d8e615d1 128 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 21:0b10d8e615d1 129 for(short int i=0; i<N_CHANNELS; i++) { // For each LL
WD40andTape 21:0b10d8e615d1 130 llcomms.mutChannel[i].lock(); // MUTEX LOCK
WD40andTape 26:7c59002c9cd7 131 llcomms.demandPosition_mm[i] = dblDemandPosition_mm[i];
WD40andTape 26:7c59002c9cd7 132 llcomms.demandSpeed_mmps[i] = dblDemandSpeed_mmps[i];
WD40andTape 21:0b10d8e615d1 133 llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
WD40andTape 22:82871f00f89d 134 llcomms.isDataReady[i] = 1; // Signal that data ready
WD40andTape 21:0b10d8e615d1 135 } // end for
WD40andTape 21:0b10d8e615d1 136 mutPathIn.unlock(); // Unlock relevant mutex
WD40andTape 24:bc852aa89e7a 137 } // end while(1)
WD40andTape 26:7c59002c9cd7 138
WD40andTape 3:c83291bf9fd2 139 }
dofydoink 0:607bc887b6e0 140
WD40andTape 13:a373dfc57b89 141 int main() {
WD40andTape 7:5b6a2cefbf3b 142 pc.baud(BAUD_RATE);
WD40andTape 31:08cb04eb75fc 143 //printf("ML engage. Compiled at %s\r\n.",__TIME__);
dofydoink 5:712e7634c779 144 wait(3);
WD40andTape 9:cd3607ba5643 145
WD40andTape 10:1b6daba32452 146 threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
WD40andTape 7:5b6a2cefbf3b 147 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 21:0b10d8e615d1 148 threadSetDemands.start(setDemandsForLL); // Start planning thread
WD40andTape 17:bbaf3e8440ad 149 threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
WD40andTape 7:5b6a2cefbf3b 150
WD40andTape 24:bc852aa89e7a 151 setDemandsTicker.attach(&startLLcomms, 1/(float)LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 152
WD40andTape 7:5b6a2cefbf3b 153 Thread::wait(1);
WD40andTape 1:2a43cf183a62 154 while(1) {
WD40andTape 1:2a43cf183a62 155 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 156 }
WD40andTape 7:5b6a2cefbf3b 157 }