File content as of revision 6:6142902e20d1:

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

#include "QEI.h"

#include "rtos.h"
#include "FastPWM.h"

#define ERROR_LIMIT_POS 0.008 //limits for when any error measurement is considered to be zero.
#define ERROR_LIMIT_NEG -0.008

#define CHAN_1 0x80
#define CHAN_2 0x90
#define CHAN_3 0xA0
#define CHAN_4 0xB0

#define PRESSURE_CHAN 1
#define POSITION_CHAN 3

int intDummy;
//SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel

//InterruptIn pinPWMin(PA_6);
InterruptIn pinPWMin_pos(PA_8);
InterruptIn pinPWMin_vel(PA_6);
QEI wheel (PB_5, PB_4, NC, 128, QEI::X2_ENCODING);

//DigitalOut Mntr(D3);
//DigitalIn swDem(D2);
//DigitalOut Mntr2(PB_8);
   
#define EXTERNAL_CLOCK_MODE 0x08
#define RANGE_CONFIG 0x03

#define EXTERNAL_CLOCK_MODE 0x08
#define RANGE_CONFIG 0x03 //config for 1.5*Vref = 6.144V

#define PRESSURE_BIAS_VOLTAGE 0.15151515151515 

const double MAX_ACTUATOR_LENGTH = 52.2;

//const double MAX_POSITION_MM = 40.0; //maximum actuator position position in mm
const double MAX_POSITION_MM = 8.0; //maximum actuator position position in [bar] because this is a total hack. I've replaced position readings with pressure 
const double MAX_SPEED_MMPS = 4.0;
//sample time variables
double dblSampleTime_s = 0.001;

#define POT_2_MM 0.006750412 //convert potentiometer reading to mm (Tested and is right)
#define POT_OFFSET 7500//6666

Serial pc(USBTX, USBRX); // tx, rx

Thread PositionControlThread(osPriorityHigh);
Thread DebugThread(osPriorityNormal);
Thread GateControlThread(osPriorityNormal);

Thread DutyCycleThread(osPriorityRealtime);
Mutex mutDutyCycle_pos;
Mutex mutDutyCycle_vel;
Semaphore semDutyCycle(1);
Timer timerDutyCycle;

double dblSensorDriftError;

short randomvar;

volatile int dataRx;


Semaphore semPosCtrl(1);
Semaphore semDebug(1);
Semaphore semGate(1);

Timer timer;

long pulsesTest;

//define all pins
SPI spi(PB_15, PB_14, PB_13); // mosi, miso, sclk //DO NOT USE D4 OR D5 !!!
   
DigitalOut cs_ADC(PB_12);

//DigitalOut pinGate(PA_8);

//AnalogIn pinDemand1(PA_0);
//AnalogIn pinDemand2(PA_1);

AnalogIn pinCurSense(PB_0);
AnalogIn pinPresSens(PC_0);

FastPWM pinPwmOutput(PC_8);

FastPWM pinAltOutB(PA_0);
FastPWM pinAltOutA(PA_1);

//FastPWM pinSigOut(D2);

DigitalOut pinDirectionFwd(PA_13);//INB
DigitalOut pinDirectionRev(PA_14);//INA

//define all variables

int ii,jj,kk,nn, spiTick; //counting variables

volatile int slaveReceivePos;
volatile int slaveReceiveVel;
    
volatile int dataFlag;
volatile int intFeedBack_pos;
volatile int intFeedBack_pres;

int intChkSum_pos;
int intChkSum_vel;

int intPar_pos;
int intPar_vel;

//raw Data Readings
int intPotRead = 0;
int intPressureRead = 0;
double analogReadingDemPos;
double analogReadingDemVel;
double analogReadingCurSens;

char buf[10];
int dataReceived;




//system state variables 
double dblPos[10];//current position in mm
double dblPosFil[10];//current position in mm
double dblLastPos;//previous position in mm
double dblVel[10];//velocity in mm/s
double dblAccel; //acceleration in mm/s^2
double dblIntegral = 0;//integral of position;

