File content as of revision 3:c83291bf9fd2:

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

#define MAX_LENGTH_MM 100.0
#define MAX_ACTUATOR_LENGTH 52.2
#define MAX_SPEED_MMPS 24.3457
#define PATH_TOLERANCE_MM 0.2 

double dblMaxChamberLengths_mm[N_CHANNELS] = {100.0,50.0,50.0,50.0,50.0,50.0,30.0,30.0};



int ii,jj; //counting varaibles

//-----These are the variables being shared between High-Level and Mid-Level-----------------------------------------------//

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 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).
double dblTargetTime_s; //the time in seconds desired to reach the target (>0...!)

//
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];//the linear path velocity (not sent to actuator)
double dblLinearPathCurrentPos_mm[N_CHANNELS]; //the current position of the linear path (not sent to actuator)
double dblSmoothPathCurrentPos_mm[N_CHANNELS]; //the current position of the smooth path (not sent to actuator)

double dblTargetActLen_mm[N_CHANNELS];
double dblPathToActuator[N_CHANNELS];//the target position for the actuator (sent to actuator)

int intDemandPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator
int intPosSPI_Rx[N_CHANNELS]; //13 bit value received over SPI from the actuator
int intPosADC_Rx[N_CHANNELS]; //12-bit ADC reading of potentiometer on actuator

double dblPathTolerance; //how close the linear path must get to the target position before considering it a success.

double dblActuatorConversion[N_CHANNELS] = {0.24176,0.24176,0.24176,0.24176,0.24176,0.36264,0.36264};// covert from chamber lengths to actuator lengths

double dblSmoothingFactor = 0.5; // 0<x<=1, where 1 is no smoothing

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

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

//InterruptIn pinGate[N_CHANNELS] ={PF_11, PG_14, PF_15, PF_12, PF_3, PF_13, PE_11, PE_13};//gate interrupt pins

//WRONG!!!!!!!!!!
//InterruptIn pinGate[N_CHANNELS] ={PG_11, PG_14, PF_15, PF_12, PF_3, PF_13, PE_11, PE_13};//gate interrupt pins
//WRONG!!!!!

//InterruptIn testPinGate(PF_11);
//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); //
InterruptIn pinGate7(PE_13);

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


EventQueue queue(32 * EVENTS_EVENT_SIZE);

Thread t(osPriorityRealtime);

Thread threadReplan(osPriorityBelowNormal);
Thread threadPathPlan(osPriorityNormal);

Thread threadSimulateDemand(osPriorityHigh);

Mutex mutChannel[N_CHANNELS];
Mutex mutPsi;

Semaphore semPathPlan(1);//

Timer timer;//timers are nice


/*
  unsigned int result1 = 0;
  unsigned int result2 = 0;
  unsigned int result3 = 0;
  unsigned int result4 = 0;
  
  unsigned int result1a = 0;
  unsigned int result1b = 0;
*/



/*
double dblDemandPosition[N_CHANNELS];
double dblPosition[N_CHANNELS];
double dblPressure[N_CHANNELS];
*/

int ThreadID[N_CHANNELS];

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

double ReadADCPosition(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*100.0;
    return dblOutput;
}
    
double ReadADCPressure(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) 
{
    
    //get data from controller
    spi.format(16,2);
    spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
    
    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/dblActuatorConversion[channel])/1000;
    //printf("%d, %d\r\n",intPosSPI_Rx[0],intDemandPos_Tx[0]);
    
}

void testSendReceiveData() 
{
    printf("x\r\n");
    //get data from controller
    spi.format(16,2);
    spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
    
    cs_LL[0] = 0;//select relevant chip
    intPosSPI_Rx[0] = spi.write(intDemandPos_Tx[0]); //transmit & receive
    cs_LL[0] = 1;//deselect chip
    isDataReady[0] = 0;//data no longer ready, i.e. we now require new data
    
    //sort out received data
    chrErrorFlag[0] = intPosSPI_Rx[0]>>13;
    intPosSPI_Rx[0] = intPosSPI_Rx[0] & 0x1FFF;
    dblPosition_mtrs[0] = (double)intPosSPI_Rx[0]/8191*(MAX_ACTUATOR_LENGTH/dblActuatorConversion[0])/1000;
    
    
}

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

void testRiseFunction(void)
{
    pinCheck = 1;
    //check if data is ready for tranmission
    if (isDataReady[0])
    {
        // Add transmit task to event queue
        ThreadID[0] = queue.call(testSendReceiveData);//schedule transmission
    }
}

