File content as of revision 0:607bc887b6e0:

#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

#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 50.0
#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,kk; //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 dblLinearPath_mm[N_CHANNELS]; //the current position of the linear path (not sent to actuator)
double dblSmoothPath_mm[N_CHANNELS]; //the current position of the smooth path (not sent to actuator)
double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
double dblTargetActLen_mm[N_CHANNELS];
double dblPathToActuator[N_CHANNELS];//the target position for the actuator (sent to actuator)

int intDemPos_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};

double dblSmoothingFactor = 0.5;

char chrErrorFlag[N_CHANNELS];

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

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

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

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

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 mut[N_CHANNELS];

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 dblDemPosition[N_CHANNELS];
double dblPosition[N_CHANNELS];
double dblPressure[N_CHANNELS];

int ThreadID[N_CHANNELS];

bool blnDataReady[N_CHANNELS];

unsigned int ReadADCPosition(int channel)
{
    unsigned int outputA;
    unsigned int outputB;
    unsigned int output;

    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);
    
    return output;
}
    
unsigned int ReadADCPressure(int channel)
{
    unsigned int outputA;
    unsigned int outputB;
    unsigned int output;

    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);
    
    return output;
}

void TransmitData(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(intDemPos_Tx[channel]); //transmit & receive
    cs_LL[channel] = 1;//deselect chip
    
    //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];
}

//common rise handler function 

void common_rise_handler(int channel)
{
    //check if data is ready for tranmission
    if (blnDataReady[channel])
    {
        // Add transmit task to event queue
        blnDataReady[channel] = 0;//data no longer ready
        ThreadID[channel] = queue.call(TransmitData,channel);//schedule transmission
    }
}



//common_fall handler functions
void common_fall_handler(int channel) 
{
    //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()
{
    semPathPlan.release();
}

//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++)
        {
            mut[ii].lock();
        }
        
        //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++)
        {
            mut[ii].unlock();
        }

        for (ii = 0; ii< N_CHANNELS; ii++)
        {
            mut[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] - dblLinearPath_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;
                }
            }
            
            mut[ii].unlock(); //unlock mutex.
        }
    //}
    
    
}

void CalculatePath()
{
    while(1)
    {
        semPathPlan.wait();
        //if(blnReplan)
//        {
//            blnReplan = 0;//remove flag
//            ReplanPath(dblPSI, dblTargetTime_s);
//        }
        for(ii = 0; ii < N_CHANNELS; ii++)
        {
            //calculate next step in linear path
            mut[ii].lock();//lock relevant mutex
            dblLinearPath_mm[ii] = dblLinearPath_mm[ii] + dblVelocity_mmps[ii]*PATH_SAMPLE_TIME_S;
            //check tolerance
            if (abs(dblLinearPath_mm[ii] - dblTargetActLen_mm[ii]) <= dblPathTolerance)
            {
                dblVelocity_mmps[ii] = 0; //stop linear path generation when linear path is within tolerance of target position.
            }
            mut[ii].unlock();//unlock relevant mutex
            
            //calculate next step in smooth path
            dblSmoothPath_mm[ii] = dblSmoothingFactor*dblLinearPath_mm[ii] + (1.0-dblSmoothingFactor)*dblSmoothPath_mm[ii];
            
            //convert to actuator distances
            dblPathToActuator[ii] = dblSmoothPath_mm[ii];
            
            intDemPos_Tx[ii] = (int) dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH*8191;//convert to a 13-bit number;
            intDemPos_Tx[ii] = intDemPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit 
            
          
            blnDataReady[ii] = 1;//signal that data ready

        }
        printf("%d, %f,%f,%f, %f\r\n",timer.read_ms(), dblTargetActLen_mm[0] ,dblVelocity_mmps[0], dblLinearPath_mm[0], dblSmoothPath_mm[0]);
    }
}

void SimulateDemand()
{
    while(1)
    {
        mut[0].lock();
        if(kk == 0)
        {
            dblPSI[0][0] = (double) 10.0;
            dblTargetTime_s = 1.0;
        }
        else
        { 
            dblPSI[0][0] = (double) 0.0; 
            dblTargetTime_s = 2.0;
        }
        
        kk = 1 - kk;
        
        
        //semReplan.release();
        
        mut[0].unlock();
        ReplanPath();
    
        Thread::wait(7000);
    }
}



Ticker PathCalculationTicker;

int main() 
{
    //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
        blnDataReady[ii] = 0;
    }
    
    //calculate some control variables
    dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance. 
    
    pinReset = 1; //initialise reset pin to not reset the controllers.
    
    //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(&rise0);
    pinGate[1].rise(&rise1);
    pinGate[2].rise(&rise2);
    pinGate[3].rise(&rise3);
    pinGate[4].rise(&rise4);
    pinGate[5].rise(&rise5);
    pinGate[6].rise(&rise6);
    pinGate[7].rise(&rise7);
    
    //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
    pinGate[0].fall(&fall0);
    pinGate[1].fall(&fall1);
    pinGate[2].fall(&fall2);
    pinGate[3].fall(&fall3);
    pinGate[4].fall(&fall4);
    pinGate[5].fall(&fall5);
    pinGate[6].fall(&fall6);
    pinGate[7].fall(&fall7);
    
    timer.start();
    kk = 0;
    
    threadSimulateDemand.start(SimulateDemand);
    threadPathPlan.start(CalculatePath); //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); 
    }
}