File content as of revision 4:303584310071:

#include "mbed.h"
#include "math.h"

//#include "mbed.h"
#include "mbed_events.h"

//ADC SPI stuff
#define PREAMBLE 0x06
#define CHAN_1 0x30
#define CHAN_2 0x70
#define CHAN_3 0xB0
#define CHAN_4 0xF0

#define DATA_MASK 0x0F

//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
                
//parameters to manually change                       
#define LOW_LEVEL_SPI_FREQUENCY 10000000

#define PATH_SAMPLE_TIME_S 0.005

#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.

const double DBL_MAX_CHAMBER_LENGTHS_MM[N_CHANNELS] = {100.0,50.0,50.0,50.0,50.0,50.0,30.0,30.0};
const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {0.24176,0.24176,0.24176,0.24176,0.24176,0.36264,0.36264};// covert from chamber lengths to actuator lengths
const double DBL_SMOOTHING_FACTOR = 0.5; // 0<x<=1, where 1 is no smoothing
const double DBL_PATH_TOLERANCE = 0.2;
//-----These are the variables being shared between High-Level and Mid-Level-----------------------------------------------//

//
Semaphore semReplan(1);// this must be set every time new high-level transmissions are received to allow replanning to take place!
//--------------------------------------------------------------------------------------------------------------------------//

//boolean flag to indicate that new target information has been received
//bool blnReplan;//set this when new transmission is received over ethernet

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

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

char chrErrorFlag[N_CHANNELS];// 3 error bits from LL KEEP GLOBAL

//pin declarations----------------
Serial pc(USBTX, USBRX); // tx, rx for usb debugging

InterruptIn pinGate6(PE_11); //this pin HAS TO BE defined before SPI set up. No Clue Why.

SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk

DigitalOut cs_LL[N_CHANNELS] = {PD_15, PE_10, PD_14, PD_11, PE_7, PD_12, PF_10, PD_13};//chip select for low level controller
DigitalOut cs_ADC[N_CHANNELS] = {PG_12, PG_9, PE_1, PG_0, PD_0, PD_1, PF_0, PF_1}; //chip select for ADC

DigitalOut pinCheck(PE_5);

//these interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
InterruptIn pinGate0(PF_11);
InterruptIn pinGate1(PG_14);
InterruptIn pinGate2(PF_15);
InterruptIn pinGate3(PF_12);
InterruptIn pinGate4(PF_3);
InterruptIn pinGate5(PF_13);
//InterruptIn pinGate6(PE_11); //see above nonsense
InterruptIn pinGate7(PE_13);

DigitalOut pinReset(PD_2); //reset pin for all controllers.


EventQueue queue(32 * EVENTS_EVENT_SIZE);

Thread t(osPriorityRealtime);

Thread threadReceiveAndReplan(osPriorityBelowNormal);
Thread threadSmoothPathPlan(osPriorityNormal);

Thread threadSimulateDemand(osPriorityHigh);

Mutex mutChannel[N_CHANNELS];
Mutex mutPathIn;

Semaphore semPathPlan(1);//

Timer timer;//timers are nice

int ThreadID[N_CHANNELS];

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


double ReadADCPosition_mtrs(int channel)
{
    unsigned int outputA;
    unsigned int outputB;
    int output;
    double dblOutput;

    spi.format(8,0);
    spi.frequency(1000000);
    
    cs_ADC[channel] = 0;
    spi.write(PREAMBLE);
    outputA = spi.write(CHAN_3);
    outputB = spi.write(0xFF);
    cs_ADC[channel]= 1;
    
    outputA = outputA & DATA_MASK;
    outputA = outputA<<8;
    output = (outputA | outputB);
    output = 4095- output;
    dblOutput = (double) ((output - 1504)/4095*0.1);
    return dblOutput;
}
    
