Revision 6:042db7596e55, committed 2021-06-24

Comitter:

dofydoink

Date:

Thu Jun 24 20:45:25 2021 +0000

Parent:

5:4e710cef655e

Commit message:

PWM low level;

Changed in this revision

--- a/main.cpp	Mon Apr 01 14:15:31 2019 +0000
+++ b/main.cpp	Thu Jun 24 20:45:25 2021 +0000
@@ -21,8 +21,9 @@
 
 const double MAX_SPEED_MMPS = 24.3457;
 int intDummy;
-SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel
-
+//SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel
+InterruptIn pinPWMin_pos(PA_8);
+InterruptIn pinPWMin_vel(PB_9);
 QEI wheel (PB_5, PB_4, NC, 256, QEI::X4_ENCODING);
 
 DigitalOut Mntr(D3);
@@ -53,31 +54,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);
 Semaphore semGate(1);
@@ -91,7 +78,7 @@
    
 DigitalOut cs_ADC(PB_12);
 
-DigitalOut pinGate(PA_8);
+//DigitalOut pinGate(PA_8);
 
 AnalogIn pinDemand1(PA_0);
 AnalogIn pinDemand2(PA_1);
@@ -132,9 +119,6 @@
 char buf[10];
 int dataReceived;
 
-
-
-
 //system state variables 
 double dblPos[10];//current position in mm
 double dblPosFil[10];//current position in mm
@@ -161,8 +145,6 @@
 double dblLastLinearPath;
 double dblPathSign;
 
-
-
 double dblError;
 double dblLastError;
 double dblErrorDot;
@@ -174,6 +156,7 @@
 int intDemVelFilOrder = 6;
 int intOutFilOrder = 0;
 
+double dblPressureVal_norm;
 double dblPressureVal_bar;
 
 //controller variables 
@@ -198,11 +181,95 @@
 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();
+    
+}
+
+void PWMFall_pos() {
+    //mutDutyCycle_pos.lock();
+    intDeltaT_pos = timerDutyCycle_pos.read_us();
+    //mutDutyCycle_pos.unlock();
+    
+}
+
+void PWMRise_vel() {
+    //mutDutyCycle_vel.lock();
+    intT_vel = timerPeriod_vel.read_us();
+    timerPeriod_vel.reset();
+    timerDutyCycle_vel.reset();
+    //mutDutyCycle_vel.unlock();
+    isRiseWorking = 1;
+}
+
+void PWMFall_vel() {
+    //mutDutyCycle_vel.lock();
+    intDeltaT_vel = timerDutyCycle_vel.read_us();
+    //mutDutyCycle_vel.unlock();
+    isFallWorking = 1;
+}
+
+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,7 +290,11 @@
     return isCheckPassed;
 }
 
-bool PerformSlaveSPI(SPISlave *spiSlave, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]) {
+////////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;
@@ -235,7 +306,7 @@
                 numPacketsReceived++;
                 
                 inboundMsg = spiSlave->read();
-                Mntr = 1;
+                Mntr = 1;//dummy variable used to check function
                 if(i==0) { 
                      spiSlave->reply(outboundMsgs[0]);
                 } else if(i==1) {
@@ -246,8 +317,8 @@
                 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;
+                    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;
                     }
@@ -256,11 +327,13 @@
             }
         }
     }
-    if( numPacketsReceived != 3 ) {
+    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)
 {
@@ -383,297 +456,369 @@
 
     return 1;
 }
