Diff: main.cpp

Revision:

1:cb2859df7a4c

Parent:

0:20018747657d

Child:

2:aee7d4724915

--- a/main.cpp	Mon Dec 17 15:11:37 2018 +0000
+++ b/main.cpp	Mon Dec 17 15:54:28 2018 +0000
@@ -34,6 +34,7 @@
 #define RANGE_CONFIG 0x03 //config for 1.5*Vref = 6.144V
 
 #define PRESSURE_BIAS_VOLTAGE 0.15151515151515 
+#define MAX_ACTUATOR_LENGTH 52.2
 
 #define POT_2_MM 0.006750412 //convert potentiometer reading to mm (Tested and is right)
 #define POT_OFFSET 7500//6666
@@ -152,7 +153,7 @@
     double dblTargetVel;
     
     double dblMaxPos = 41.0; //maximum actuator position position in mm
-    double dblPosConv;
+
     
     double dblError;
     double dblLastError;
@@ -334,8 +335,9 @@
         //sensor readings
         
         intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure
-        intFeedBack_pres = intPressureRead & 0xFFE0;
-        intFeedBack_pres = (intFeedBack_pres) | SumDigits(intFeedBack_pres>>5);
+        intPressureRead = intPressureRead-1333;
+        intPressureRead = intPressureRead/11997 * 511;
+        intFeedBack_pres = (intFeedBack_pres<<5) | SumDigits(intFeedBack_pres);
         intFeedBack_pres = (intFeedBack_pres <<1) &0x1;
         intFeedBack_pres = (intFeedBack_pres <<1) | EvenParityBitGen(intFeedBack_pres);
                 
@@ -390,7 +392,7 @@
         
         //printf("%f\r\n",dblPosD[intDemPosFilOrder]);
         
-        intFeedBack_pos = dblPos[intPosFilOrder]/dblMaxPos*511;
+        intFeedBack_pos = dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH*511;
         
         if(intFeedBack_pos>511)
         {
@@ -554,7 +556,9 @@
                 //intFeedBack = intPressureRead & 0xFFFF;
                 slave.reply(intFeedBack_pres); //prepare pressure reply
                 
-                slave.reply(0xFFFF); 
+                slave.read();//get next message, send reply
+                
+                slave.reply(0xFFFF); //prepare next dummy reply
                 pinGate = 0;
                 
                 //deal with position
@@ -565,7 +569,7 @@
                 //check if parity, check Sum and mesage type matches
                 if((intPar_pos == (slaveReceivePos&0x1))&&(intChkSum_pos == ((slaveReceivePos>>2)&0x1F))&&( ( (slaveReceivePos>>1) &0x1) ) == 0) {
                     slaveReceivePos = slaveReceivePos>>7;
-                    dblTargetPos = (double) dblMaxPos*slaveReceivePos/511;
+                    dblTargetPos = (double) MAX_ACTUATOR_LENGTH*slaveReceivePos/511;
                 
                     //limit demand to ensure safety
                     if(dblTargetPos > dblMaxPos)
@@ -636,8 +640,6 @@
        Kp = Kp/dblMotorVoltage;
        Kd = Kd/dblSampleTime_s/dblMotorVoltage;
        Ki = Ki*dblSampleTime_s/dblMotorVoltage;
-
-       dblPosConv = dblMaxPos/0.8;
     }
 
 Ticker debugTicker;