double ReadADCPressure_bar(int channel)
{
    unsigned int outputA;
    unsigned int outputB;
    int output;
    double dblOutput;

    spi.format(8,0);
    spi.frequency(1000000);
    
    cs_ADC[channel] = 0;
    spi.write(PREAMBLE);
    outputA = spi.write(CHAN_1);
    outputB = spi.write(0xFF);
    cs_ADC[channel] = 1;
    
    outputA = outputA & DATA_MASK;
    outputA = outputA<<8;
    output = (outputA | outputB);
    
    dblOutput = (double) ((output-502)/4095*8.0);
    return dblOutput;
}

void SendReceiveData(int channel) 
{
    
    
    int intPosSPI_Rx[N_CHANNELS]; //13 bit value received over SPI from the actuator
    
    
    //get data from controller
    spi.format(16,2);
    spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
    mutChannel[channel].lock();//lock mutex for specific Channel
    cs_LL[channel] = 0;//select relevant chip
    intPosSPI_Rx[channel] = spi.write(intDemandPos_Tx[channel]); //transmit & receive
    cs_LL[channel] = 1;//deselect chip
    isDataReady[channel] = 0;//data no longer ready, i.e. we now require new data
    

    
    //sort out received data
    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;
    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)
{
    pinCheck = 1;
    //check if data is ready for tranmission
    if (isDataReady[channel])
    {
        // Add transmit task to event queue
        ThreadID[channel] = queue.call(SendReceiveData,channel);//schedule transmission
    }
}




//common_fall handler functions
void common_fall_handler(int channel) 
{
    pinCheck = 0;
    //cancel relevant queued event    
    queue.cancel(ThreadID[channel]);
}

//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); }

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 ii;

    double dblPSI[3][3]; //the message from high-level containing the chamber lengths in meters in following format:
            /*
            {L(front,1),   L(front,2),    L(front,3);
             L(mid,1)  ,   L(mid,2)  ,    L(mid,3);
             L(rear,1) ,   L(rear,2) ,    L(rear,3);}
            */
    double dblTargetTime_s; //the time in seconds desired to reach the target (>0...!)
    
    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).
    
    //!!!!!!!!!!!!!!! If received messages faster than path replanning, will get increasingly behind time --> need to throw away some instructions
    
    //receive
    
    double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
    
    bool kk = 0;
    while(1) {
        
        if(kk == 0) {
            dblPSI[0][0] = (double) 10.0;
            dblTargetTime_s = 1.5;
        } else { 
            dblPSI[0][0] = (double) 0.0; 
            dblTargetTime_s = 2.0;
        }
        kk = !kk;
        printf("REPLAN, %f,\r\n", dblPSI[0][0] );
        //update front segment
        dblTargetChambLen_mm[0] = dblPSI[0][0]*1000;
        dblTargetChambLen_mm[1] = dblPSI[0][1]*1000;
        dblTargetChambLen_mm[2] = dblPSI[0][2]*1000;
        //update mid segment
        dblTargetChambLen_mm[6] = dblPSI[1][0]*1000;
        dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; //same because two pumps are used
        //update rear segment
        dblTargetChambLen_mm[3] = dblPSI[2][0]*1000;
        dblTargetChambLen_mm[4] = dblPSI[2][1]*1000;
        dblTargetChambLen_mm[5] = dblPSI[2][2]*1000;

        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]; 
            }
            if(dblTargetChambLen_mm[ii]<0.0) {
                dblTargetChambLen_mm[ii] = 0.0; 
            }
            
            //mutPathIn.lock();//lock variables to avoid race condition
            
            dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
            
            //work out new velocities
            dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblTargetTime_s;
            
            //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;
                }
            }
            //mutPathIn.unlock(); //unlock mutex.
            
            dblPressure_bar[ii] = ReadADCPressure_bar(ii);//read pressure from channel
            dblPosition_mtrs[ii] = ReadADCPosition_mtrs(ii); //read position from channel
            
            //send info back to HL
        } // end of for
        //printf("%f\r\n", dblVelocity_mmps[0]);
        Thread::wait(7000);
    } // end of while(1)    
}

