Revision 6:6142902e20d1, committed 2021-06-25

Comitter:

dofydoink

Date:

Fri Jun 25 07:57:57 2021 +0000

Parent:

5:4e710cef655e

Commit message:

Working pressure control low level code;

Changed in this revision

--- a/main.cpp	Mon Apr 01 14:15:31 2019 +0000
+++ b/main.cpp	Fri Jun 25 07:57:57 2021 +0000
@@ -17,16 +17,17 @@
 #define PRESSURE_CHAN 1
 #define POSITION_CHAN 3
 
-
+int intDummy;
+//SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel
 
-const double MAX_SPEED_MMPS = 24.3457;
-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);
 
-QEI wheel (PB_5, PB_4, NC, 256, QEI::X4_ENCODING);
-
-DigitalOut Mntr(D3);
-DigitalOut Mntr2(PB_8);
+//DigitalOut Mntr(D3);
+//DigitalIn swDem(D2);
+//DigitalOut Mntr2(PB_8);
    
 #define EXTERNAL_CLOCK_MODE 0x08
 #define RANGE_CONFIG 0x03
@@ -38,8 +39,9 @@
 
 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 = 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;
 
@@ -53,30 +55,17 @@
 Thread GateControlThread(osPriorityNormal);
 
 Thread DutyCycleThread(osPriorityRealtime);
-Mutex mutDutyCycle;
+Mutex mutDutyCycle_pos;
+Mutex mutDutyCycle_vel;
 Semaphore semDutyCycle(1);
 Timer timerDutyCycle;
 
-volatile unsigned int intT;
-volatile unsigned int intDeltaT;
-double dblDutyCycle;
-
 double dblSensorDriftError;
 
 short randomvar;
 
 volatile int dataRx;
 
-void CalculateDutyCycle()
-{
-    while(1)
-    {
-        semDutyCycle.wait();
-        mutDutyCycle.lock();
-        dblDutyCycle = (double) intDeltaT/intT;
-        mutDutyCycle.unlock();
-    }
-}
 
 Semaphore semPosCtrl(1);
 Semaphore semDebug(1);
@@ -91,16 +80,20 @@
    
 DigitalOut cs_ADC(PB_12);
 
-DigitalOut pinGate(PA_8);
+//DigitalOut pinGate(PA_8);
 
-AnalogIn pinDemand1(PA_0);
-AnalogIn pinDemand2(PA_1);
+//AnalogIn pinDemand1(PA_0);
+//AnalogIn pinDemand2(PA_1);
 
 AnalogIn pinCurSense(PB_0);
+AnalogIn pinPresSens(PC_0);
 
 FastPWM pinPwmOutput(PC_8);
 
-FastPWM pinSigOut(D2);
+FastPWM pinAltOutB(PA_0);
+FastPWM pinAltOutA(PA_1);
+
+//FastPWM pinSigOut(D2);
 
 DigitalOut pinDirectionFwd(PA_13);//INB
 DigitalOut pinDirectionRev(PA_14);//INA
@@ -143,6 +136,15 @@
 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;
@@ -155,10 +157,13 @@
 double dblVelD[10];
 double dblAccelD;
 
-double dblTargetPos;
-double dblTargetVel;
-double dblLinearPath;
-double dblLastLinearPath;
+double dblPresD;
+double dblPresDotD;
+
+double dblTargetPos = 0;
+double dblTargetVel =0;
+double dblLinearPath = 0;
+double dblLastLinearPath = 0;
 double dblPathSign;
 
 
@@ -169,26 +174,29 @@
 
 //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 = 20.0;
-double Ki = 2.0;
-double Kd = 1.5;
+double Kp = 130.0;
+double Ki = 0.0;
+double Kd = 0.0;
 double dblIntTerm; 
-double dblIntLimit = 0.8;
+double dblIntLimit = 1.0;
 int RXFlag;
 double dblControlBias = 0.0;
 
