File content as of revision 26:7c59002c9cd7:

// 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"

// Maximum achievable mm path tolerance plus additional % tolerance
//const float FLT_PATH_TOLERANCE_MM = MAX_SPEED_MMPS * (1/LL_DEMANDS_FREQ_HZ) * (1.0f+FLT_PERCENT_PATH_TOLERANCE);

// 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(SERVER_PORT);

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();
        int error_code = hlcomms.send_sensor_message(llcomms.positionSensor_mm,llcomms.pressureSensor_bar);
        /*if( error_code < 0 ) {
            if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
                "Perhaps the server socket is not bound or not set to listen for connections? "
                "Or the socket is set to non-blocking or timed out?\n\r", error_code);
            hlcomms.close_server();
            return;
        }*/
    }
}

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

// This function will be called when a new transmission is received from high level
void ReceiveAndReplan() {
    
    int error_code;
    error_code = hlcomms.setup_server();
    if( error_code == -1 ) return;
    error_code = hlcomms.accept_connection();
    if( error_code == -1 ) {
        hlcomms.close_server();
        return;
    }
    
    SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals
    
    struct msg_format input; //hlcomms.msg_format
    
    while( true ) {
        // RECEIVE MESSAGE
        error_code = hlcomms.receive_message();
        if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
            if(IS_PRINT_OUTPUT) printf("Client disconnected.\n\r");
            hlcomms.close_server();
            return;
        } else if( error_code < 0 ) {
            if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
                "Perhaps the server socket is not connected to a remote host? "
                "Or the socket is set to non-blocking or timed out?\n\r", error_code);
            hlcomms.close_server();
            return;
        }
        // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! RECV MUTEX
        input = hlcomms.process_message();
        
        // PROCESS INPUT
        if(IS_PRINT_OUTPUT) printf("REPLAN, %f\r\n",input.speed);
        
        double dblTarget_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
        // Update rear segment
        dblTarget_mm[3] = input.psi[0][0]*1000;
        dblTarget_mm[5] = 0.0; //input.psi[0][1]*1000;
        dblTarget_mm[6] = 0.0; //input.psi[0][2]*1000;
        // Update mid segment
        dblTarget_mm[4] = input.psi[1][0]*1000;
        dblTarget_mm[7] = dblTarget_mm[4]; // Same because two pumps are used
        // Update front segment
        dblTarget_mm[0] = input.psi[2][0]*1000;
        dblTarget_mm[1] = input.psi[2][1]*1000;
        dblTarget_mm[2] = input.psi[2][2]*1000;

        // Lock mutex, preventing setDemandsForLL from running
        mutPathIn.lock();        
        // Limit requested speed
        double limitedSpeed_mmps = min( max( 0.0 , input.speed ) , (double)MAX_SPEED_MMPS );
        // For each actuator, limit the input position, calculate the position change, and select the absolute max
        double dblDisplacementToTarget_mm[N_CHANNELS];
        double maxDistanceToTarget_mm = 0.0;
        for(int i=0; i<N_CHANNELS; i++) {
            double dblCurrentPosition_mm = llcomms.positionSensor_mm[i];
            if(dblTarget_mm[i]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
                // Set actuator position change to 0
                dblDisplacementToTarget_mm[i] = 0.0;
            } else { // Requested position is positive
                // ? Limit requested chamber lengths
                // ? Convert from chamber length to actuator space
                // Limit actuator position
                dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , (double)MAX_ACTUATOR_LIMIT_MM );
                // Calculate actuator position change
                dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition_mm;
                // Select the max absolute actuator position change
                if(fabs(dblDisplacementToTarget_mm[i])>maxDistanceToTarget_mm) {
                    maxDistanceToTarget_mm = fabs(dblDisplacementToTarget_mm[i]);
                }
            }
        }
        // For max actuator position change, calculate the time to destination at the limited speed
        double maxTimeToTarget_s = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
        // For each actuator, replan target position and velocity as required
        for(int i=0; i<N_CHANNELS; i++) {
            // If requested actuator position change is already within tolerance, do NOT replan that actuator
            //printf("%d\t%0.5f\t%0.5f\r\n",i,dblDisplacementToTarget_mm[i],FLT_PATH_TOLERANCE_MM);
            //printf("%d\t%0.5f\t%0.5f\t%0.5f\r\n",i,dblTarget_mm[i],fabs(dblDisplacementToTarget_mm[i]),maxTimeToTarget_s);
            if( fabs(dblDisplacementToTarget_mm[i]) < FLT_PATH_TOLERANCE_MM ) continue;
            // Calculate velocity for each motor to synchronise movements to complete in max time
            // Set dblDemandPosition_mm and dblDemandSpeed_mmps
            //printf("%d\t%0.5f\t%0.5f\t%0.5f\r\n",i,dblTarget_mm[i],fabs(dblDisplacementToTarget_mm[i]),maxTimeToTarget_s);
            dblDemandPosition_mm[i] = dblTarget_mm[i];
            dblDemandSpeed_mmps[i] = fabs(dblDisplacementToTarget_mm[i]) / maxTimeToTarget_s;
        }
        // Unlock mutex, allowing setDemandsForLL to run again
        mutPathIn.unlock();
        
        // SEND MESSAGE
        error_code = hlcomms.send_duration_message(&maxTimeToTarget_s);
        if( error_code < 0 ) {
            if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
                "Perhaps the server socket is not bound or not set to listen for connections? "
                "Or the socket is set to non-blocking or timed out?\n\r", error_code);
            hlcomms.close_server();
            return;
        }
    }

}

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

Timer timerA;
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] = 2*sin(timerA.read())+4;
            //llcomms.demandSpeed_mmps[i] = 4.0;
            //printf("%d\t%0.5f\t%0.5f\r\n",i,dblDemandPosition_mm[i],dblDemandSpeed_mmps[i]);
            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);
    
    timerA.start();
    
    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); 
    }
}