Revision 6:f4d91041b8af, committed 2021-06-25

Comitter:

dofydoink

Date:

Fri Jun 25 07:38:21 2021 +0000

Parent:

5:4e710cef655e

Commit message:

Control Code for Low Level controllers;

Changed in this revision

--- a/main.cpp	Mon Apr 01 14:15:31 2019 +0000
+++ b/main.cpp	Fri Jun 25 07:38:21 2021 +0000
@@ -174,6 +174,7 @@
 int intDemVelFilOrder = 6;
 int intOutFilOrder = 0;
 
+double dblPressureVal_norm;
 double dblPressureVal_bar;
 
 //controller variables 
@@ -183,6 +184,8 @@
 double Kp = 20.0;
 double Ki = 2.0;
 double Kd = 1.5;
+
+
 double dblIntTerm; 
 double dblIntLimit = 0.8;
 int RXFlag;
@@ -198,11 +201,20 @@
 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;
+
 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 +235,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 +251,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 +262,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 +272,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)
 {
@@ -411,13 +429,45 @@
     //sensor readings
     
     intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure
-    intPressureRead = intPressureRead-1334;
+    
+    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
     
-    dblPressureVal_bar = ( (double) intPressureRead/10667)*10.0;
+    if (dblPressureVal_norm >1.0)
+    {
+        dblPressureVal_norm = 1.0;
+    }
+    if (dblPressureVal_norm < 0.0)
+    {
+        dblPressureVal_norm = 0.0;
+    }
+    
+    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);
-    //printf("%d\r\n",intFeedBack_pres);
+    //intFeedBack_pres = (int)(((double)intPressureRead/10667) * 511);
+    intFeedBack_pres = (int) (dblPressureVal_bar/12*511);
+    
+    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;
@@ -451,7 +501,7 @@
         //dblPos[0] = dblPotPositionRead;
     }
     
-    //printf("%d, %f, %f\r\n",pulsesTest,dblPos[0],dblPotPositionRead);
+    
     if(intPosFilOrder > 0)
     {
         for (ii = 1; ii<intPosFilOrder+1; ii++)
@@ -479,7 +529,6 @@
     }
     
     
-    //printf("%f\r\n",dblPosD[intDemPosFilOrder]);
     
     intFeedBack_pos = (int) ((dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH)*511);
     
@@ -493,13 +542,13 @@
         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
@@ -543,7 +592,6 @@
     dblError = dblPosD[intDemPosFilOrder] - dblPos[intPosFilOrder];
     dblErrorDot = dblVelD[intDemVelFilOrder] - dblVel[intVelFilOrder];
     //get integral
-    //printf("%f\r\n",dblError);
 
     
     dblIntTerm = dblIntTerm + Ki*dblError;
@@ -571,8 +619,21 @@
     }
     
     //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)
     {
@@ -594,7 +655,46 @@
     {
         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)
     {
@@ -604,6 +704,16 @@
     {
         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
@@ -628,26 +738,28 @@
     dblLastPos = dblPos[intPosFilOrder];
     dblLastPosD = dblPosD[intDemPosFilOrder];
     
-    //GateControl();
-    gateTimer.reset();
-    pinGate = 1;
-    unsigned int outboundMsgs[2] = { intFeedBack_pos , intFeedBack_pres };
+    ///////////////////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(gateTimer.read_us() < 500) {//while gate is open
         
-        if(slave.receive()) {
+        if(slave.receive()) {//when a message is received
             
-            bool isSPIsuccess = PerformSlaveSPI(&slave,outboundMsgs,inboundMsgsData);
+            bool isSPIsuccess = PerformSlaveSPI(&slave,outboundMsgs,inboundMsgsData);//process the data and check if it was corrupted
             
-            if( isSPIsuccess ) {
-                dblTargetPos = (double)MAX_POSITION_MM*inboundMsgsData[0]/511; // dblMaxPos should be a constant double MAX_POSITION_MM
+            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;
+                dblTargetVel = (double)MAX_SPEED_MMPS*inboundMsgsData[1]/511;//set target velocity  (9-bit value)
                 
                 if(dblTargetVel>MAX_SPEED_MMPS) {
                     dblTargetVel = MAX_SPEED_MMPS;
@@ -655,20 +767,26 @@
                     dblTargetPos = 0.0;
                 }
                 
-                break;
+                break;//bail out of while loop
             }
             
         }
         
     }
     
-    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);
+    pinGate = 0;//close gate
+
+/*
+   printf("%f\t",dblPos[intPosFilOrder]);
+   printf("%f\t",dblPosD[intDemPosFilOrder]);
+   printf("%f\t",dblTargetPos);
+   printf("%f\t",dblError);
+   printf("\r\n");
+   */
    
     }
 }
+   //////////////////////////////////////////////////For Carafino: End
    
    
 //configure all control parameters
@@ -695,7 +813,7 @@
    
 int main() {
     pinGate = 0;
-    
+    pc.baud(115200);
     cs_ADC = 1;
     Mntr = 0;
     Mntr2 = 0;
@@ -734,6 +852,10 @@
     dblTargetVel = 0.0;
     dblPosD[intDemPosFilOrder] = dblStartingPos;
     
+    dblTargetPos = 0.0;
+    dblPos[intPosFilOrder] = dblStartingPos;
+    dblTargetVel = 3.0;
+    
     printf("\r\nPotRead: %d, Current Pos: %f, Target Pos: %f\r\n", intPotRead, dblStartingPos, dblTargetPos);
         
     //calculate/convert variables