Low level control code for syringe pumps
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
Diff: main.cpp
- Revision:
- 0:20018747657d
- Child:
- 1:cb2859df7a4c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Dec 17 15:11:37 2018 +0000 @@ -0,0 +1,718 @@ +#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; + +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); + + #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 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.001; + + + //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 dblMaxPos = 41.0; //maximum actuator position position in mm + double dblPosConv; + + double dblError; + double dblLastError; + double dblErrorDot; + + //filter Variables + int intPosFilOrder = 1; + int intVelFilOrder = 1; + int intDemPosFilOrder = 6; + int intDemVelFilOrder = 6; + int intOutFilOrder = 0; + + //controller variables + double omega; + double zeta; + + double Kp = 20.0; + double Ki = 2.0; + double Kd = 1.5; + double dblIntTerm; + double dblIntLimit = 0.8; + + 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 = 1;//!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 + intFeedBack_pres = intPressureRead & 0xFFE0; + intFeedBack_pres = (intFeedBack_pres) | SumDigits(intFeedBack_pres>>5); + intFeedBack_pres = (intFeedBack_pres <<1) &0x1; + intFeedBack_pres = (intFeedBack_pres <<1) | EvenParityBitGen(intFeedBack_pres); + + 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 = dblPos[intPosFilOrder]/dblMaxPos*511; + + if(intFeedBack_pos>511) + { + intFeedBack_pos = 511; + } + + if(intFeedBack_pos<0) + { + intFeedBack_pos = 0; + } + + + intFeedBack_pos = (intFeedBack_pos<<5) | SumDigits(intFeedBack_pos); + intFeedBack_pos = intFeedBack_pos <<1; + intFeedBack_pos = (intFeedBack_pos <<1) | EvenParityBitGen(intFeedBack_pos); + + //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) { + //is velocity positive or negative? + double dblPathSign = dblPathDifference/fabs(dblPathDifference); + + dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path + + //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; + + //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]; + + Mntr = 0; + + //GateControl(); + gateTimer.reset(); + pinGate = 1; + + while((gateTimer.read_us() < 600) && pinGate == 1) + { + if(slave.receive()) + { + //receive first msg + slaveReceivePos = slave.read(); //reply with 0xFFFF + + slave.reply(intFeedBack_pos);//prepare position reply + slaveReceiveVel = slave.read();//get next message + + //intFeedBack = intPressureRead & 0xFFFF; + slave.reply(intFeedBack_pres); //prepare pressure reply + + slave.reply(0xFFFF); + pinGate = 0; + + //deal with position + //find parity & checkSum + intPar_pos = EvenParityBitGen(slaveReceivePos>>1); + intChkSum_pos = SumDigits(slaveReceivePos>>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; + + //limit demand to ensure safety + if(dblTargetPos > dblMaxPos) + { + dblTargetPos = dblMaxPos; + } + if(dblTargetPos < 0.0) + { + dblTargetPos = 0.0; + } + } + + //deal with velocity + //find parity & checkSum + intPar_vel = EvenParityBitGen(slaveReceiveVel>>1); + intChkSum_vel = SumDigits(slaveReceiveVel>>7); + + //check if parity, check Sum and mesage type matches + if((intPar_vel == (slaveReceiveVel&0x1))&&(intChkSum_vel == ((slaveReceiveVel>>2)&0x1F))&&( ( (slaveReceiveVel>>1) &0x1) ) == 0) { + slaveReceiveVel = slaveReceiveVel>>7; + dblTargetVel = (double) MAX_SPEED_MMPS*slaveReceiveVel/511; + + //limit demand to ensure safety + if(dblTargetVel > MAX_SPEED_MMPS) + { + dblTargetVel = MAX_SPEED_MMPS; + } + if(dblTargetVel < 0.0) + { + 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; +// } +// } + + } + } + pinGate = 0; + + + } + } + + + //configure all control parameters + void ControlParameterConfig() + { + Kp = Kp/dblMotorVoltage; + Kd = Kd/dblSampleTime_s/dblMotorVoltage; + Ki = Ki*dblSampleTime_s/dblMotorVoltage; + + dblPosConv = dblMaxPos/0.8; + } + +Ticker debugTicker; + +void startPositionControl() +{ + semPosCtrl.release(); +} + +void startDebug() +{ + semDebug.release(); +} + +Ticker positionCtrlTicker; + +int main() { + cs_ADC = 1; + + 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); + dblTargetPos = dblStartingPos; + dblPosD[intDemPosFilOrder] = dblStartingPos; + + printf("\r\n%d, %f\r\n", intPotRead, dblStartingPos); + //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; + PositionControlThread.start(PositionControlPID); + DutyCycleThread.start(CalculateDutyCycle); + positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s); + + intFeedBack_pos = 0; + intFeedBack_pres = 0; + + + while(1) + { + Thread::wait(osWaitForever); + } +} + + +