Pressure control for drive segment in rebuild of control unit.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
Diff: main.cpp
- Revision:
- 1:cb2859df7a4c
- Parent:
- 0:20018747657d
- Child:
- 2:aee7d4724915
diff -r 20018747657d -r cb2859df7a4c main.cpp --- 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;