File content as of revision 32:8c59c536a2a6:

// STANDARD IMPORTS
#include "math.h"
// MBED IMPORTS
#include "mbed.h"
#include "mbed_events.h"
// CUSTOM IMPORTS
#include "MLSettings.h"
#include "HLComms.h"
#include "LLComms.h"

// DEMAND VARIABLES
double dblDemandSpeed_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
double dblDemandPosition_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator

Serial pc(USBTX, USBRX); // tx, rx for usb debugging
LLComms llcomms;
HLComms hlcomms(HL_COMMS_FREQ_HZ);

Thread threadLowLevelSPI(osPriorityRealtime);
Thread threadSetDemands(osPriorityNormal);
Thread threadReceiveAndReplan(osPriorityBelowNormal);
Thread threadSensorFeedback(osPriorityBelowNormal);

Mutex mutPathIn;
Semaphore semLLcomms(1);
Semaphore semSensorData(1);

Ticker setDemandsTicker;
Ticker SendSensorDataTicker;

void sendSensorData() {
    while( true ) {
        semSensorData.wait();
        hlcomms.send_sensor_message(llcomms.positionSensor_uint,llcomms.pressureSensor_uint);
    }
}

void signalSendSensorData() {
    semSensorData.release();
}

// This function will be called when a new transmission is received from high level
void ReceiveAndReplan() {
    
    SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals
    
    struct demands_struct input;
    
    while( true ) {
        hlcomms.newData.wait();
        input = hlcomms.get_demands();
        // PROCESS INPUT
        double maxTimesToTarget_s[3] = { -1.0 };
        //[8,7,6,4,3,-1,-1,0,-1]
        //FRONT = channels 0,1,2
        //MID = channels 3,4(,5)
        //REAR = channels (6,)7(,8)
        // Lock mutex, preventing setDemandsForLL from running
        mutPathIn.lock();
        for(short int segNum=0; segNum<3; segNum++) {
            // Limit requested speed
            if( input.speeds_mmps[segNum] > 0 ) {
                double limitedSpeed_mmps = min( max( 0.0 , input.speeds_mmps[segNum] ) , (double)MAX_SPEED_MMPS );
                // For each actuator, limit the input position, calculate the position change, and select the absolute max
                double dblDistanceToTarget_mm[3] = { -1.0 };
                double maxDistanceToTarget_mm = 0.0;
                for(short int i=0; i<3; i++) {
                    short int channel = segNum*3+i;
                    if(channel==8) { // !!!!!!!!!!!!!!!! This is horrible
                        continue;
                    }
                    double dblCurrentPosition_mm = llcomms.positionSensor_mm[channel];
                    if(input.psi_mm[channel]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
                        continue;
                    }
                    // Limit actuator position
                    input.psi_mm[channel] = min( max( 0.0 , input.psi_mm[channel] ) , (double)MAX_ACTUATOR_LIMIT_MM );
                    // Calculate actuator position change
                    dblDistanceToTarget_mm[i] = fabs(input.psi_mm[channel] - dblCurrentPosition_mm);
                    // Select the max absolute actuator position change
                    if(dblDistanceToTarget_mm[i]>maxDistanceToTarget_mm) {
                        maxDistanceToTarget_mm = dblDistanceToTarget_mm[i];
                    }
                }
                // For max actuator position change, calculate the time to destination at the limited speed
                maxTimesToTarget_s[segNum] = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
                // For each actuator, replan target position and velocity as required
                for(short int i=0; i<3; i++) {
                    short int channel = segNum*3+i;
                    // If requested actuator position change is already within tolerance, do NOT replan that actuator
                    if( dblDistanceToTarget_mm[i] <= 0.0 ) continue;
                    // Calculate velocity for each motor to synchronise movements to complete in max time
                    // Set dblDemandPosition_mm and dblDemandSpeed_mmps
                    dblDemandPosition_mm[channel] = input.psi_mm[channel];
                    dblDemandSpeed_mmps[channel] = dblDistanceToTarget_mm[i] / maxTimesToTarget_s[segNum];
                }
            } else {
                maxTimesToTarget_s[segNum] = -1.0;
            }
        }
        // Unlock mutex, allowing setDemandsForLL to run again
        mutPathIn.unlock();
        
        // SEND MESSAGE
        hlcomms.send_durations_message(maxTimesToTarget_s);
    }

}

void startLLcomms() { // Send new demands to LL after receiving new target data
    semLLcomms.release(); // Uses threadSetDemands which is normal priority
}

void setDemandsForLL() {
    
    while(1) {
        semLLcomms.wait();
        mutPathIn.lock(); // Lock relevant mutex
        for(short int i=0; i<N_CHANNELS; i++) { // For each LL
            llcomms.mutChannel[i].lock(); // MUTEX LOCK
            llcomms.demandPosition_mm[i] = dblDemandPosition_mm[i];
            llcomms.demandSpeed_mmps[i] = dblDemandSpeed_mmps[i];
            llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
            llcomms.isDataReady[i] = 1; // Signal that data ready
        } // end for
        mutPathIn.unlock(); // Unlock relevant mutex
    } // end while(1)

}

int main() {
    pc.baud(BAUD_RATE);
    //printf("ML engage. Compiled at %s\r\n.",__TIME__);
    wait(3);
    
    threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
    threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
    threadSetDemands.start(setDemandsForLL); // Start planning thread
    threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread

    setDemandsTicker.attach(&startLLcomms, 1/(float)LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
    
    Thread::wait(1);
    while(1) {
        Thread::wait(osWaitForever); 
    }
}