double dblPres; //current pressure in bar
double dblPresFil[10]; //current pressure after filter in bar
double dblPresDot; //time derivative of pressure
double dblPresDotFil[10]; //filtered time derivative of pressure 
double dblLastPres; //previous pressure
double dblLastPresDot;
double dblIntPres = 0.0; //time integral of pressure


double dblPotPositionRead;

double dblPosBias = 48.0;

double dblStartingPos;

//demand variables
double dblPosD[10];
double dblLastPosD;
double dblVelD[10];
double dblAccelD;

double dblPresD;
double dblPresDotD;

double dblTargetPos = 0;
double dblTargetVel =0;
double dblLinearPath = 0;
double dblLastLinearPath = 0;
double dblPathSign;



double dblError;
double dblLastError;
double dblErrorDot;

//filter Variables
int intPosFilOrder = 0;
int intPresFilOrder = 4;
int intPresDotFilOrder = 4;
int intVelFilOrder = 1;
int intDemPosFilOrder = 6;
int intDemVelFilOrder = 6;
int intOutFilOrder = 0;

double dblPressureVal_norm;
double dblPressureVal_bar;

//controller variables 
double omega;
double zeta;

double Kp = 130.0;
double Ki = 0.0;
double Kd = 0.0;
double dblIntTerm; 
double dblIntLimit = 1.0;
int RXFlag;
double dblControlBias = 0.0;

double dblMotorVoltage = 24.0;

double output[10];//controller output 

//current limit variables
double CurrentLimitSet;
double dblCurrentLimitAmps = 3.0;
double currentBuck;
double currentBuckGain = 3.0;

//Pressure Limitation
const double dblPressureLimitBar = 10.0;
double dblPressureLimitGain = 2.0;
double dblPressureLimitBuck;
double dblPressureLimitDerivativeGain = 0.1;
double dblLastErrorPressure;
double dblErrorPressure;
double dblDeltaErrorPressure;

int intT_pos= 100;
int intDeltaT_pos =0;
int intT_vel= 100;
int intDeltaT_vel =0;

Timer timerPeriod_pos;
Timer timerDutyCycle_pos;
Timer timerPeriod_vel;
Timer timerDutyCycle_vel;

bool isFallWorking = 0;
bool isRiseWorking = 0;

const int intPeriod_us = 2000;

void PWMRise_pos() {
    mutDutyCycle_pos.lock();
    intT_pos = timerPeriod_pos.read_us();
    timerPeriod_pos.reset();
    timerDutyCycle_pos.reset();
    mutDutyCycle_pos.unlock();
    isRiseWorking = 1;
}

void PWMFall_pos() {
    mutDutyCycle_pos.lock();
    intDeltaT_pos = timerDutyCycle_pos.read_us();
    mutDutyCycle_pos.unlock();
    isFallWorking = 1;
}

void PWMRise_vel() {
    mutDutyCycle_vel.lock();
    intT_vel = timerPeriod_vel.read_us();
    timerPeriod_vel.reset();
    timerDutyCycle_vel.reset();
    mutDutyCycle_vel.unlock();
}

void PWMFall_vel() {
    mutDutyCycle_vel.lock();
    intDeltaT_vel = timerDutyCycle_vel.read_us();
    mutDutyCycle_vel.unlock();
}

double CalculateDemand(Mutex mutex, int intT, int intDeltaT, int intPeriod) {
    mutex.lock();
    int _intT = intT;
    int _intDeltaT = intDeltaT;
    mutex.unlock();
    double dblOutput, dblDutyCycle;
    if(_intT > 0){
        if(_intT>(intPeriod_us-10) && _intT <(intPeriod_us+10)){
            dblDutyCycle = (double)_intDeltaT/_intT;
            
            dblDutyCycle -= 0.1;
            dblDutyCycle /=0.8; 
            if(dblDutyCycle >1.0){ 
                dblDutyCycle = 1.0;
            }
            if(dblDutyCycle <0.0){ 
                dblDutyCycle = 0.0;
            }
        }
    }
    
    int intInput = (int)(dblDutyCycle*400.0);
    //dblTargetPos = (double)MAX_POSITION_MM*dblDutyCycle; //set target position  (9-bit value)
    dblOutput = (double)intInput/400.0; //set target position  (9-bit value)
    return dblOutput;
}

