Pressure control for drive segment in rebuild of control unit.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
Diff: main.cpp
- Revision:
- 2:aee7d4724915
- Parent:
- 1:cb2859df7a4c
- Child:
- 3:9bd35e5b05ba
--- a/main.cpp Mon Dec 17 15:54:28 2018 +0000 +++ b/main.cpp Mon Jan 28 15:30:36 2019 +0000 @@ -20,12 +20,13 @@ const float MAX_SPEED_MMPS = 24.3457; - +int intDummy; SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel QEI wheel (PB_5, PB_4, NC, 256, QEI::X4_ENCODING); DigitalOut Mntr(D3); +DigitalOut Mntr2(PB_8); #define EXTERNAL_CLOCK_MODE 0x08 #define RANGE_CONFIG 0x03 @@ -126,7 +127,7 @@ int dataReceived; //sample time variables - double dblSampleTime_s = 0.001; + double dblSampleTime_s = 0.99; //system state variables @@ -151,6 +152,9 @@ double dblTargetPos; double dblTargetVel; + double dblLinearPath; + double dblLastLinearPath; + double dblPathSign; double dblMaxPos = 41.0; //maximum actuator position position in mm @@ -166,6 +170,8 @@ int intDemVelFilOrder = 6; int intOutFilOrder = 0; + double dblPressureVal_bar; + //controller variables double omega; double zeta; @@ -175,7 +181,7 @@ double Kd = 1.5; double dblIntTerm; double dblIntLimit = 0.8; - + int RXFlag; double dblControlBias = 0.0; double dblMotorVoltage = 12.0; @@ -323,8 +329,9 @@ while(1) { semPosCtrl.wait(); - - Mntr = 1;//!led; + Mntr = !Mntr; + //Mntr2 = 1; + //Mntr2 = 1 - Mntr2;//!led; pulsesTest = wheel.getPulses(); double testy = (double) abs(pulsesTest)/2000; @@ -335,12 +342,25 @@ //sensor readings intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure - 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); - + 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) | EvenParityBitGen(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); @@ -392,7 +412,7 @@ //printf("%f\r\n",dblPosD[intDemPosFilOrder]); - intFeedBack_pos = dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH*511; + intFeedBack_pos = (int) dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH*511; if(intFeedBack_pos>511) { @@ -405,28 +425,30 @@ } - intFeedBack_pos = (intFeedBack_pos<<5) | SumDigits(intFeedBack_pos); - intFeedBack_pos = intFeedBack_pos <<1; - intFeedBack_pos = (intFeedBack_pos <<1) | EvenParityBitGen(intFeedBack_pos); + 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) | EvenParityBitGen(intFeedBack_pos);//add parity //intFeedBack = dblSensorDriftError*8191; - - - - + ///////////////PATH GENERATION//////////////////////// //work out next path point double dblPathDifference; dblPathDifference = dblTargetPos - dblPosD[intDemPosFilOrder]; - double dblLinearPath; - double dblLastLinearPath; + //check if target has been reached - if (dblPathDifference > 1.05*MAX_SPEED_MMPS*dblSampleTime_s) { + if (fabs(dblPathDifference) > 1.05*MAX_SPEED_MMPS*dblSampleTime_s) { //is velocity positive or negative? - double dblPathSign = dblPathDifference/fabs(dblPathDifference); + dblPathSign = dblPathDifference/fabs(dblPathDifference); dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path + if(dblLinearPath > dblMaxPos){ + dblLinearPath = dblMaxPos; + } + if (dblLinearPath < 0.0){ + dblLinearPath = 0.0; + } //now smooth dblPosD[intDemPosFilOrder] = 0.2*dblLinearPath + 0.8*dblPosD[intDemPosFilOrder]; @@ -447,6 +469,10 @@ dblVelD[0] = dblPosD[intDemPosFilOrder] - dblLastPosD; + + ///////////////////////////////////////////////////// End of Path Generation + + //run PID calculations //get errors dblError = dblPosD[intDemPosFilOrder] - dblPos[intPosFilOrder]; @@ -537,37 +563,79 @@ dblLastPos = dblPos[intPosFilOrder]; dblLastPosD = dblPosD[intDemPosFilOrder]; - Mntr = 0; - //GateControl(); + pinGate = 1; + gateTimer.reset(); - pinGate = 1; + - while((gateTimer.read_us() < 600) && pinGate == 1) + while((gateTimer.read_us() < 800) && pinGate == 1) { if(slave.receive()) { + //Mntr = !1; //receive first msg - slaveReceivePos = slave.read(); //reply with 0xFFFF + slaveReceivePos = slave.read(); //reply with 0xFFFF 1 slave.reply(intFeedBack_pos);//prepare position reply - slaveReceiveVel = slave.read();//get next message - - //intFeedBack = intPressureRead & 0xFFFF; + + slaveReceiveVel = slave.read();//get next message, send position reply 2 slave.reply(intFeedBack_pres); //prepare pressure reply + //slave.reply(0x5555); - slave.read();//get next message, send reply + intDummy = slave.read();//get next message, send pressure reply 3 + slave.reply(0xFFFF); //prepare next dummy reply for following msg + + //slave.read(); - slave.reply(0xFFFF); //prepare next dummy reply + // intDummy = slave.read();//get next message, send status reply +// + + pinGate = 0; - - //deal with position + printf("P:%d\t V:%d\t D:%d\r\n", slaveReceivePos,slaveReceiveVel,intDummy); + //deal with pos msg //find parity & checkSum intPar_pos = EvenParityBitGen(slaveReceivePos>>1); intChkSum_pos = SumDigits(slaveReceivePos>>7); + bool isParPos = (intPar_pos == (slaveReceivePos&0x1)); + bool isChkPos = (intChkSum_pos == ((slaveReceivePos>>2)&0x1F)); + bool isTypePos = (( (slaveReceivePos>>1) &0x1 ) == 0); + + + + //deal with velocity msg + //find parity & checkSum + intPar_vel = EvenParityBitGen(slaveReceiveVel>>1); + intChkSum_vel = SumDigits(slaveReceiveVel>>7); + + bool isParVel = (intPar_vel == (slaveReceiveVel&0x1)); + bool isChkVel = (intChkSum_vel == ((slaveReceiveVel>>2)&0x1F)); + bool isTypeVel = ( (slaveReceiveVel>>1) &0x1 ) == 1; + + //if(isParVel){ +// Mntr = 1; +// } else{ +// Mntr = 0; +// } +// +// if(isChkVel){ +// Mntr2 = 1; +// } +// else { +// Mntr2 = 0; +// } + + //if(isParPos && isChkPos && isTypePos && isParVel && isChkVel && isTypeVel){ +// slave.reply(0x5555);//prepare specific reply that all is well +// } +// else { +// slave.reply(0x0000); // otherwise give it something wrong +// } + //check if parity, check Sum and mesage type matches - if((intPar_pos == (slaveReceivePos&0x1))&&(intChkSum_pos == ((slaveReceivePos>>2)&0x1F))&&( ( (slaveReceivePos>>1) &0x1) ) == 0) { + if(isParPos && isChkPos && isTypePos) { slaveReceivePos = slaveReceivePos>>7; dblTargetPos = (double) MAX_ACTUATOR_LENGTH*slaveReceivePos/511; @@ -581,17 +649,14 @@ dblTargetPos = 0.0; } } - - //deal with velocity - //find parity & checkSum - intPar_vel = EvenParityBitGen(slaveReceiveVel>>1); - intChkSum_vel = SumDigits(slaveReceiveVel>>7); - + //printf("P:%d\tC:%d\tT:%d\t%f\r\n", slaveReceiveVel&0x1, ((slaveReceiveVel>>2)&0x1F), ((slaveReceiveVel>>1) & 0x1),dblTargetVel); + + //parityFail = 0; //check if parity, check Sum and mesage type matches - if((intPar_vel == (slaveReceiveVel&0x1))&&(intChkSum_vel == ((slaveReceiveVel>>2)&0x1F))&&( ( (slaveReceiveVel>>1) &0x1) ) == 0) { + if(isParVel && isChkVel && isTypeVel) { slaveReceiveVel = slaveReceiveVel>>7; - dblTargetVel = (double) MAX_SPEED_MMPS*slaveReceiveVel/511; - + dblTargetVel = (double) slaveReceiveVel/511.0; + dblTargetVel = MAX_SPEED_MMPS*dblTargetVel; //limit demand to ensure safety if(dblTargetVel > MAX_SPEED_MMPS) { @@ -601,34 +666,16 @@ { dblTargetVel = 0.0; } + } - - - - -// dataFlag = slaveReceive>>13; -// -// if(dataFlag == 0) -// { -// slaveReceive = slaveReceive>>7 & 0x1FF; -// //get demand and filter -// dblPosD[intDemPosFilOrder] = (double) dblMaxPos*slaveReceive/511; -// -// //limit demand to ensure safety -// if(dblPosD[intDemPosFilOrder] > dblMaxPos) -// { -// dblPosD[intDemPosFilOrder] = dblMaxPos; -// } -// if(dblPosD[intDemPosFilOrder] < 0.0) -// { -// dblPosD[intDemPosFilOrder] = 0.0; -// } -// } - + + //Mntr2 = 0; } } pinGate = 0; - + + + //printf("Demand Pos: %f\t RXFlag: %d\t parity?%d \r\n",dblTargetPos, RXFlag, parityFail); } } @@ -657,8 +704,11 @@ Ticker positionCtrlTicker; int main() { + pinGate = 0; + cs_ADC = 1; - + Mntr = 0; + Mntr2 = 0; pinPwmOutput.period_us(50); printf("\r\nYo Yo Yo, Low Level innit\r\n\n\n"); wait(0.5); @@ -676,6 +726,7 @@ // slave.format(16,2); slave.frequency(10000000); + dblPosD[intDemPosFilOrder] = 0.0; slaveReceivePos = 0.0; slaveReceiveVel = 0.0; @@ -687,13 +738,16 @@ 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; + dblTargetVel = 0.0; dblPosD[intDemPosFilOrder] = dblStartingPos; - printf("\r\n%d, %f\r\n", intPotRead, dblStartingPos); + printf("\r\nPotRead: %d, Current Pos: %f, Target Pos: %f\r\n", intPotRead, dblStartingPos, dblTargetPos); //wait(0.05); + //printf("\n\n\n"); //dblStartingPos = (double) POT_2_MM*(uintPotRead - POT_OFFSET); @@ -702,6 +756,7 @@ //calculate/convert variables CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3; + slave.reply(0x5555); PositionControlThread.start(PositionControlPID); DutyCycleThread.start(CalculateDutyCycle); positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s);