// lock mutex
//  get variables
// unlock mutex
// do calculations
// lock mutex
//  set variables
// unlock mutex

void CalculateSmoothPath() {
    int ii;
    double dblMeasuredSampleTime;
    double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; //the current position of the smooth path (not sent to actuator)
    
    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(ii = 0; ii < N_CHANNELS; ii++) {
            if(ii == 0) printf("BEFORE: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblVelocity_mmps[0],dblMeasuredSampleTime);
            //calculate next step in linear path
            //mutPathIn.lock();//lock relevant mutex
            dblLinearPathCurrentPos_mm[ii] = dblLinearPathCurrentPos_mm[ii] + dblVelocity_mmps[ii]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
            
            if(dblLinearPathCurrentPos_mm[ii] < 0.0) {
                dblLinearPathCurrentPos_mm[ii] = 0.00;
            }
            
            //check tolerance
            if (fabs(dblLinearPathCurrentPos_mm[ii] - dblTargetActPos_mm[ii]) <= DBL_PATH_TOLERANCE) {
                dblVelocity_mmps[ii] = 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[ii] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[ii] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[ii];
            //mutChannel[ii].lock();
            intDemandPos_Tx[ii] = (int) ((dblSmoothPathCurrentPos_mm[ii]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
            intDemandPos_Tx[ii] = intDemandPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit 
            //mutChannel[ii].unlock();
            if(ii == 0) printf("AFTER: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[ii],dblVelocity_mmps[ii],dblMeasuredSampleTime);
          
            isDataReady[ii] = 1;//signal that data ready

        } // end for
        //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]); 
        //printf("%f,%f, %f, %f\r\n", dblTargetActPos_mm[0], dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
    } // end while
}



Ticker PathCalculationTicker;

int main() 
{
    int ii;
    
    //initialise relevant variables
    for(ii = 0; ii<N_CHANNELS; ii++)
    {
        
        //all chip selects in off state
        cs_LL[ii] = 1;
        cs_ADC[ii] = 1;
        
        //data ready flags set to not ready
        isDataReady[ii] = 0;
    }
    
    //calculate some control variables
    //dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm. 
    
    pinReset = 1; //initialise reset pin to not reset the controllers.
    wait(0.1);
    pinReset=0; //reset controllers to be safe
    wait(0.1);
    pinReset = 1;//ready to go
    
    //say something nice to the user.
    pc.baud(9600);
    printf("Hi, there! I'll be your mid-level controller for today.\r\n");
    
    // Start the event queue
    t.start(callback(&queue, &EventQueue::dispatch_forever));
    
    //set up the interrupts

    //set up rise interrupts MIGHT NOT NEED TO BE POINTERS
    //
    //pinGate[0].rise(&testRiseFunction);
    //testPinGate.rise(&testRiseFunction);//<working
    //testPinGate.rise(&rise0);
    
    pinGate0.rise(&rise0);
    /*
    pinGate1.rise(&rise1);
    pinGate2.rise(&rise2);
    pinGate3.rise(&rise3);
    pinGate4.rise(&rise4);
    pinGate5.rise(&rise5);
    pinGate6.rise(&rise6);
    pinGate7.rise(&rise7);
    */
    
    //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
    //
    //pinGate[0].fall(&testFallFunction);
    //testPinGate.fall(&testFallFunction); <working
    //testPinGate.fall(&fall0);
    
    pinGate0.fall(&fall0);
    /*
    pinGate1.fall(&fall1);
    pinGate2.fall(&fall2);
    pinGate3.fall(&fall3);
    pinGate4.fall(&fall4);
    pinGate5.fall(&fall5);
    pinGate6.fall(&fall6);
    pinGate7.fall(&fall7);
    */
    
    timer.start();
    
    //threadSimulateDemand.start(SimulateDemand);
    threadReceiveAndReplan.start(ReceiveAndReplan);//start Replanning thread
    //ReceiveAndReplan();
    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); 
    }
}