Control code for the peristaltic pump. Includes current control.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
main.cpp
- Committer:
- dofydoink
- Date:
- 2019-01-30
- Revision:
- 4:3bab17dfae4e
- Parent:
- 3:9bd35e5b05ba
- Child:
- 5:59dd1467762e
- Child:
- 6:4e710cef655e
File content as of revision 4:3bab17dfae4e:
#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 double 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 const double MAX_ACTUATOR_LENGTH = 52.2; const double MAX_POSITION_MM = 40.0; //maximum actuator position position in mm //sample time variables double dblSampleTime_s = 0.001; #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; //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 dblError; double dblLastError; double dblErrorDot; //filter Variables int intPosFilOrder = 0; 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; Timer gateTimer; //define custom Functions bool CheckMessage(int msg) { // Find message parity short int count = 0; for(short int i=0; i<32; i++) { if( msg>>1 & (1<<i) ) count++; } int intParity = !(count%2); // Find message CheckSum int intChkSum = 0; int intTempVar = msg>>7; while(intTempVar > 0) { intChkSum += intTempVar%10; intTempVar = int(intTempVar/10); } // Check if parity, CheckSum and mesage type match bool isParityCorrect = (intParity == (msg&0x1)); bool isChkSumCorrect = (intChkSum == ((msg>>2)&0x1F)); bool isCheckPassed = (isParityCorrect && isChkSumCorrect); return isCheckPassed; } bool PerformSlaveSPI(SPISlave *spiSlave, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]) { unsigned int dummyMsg = 0x5555; bool isSuccess = true; unsigned int inboundMsg, typeBit; short int numPacketsReceived = 0; for(short int i=0; i<3; i++) { // Loop 3 times for 3 SPI messages while( gateTimer.read_us() < 500 ) { if( spiSlave->receive() ) { numPacketsReceived++; inboundMsg = spiSlave->read(); Mntr = 1; if(i==0) { spiSlave->reply(outboundMsgs[0]); } else if(i==1) { spiSlave->reply(outboundMsgs[1]); } else { spiSlave->reply(dummyMsg); } 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; if( !CheckMessage(inboundMsg) ) { isSuccess = false; } } break; } } } if( numPacketsReceived != 3 ) { isSuccess = false; } return isSuccess; } 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) int OddParityBitGen(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; } void PositionControlPID() { while(1) { semPosCtrl.wait(); Mntr2 = !Mntr2; //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) | OddParityBitGen(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) | OddParityBitGen(intFeedBack_pos);//add parity //intFeedBack = dblSensorDriftError*8191; ///////////////PATH GENERATION//////////////////////// //work out next path point double dblPathDifference; dblPathDifference = dblTargetPos - dblLinearPath; dblPathSign = dblPathDifference/fabs(dblPathDifference); //is velocity positive or negative? //check if target has not been reached (with additional 1% of step to be sure) if (fabs(dblPathDifference) > 1.01*dblTargetVel*dblSampleTime_s) { dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path } else { //if path is very close to target position dblLinearPath = dblTargetPos; } //limit position if(dblLinearPath > MAX_POSITION_MM){ dblLinearPath = MAX_POSITION_MM; } if (dblLinearPath < 0.0){ dblLinearPath = 0.0; } dblPosD[intDemPosFilOrder] = 0.07*dblLinearPath + 0.93*dblPosD[intDemPosFilOrder]; //make sure path is safe if (dblPosD[intDemPosFilOrder] > MAX_POSITION_MM) { dblPosD[intDemPosFilOrder] = MAX_POSITION_MM; } 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(); gateTimer.reset(); pinGate = 1; unsigned int outboundMsgs[2] = { intFeedBack_pos , intFeedBack_pres }; unsigned int inboundMsgsData[2] = {0,0}; while(gateTimer.read_us() < 500) { if(slave.receive()) { bool isSPIsuccess = PerformSlaveSPI(&slave,outboundMsgs,inboundMsgsData); if( isSPIsuccess ) { dblTargetPos = (double)MAX_POSITION_MM*inboundMsgsData[0]/511; // dblMaxPos should be a constant double MAX_POSITION_MM 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; if(dblTargetVel>MAX_SPEED_MMPS) { dblTargetVel = MAX_SPEED_MMPS; } else if(dblTargetVel<0.0) { dblTargetPos = 0.0; } break; } } } 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); } } //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; dblPos[intPosFilOrder] = dblStartingPos; dblTargetVel = 0.0; dblPosD[intDemPosFilOrder] = dblStartingPos; printf("\r\nPotRead: %d, Current Pos: %f, Target Pos: %f\r\n", intPotRead, dblStartingPos, dblTargetPos); //calculate/convert variables CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3; slave.reply(0x5555); PositionControlThread.start(PositionControlPID); positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s); intFeedBack_pos = 0; intFeedBack_pres = 0; while(1) { Thread::wait(osWaitForever); } }