-    
-void CloseGate()
-{
-    pinGate = 0;
-    
-}
-    
-  
-  
 
 void PositionControlPID()
 {
    while(1)
    {
-   semPosCtrl.wait();
-    
-    Mntr2 = !Mntr2;
-   //Mntr2 = 1 - Mntr2;//!led; 
-   pulsesTest = wheel.getPulses();
-
-   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);
-    
-    
-    //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;
-
-    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
+       semPosCtrl.wait();
+        
+        Mntr2 = !Mntr2;
+       //Mntr2 = 1 - Mntr2;//!led; 
+       pulsesTest = wheel.getPulses();
     
-     //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++)
+       double testy = (double) abs(pulsesTest)/2000;
+       pinSigOut.write(testy);
+       
+       //take all readings
+        
+        //sensor readings
+        
+        intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure
+        
+        dblPressureVal_norm = ((double) intPressureRead/16383.0);
+        dblPressureVal_norm = dblPressureVal_norm*6.144;//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)
         {
-            dblPos[ii] = (double) 0.7*dblPos[ii-1] + 0.3*dblPos[ii];
+            dblPressureVal_norm = 1.0;
+        }
+        if (dblPressureVal_norm < 0.0)
+        {
+            dblPressureVal_norm = 0.0;
         }
-    }
-    else
-    {
-        dblPos[intPosFilOrder] = dblPos[0];
-    }
-
-    //get velocity and filter
-    dblVel[0] = dblPos[intPosFilOrder] - dblLastPos;
-    if(intVelFilOrder>0)
-    {
-        for (ii = 1; ii<intVelFilOrder+1; ii++)
+        
+        double pressureCheck;
+        
+        //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;
+         
+        //intFeedBack_pres = (int)(((double)intPressureRead/10667) * 511);
+        intFeedBack_pres = (int) (dblPressureVal_bar/12*511);
+        
+        if(intFeedBack_pres > 511)
         {
-            dblVel[ii] = (double) 0.7*dblVel[ii-1] + 0.3*dblVel[ii];
+            intFeedBack_pres = 511;
+        }
+        if(intFeedBack_pres < 0)
+        {
+            intFeedBack_pres = 0;
         }
-    }
-    else
-    {
-        dblVel[intVelFilOrder] = dblVel[0];
-    }
-    
-    
-    //printf("%f\r\n",dblPosD[intDemPosFilOrder]);
+        
+        //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;
     
-    intFeedBack_pos = (int) ((dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH)*511);
-    
-    if(intFeedBack_pos>511)
-    {
-        intFeedBack_pos = 511;
-    }
+        unsigned short garb = 0x01;
+               
+        intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
+        dblPotPositionRead = (double) POT_2_MM*(intPotRead  - POT_OFFSET);
         
-    if(intFeedBack_pos<0)
-    {
-        intFeedBack_pos = 0;
-    }
+        
+        //demand Readings
     
         
-    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;
-          
-    
-    ///////////////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;
-    }
+        //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;
+        }
+        
+        
+        if(intPosFilOrder > 0)
+        {
+            for (ii = 1; ii<intPosFilOrder+1; ii++)
+            {
+                dblPos[ii] = (double) 0.7*dblPos[ii-1] + 0.3*dblPos[ii];
+            }
+        }
+        else
+        {
+            dblPos[intPosFilOrder] = dblPos[0];
+        }
     
-    //limit position
-    if(dblLinearPath > MAX_POSITION_MM){
-        dblLinearPath = MAX_POSITION_MM;
-    }
-    if (dblLinearPath < 0.0){
-        dblLinearPath = 0.0;
-    }
-    
-    dblPosD[intDemPosFilOrder] = 0.07*dblLinearPath + 0.93*dblPosD[intDemPosFilOrder];
+        //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];
+        }
+        
+        
+        
+        intFeedBack_pos = (int) ((dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH)*511);
+        
+        if(intFeedBack_pos>511)
+        {
+            intFeedBack_pos = 511;
+        }
+            
+        if(intFeedBack_pos<0)
+        {
+            intFeedBack_pos = 0;
+        }
+        
+        //printf("%d\r\n",dblPos[intPosFilOrder]);      
+        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;
+            
+        
+        ///////////////PATH GENERATION////////////////////////
+        //work out next path point
+        double dblPathDifference;
+        dblPathDifference = dblTargetPos - dblLinearPath;
+        dblPathSign = dblPathDifference/fabs(dblPathDifference); //is velocity positive or negative?
     
-    //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;
-    
-    ///////////////////////////////////////////////////// End of Path Generation
-    
+        //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.07*dblLinearPath + 0.93*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;
+        
+        ///////////////////////////////////////////////////// End of Path Generation
+        
+        
+        //run PID calculations
+        //get errors
+        dblError = dblPosD[intDemPosFilOrder] - dblPos[intPosFilOrder];
+        dblErrorDot = dblVelD[intDemVelFilOrder] - dblVel[intVelFilOrder];
+        //get integral
     
-    //run PID calculations
-    //get errors
-    dblError = dblPosD[intDemPosFilOrder] - dblPos[intPosFilOrder];
-    dblErrorDot = dblVelD[intDemVelFilOrder] - dblVel[intVelFilOrder];
-    //get integral
-    //printf("%f\r\n",dblError);
-
-    
-    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.01)
-    {
-        dblError = 0.0;
+        
+        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.01)
+        {
+            dblError = 0.0;
+            dblErrorDot = 0.0;
+            
+        }
+            
+        if (fabs(dblErrorDot) < 0.1)
+        {
         dblErrorDot = 0.0;
+        }
         