-double dblMotorVoltage = 12.0;
+double dblMotorVoltage = 24.0;
 
 double output[10];//controller output 
 
@@ -198,11 +206,92 @@
 double currentBuck;
 double currentBuckGain = 3.0;
 
-Timer gateTimer;  
+//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) {
+bool CheckMessage(int msg) {//checks if message was corrupted
     // Find message parity
     short int count = 0;
     for(short int i=0; i<32; i++) {
@@ -223,44 +312,53 @@
     return isCheckPassed;
 }
 
-bool PerformSlaveSPI(SPISlave *spiSlave, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]) {
-   
-    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;
-                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;
-                    inboundMsgsData[typeBit] = inboundMsg>>7 & 0x1FF;
-                    if( !CheckMessage(inboundMsg) ) {
-                        isSuccess = false;
-                    }
-                }
-                break;
-            }
-        }
-    }
-    if( numPacketsReceived != 3 ) {
-        isSuccess = false;
-    }
-    return isSuccess;
-}
+////////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)
 {
@@ -310,8 +408,7 @@
     
 void ADC_Config()
 {
-    
-     unsigned int msg;
+    unsigned int msg;
     spi.format(8,0);
     spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
     
@@ -350,7 +447,6 @@
     cs_ADC = 0;
     spi.write(EXTERNAL_CLOCK_MODE);
     cs_ADC = 1;
-    
 }
     
     
@@ -364,7 +460,6 @@
         intTempVar = int(intTempVar/10);
         //intSumResult += int(var/100)%100;
     }
-    
     return intSumResult;
 }
 
@@ -384,42 +479,107 @@
     return 1;
 }
     
