MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

main.cpp@28:8e0c502c1a50, 2019-02-06 (annotated)

Committer:
WD40andTape
Date:
Wed Feb 06 16:10:18 2019 +0000
Revision:
28:8e0c502c1a50
Parent:
27:6853ee8ffefd
Child:
29:10a5cf37a875
Tested and working (separate from HL).

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 28:8e0c502c1a50 34 hlcomms.send_sensor_message(llcomms.positionSensor_mm,llcomms.pressureSensor_bar);
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 4:303584310071 48
WD40andTape 6:f0a18e28a322 49 while( true ) {
WD40andTape 28:8e0c502c1a50 50 hlcomms.newData.wait();
WD40andTape 28:8e0c502c1a50 51 input = hlcomms.get_demands();
WD40andTape 1:2a43cf183a62 52
WD40andTape 7:5b6a2cefbf3b 53 // PROCESS INPUT
WD40andTape 14:54c3759e76ed 54
WD40andTape 19:e5acb2183d4e 55 double dblTarget_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
WD40andTape 19:e5acb2183d4e 56 // Update rear segment
WD40andTape 28:8e0c502c1a50 57 dblTarget_mm[3] = input.psi_mm[0]*1000;
WD40andTape 28:8e0c502c1a50 58 dblTarget_mm[5] = -1.0;
WD40andTape 28:8e0c502c1a50 59 dblTarget_mm[6] = -1.0;
WD40andTape 19:e5acb2183d4e 60 // Update mid segment
WD40andTape 28:8e0c502c1a50 61 dblTarget_mm[4] = input.psi_mm[3]*1000;
WD40andTape 19:e5acb2183d4e 62 dblTarget_mm[7] = dblTarget_mm[4]; // Same because two pumps are used
WD40andTape 14:54c3759e76ed 63 // Update front segment
WD40andTape 28:8e0c502c1a50 64 dblTarget_mm[0] = input.psi_mm[6]*1000;
WD40andTape 28:8e0c502c1a50 65 dblTarget_mm[1] = input.psi_mm[7]*1000;
WD40andTape 28:8e0c502c1a50 66 dblTarget_mm[2] = input.psi_mm[8]*1000;
WD40andTape 28:8e0c502c1a50 67 /*input.psi_mm[5] = -1; // Disable additional mid actuator
WD40andTape 28:8e0c502c1a50 68 input.psi_mm[7] = -1; // Disable additional rear actuator
WD40andTape 28:8e0c502c1a50 69 input.psi_mm[8] = -1; // Disable additional rear actuator*/
dofydoink 11:7029367a1840 70
WD40andTape 21:0b10d8e615d1 71 // Lock mutex, preventing setDemandsForLL from running
WD40andTape 19:e5acb2183d4e 72 mutPathIn.lock();
WD40andTape 19:e5acb2183d4e 73 // Limit requested speed
WD40andTape 28:8e0c502c1a50 74 double limitedSpeed_mmps = min( max( 0.0 , input.speed_mmps ) , (double)MAX_SPEED_MMPS );
WD40andTape 19:e5acb2183d4e 75 // For each actuator, limit the input position, calculate the position change, and select the absolute max
WD40andTape 19:e5acb2183d4e 76 double dblDisplacementToTarget_mm[N_CHANNELS];
WD40andTape 19:e5acb2183d4e 77 double maxDistanceToTarget_mm = 0.0;
WD40andTape 19:e5acb2183d4e 78 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 26:7c59002c9cd7 79 double dblCurrentPosition_mm = llcomms.positionSensor_mm[i];
WD40andTape 26:7c59002c9cd7 80 if(dblTarget_mm[i]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
WD40andTape 19:e5acb2183d4e 81 // Set actuator position change to 0
WD40andTape 19:e5acb2183d4e 82 dblDisplacementToTarget_mm[i] = 0.0;
WD40andTape 19:e5acb2183d4e 83 } else { // Requested position is positive
WD40andTape 19:e5acb2183d4e 84 // ? Limit requested chamber lengths
WD40andTape 19:e5acb2183d4e 85 // ? Convert from chamber length to actuator space
WD40andTape 19:e5acb2183d4e 86 // Limit actuator position
WD40andTape 26:7c59002c9cd7 87 dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , (double)MAX_ACTUATOR_LIMIT_MM );
WD40andTape 19:e5acb2183d4e 88 // Calculate actuator position change
WD40andTape 26:7c59002c9cd7 89 dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition_mm;
WD40andTape 19:e5acb2183d4e 90 // Select the max absolute actuator position change
WD40andTape 19:e5acb2183d4e 91 if(fabs(dblDisplacementToTarget_mm[i])>maxDistanceToTarget_mm) {
WD40andTape 19:e5acb2183d4e 92 maxDistanceToTarget_mm = fabs(dblDisplacementToTarget_mm[i]);
WD40andTape 19:e5acb2183d4e 93 }
WD40andTape 19:e5acb2183d4e 94 }
dofydoink 11:7029367a1840 95 }
WD40andTape 19:e5acb2183d4e 96 // For max actuator position change, calculate the time to destination at the limited speed
WD40andTape 19:e5acb2183d4e 97 double maxTimeToTarget_s = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
WD40andTape 19:e5acb2183d4e 98 // For each actuator, replan target position and velocity as required
WD40andTape 19:e5acb2183d4e 99 for(int i=0; i<N_CHANNELS; i++) {
WD40andTape 19:e5acb2183d4e 100 // If requested actuator position change is already within tolerance, do NOT replan that actuator
WD40andTape 26:7c59002c9cd7 101 //printf("%d\t%0.5f\t%0.5f\r\n",i,dblDisplacementToTarget_mm[i],FLT_PATH_TOLERANCE_MM);
WD40andTape 26:7c59002c9cd7 102 //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 103 if( fabs(dblDisplacementToTarget_mm[i]) < FLT_PATH_TOLERANCE_MM ) continue;
WD40andTape 19:e5acb2183d4e 104 // Calculate velocity for each motor to synchronise movements to complete in max time
WD40andTape 26:7c59002c9cd7 105 // Set dblDemandPosition_mm and dblDemandSpeed_mmps
WD40andTape 26:7c59002c9cd7 106 //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 107 dblDemandPosition_mm[i] = dblTarget_mm[i];
WD40andTape 26:7c59002c9cd7 108 dblDemandSpeed_mmps[i] = fabs(dblDisplacementToTarget_mm[i]) / maxTimeToTarget_s;
WD40andTape 19:e5acb2183d4e 109 }
WD40andTape 21:0b10d8e615d1 110 // Unlock mutex, allowing setDemandsForLL to run again
WD40andTape 19:e5acb2183d4e 111 mutPathIn.unlock();
dofydoink 12:595ed862e52f 112
WD40andTape 6:f0a18e28a322 113 // SEND MESSAGE
WD40andTape 28:8e0c502c1a50 114 hlcomms.send_duration_message(maxTimeToTarget_s);
WD40andTape 6:f0a18e28a322 115 }
WD40andTape 6:f0a18e28a322 116
dofydoink 0:607bc887b6e0 117 }
dofydoink 0:607bc887b6e0 118
WD40andTape 24:bc852aa89e7a 119 void startLLcomms() { // Send new demands to LL after receiving new target data
WD40andTape 24:bc852aa89e7a 120 semLLcomms.release(); // Uses threadSetDemands which is normal priority
WD40andTape 24:bc852aa89e7a 121 }
WD40andTape 24:bc852aa89e7a 122
WD40andTape 21:0b10d8e615d1 123 void setDemandsForLL() {
WD40andTape 21:0b10d8e615d1 124
WD40andTape 4:303584310071 125 while(1) {
WD40andTape 21:0b10d8e615d1 126 semLLcomms.wait();
WD40andTape 21:0b10d8e615d1 127 mutPathIn.lock(); // Lock relevant mutex
WD40andTape 21:0b10d8e615d1 128 for(short int i=0; i<N_CHANNELS; i++) { // For each LL
WD40andTape 21:0b10d8e615d1 129 llcomms.mutChannel[i].lock(); // MUTEX LOCK
WD40andTape 26:7c59002c9cd7 130 llcomms.demandPosition_mm[i] = dblDemandPosition_mm[i];
WD40andTape 26:7c59002c9cd7 131 llcomms.demandSpeed_mmps[i] = dblDemandSpeed_mmps[i];
WD40andTape 21:0b10d8e615d1 132 llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
WD40andTape 22:82871f00f89d 133 llcomms.isDataReady[i] = 1; // Signal that data ready
WD40andTape 21:0b10d8e615d1 134 } // end for
WD40andTape 21:0b10d8e615d1 135 mutPathIn.unlock(); // Unlock relevant mutex
WD40andTape 24:bc852aa89e7a 136 } // end while(1)
WD40andTape 26:7c59002c9cd7 137
WD40andTape 3:c83291bf9fd2 138 }
dofydoink 0:607bc887b6e0 139
WD40andTape 13:a373dfc57b89 140 int main() {
WD40andTape 7:5b6a2cefbf3b 141 pc.baud(BAUD_RATE);
WD40andTape 17:bbaf3e8440ad 142 printf("ML engage. Compiled at %s\r\n.",__TIME__);
dofydoink 5:712e7634c779 143 wait(3);
WD40andTape 9:cd3607ba5643 144
WD40andTape 10:1b6daba32452 145 threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
WD40andTape 7:5b6a2cefbf3b 146 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
WD40andTape 21:0b10d8e615d1 147 threadSetDemands.start(setDemandsForLL); // Start planning thread
WD40andTape 17:bbaf3e8440ad 148 threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
WD40andTape 7:5b6a2cefbf3b 149
WD40andTape 24:bc852aa89e7a 150 setDemandsTicker.attach(&startLLcomms, 1/(float)LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
dofydoink 0:607bc887b6e0 151
WD40andTape 7:5b6a2cefbf3b 152 Thread::wait(1);
WD40andTape 1:2a43cf183a62 153 while(1) {
WD40andTape 1:2a43cf183a62 154 Thread::wait(osWaitForever);
dofydoink 0:607bc887b6e0 155 }
WD40andTape 7:5b6a2cefbf3b 156 }