-    }
+        //calculate output
+        
+        
+        output[0] = Kp*dblError + dblIntTerm + Kd*dblErrorDot;
+        
+        //tryPressureControl
+    //    double dblPosCtrlOut = Kp*dblError + dblIntTerm + Kd*dblErrorDot;
+    //    
+    //    double Ki_pres = 10.0;
+    //    double Kp_pres = 1;
+    //    
+    //    double dblPressureError;
+    //    double dblPressureErrorIntTerm;
+    //    
+    //    dblPressureError = dblPosCtrlOut - Pressure
+        
+        //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 (fabs(dblErrorDot) < 0.1)
-    {
-    dblErrorDot = 0.0;
-    }
-    
-    //calculate output
-    output[0] = Kp*dblError + dblIntTerm + Kd*dblErrorDot;
+        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];
+        //end Current limit
     
-    //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++)
+        //find direction
+        if(output[intOutFilOrder] >=0.0)
+        {
+            pinDirectionFwd = 1;
+            pinDirectionRev = 0;
+            dblControlBias = 0.0;
+        }
+        else
         {
-            output[ii] = 0.7*output[ii-1] + 0.3*output[ii];
+            pinDirectionFwd = 0;
+            pinDirectionRev = 1;
+            dblControlBias = 0.0;
+        }
+        
+        pinPwmOutput.write(abs(output[intOutFilOrder])+dblControlBias);
+        
+        //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;
         }
-    }
-    else
-    {
-        output[intOutFilOrder] = output[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;
+        }
+        
+        /*
+        printf("%d \t %d \r\n",intDeltaT_pos,intDeltaT_vel);
+        printf("%d \t %d \r\n",intT_pos,intT_vel);
+        printf("%f \t %f \r\n",dblTargetPos,dblTargetVel);
+        //printf("%f\r\n",output[intOutFilOrder]);
+        printf("Rise: %d \t Fall:%d", isRiseWorking, isFallWorking);
+        printf("\r\n");
+        */
     }
 
-    //limit current
-    if (analogReadingCurSens> CurrentLimitSet)
-    {
-        currentBuck = CurrentLimitSet / analogReadingCurSens / currentBuckGain;
-    }
-    else
-    {
-        currentBuck = 1.0;
-    }
-                
-    output[intOutFilOrder] = currentBuck*output[intOutFilOrder];
-    //end Current limit
-
-    //find direction
-    if(output[intOutFilOrder] >=0.0)
-    {
-        pinDirectionFwd = 1;
-        pinDirectionRev = 0;
-        dblControlBias = 0.0;
-    }
-    else
-    {
-        pinDirectionFwd = 0;
-        pinDirectionRev = 1;
-        dblControlBias = 0.0;
-    }
-    
-    pinPwmOutput.write(abs(output[intOutFilOrder])+dblControlBias);
-    
-    //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;
-            }
-            
-        }
-        
-    }
-    
-    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]);
-    
-    //printf("Demand Pos: %f\t RXFlag: %d\t parity?%d \r\n",dblTargetPos, RXFlag, parityFail);
-   
-    }
 }
+   //////////////////////////////////////////////////For Carafino: End
    
    
 //configure all control parameters
-void ControlParameterConfig()
-{
+void ControlParameterConfig(){
    Kp = Kp/dblMotorVoltage;
    Kd = Kd/dblSampleTime_s/dblMotorVoltage;
    Ki = Ki*dblSampleTime_s/dblMotorVoltage;
@@ -694,17 +839,32 @@
 Ticker positionCtrlTicker;
    
 int main() {
-    pinGate = 0;
-    
+
     cs_ADC = 1;
     Mntr = 0;
     Mntr2 = 0;
     pinPwmOutput.period_us(50);
+    
+    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 +874,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;
@@ -728,18 +888,23 @@
     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);
+    //dblLinearPath = dblStartingPos;
+//    dblTargetPos = dblStartingPos;
+//    dblPos[intPosFilOrder] = dblStartingPos;
+//    dblTargetVel = 0.0;
     dblLinearPath = dblStartingPos;
-    dblTargetPos = dblStartingPos;
+    dblTargetPos = 0.0;
     dblPos[intPosFilOrder] = dblStartingPos;
-    dblTargetVel = 0.0;
+    dblTargetVel = 2.0;
     dblPosD[intDemPosFilOrder] = dblStartingPos;
     
+    //dblPosD[intDemPosFilOrder] = 0;
     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);
+    //slave.reply(0x5555);
     PositionControlThread.start(PositionControlPID);
 
     positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s);