//Timer gateTimer;  

//define custom Functions
   
bool CheckMessage(int msg) {//checks if message was corrupted
    // Find message parity
    short int count = 0;
    for(short int i=0; i<32; i++) {
        if( msg>>1 & (1<<i) ) count++;
    }
    int intParity = !(count%2);
    // Find message CheckSum    
    int intChkSum = 0;
    int intTempVar = msg>>7;
    while(intTempVar > 0) {
        intChkSum += intTempVar%10;
        intTempVar = int(intTempVar/10);
    }
    // Check if parity, CheckSum and mesage type match
    bool isParityCorrect = (intParity == (msg&0x1));
    bool isChkSumCorrect = (intChkSum == ((msg>>2)&0x1F));
    bool isCheckPassed = (isParityCorrect && isChkSumCorrect);
    return isCheckPassed;
}

////////For Carafino: Start/////////





//Message checking (For Carafino)
//bool PerformSlaveSPI(SPISlave *spiSlave, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]) {//performs the SPI transaction
//   
//    unsigned int dummyMsg = 0x5555;
//    bool isSuccess = true;
//    unsigned int inboundMsg, typeBit;
//    short int numPacketsReceived = 0;
//    for(short int i=0; i<3; i++) { // Loop 3 times for 3 SPI messages
//        while( gateTimer.read_us() < 500 ) {
//            if( spiSlave->receive() ) {
//                numPacketsReceived++;
//                
//                inboundMsg = spiSlave->read();
//                //Mntr = 1;//dummy variable used to check function
//                if(i==0) { 
//                     spiSlave->reply(outboundMsgs[0]);
//                } else if(i==1) {
//                     spiSlave->reply(outboundMsgs[1]);
//                } else {
//                     spiSlave->reply(dummyMsg);
//                }
//                //Mntr = 0;
//                
//                if((unsigned int)inboundMsg != dummyMsg) { // Message is not dummy which is only used for reply
//                    typeBit = inboundMsg>>1 & 0x1;//extracts type bit from message, 0 = target Position; 1 = target Velocity
//                    inboundMsgsData[typeBit] = inboundMsg>>7 & 0x1FF;//this contains the data recieved from master
//                    if( !CheckMessage(inboundMsg) ) {
//                        isSuccess = false;
//                    }
//                }
//                break;
//            }
//        }
//    }
//    if( numPacketsReceived != 3 ) {//if it hasn't received three messages, it failed.
//        isSuccess = false;
//    }
//    return isSuccess;
//}

////////For Carafino: End/////////
   
int Read14BitADC(int channel, DigitalOut CSpin)
{
    char message;
    unsigned int outputA;
    unsigned int outputB;
    int output;

    switch(channel)
    {
        case 1:
            message = CHAN_1;
            break;
    
        case 2:
           message = CHAN_2;
           break;
    
        case 3:
           message = CHAN_3;
           break;
        case 4:
           message = CHAN_4;
           break;
           
        default:
        message = CHAN_1;
    }
    
    spi.format(8,0);
    spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
    
    CSpin.write(0);
    spi.write(message);
    spi.write(0x00);
    outputA = spi.write(0x00);
    outputB = spi.write(0x00);
    CSpin.write(1);
    
    //convert result to sensible value
    outputA = outputA<<8;
    output =  outputA | outputB;
    output = output>>2;
    
    return output;
}
    
void ADC_Config()
{
    unsigned int msg;
    spi.format(8,0);
    spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
    
    msg = CHAN_1 | RANGE_CONFIG; //config channel 1 set Vref as 
    cs_ADC = 0;
    spi.write(msg);
    cs_ADC = 1;
    
    cs_ADC = 0;
    spi.write(EXTERNAL_CLOCK_MODE);
    cs_ADC = 1;
    
    msg = CHAN_2 | RANGE_CONFIG; //config channel 2
    cs_ADC = 0;
    spi.write(msg);
    cs_ADC = 1;
    
    cs_ADC = 0;
    spi.write(EXTERNAL_CLOCK_MODE);
    cs_ADC = 1;
        
    msg = CHAN_3 | RANGE_CONFIG; //config channel 3
    cs_ADC = 0;
    spi.write(msg);
    cs_ADC = 1;
    
    cs_ADC = 0;
    spi.write(EXTERNAL_CLOCK_MODE);
    cs_ADC = 1;
    
    msg = CHAN_4 | RANGE_CONFIG; //config channel 4
    cs_ADC = 0;
    spi.write(msg);
    cs_ADC = 1;
    
    cs_ADC = 0;
    spi.write(EXTERNAL_CLOCK_MODE);
    cs_ADC = 1;
}
    
    
int SumDigits(int var)
{
    int intSumResult = 0;
    int intTempVar = var;
    while (intTempVar >0)
    {
        intSumResult += intTempVar%10;
        intTempVar = int(intTempVar/10);
        //intSumResult += int(var/100)%100;
    }
    return intSumResult;
}

//int EvenParityBitGen(int var)
int OddParityBitGen(int var)
{
    unsigned int count = 0, i, b = 1;

    for(i = 0; i < 32; i++){
        if( var & (b << i) ){count++;}
    }

    if( (count % 2) ){
        return 0;
    }

    return 1;
}
    
//void CloseGate()
//{
//    pinGate = 0;
//    
//}
// 
 double dblReadDem = 0;
 
