Mid level control code

Dependencies:   ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }