File content as of revision 9:cd3607ba5643:

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

// SIMPLE CHANNEL SELECTION
#define ADC_PRESSURE 1
#define ADC_POSITION 3

#define N_CHANNELS 8 // Number of channels to control
// 1-3: front segment; 4-6: rear segment; 7-8: mid segment

// SPI SETTINGS
#define LOW_LEVEL_SPI_FREQUENCY 10000000
// PATH GENERATION SETTINGS
#define PATH_SAMPLE_TIME_S 0.005 //0.109
#define MAX_LENGTH_MM 100.0
#define MAX_ACTUATOR_LENGTH 52.2
#define MAX_SPEED_MMPS 24.3457
#define PATH_TOLERANCE_MM 0.2 // How close the linear path must get to the target position before considering it a success.

#define BAUD_RATE 9600 //115200

// COMMS SETTINGS
const short int SERVER_PORT = 80;

//const double DBL_MAX_CHAMBER_LENGTHS_MM[N_CHANNELS] = {80.0,80.0,80.0,80.0,80.0,80.0,80.0,80.0};
//const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {0.30586,0.30586,0.30586,0.30586,0.30586,0.4588,0.4588}; // Convert from chamber lengths to actuator lengths
const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; // Convert from chamber lengths to actuator 
const double DBL_SMOOTHING_FACTOR = 0.5; // 0<x<=1, where 1 is no smoothing
const double DBL_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; // Path tolerance in mm with 5% 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

int intDemandPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator

Serial pc(USBTX, USBRX); // tx, rx for usb debugging
LLComms llcomms(LOW_LEVEL_SPI_FREQUENCY);//(N_CHANNELS);

EventQueue queue(32 * EVENTS_EVENT_SIZE);
Thread t(osPriorityRealtime);
Thread threadReceiveAndReplan(osPriorityBelowNormal);
Thread threadSmoothPathPlan(osPriorityNormal);

Mutex mutPathIn;
Semaphore semPathPlan(1);

Timer timer;
Ticker PathCalculationTicker;

bool isDataReady[N_CHANNELS] = { 0 }; // flag to indicate path data is ready for transmission to low level.

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() {
    HLComms hlcomms(SERVER_PORT);
    
    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;
    }
    
    int ii;
    struct msg_format input; //hlcomms.msg_format
    
    while( true ) {
        // RECEIVE MESSAGE
        error_code = hlcomms.receive_message();
        if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
            printf("Client disconnected.\n\r");
            hlcomms.close_server();
            return;
        } else if( error_code < 0 ) {
            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;
        }
        //printf("Message received.\r\n");
        input = hlcomms.process_message();
        
        // PROCESS INPUT
        double dblTargetChambLen_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
        printf("REPLAN, %f\r\n",input.duration);
        // Update front segment
        dblTargetChambLen_mm[0] = input.psi[0][0]*1000;
        dblTargetChambLen_mm[1] = input.psi[0][1]*1000;
        dblTargetChambLen_mm[2] = input.psi[0][2]*1000;
        // Update mid segment
        dblTargetChambLen_mm[6] = input.psi[1][0]*1000;
        dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; // Same because two pumps are used
        // Update rear segment
        dblTargetChambLen_mm[3] = input.psi[2][0]*1000;
        dblTargetChambLen_mm[4] = input.psi[2][1]*1000;
        dblTargetChambLen_mm[5] = input.psi[2][2]*1000;

        bool isTimeChanged = 0;
        double dblMaxRecalculatedTime = input.duration;
        mutPathIn.lock(); // Lock variables to avoid race condition
        for (ii = 0; ii< N_CHANNELS; ii++) {
            //check to see if positions are achievable
            /*if(dblTargetChambLen_mm[ii]>DBL_MAX_CHAMBER_LENGTHS_MM[ii]) {
                dblTargetChambLen_mm[ii] = DBL_MAX_CHAMBER_LENGTHS_MM[ii]; 
                isTimeChanged = 1;
            }
            if(dblTargetChambLen_mm[ii]<0.0) {
                dblTargetChambLen_mm[ii] = 0.0;
                isTimeChanged = 1; 
            }*/
            dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
            //!! LIMIT CHAMBER LENGTHS TOO
            if( dblTargetActPos_mm[ii]<0.0 || dblTargetActPos_mm[ii]>25.0 ) {
                dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 25.0 );
                isTimeChanged = 1;
            }
        }
        double dblActPosChange;
        short sgn;
        for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
            dblActPosChange = dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii];
            if( fabs(dblActPosChange) < DBL_PATH_TOLERANCE ) {
                dblActPosChange = 0.0;
                isTimeChanged = 1;
            }
            if( input.duration < 0.000000001 ) {
                sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
                dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
                isTimeChanged = 1;
            } else {
                dblVelocity_mmps[ii] = dblActPosChange / input.duration;
            }
            // Check to see if velocities are achievable
            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;
            }
            double dblRecalculatedTime;
            if( fabs(dblVelocity_mmps[ii]) < 0.000000001 ) {
                dblRecalculatedTime = 0;
            } else {
                dblRecalculatedTime = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblVelocity_mmps[ii];
            }
            if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
                dblMaxRecalculatedTime = dblRecalculatedTime;
            }
        }
        if( isTimeChanged ) {
            if( dblMaxRecalculatedTime < input.duration ) {
                dblMaxRecalculatedTime = input.duration;
            }
            for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
                dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblMaxRecalculatedTime;
            }
        }
        mutPathIn.unlock(); // Unlock mutex
        
        printf("Sending message...\r\n");
        // SEND MESSAGE
        hlcomms.make_message(&dblMaxRecalculatedTime);
        error_code = hlcomms.send_message();
        if( error_code < 0 ) {
            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;
        }
        printf("Message sent.\r\n");
    }

}

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();
        
        // 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
            dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
            if(dblLinearPathCurrentPos_mm[jj] < 0.0) {
                dblLinearPathCurrentPos_mm[jj] = 0.00;
            }
            // Check tolerance
            if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= DBL_PATH_TOLERANCE) {
                dblVelocity_mmps[jj] = 0.0; // Stop linear path generation when linear path is within tolerance of target position
            }
            mutPathIn.unlock(); // Unlock relevant mutex
            
            // Calculate next step in smooth path
            dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
            llcomms.mutChannel[jj].lock(); // MUTEX LOCK
            intDemandPos_Tx[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
            intDemandPos_Tx[jj] = intDemandPos_Tx[jj] & 0x1FFF; // Ensure number is 13-bit 
            llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
          
            isDataReady[jj] = 1;//signal that data ready
        } // end for
        
        //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
        /*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]);*/
        //printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
        
    } // end while
}

// NEED TO FIGURE OUT POINTER-TO-MEMBER FUNCTIONS TO MOVE THESE INTO LLComms CLASS
void SendReceiveData(int channel, int _intDemandPos_Tx[], bool (*_isDataReady)[8]) {
    int intPosSPI_Rx[N_CHANNELS]; // 13 bit value received over SPI from the actuator
    
    // Get data from controller
    llcomms.spi.format(16,2);
    llcomms.spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
    llcomms.mutChannel[channel].lock(); // Lock mutex for specific Channel
    *llcomms.cs_LL[channel] = 0; // Select relevant chip
    intPosSPI_Rx[channel] = llcomms.spi.write(_intDemandPos_Tx[channel]); // Transmit & receive
    *llcomms.cs_LL[channel] = 1; // Deselect chip
    *_isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
    /*if(channel == 0) {
        intGlobalTest = intPosSPI_Rx[channel];
        dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
    }*/
    
    // Sort out received data
    llcomms.chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
    
    intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
    //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
    llcomms.mutChannel[channel].unlock();//unlock mutex for specific channel
    //printf("%d, %d\r\n",intPosSPI_Rx[0],_intDemandPos_Tx[0]);
}
// Common rise handler function 
void common_rise_handler(int channel) {
    llcomms.pinCheck = 1;
    if (isDataReady[channel]) { // Check if data is ready for tranmission
        //llcomms.ThreadID[channel] = queue.call(&llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
        //llcomms.ThreadID[channel] = queue.call(&llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
        //llcomms.ThreadID[channel] = queue.call(mbed::Callback(&llcomms,&LLComms::SendReceiveData),channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
       // llcomms.ThreadID[channel] = queue.call(mbed::Callback<void()>(&llcomms,&LLComms::SendReceiveData),channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
        //llcomms.ThreadID[channel] = queue.call(&llcomms,*llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
        llcomms.ThreadID[channel] = queue.call(&SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
        //llcomms.ThreadID[channel] = queue.call(&llcomms,&LLComms::dSendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
        //callback(&low_pass1, &LowPass::step)
    }
}
// Common fall handler functions
void common_fall_handler(int channel) {
    llcomms.pinCheck = 0;
    queue.cancel(llcomms.ThreadID[channel]); // Cancel relevant queued event
}
// Stub rise functions
void rise0(void) { common_rise_handler(0); }
void rise1(void) { common_rise_handler(1); }
void rise2(void) { common_rise_handler(2); }
void rise3(void) { common_rise_handler(3); }
void rise4(void) { common_rise_handler(4); }
void rise5(void) { common_rise_handler(5); }
void rise6(void) { common_rise_handler(6); }
void rise7(void) { common_rise_handler(7); }
// Stub fall functions
void fall0(void) { common_fall_handler(0); }
void fall1(void) { common_fall_handler(1); }
void fall2(void) { common_fall_handler(2); }
void fall3(void) { common_fall_handler(3); }
void fall4(void) { common_fall_handler(4); }
void fall5(void) { common_fall_handler(5); }
void fall6(void) { common_fall_handler(6); }
void fall7(void) { common_fall_handler(7); }

int main() {    
    pc.baud(BAUD_RATE);
    printf("Hi, there! I'll be your mid-level controller for today.\r\n");
    wait(3);
    
    llcomms.reset();
    t.start(callback(&queue, &EventQueue::dispatch_forever)); // Start the event queue
    
    // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
    llcomms.pinGate0.rise(&rise0);
    llcomms.pinGate1.rise(&rise1);
    llcomms.pinGate2.rise(&rise2);
    llcomms.pinGate3.rise(&rise3);
    llcomms.pinGate4.rise(&rise4);
    llcomms.pinGate5.rise(&rise5);
    llcomms.pinGate6.rise(&rise6);
    llcomms.pinGate7.rise(&rise7);
    // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
    llcomms.pinGate0.fall(&fall0);
    //llcomms.pinGate0.fall(&llcomms,&LLComms::dfall0);
    llcomms.pinGate1.fall(&fall1);
    llcomms.pinGate2.fall(&fall2);
    llcomms.pinGate3.fall(&fall3);
    llcomms.pinGate4.fall(&fall4);
    llcomms.pinGate5.fall(&fall5);
    llcomms.pinGate6.fall(&fall6);
    llcomms.pinGate7.fall(&fall7);
    
    timer.start();

    threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
    threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning 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); 
    }
}