void PositionControlPID()
{
   while(1){
   semPosCtrl.wait();
    //slave.reply(0x5555);
//Mntr2 = !Mntr2;
   //Mntr2 = 1 - Mntr2;//!led; 
   pulsesTest = wheel.getPulses();
   
   dblReadDem = (double)pulsesTest/256;
   
   if (dblReadDem > 1.0){
       dblReadDem = 1.0;
    }
    if (dblReadDem < 0){
        dblReadDem = 0.0;
    }
    
    dblReadDem = dblReadDem * 4.0;

   double testy = (double) abs(pulsesTest)/2000;
   //take all readings
    //sensor readings
    
    intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure
    intPressureRead = pinPresSens.read();
    
    
    dblPressureVal_norm = pinPresSens.read();
    dblPressureVal_norm = dblPressureVal_norm*3.3;//convert to voltage
    dblPressureVal_norm = dblPressureVal_norm - 0.5;//subtract offset
    dblPressureVal_norm = dblPressureVal_norm/4.0;//calculate normalised pressure
    
    if (dblPressureVal_norm >1.0){
        dblPressureVal_norm = 1.0;
    }
    if (dblPressureVal_norm < 0.0){
        dblPressureVal_norm = 0.0;
    }
    
   
    //intPressureRead = intPressureRead-1334;
    //intPressureRead = intPressureRead-1679;
    
    //dblPressureVal_bar = ( (double) intPressureRead/10667)*10.0;
    //pressureCheck = ( (double) intPressureRead/10667)*10.0;
    dblPressureVal_bar = dblPressureVal_norm * 25.0;
    dblPres = dblPressureVal_bar; 
    //intFeedBack_pres = (int)(((double)intPressureRead/10667) * 511);

    //unsigned short garb = 0x01;
//           
//    intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
//    dblPotPositionRead = (double) POT_2_MM*(intPotRead  - POT_OFFSET);
    
    //demand Readings
    
    //current reading
    analogReadingCurSens = (double) 0.3*pinCurSense.read()+0.7*analogReadingCurSens;
    
    //convert units and filter
    
     //get position and filter
    //dblPos[0] = (double) pulsesTest*-0.0078125 + dblStartingPos;
//    dblSensorDriftError = dblPotPositionRead - dblPos[0];
    
    double alpha = (6.283*dblSampleTime_s*100)/(1 + 6.283*dblSampleTime_s*100);
    
    //filter pressure reading
    if (intPresFilOrder>0) {
        dblPresFil[0] = alpha*dblPres + (1-alpha)*dblPresFil[intPresFilOrder];//filter at 100Hz cutoff
        if (intPresFilOrder>1) {
            for (ii = 1;ii<intPresFilOrder+1; ii++){
                dblPresFil[ii] = alpha*dblPresFil[ii-1]+ (1-alpha)*dblPresFil[ii];
            }
        }
    }
    else{
      dblPresFil[intPresFilOrder] = dblPres;
    }
    
    intFeedBack_pres = (int) (dblPresFil[intPresFilOrder]/12*511);//scale pressure to 12bar
    
    if(intFeedBack_pres > 511){
        intFeedBack_pres = 511;
    }
    if(intFeedBack_pres < 0){
        intFeedBack_pres = 0;
    }
    
    //printf("%f\t",dblPos[0]);
    //printf("%d\t",intPressureRead);
    //printf("\r\n");
    //intFeedBack_pres = intFeedBack_pres>>5;
   
    intFeedBack_pres = (intFeedBack_pres<<5) | SumDigits(intFeedBack_pres);//add checksum
    intFeedBack_pres = (intFeedBack_pres<<1);
    intFeedBack_pres = intFeedBack_pres | 0x0001; //add type (1 for pressure)
    intFeedBack_pres = (intFeedBack_pres <<1) | OddParityBitGen(intFeedBack_pres);//add parity
    intFeedBack_pres =  intFeedBack_pres & 0xFFFF;
    
    
    dblPresDot = dblPresFil[intPresFilOrder] - dblLastPres;//calculate difference

    
    //filter pressure time derivative
    if (intPresDotFilOrder>0){
        dblPresDotFil[0] = alpha*dblPresDot + (1-alpha)*dblPresDotFil[intPresDotFilOrder];//filter at 100Hz cutoff
        if (intPresDotFilOrder>1){
            for (ii = 1;ii<intPresDotFilOrder+1; ii++){
                dblPresDotFil[ii] = alpha*dblPresDotFil[ii-1]+ (1-alpha)*dblPresDotFil[ii];
            }
        }
    }
    else{
      dblPresDotFil[intPresDotFilOrder] = dblPresDot;
    }
    
    //feedback stuff, set position equal to pressure
    intFeedBack_pos = intFeedBack_pres;
    intFeedBack_pos = (intFeedBack_pos<<5) | SumDigits(intFeedBack_pos);//add checkSum
    intFeedBack_pos = intFeedBack_pos <<1; // add type (0 for position)
    intFeedBack_pos = (intFeedBack_pos <<1) | OddParityBitGen(intFeedBack_pos);//add parity  
  
  //self pressure demant set  
//    int intState = swDem.read();
//    if (intState){
//        dblTargetPos = 6.0;
//        dblTargetVel = 6.0;
//    }
//    else {
//        dblTargetPos = 1.5;
//        dblTargetVel = 6.0;
//    }      
    
    ///////////////PATH GENERATION////////////////////////
    //work out next path point
    double dblPathDifference;
    dblPathDifference = dblTargetPos - dblLinearPath;
    dblPathSign = dblPathDifference/fabs(dblPathDifference); //is velocity positive or negative?

    //check if target has not been reached (with additional 1% of step to be sure)
    if (fabs(dblPathDifference) > 1.01*dblTargetVel*dblSampleTime_s) {
        dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path
    }
    else { //if path is very close to target position
        dblLinearPath = dblTargetPos;
    }
    
    //limit position
    if(dblLinearPath > MAX_POSITION_MM){
        dblLinearPath = MAX_POSITION_MM;
    }
    if (dblLinearPath < 0.0){
        dblLinearPath = 0.0;
    }
    
    dblPosD[intDemPosFilOrder] = 0.3*dblLinearPath + 0.7*dblPosD[intDemPosFilOrder];
    
    //make sure path is safe
    if (dblPosD[intDemPosFilOrder] > MAX_POSITION_MM) {
        dblPosD[intDemPosFilOrder] = MAX_POSITION_MM;
    }
    if (dblPosD[intDemPosFilOrder] < 0.0) {
        dblPosD[intDemPosFilOrder] = 0.0;
    }
     
    dblVelD[0] = dblPosD[intDemPosFilOrder] - dblLastPosD;
    dblPresD = dblPosD[intDemPosFilOrder];//set demand pressure to calculated demand 
    dblPresDotD =  dblVelD[0];//set demand rate of change in pressure 
    
    ///////////////////////////////////////////////////// End of Path Generation
    //run PID calculations
    //get errors
    
    dblError = dblPresD - dblPresFil[intPresFilOrder];
    dblErrorDot = dblPresDotD - dblPresDotFil[intPresDotFilOrder];
    //get integral
    dblIntTerm = dblIntTerm + Ki*dblError;
    
    //limit integral term
    if (dblIntTerm > dblIntLimit){
        dblIntTerm = dblIntLimit;
    }
    if (dblIntTerm < -1.0*dblIntLimit){
        dblIntTerm = (double) -1.0*dblIntLimit;
    }
    
    if(fabs(dblError) <0.0321) {
        dblError = 0.0;
        //dblErrorDot = 0.0;
        
    }
        
    if (fabs(dblErrorDot) < 0.2) {
    dblErrorDot = 0.0;
    }
    
    //calculate output
    
    
    output[0] = Kp*dblError + dblIntTerm + Kd*dblErrorDot;
    //printf("%f\r\n",dblError);   
    //limit output
    if (output[0] > 0.95){
        output[0] = 0.95;
    }
    if (output[0] < -0.95) {
        output[0] = -0.95;
    }
    
    if(intOutFilOrder>0){
        for (ii = 1; ii < intOutFilOrder+1; ii++) {
            output[ii] = 0.7*output[ii-1] + 0.3*output[ii];
        }
    }
    else{
        output[intOutFilOrder] = output[0];
    }
        
    
    //limit pressure
    if (dblPressureVal_bar >dblPressureLimitBar){
        dblErrorPressure = dblPressureVal_bar - dblPressureLimitBar;
        dblDeltaErrorPressure = dblErrorPressure - dblLastErrorPressure;
        
        dblPressureLimitBuck = dblPressureLimitGain * dblErrorPressure;
        dblPressureLimitBuck = dblPressureLimitBuck + (dblPressureLimitDerivativeGain*dblDeltaErrorPressure);
        dblPressureLimitBuck = dblPressureLimitBuck *1.9/2.0;
        dblLastErrorPressure = dblErrorPressure;
    }
    else {
        dblPressureLimitBuck = 0.0;
    }
    
    if (dblPressureLimitBuck < 0.0){
        dblPressureLimitBuck = 0.0;
    }
    if (dblPressureLimitBuck > 1.9){
        dblPressureLimitBuck = 1.9;
    }
    
    output[intOutFilOrder] = output[intOutFilOrder] - dblPressureLimitBuck;
    
    //limit output
    if (output[intOutFilOrder] > 0.95){
        output[intOutFilOrder] = 0.95;
    }
    if (output[intOutFilOrder] < -0.95){
        output[intOutFilOrder] = -0.95;
    }
    
    //limit current
    if (analogReadingCurSens> CurrentLimitSet){
        currentBuck = CurrentLimitSet / analogReadingCurSens / currentBuckGain;
    }
    else{
        currentBuck = 1.0;
    }
    
    if (currentBuck >1.0){
        currentBuck = 1.0;
    }
    
    if (currentBuck <0.0){
        currentBuck = 0.0;
    }
                
    output[intOutFilOrder] = currentBuck*output[intOutFilOrder];
    
    
    //output[intOutFilOrder] = dblPresD;
    //end Current limit
    
    double dblBias = 0.08*dblPresFil[intPresFilOrder] + 0.2;
    dblBias = 0.0;

    //find direction
    if(output[intOutFilOrder] >=0.0)
    {
        //pinDirectionFwd = 1;
        //pinDirectionRev = 0;
        //dblControlBias = 0.0;
        pinAltOutB.write(abs(output[intOutFilOrder])+dblBias);
        pinAltOutA.write(0.0);
        pinPwmOutput.write(1.0);

    }
    else
    {
        //pinDirectionFwd = 0;
//        pinDirectionRev = 1;
//        dblControlBias = 0.0;
        pinAltOutA.write(abs(output[intOutFilOrder])+dblBias);
        pinAltOutB.write(0.0);
        pinPwmOutput.write(1.0);
    }
    
    //printf("O:%f\t\r\n",abs(output[intOutFilOrder]));
    //pinPwmOutput.write(abs(output[intOutFilOrder]));
    
    //update all past variables
    dblLastPos = dblPos[intPosFilOrder];
    dblLastPosD = dblPosD[intDemPosFilOrder];
    
    dblTargetPos = CalculateDemand(mutDutyCycle_pos, intT_pos, intDeltaT_pos, intPeriod_us);
    dblTargetPos = (double)MAX_POSITION_MM*dblTargetPos; //set target position  (9-bit value)
    if(dblTargetPos>MAX_POSITION_MM) { // Limit demand to ensure safety
        dblTargetPos = MAX_POSITION_MM;
    } else if(dblTargetPos<0.0) {
        dblTargetPos = 0.0;
    }
    
    //dblTargetVel = (double)MAX_SPEED_MMPS*inboundMsgsData[1]/511;//set target velocity  (9-bit value)
    dblTargetVel = CalculateDemand(mutDutyCycle_vel, intT_vel, intDeltaT_vel, intPeriod_us);
    dblTargetVel = (double)MAX_SPEED_MMPS*dblTargetVel; //set target position  (9-bit value)
    if(dblTargetVel>MAX_SPEED_MMPS) {
        dblTargetVel = MAX_SPEED_MMPS;
    } else if(dblTargetVel<0.0) {
        dblTargetVel = 0.0;
    }
    //intInput = (int)(dblDutyCycle*1000.0);
    
    //printf("%f\t%f\r\n",dblTargetPos, dblTargetVel);
    
    //////print debug//////

    //printf("P: %f\tPd: %f\t lin: %f\tTarget: %f\r\n", dblPresFil[intPresFilOrder],dblPresD, dblLinearPath,dblTargetPos);
    //prinntf("E: %f\tBias: %f\r\n",output[intOutFilOrder], dblBias);
    
    ///////////////////Communication (For Carafino: Start)////////////////////////////////
    
    //Message handling MCU can only receive messages when gate is 'open'. Gate opens for 500us every 1ms.
    //gateTimer.reset();//resets the gate timer
//    pinGate = 1;//gate is open. Sets digital output pin to HIGH
//    unsigned int outboundMsgs[2] = {intFeedBack_pos, intFeedBack_pres};//prepare feedback messages
//    unsigned int inboundMsgsData[2] = {0,0};
    //while(gateTimer.read_us() < 500) {//while gate is open
//        
//        if(slave.receive()) {//when a message is received
//            
//            bool isSPIsuccess = PerformSlaveSPI(&slave,outboundMsgs,inboundMsgsData);//process the data and check if it was corrupted
//            
//            if( isSPIsuccess ) {//if message looks OK
//                dblTargetPos = (double)MAX_POSITION_MM*inboundMsgsData[0]/511; //set target position  (9-bit value)
//                if(dblTargetPos>MAX_POSITION_MM) { // Limit demand to ensure safety
//                    dblTargetPos = MAX_POSITION_MM;
//                } else if(dblTargetPos<0.0) {
//                    dblTargetPos = 0.0;
//                }
//                
//                dblTargetVel = (double)MAX_SPEED_MMPS*inboundMsgsData[1]/511;//set target velocity  (9-bit value)
//                
//                if(dblTargetVel>MAX_SPEED_MMPS) {
//                    dblTargetVel = MAX_SPEED_MMPS;
//                } else if(dblTargetVel<0.0) {
//                    dblTargetPos = 0.0;
//                }
//                
//                break;//bail out of while loop
//            }
//        }
//    }
//    
//    pinGate = 0;//close gate
   
    dblLastPres = dblPresFil[intPresFilOrder];//update previous pressure reading
    dblLastPresDot = dblPresDotFil[intPresDotFilOrder];
    }
}
   //////////////////////////////////////////////////For Carafino: End



