Mid level control code
Dependencies: ros_lib_kinetic
main.cpp
00001 // STANDARD IMPORTS 00002 #include "math.h" 00003 // MBED IMPORTS 00004 #include "mbed.h" 00005 #include "mbed_events.h" 00006 // CUSTOM IMPORTS 00007 #include "MLSettings.h" 00008 #include "HLComms.h" 00009 #include "LLComms.h" 00010 00011 // DEMAND VARIABLES 00012 double dblDemandSpeed_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator) 00013 double dblDemandPosition_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator 00014 00015 Serial pc(USBTX, USBRX); // tx, rx for usb debugging 00016 LLComms llcomms; 00017 HLComms hlcomms(HL_COMMS_FREQ_HZ); 00018 00019 Thread threadLowLevelSPI(osPriorityRealtime); 00020 Thread threadSetDemands(osPriorityNormal); 00021 Thread threadReceiveAndReplan(osPriorityBelowNormal); 00022 Thread threadSensorFeedback(osPriorityBelowNormal); 00023 00024 Mutex mutPathIn; 00025 Semaphore semLLcomms(1); 00026 Semaphore semSensorData(1); 00027 00028 Ticker setDemandsTicker; 00029 Ticker SendSensorDataTicker; 00030 00031 void sendSensorData() { 00032 while( true ) { 00033 semSensorData.wait(); 00034 hlcomms.send_sensor_message(llcomms.positionSensor_uint,llcomms.pressureSensor_uint); 00035 } 00036 } 00037 00038 void signalSendSensorData() { 00039 semSensorData.release(); 00040 } 00041 00042 // This function will be called when a new transmission is received from high level 00043 void ReceiveAndReplan() { 00044 00045 SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals 00046 00047 struct demands_struct input; 00048 DigitalOut SupportPins[4] = {PE_4, PE_2, PE_3, PE_6}; 00049 00050 while( true ) { 00051 hlcomms.newData.wait(); 00052 input = hlcomms.get_demands(); 00053 // SUPPORT FUNCTIONS 00054 // [isInsufflate,isSuction,isWashLens,isJet] 00055 for(short int i=0; i<4; i++) { 00056 if(i<2) { // Active low, i.e. 0 = Off 00057 SupportPins[i].write((short int)input.utitilies_bool[i]); 00058 } else { // Active high, i.e. 1 = Off 00059 SupportPins[i].write((short int)(!input.utitilies_bool[i])); 00060 } 00061 } 00062 // PROCESS INPUT 00063 double maxTimesToTarget_s[3] = { -1.0 }; 00064 //[8,7,6,4,3,-1,-1,0,-1] 00065 //FRONT = channels 0,1,2 00066 //MID = channels 3,4(,5) 00067 //REAR = channels (6,)7(,8) 00068 // Lock mutex, preventing setDemandsForLL from running 00069 mutPathIn.lock(); 00070 for(short int segNum=0; segNum<3; segNum++) { 00071 // Limit requested speed 00072 if( input.speeds_mmps[segNum] > 0 ) { 00073 double limitedSpeed_mmps = min( max( 0.0 , input.speeds_mmps[segNum] ) , (double)MAX_SPEED_MMPS ); 00074 // For each actuator, limit the input position, calculate the position change, and select the absolute max 00075 double dblDistanceToTarget_mm[3] = { -1.0 }; 00076 double maxDistanceToTarget_mm = 0.0; 00077 for(short int i=0; i<3; i++) { 00078 short int channel = segNum*3+i; 00079 if(channel>=N_CHANNELS) { 00080 continue; 00081 } 00082 double dblCurrentPosition_mm = llcomms.positionSensor_mm[channel]; 00083 if(input.psi_mm[channel]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous 00084 continue; 00085 } 00086 // Limit actuator position 00087 input.psi_mm[channel] = min( max( 0.0 , input.psi_mm[channel] ) , (double)MAX_ACTUATOR_LIMIT_MM ); 00088 // Calculate actuator position change 00089 dblDistanceToTarget_mm[i] = fabs(input.psi_mm[channel] - dblCurrentPosition_mm); 00090 // Select the max absolute actuator position change 00091 if(dblDistanceToTarget_mm[i]>maxDistanceToTarget_mm) { 00092 maxDistanceToTarget_mm = dblDistanceToTarget_mm[i]; 00093 } 00094 } 00095 // For max actuator position change, calculate the time to destination at the limited speed 00096 maxTimesToTarget_s[segNum] = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps; 00097 // For each actuator, replan target position and velocity as required 00098 for(short int i=0; i<3; i++) { 00099 short int channel = segNum*3+i; 00100 // If requested actuator position change is already within tolerance, do NOT replan that actuator 00101 if( dblDistanceToTarget_mm[i] <= 0.0 ) continue; 00102 // Calculate velocity for each motor to synchronise movements to complete in max time 00103 // Set dblDemandPosition_mm and dblDemandSpeed_mmps 00104 dblDemandPosition_mm[channel] = input.psi_mm[channel]; 00105 dblDemandSpeed_mmps[channel] = dblDistanceToTarget_mm[i] / maxTimesToTarget_s[segNum]; 00106 } 00107 } else { 00108 maxTimesToTarget_s[segNum] = -1.0; 00109 } 00110 } 00111 // Unlock mutex, allowing setDemandsForLL to run again 00112 mutPathIn.unlock(); 00113 00114 // SEND MESSAGE 00115 hlcomms.send_durations_message(maxTimesToTarget_s); 00116 } 00117 00118 } 00119 00120 void startLLcomms() { // Send new demands to LL after receiving new target data 00121 semLLcomms.release(); // Uses threadSetDemands which is normal priority 00122 } 00123 00124 void setDemandsForLL() { 00125 00126 while(1) { 00127 semLLcomms.wait(); 00128 mutPathIn.lock(); // Lock relevant mutex 00129 for(short int i=0; i<N_CHANNELS; i++) { // For each LL 00130 llcomms.mutChannel[i].lock(); // MUTEX LOCK 00131 llcomms.demandPosition_mm[i] = dblDemandPosition_mm[i]; 00132 llcomms.demandSpeed_mmps[i] = dblDemandSpeed_mmps[i]; 00133 llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK 00134 llcomms.isDataReady[i] = 1; // Signal that data ready 00135 } // end for 00136 mutPathIn.unlock(); // Unlock relevant mutex 00137 } // end while(1) 00138 00139 } 00140 00141 int main() { 00142 pc.baud(BAUD_RATE); 00143 //printf("ML engage. Compiled at %s\r\n.",__TIME__); 00144 wait(3); 00145 00146 threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue 00147 threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread 00148 threadSetDemands.start(setDemandsForLL); // Start planning thread 00149 threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread 00150 00151 setDemandsTicker.attach(&startLLcomms, 1/(float)LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals 00152 00153 Thread::wait(1); 00154 while(1) { 00155 Thread::wait(osWaitForever); 00156 } 00157 }
Generated on Mon Jul 25 2022 18:11:35 by
1.7.2