-void CloseGate()
-{
-    pinGate = 0;
-    
-}
-    
-  
-  
-
+//void CloseGate()
+//{
+//    pinGate = 0;
+//    
+//}
+// 
+ double dblReadDem = 0;
+ 
 void PositionControlPID()
 {
-   while(1)
-   {
+   while(1){
    semPosCtrl.wait();
-    
-    Mntr2 = !Mntr2;
+    //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;
-   pinSigOut.write(testy);
-   
    //take all readings
-    
     //sensor readings
     
     intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure
-    intPressureRead = intPressureRead-1334;
-    
-    dblPressureVal_bar = ( (double) intPressureRead/10667)*10.0;
-    
-     
-    intFeedBack_pres = (int)(((double)intPressureRead/10667) * 511);
-    //printf("%d\r\n",intFeedBack_pres);
+    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
@@ -427,79 +587,40 @@
     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;
-
-    unsigned short garb = 0x01;
-           
-    intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
-    dblPotPositionRead = (double) POT_2_MM*(intPotRead  - POT_OFFSET);
     
     
-    //demand Readings
+    dblPresDot = dblPresFil[intPresFilOrder] - dblLastPres;//calculate difference
 
     
-    //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];
-    
-    if(dblSensorDriftError > 2.0)//if encoder reading is seriously wrong
-    {
-        //dblPos[0] = dblPotPositionRead;
-    }
-    
-    //printf("%d, %f, %f\r\n",pulsesTest,dblPos[0],dblPotPositionRead);
-    if(intPosFilOrder > 0)
-    {
-        for (ii = 1; ii<intPosFilOrder+1; ii++)
-        {
-            dblPos[ii] = (double) 0.7*dblPos[ii-1] + 0.3*dblPos[ii];
+    //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
-    {
-        dblPos[intPosFilOrder] = dblPos[0];
-    }
-
-    //get velocity and filter
-    dblVel[0] = dblPos[intPosFilOrder] - dblLastPos;
-    if(intVelFilOrder>0)
-    {
-        for (ii = 1; ii<intVelFilOrder+1; ii++)
-        {
-            dblVel[ii] = (double) 0.7*dblVel[ii-1] + 0.3*dblVel[ii];
-        }
-    }
-    else
-    {
-        dblVel[intVelFilOrder] = dblVel[0];
+    else{
+      dblPresDotFil[intPresDotFilOrder] = dblPresDot;
     }
     
-    
-    //printf("%f\r\n",dblPosD[intDemPosFilOrder]);
-    
-    intFeedBack_pos = (int) ((dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH)*511);
-    
-    if(intFeedBack_pos>511)
-    {
-        intFeedBack_pos = 511;
-    }
-        
-    if(intFeedBack_pos<0)
-    {
-        intFeedBack_pos = 0;
-    }
-    
-        
+    //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
-    
-    //intFeedBack = dblSensorDriftError*8191;
-          
+    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
@@ -523,7 +644,7 @@
         dblLinearPath = 0.0;
     }
     
-    dblPosD[intDemPosFilOrder] = 0.07*dblLinearPath + 0.93*dblPosD[intDemPosFilOrder];
+    dblPosD[intDemPosFilOrder] = 0.3*dblLinearPath + 0.7*dblPosD[intDemPosFilOrder];
     
     //make sure path is safe
     if (dblPosD[intDemPosFilOrder] > MAX_POSITION_MM) {
@@ -534,177 +655,261 @@
     }
      
     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 = dblPosD[intDemPosFilOrder] - dblPos[intPosFilOrder];
-    dblErrorDot = dblVelD[intDemVelFilOrder] - dblVel[intVelFilOrder];
+    
+    dblError = dblPresD - dblPresFil[intPresFilOrder];
+    dblErrorDot = dblPresDotD - dblPresDotFil[intPresDotFilOrder];
     //get integral
-    //printf("%f\r\n",dblError);
-
-    
     dblIntTerm = dblIntTerm + Ki*dblError;
     
     //limit integral term
-    if (dblIntTerm > dblIntLimit)
-    {
+    if (dblIntTerm > dblIntLimit){
         dblIntTerm = dblIntLimit;
     }
-    if (dblIntTerm < -1.0*dblIntLimit)
-    {
+    if (dblIntTerm < -1.0*dblIntLimit){
         dblIntTerm = (double) -1.0*dblIntLimit;
     }
     
-    if(fabs(dblError) <0.01)
-    {
+    if(fabs(dblError) <0.0321) {
         dblError = 0.0;
-        dblErrorDot = 0.0;
+        //dblErrorDot = 0.0;
         
     }
         
-    if (fabs(dblErrorDot) < 0.1)
-    {
+    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)
-    {
+    if (output[0] > 0.95){
         output[0] = 0.95;
     }
-    if (output[0] < -0.95)
-    {
+    if (output[0] < -0.95) {
         output[0] = -0.95;
     }
     
-    if(intOutFilOrder>0)
-    {
-        for (ii = 1; ii < intOutFilOrder+1; ii++)
-        {
+    if(intOutFilOrder>0){
+        for (ii = 1; ii < intOutFilOrder+1; ii++) {
             output[ii] = 0.7*output[ii-1] + 0.3*output[ii];
         }
     }
-    else
-    {
+    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)
-    {
+    if (analogReadingCurSens> CurrentLimitSet){
         currentBuck = CurrentLimitSet / analogReadingCurSens / currentBuckGain;
     }
-    else
-    {
+    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;
+        //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;
+        //pinDirectionFwd = 0;
+//        pinDirectionRev = 1;
+//        dblControlBias = 0.0;
+        pinAltOutA.write(abs(output[intOutFilOrder])+dblBias);
+        pinAltOutB.write(0.0);
+        pinPwmOutput.write(1.0);
     }
     
-    pinPwmOutput.write(abs(output[intOutFilOrder])+dblControlBias);
+    //printf("O:%f\t\r\n",abs(output[intOutFilOrder]));
+    //pinPwmOutput.write(abs(output[intOutFilOrder]));
     
     //update all past variables
     dblLastPos = dblPos[intPosFilOrder];
     dblLastPosD = dblPosD[intDemPosFilOrder];
     
-    //GateControl();
-    gateTimer.reset();
-    pinGate = 1;
-    unsigned int outboundMsgs[2] = { intFeedBack_pos , intFeedBack_pres };
-    unsigned int inboundMsgsData[2] = {0,0};
-    while(gateTimer.read_us() < 500) {
-        
-        if(slave.receive()) {
-            
-            bool isSPIsuccess = PerformSlaveSPI(&slave,outboundMsgs,inboundMsgsData);
-            
-            if( isSPIsuccess ) {
-                dblTargetPos = (double)MAX_POSITION_MM*inboundMsgsData[0]/511; // dblMaxPos should be a constant double MAX_POSITION_MM
-                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;
-                
-                if(dblTargetVel>MAX_SPEED_MMPS) {
-                    dblTargetVel = MAX_SPEED_MMPS;
-                } else if(dblTargetVel<0.0) {
-                    dblTargetPos = 0.0;
-                }
-                
-                break;
-            }
-            
-        }
-        
+    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;
     }
     
-    pinGate = 0;
-    //printf("EncPos: %f \t TargetPos: %f \t TargetVel: %f \t LinPath: %f \t DemPos: %f\r\n", dblPos[intPosFilOrder],dblTargetPos, dblTargetVel,dblLinearPath, dblPosD[intDemPosFilOrder]);
+    //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)////////////////////////////////
     
-    //printf("Demand Pos: %f\t RXFlag: %d\t parity?%d \r\n",dblTargetPos, RXFlag, parityFail);
+    //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()
-{
+void ControlParameterConfig(){
    Kp = Kp/dblMotorVoltage;
    Kd = Kd/dblSampleTime_s/dblMotorVoltage;
    Ki = Ki*dblSampleTime_s/dblMotorVoltage;
+   
+   dblReadDem = 0;
 }
 
 Ticker debugTicker;
 
-void startPositionControl()
-{
+void startPositionControl(){
     semPosCtrl.release();
 }
 
-void startDebug()
-{
+void startDebug(){
     semDebug.release();
 }
 
 Ticker positionCtrlTicker;
    
 int main() {
-    pinGate = 0;
-    
+    //pinGate = 0;
+    //swDem.mode(PullDown);
     cs_ADC = 1;
-    Mntr = 0;
-    Mntr2 = 0;
-    pinPwmOutput.period_us(50);
+    //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();
+    //gateTimer.start();
     //cs_ADC = 1;
     
     ControlParameterConfig();
@@ -714,8 +919,8 @@
 //        dblStartingPos = (double) POT_2_MM*uintPotRead  - dblPosBias;
 //    }
 //    
-    slave.format(16,2);
-    slave.frequency(10000000);
+//    slave.format(16,2);
+//    slave.frequency(10000000);
     
     dblPosD[intDemPosFilOrder] = 0.0;
     slaveReceivePos = 0.0;
@@ -727,7 +932,7 @@
     wait(0.1);
     intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
     //intPotRead = (Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
-    dblStartingPos = (double) POT_2_MM*(intPotRead  - POT_OFFSET);
+    dblStartingPos = 0.0;//(double) POT_2_MM*(intPotRead  - POT_OFFSET);
     dblLinearPath = dblStartingPos;
     dblTargetPos = dblStartingPos;
     dblPos[intPosFilOrder] = dblStartingPos;
@@ -739,7 +944,7 @@
     //calculate/convert variables
     
     CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3;
-    slave.reply(0x5555);
+    //slave.reply(0x5555);
     PositionControlThread.start(PositionControlPID);
 
     positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s);
@@ -748,8 +953,7 @@
     intFeedBack_pres = 0;
     
     
-    while(1) 
-    {
+    while(1){
         Thread::wait(osWaitForever);
     }
 }
\ No newline at end of file