//configure all control parameters
void ControlParameterConfig(){
   Kp = Kp/dblMotorVoltage;
   Kd = Kd/dblSampleTime_s/dblMotorVoltage;
   Ki = Ki*dblSampleTime_s/dblMotorVoltage;
   
   dblReadDem = 0;
}

Ticker debugTicker;

void startPositionControl(){
    semPosCtrl.release();
}

void startDebug(){
    semDebug.release();
}

Ticker positionCtrlTicker;
   
int main() {
    //pinGate = 0;
    //swDem.mode(PullDown);
    cs_ADC = 1;
    //Mntr = 0;
    //Mntr2 = 0;
    pinPwmOutput.period_us(1000);
    
    pinPWMin_pos.mode(PullNone);
    timerDutyCycle_pos.start();
    timerPeriod_pos.start();

    pinPWMin_vel.mode(PullNone);
    timerDutyCycle_vel.start();
    timerPeriod_vel.start();    
    //calculateTicker.attach(&CalculateDutyCycle,0.1);
    
    pinPWMin_pos.rise(&PWMRise_pos);
    pinPWMin_pos.fall(&PWMFall_pos);
    pinPWMin_vel.rise(&PWMRise_vel);
    pinPWMin_vel.fall(&PWMFall_vel);
    
    pc.baud(115200);
    printf("\r\nYo Yo Yo, Low Level innit\r\n\n\n");
    wait(0.5);
    
    timer.start();
    //gateTimer.start();
    //cs_ADC = 1;
    
    ControlParameterConfig();
//    for (ii = 0; ii< 10; ii++)
//    {
//        uintPotRead = Read14BitADC(3, cs_ADC);//read potentiometer
//        dblStartingPos = (double) POT_2_MM*uintPotRead  - dblPosBias;
//    }
//    
//    slave.format(16,2);
//    slave.frequency(10000000);
    
    dblPosD[intDemPosFilOrder] = 0.0;
    slaveReceivePos = 0.0;
    slaveReceiveVel = 0.0;
    wait(0.1);
    ADC_Config();
    wait(0.1);
    ADC_Config();
    wait(0.1);
    intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
    //intPotRead = (Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
    dblStartingPos = 0.0;//(double) POT_2_MM*(intPotRead  - POT_OFFSET);
    dblLinearPath = dblStartingPos;
    dblTargetPos = dblStartingPos;
    dblPos[intPosFilOrder] = dblStartingPos;
    dblTargetVel = 0.0;
    dblPosD[intDemPosFilOrder] = dblStartingPos;
    
    printf("\r\nPotRead: %d, Current Pos: %f, Target Pos: %f\r\n", intPotRead, dblStartingPos, dblTargetPos);
        
    //calculate/convert variables
    
    CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3;
    //slave.reply(0x5555);
    PositionControlThread.start(PositionControlPID);

    positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s);
    
    intFeedBack_pos = 0;
    intFeedBack_pres = 0;
    
    
    while(1){
        Thread::wait(osWaitForever);
    }
}