File content as of revision 19:e5acb2183d4e:

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

//DigitalOut pinTesty(PB_8);

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

// PATH VARIABLES
double dblVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
double dblLinearPathCurrentPos_mm[N_CHANNELS]= { 0.0 }; // The current position of the linear path (not sent to actuator)
double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator

// These have to be declared in the global scope or we get a stack overflow. No idea why.
double _dblVelocity_mmps[N_CHANNELS];
//double _dblLinearPathCurrentPos_mm[N_CHANNELS];

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

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

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

Timer timer;
Ticker PathCalculationTicker;
Ticker SendSensorDataTicker;


void sendSensorData() {
    while( true ) {
        semSensorData.wait();
        double pressures[N_CHANNELS];
        // !!! READ ADC PRESSURE ASSUMES THAT LL ARE ON FIRST
        for(short int i=0; i<N_CHANNELS; i++) {
            pressures[i] = llcomms.ReadADCPressure_bar(i);
        }
        int error_code = hlcomms.send_sensor_message(dblLinearPathCurrentPos_mm,pressures);
        /*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();
}

void startPathPlan() { // Plan a new linear path after receiving new target data
    semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
}

// 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, 0.1); // 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 CalculateSmoothPath 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++) {
            // If requested position is negative
            if(dblTarget_mm[i]<0) {
                // 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] ) , 40.0 ); // !!! USE A CONSTANT FOR THIS
                // Calculate actuator position change
                double dblCurrentPosition = dblLinearPathCurrentPos_mm[i];
                //dblCurrentPosition[i] = llcomms.ReadADCPosition_mtrs(i); // Read position from channel
                dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition;
                // 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
            if( fabs(dblDisplacementToTarget_mm[i]) < FLT_PATH_TOLERANCE ) continue;
            // Calculate velocity for each motor to synchronise movements to complete in max time
            // Set dblTargetActPos_mm and dblVelocity_mmps
            dblTargetActPos_mm[i] = dblTarget_mm[i];
            dblVelocity_mmps[i] = dblDisplacementToTarget_mm[i] / maxTimeToTarget_s;
        }
        // Unlock mutex, allowing CalculateSmoothPath to run again
        mutPathIn.unlock();
        
/*
        //bool isTimeChanged = 0;
        double dblMaxRecalculatedTime = input.duration;
        // Convert from requested chamber to actuator space and limit actuator positions
        for (ii = 0; ii< N_CHANNELS; ii++) {
            // If sent a negative requested position, do NOT replan that actuator
            if( dblTargetChambLen_mm[ii] < 0.0 ) continue;
            //check to see if positions are achievable
            dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*FLT_ACTUATOR_CONVERSION[ii];
            dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 40.0 );
            //!! LIMIT CHAMBER LENGTHS TOO
        }
        // Calculate achievable velocities, and hence time, for the requested move to complete within tolerance
        double dblActPosChange;
        short sgn;
        for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
            // If sent a negative requested position, do NOT replan that actuator
            if( dblTargetChambLen_mm[ii] < 0.0 ) continue;
            
            //dblActPosChange = 1.0; // or = 0.0;
            //_dblVelocity_mmps[ii] = 0.0; // DOESN'T CRASH
            
            //dblActPosChange = dblTargetActPos_mm[ii];
            //_dblVelocity_mmps[ii] = 0.0; // DOESN'T CRASH
            
            //dblActPosChange = _dblLinearPathCurrentPos_mm[ii]; // DOESN'T CRASH
            
            //_dblVelocity_mmps[ii] = _dblLinearPathCurrentPos_mm[ii]; // DOESN'T CRASH
            
            //dblActPosChange = 0.0;
            //_dblVelocity_mmps[ii] = _dblLinearPathCurrentPos_mm[ii]; // DOESN'T CRASH
            
            // DOES CRASH but not with a return at the end of while OR if _ variables are declared globally
            //dblActPosChange = _dblLinearPathCurrentPos_mm[ii];
            //_dblVelocity_mmps[ii] = 0.0;
            
            dblActPosChange = dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii];
            if( fabs(dblActPosChange) < FLT_PATH_TOLERANCE ) { // If actuator ii is already within tolerance
                dblActPosChange = 0.0;
                //isTimeChanged = 1;
            }
            //IS BELOW
            if( input.duration < 0.000000001 ) { // If max (safe) velocity was requested
                sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
                _dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
                //isTimeChanged = 1;
            } else {
                _dblVelocity_mmps[ii] = dblActPosChange / input.duration;
            }
            //IS ABOVE
            // Check to see if velocities are achievable -- this can move into the else section of the above if statement
            if(abs(_dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
                if(_dblVelocity_mmps[ii]>0) {
                    _dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
                } else {
                    _dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
                }
                //isTimeChanged = 1;
            }
            // Recalculate the move's time after altering the position and/or velocity
            double dblRecalculatedTime;
            if( fabs(_dblVelocity_mmps[ii]) < 0.000000001 ) {
                dblRecalculatedTime = 0;
            } else {
                dblRecalculatedTime = dblActPosChange / _dblVelocity_mmps[ii];
            }
            // Find the maximum time for any actuator's move for synchronization
            if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
                dblMaxRecalculatedTime = dblRecalculatedTime;
            }
        }
        // Finally recalculate all of the velocities based upon this maximum time for synchronization
        // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!MUTEX FOR dblVelocity_mmps and dblTargetActPos_mm??
        for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
            // If sent a negative requested position, do NOT replan that actuator
            if( dblTargetChambLen_mm[ii] < 0.0 ) continue;
            
            _dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii]) / dblMaxRecalculatedTime;
            dblVelocity_mmps[ii] = _dblVelocity_mmps[ii];
        }
*/
        // 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 CalculateSmoothPath() {
    int jj;
    double dblMeasuredSampleTime;
    double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; // The current position of the smooth path (not sent to actuator)
    //double dblPosition_mtrs[N_CHANNELS]; // The actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
    //double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar  = 100,000 Pa)
    while(1) {
        semPathPlan.wait();
        //pinTesty = 1;
        // If run time is more than 50 us from expected, calculate from measured time step
        if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
            dblMeasuredSampleTime = timer.read(); 
        } else {
            dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
        }
        timer.reset();
            
        for(jj = 0; jj < N_CHANNELS; jj++) {
            //dblPressure_bar[jj] = ReadADCPressure_bar(jj); // Read pressure from channel
            //dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); // Read position from channel
            
            // Calculate next step in linear path
            mutPathIn.lock(); // Lock relevant mutex
            // Check tolerance
            if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= FLT_PATH_TOLERANCE) {
                dblVelocity_mmps[jj] = 0.0; // Stop linear path generation when linear path is within tolerance of target position
            }
            dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
            dblLinearPathCurrentPos_mm[jj] = min( max( 0.0 , dblLinearPathCurrentPos_mm[jj] ) , 40.0 );
            mutPathIn.unlock(); // Unlock relevant mutex
            
            // Calculate next step in smooth path
            dblSmoothPathCurrentPos_mm[jj] = FLT_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0f-FLT_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
            dblSmoothPathCurrentPos_mm[jj] = max( 0.0 , dblSmoothPathCurrentPos_mm[jj] );
            llcomms.mutChannel[jj].lock(); // MUTEX LOCK            
            llcomms.demandPosition[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
            llcomms.demandPosition[jj] = llcomms.demandPosition[jj] & 0x1FFF; // Ensure number is 13-bit
            llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
          
            llcomms.isDataReady[jj] = 1; // Signal that data ready
        } // end for
        
        //if(IS_PRINT_OUTPUT) printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
        //if(IS_PRINT_OUTPUT) printf("%f, %f, %f, %f, %f, %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblLinearPathCurrentPos_mm[1],dblLinearPathCurrentPos_mm[2],
        //  dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);
        //if(IS_PRINT_OUTPUT) printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
        //pinTesty = 0;
    } // end while
}

int main() {
    pc.baud(BAUD_RATE);
    printf("ML engage. Compiled at %s\r\n.",__TIME__);
    wait(3);
    
    timer.start();

    threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
    threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
    threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning thread
    threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread

    PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); // Set up planning thread to recur at fixed intervals
    
    Thread::wait(1);
    while(1) {
        Thread::wait(osWaitForever); 
    }
}