Pressure control for drive segment in rebuild of control unit.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
main.cpp
- Committer:
- dofydoink
- Date:
- 2019-01-28
- Revision:
- 2:aee7d4724915
- Parent:
- 1:cb2859df7a4c
- Child:
- 3:9bd35e5b05ba
File content as of revision 2:aee7d4724915:
#include "mbed.h" #include "math.h" #include "QEI.h" #include "rtos.h" #include "FastPWM.h" #define ERROR_LIMIT_POS 0.008 //limits for when any error measurement is considered to be zero. #define ERROR_LIMIT_NEG -0.008 #define CHAN_1 0x80 #define CHAN_2 0x90 #define CHAN_3 0xA0 #define CHAN_4 0xB0 #define PRESSURE_CHAN 1 #define POSITION_CHAN 3 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 #define EXTERNAL_CLOCK_MODE 0x08 #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 Serial pc(USBTX, USBRX); // tx, rx Thread PositionControlThread(osPriorityHigh); Thread DebugThread(osPriorityNormal); Thread GateControlThread(osPriorityNormal); Thread DutyCycleThread(osPriorityRealtime); Mutex mutDutyCycle; 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); Timer timer; long pulsesTest; //define all pins SPI spi(PB_15, PB_14, PB_13); // mosi, miso, sclk //DO NOT USE D4 OR D5 !!! DigitalOut cs_ADC(PB_12); DigitalOut pinGate(PA_8); AnalogIn pinDemand1(PA_0); AnalogIn pinDemand2(PA_1); AnalogIn pinCurSense(PB_0); FastPWM pinPwmOutput(PC_8); FastPWM pinSigOut(D2); DigitalOut pinDirectionFwd(PA_13);//INB DigitalOut pinDirectionRev(PA_14);//INA //define all variables int ii,jj,kk,nn, spiTick; //counting variables volatile int slaveReceivePos; volatile int slaveReceiveVel; volatile int dataFlag; volatile int intFeedBack_pos; volatile int intFeedBack_pres; int intChkSum_pos; int intChkSum_vel; int intPar_pos; int intPar_vel; //raw Data Readings int intPotRead = 0; int intPressureRead = 0; double analogReadingDemPos; double analogReadingDemVel; double analogReadingCurSens; char buf[10]; int dataReceived; //sample time variables double dblSampleTime_s = 0.99; //system state variables double dblPos[10];//current position in mm double dblPosFil[10];//current position in mm double dblLastPos;//previous position in mm double dblVel[10];//velocity in mm/s double dblAccel; //acceleration in mm/s^2 double dblIntegral = 0;//integral of position; double dblPotPositionRead; double dblPosBias = 48.0; double dblStartingPos; //demand variables double dblPosD[10]; double dblLastPosD; double dblVelD[10]; double dblAccelD; double dblTargetPos; double dblTargetVel; double dblLinearPath; double dblLastLinearPath; double dblPathSign; double dblMaxPos = 41.0; //maximum actuator position position in mm double dblError; double dblLastError; double dblErrorDot; //filter Variables int intPosFilOrder = 1; int intVelFilOrder = 1; int intDemPosFilOrder = 6; int intDemVelFilOrder = 6; int intOutFilOrder = 0; double dblPressureVal_bar; //controller variables double omega; double zeta; double Kp = 20.0; double Ki = 2.0; double Kd = 1.5; double dblIntTerm; double dblIntLimit = 0.8; int RXFlag; double dblControlBias = 0.0; double dblMotorVoltage = 12.0; double output[10];//controller output //current limit variables double CurrentLimitSet; double dblCurrentLimitAmps = 3.0; double currentBuck; double currentBuckGain = 3.0; //define custom Functions int Read14BitADC(int channel, DigitalOut CSpin) { char message; unsigned int outputA; unsigned int outputB; int output; switch(channel) { case 1: message = CHAN_1; break; case 2: message = CHAN_2; break; case 3: message = CHAN_3; break; case 4: message = CHAN_4; break; default: message = CHAN_1; } spi.format(8,0); spi.frequency(3000000);//3MHz clock speed (3.67MHz max) CSpin.write(0); spi.write(message); spi.write(0x00); outputA = spi.write(0x00); outputB = spi.write(0x00); CSpin.write(1); //convert result to sensible value outputA = outputA<<8; output = outputA | outputB; output = output>>2; return output; } void ADC_Config() { unsigned int msg; spi.format(8,0); spi.frequency(3000000);//3MHz clock speed (3.67MHz max) msg = CHAN_1 | RANGE_CONFIG; //config channel 1 set Vref as cs_ADC = 0; spi.write(msg); cs_ADC = 1; cs_ADC = 0; spi.write(EXTERNAL_CLOCK_MODE); cs_ADC = 1; msg = CHAN_2 | RANGE_CONFIG; //config channel 2 cs_ADC = 0; spi.write(msg); cs_ADC = 1; cs_ADC = 0; spi.write(EXTERNAL_CLOCK_MODE); cs_ADC = 1; msg = CHAN_3 | RANGE_CONFIG; //config channel 3 cs_ADC = 0; spi.write(msg); cs_ADC = 1; cs_ADC = 0; spi.write(EXTERNAL_CLOCK_MODE); cs_ADC = 1; msg = CHAN_4 | RANGE_CONFIG; //config channel 4 cs_ADC = 0; spi.write(msg); cs_ADC = 1; cs_ADC = 0; spi.write(EXTERNAL_CLOCK_MODE); cs_ADC = 1; } int SumDigits(int var) { int intSumResult = 0; int intTempVar = var; while (intTempVar >0) { intSumResult += intTempVar%10; intTempVar = int(intTempVar/10); //intSumResult += int(var/100)%100; } return intSumResult; } int EvenParityBitGen(int var) { unsigned int count = 0, i, b = 1; for(i = 0; i < 32; i++){ if( var & (b << i) ){count++;} } if( (count % 2) ){return 0;} return 1; } void CloseGate() { pinGate = 0; } Timer gateTimer; void PositionControlPID() { while(1) { semPosCtrl.wait(); Mntr = !Mntr; //Mntr2 = 1; //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) | 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); //demand Readings //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; } //printf("%d, %f, %f\r\n",pulsesTest,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]; } //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]; } //printf("%f\r\n",dblPosD[intDemPosFilOrder]); intFeedBack_pos = (int) dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH*511; if(intFeedBack_pos>511) { intFeedBack_pos = 511; } if(intFeedBack_pos<0) { intFeedBack_pos = 0; } 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]; //check if target has been reached if (fabs(dblPathDifference) > 1.05*MAX_SPEED_MMPS*dblSampleTime_s) { //is velocity positive or negative? 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]; } else { //set velocity to zero dblTargetVel = 0.0; } //make sure path is safe if (dblPosD[intDemPosFilOrder] > dblMaxPos) { dblPosD[intDemPosFilOrder] = dblMaxPos; } 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 //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; dblErrorDot = 0.0; } if (fabs(dblErrorDot) < 0.1) { dblErrorDot = 0.0; } //calculate output output[0] = Kp*dblError + dblIntTerm + Kd*dblErrorDot; //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 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(); pinGate = 1; gateTimer.reset(); while((gateTimer.read_us() < 800) && pinGate == 1) { if(slave.receive()) { //Mntr = !1; //receive first msg slaveReceivePos = slave.read(); //reply with 0xFFFF 1 slave.reply(intFeedBack_pos);//prepare position reply slaveReceiveVel = slave.read();//get next message, send position reply 2 slave.reply(intFeedBack_pres); //prepare pressure reply //slave.reply(0x5555); intDummy = slave.read();//get next message, send pressure reply 3 slave.reply(0xFFFF); //prepare next dummy reply for following msg //slave.read(); // intDummy = slave.read();//get next message, send status reply // pinGate = 0; 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(isParPos && isChkPos && isTypePos) { slaveReceivePos = slaveReceivePos>>7; dblTargetPos = (double) MAX_ACTUATOR_LENGTH*slaveReceivePos/511; //limit demand to ensure safety if(dblTargetPos > dblMaxPos) { dblTargetPos = dblMaxPos; } if(dblTargetPos < 0.0) { dblTargetPos = 0.0; } } //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(isParVel && isChkVel && isTypeVel) { slaveReceiveVel = slaveReceiveVel>>7; dblTargetVel = (double) slaveReceiveVel/511.0; dblTargetVel = MAX_SPEED_MMPS*dblTargetVel; //limit demand to ensure safety if(dblTargetVel > MAX_SPEED_MMPS) { dblTargetVel = MAX_SPEED_MMPS; } if(dblTargetVel < 0.0) { dblTargetVel = 0.0; } } //Mntr2 = 0; } } pinGate = 0; //printf("Demand Pos: %f\t RXFlag: %d\t parity?%d \r\n",dblTargetPos, RXFlag, parityFail); } } //configure all control parameters void ControlParameterConfig() { Kp = Kp/dblMotorVoltage; Kd = Kd/dblSampleTime_s/dblMotorVoltage; Ki = Ki*dblSampleTime_s/dblMotorVoltage; } Ticker debugTicker; void startPositionControl() { semPosCtrl.release(); } void startDebug() { semDebug.release(); } 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); timer.start(); gateTimer.start(); //cs_ADC = 1; ControlParameterConfig(); // for (ii = 0; ii< 10; ii++) // { // uintPotRead = Read14BitADC(3, cs_ADC);//read potentiometer // dblStartingPos = (double) POT_2_MM*uintPotRead - dblPosBias; // } // slave.format(16,2); slave.frequency(10000000); dblPosD[intDemPosFilOrder] = 0.0; slaveReceivePos = 0.0; slaveReceiveVel = 0.0; wait(0.1); ADC_Config(); wait(0.1); ADC_Config(); wait(0.1); 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\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); timerDutyCycle.start(); //calculate/convert variables CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3; slave.reply(0x5555); PositionControlThread.start(PositionControlPID); DutyCycleThread.start(CalculateDutyCycle); positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s); intFeedBack_pos = 0; intFeedBack_pres = 0; while(1) { Thread::wait(osWaitForever); } }