void testFallFunction(void)
{
    pinCheck = 0;
    //cancel relevant queued event    
    queue.cancel(ThreadID[0]);
}



//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 threadReplan 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 ReplanPath()
{
    
    //while(1)
    //{
        //semReplan.wait();//wait until called
        //printf("replan!\r\n");
        //for (ii = 0; ii < N_CHANNELS; ii++)
//        {
//            mutChannel[ii].lock();
//        }
        
        mutPsi.lock();// lock mutex for PSI to ensure no conflict when receiving new messages while already replanning
        //!!!!!!!!!!!!!!! If received messages faster than path replanning, will get increasingly behind time --> need to throw away some instructions
        
        double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
        //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;
       /* dblTargetChambLen_mm = { psi.
        
        dblTargetTime_s*/
        
        mutPsi.unlock();// unlock mutex for PSI to ensure no conflict when receiving new messages while already replanning
        
//        for (ii = 0; ii < N_CHANNELS; ii++)
//        {
//            mutChannel[ii].unlock();
//        }

        for (ii = 0; ii< N_CHANNELS; ii++)
        {
            mutChannel[ii].lock();//lock variables to avoid race condition
            
            //check to see if positions are achievable
            if(dblTargetChambLen_mm[ii]>dblMaxChamberLengths_mm[ii]) 
            {
                dblTargetChambLen_mm[ii] = dblMaxChamberLengths_mm[ii]; 
            }
            
            if(dblTargetChambLen_mm[ii]<0.0) 
            {
                dblTargetChambLen_mm[ii] = 0.0; 
            }
            
            dblTargetActLen_mm[ii] = dblTargetChambLen_mm[ii]*dblActuatorConversion[ii];
                
            //work out new velocities
            dblVelocity_mmps[ii] = (dblTargetActLen_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;
                }
            }
            
            mutChannel[ii].unlock(); //unlock mutex.
        }
    //}
    
    
}

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

void CalculateSmoothPath()
{
    double dblMeasuredSampleTime;
    while(1)
    {
        semPathPlan.wait();
        //if(blnReplan)
//        {
//            blnReplan = 0;//remove flag
//            ReplanPath(dblPSI, dblTargetTime_s);
//        }
        
        // 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++)
        {
            //calculate next step in linear path
            mutChannel[ii].lock();//lock relevant mutex
            dblLinearPathCurrentPos_mm[ii] = dblLinearPathCurrentPos_mm[ii] + dblVelocity_mmps[ii]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
            
            //check tolerance
            if (fabs(dblLinearPathCurrentPos_mm[ii] - dblTargetActLen_mm[ii]) <= dblPathTolerance)
            {
                dblVelocity_mmps[ii] = 0; //stop linear path generation when linear path is within tolerance of target position.
            }
            mutChannel[ii].unlock();//unlock relevant mutex
            
            //calculate next step in smooth path
            dblSmoothPathCurrentPos_mm[ii] = dblSmoothingFactor*dblLinearPathCurrentPos_mm[ii] + (1.0-dblSmoothingFactor)*dblSmoothPathCurrentPos_mm[ii];
            
            //convert to actuator distances
            dblPathToActuator[ii] = dblSmoothPathCurrentPos_mm[ii];
            
            intDemandPos_Tx[ii] = (int) ((dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
            
            intDemandPos_Tx[ii] = intDemandPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit 
            
            dblPressure_bar[ii] = ReadADCPressure(ii);
            dblPosition_mtrs[ii] = ReadADCPosition(ii);
          
            isDataReady[ii] = 1;//signal that data ready

        }
        printf("%f, %d\r\n",dblPathToActuator[0], intDemandPos_Tx[0]); 
        //printf("%f,%f, %f, %f\r\n", dblTargetActLen_mm[0], dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
    }
}

void SimulateDemand() // For testing purposes
{
    int kk = 0;
    while(1)
    {
        mutChannel[0].lock();
        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 = 1 - kk;
        
        
        //semReplan.release();
        
        mutChannel[0].unlock();
        ReplanPath();
    
        Thread::wait(7000);
    }
}



Ticker PathCalculationTicker;

int main() 
{
    for (ii = 0; ii <250; ii++)
    {
        pinCheck = 1;
        wait(0.001);
        pinCheck = 0;
        wait(0.001);
    }
    
    //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);
    threadPathPlan.start(CalculateSmoothPath); //start planning thread
    
    PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); //set up planning thread to recur at fixed intervals
    
    threadReplan.start(ReplanPath);//start Replanning thread 
     Thread::wait(1);  
    
      
    while(1) {
        Thread::wait(osWaitForever); 
    }
}