Low level control for MorphGI Control unit rebuild, using PWM to receive commands from Mid level.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
main.cpp
- Committer:
- dofydoink
- Date:
- 2021-06-24
- Revision:
- 6:042db7596e55
- Parent:
- 5:4e710cef655e
File content as of revision 6:042db7596e55:
#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 InterruptIn pinPWMin_pos(PA_8); InterruptIn pinPWMin_vel(PB_9); 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_pos; Mutex mutDutyCycle_vel; Semaphore semDutyCycle(1); Timer timerDutyCycle; double dblSensorDriftError; short randomvar; volatile int dataRx; 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_norm; 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; //Pressure Limitation const double dblPressureLimitBar = 10.0; double dblPressureLimitGain = 2.0; double dblPressureLimitBuck; double dblPressureLimitDerivativeGain = 0.1; double dblLastErrorPressure; double dblErrorPressure; double dblDeltaErrorPressure; int intT_pos= 100; int intDeltaT_pos =0; int intT_vel= 100; int intDeltaT_vel =0; Timer timerPeriod_pos; Timer timerDutyCycle_pos; Timer timerPeriod_vel; Timer timerDutyCycle_vel; bool isFallWorking = 0; bool isRiseWorking = 0; const int intPeriod_us = 2000; void PWMRise_pos() { //mutDutyCycle_pos.lock(); intT_pos = timerPeriod_pos.read_us(); timerPeriod_pos.reset(); timerDutyCycle_pos.reset(); //mutDutyCycle_pos.unlock(); } void PWMFall_pos() { //mutDutyCycle_pos.lock(); intDeltaT_pos = timerDutyCycle_pos.read_us(); //mutDutyCycle_pos.unlock(); } void PWMRise_vel() { //mutDutyCycle_vel.lock(); intT_vel = timerPeriod_vel.read_us(); timerPeriod_vel.reset(); timerDutyCycle_vel.reset(); //mutDutyCycle_vel.unlock(); isRiseWorking = 1; } void PWMFall_vel() { //mutDutyCycle_vel.lock(); intDeltaT_vel = timerDutyCycle_vel.read_us(); //mutDutyCycle_vel.unlock(); isFallWorking = 1; } double CalculateDemand(Mutex mutex, int intT, int intDeltaT, int intPeriod) { mutex.lock(); int _intT = intT; int _intDeltaT = intDeltaT; mutex.unlock(); double dblOutput, dblDutyCycle; if(_intT > 0){ if(_intT>(intPeriod_us-10) && _intT <(intPeriod_us+10)){ dblDutyCycle = (double)_intDeltaT/_intT; dblDutyCycle -= 0.1; dblDutyCycle /=0.8; if(dblDutyCycle >1.0){ dblDutyCycle = 1.0; } if(dblDutyCycle <0.0){ dblDutyCycle = 0.0; } } } int intInput = (int)(dblDutyCycle*400.0); //dblTargetPos = (double)MAX_POSITION_MM*dblDutyCycle; //set target position (9-bit value) dblOutput = (double)intInput/400.0; //set target position (9-bit value) return dblOutput; } Timer gateTimer; //define custom Functions bool CheckMessage(int msg) {//checks if message was corrupted // 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; } ////////For Carafino: Start///////// //Message checking (For Carafino) bool PerformSlaveSPI(SPISlave *spiSlave, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]) {//performs the SPI transaction 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;//dummy variable used to check function 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;//extracts type bit from message, 0 = target Position; 1 = target Velocity inboundMsgsData[typeBit] = inboundMsg>>7 & 0x1FF;//this contains the data recieved from master if( !CheckMessage(inboundMsg) ) { isSuccess = false; } } break; } } } if( numPacketsReceived != 3 ) {//if it hasn't received three messages, it failed. isSuccess = false; } return isSuccess; } ////////For Carafino: End///////// 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 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 dblPressureVal_norm = ((double) intPressureRead/16383.0); dblPressureVal_norm = dblPressureVal_norm*6.144;//convert to voltage dblPressureVal_norm = dblPressureVal_norm - 0.5;//subtract offset dblPressureVal_norm = dblPressureVal_norm/4.0;//calculate normalised pressure if (dblPressureVal_norm >1.0) { dblPressureVal_norm = 1.0; } if (dblPressureVal_norm < 0.0) { dblPressureVal_norm = 0.0; } double pressureCheck; //intPressureRead = intPressureRead-1334; //intPressureRead = intPressureRead-1679; //dblPressureVal_bar = ( (double) intPressureRead/10667)*10.0; //pressureCheck = ( (double) intPressureRead/10667)*10.0; dblPressureVal_bar = dblPressureVal_norm * 25.0; //intFeedBack_pres = (int)(((double)intPressureRead/10667) * 511); intFeedBack_pres = (int) (dblPressureVal_bar/12*511); if(intFeedBack_pres > 511) { intFeedBack_pres = 511; } if(intFeedBack_pres < 0) { intFeedBack_pres = 0; } //printf("%f\t",dblPos[0]); //printf("%d\t",intPressureRead); //printf("\r\n"); //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; } 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]; } intFeedBack_pos = (int) ((dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH)*511); if(intFeedBack_pos>511) { intFeedBack_pos = 511; } if(intFeedBack_pos<0) { intFeedBack_pos = 0; } //printf("%d\r\n",dblPos[intPosFilOrder]); 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 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; //tryPressureControl // double dblPosCtrlOut = Kp*dblError + dblIntTerm + Kd*dblErrorDot; // // double Ki_pres = 10.0; // double Kp_pres = 1; // // double dblPressureError; // double dblPressureErrorIntTerm; // // dblPressureError = dblPosCtrlOut - Pressure //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 pressure if (dblPressureVal_bar >dblPressureLimitBar) { dblErrorPressure = dblPressureVal_bar - dblPressureLimitBar; dblDeltaErrorPressure = dblErrorPressure - dblLastErrorPressure; dblPressureLimitBuck = dblPressureLimitGain * dblErrorPressure; dblPressureLimitBuck = dblPressureLimitBuck + (dblPressureLimitDerivativeGain*dblDeltaErrorPressure); dblPressureLimitBuck = dblPressureLimitBuck *1.9/2.0; dblLastErrorPressure = dblErrorPressure; } else { dblPressureLimitBuck = 0.0; } if (dblPressureLimitBuck < 0.0) { dblPressureLimitBuck = 0.0; } if (dblPressureLimitBuck > 1.9) { dblPressureLimitBuck = 1.9; } output[intOutFilOrder] = output[intOutFilOrder] - dblPressureLimitBuck; //limit output if (output[intOutFilOrder] > 0.95) { output[intOutFilOrder] = 0.95; } if (output[intOutFilOrder] < -0.95) { output[intOutFilOrder] = -0.95; } //limit current if (analogReadingCurSens> CurrentLimitSet) { currentBuck = CurrentLimitSet / analogReadingCurSens / currentBuckGain; } else { currentBuck = 1.0; } if (currentBuck >1.0) { currentBuck = 1.0; } if (currentBuck <0.0) { currentBuck = 0.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]; dblTargetPos = CalculateDemand(mutDutyCycle_pos, intT_pos, intDeltaT_pos, intPeriod_us); dblTargetPos = (double)MAX_POSITION_MM*dblTargetPos; //set target position (9-bit value) 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;//set target velocity (9-bit value) dblTargetVel = CalculateDemand(mutDutyCycle_vel, intT_vel, intDeltaT_vel, intPeriod_us); dblTargetVel = (double)MAX_SPEED_MMPS*dblTargetVel; //set target position (9-bit value) if(dblTargetVel>MAX_SPEED_MMPS) { dblTargetVel = MAX_SPEED_MMPS; } else if(dblTargetVel<0.0) { dblTargetVel = 0.0; } /* printf("%d \t %d \r\n",intDeltaT_pos,intDeltaT_vel); printf("%d \t %d \r\n",intT_pos,intT_vel); printf("%f \t %f \r\n",dblTargetPos,dblTargetVel); //printf("%f\r\n",output[intOutFilOrder]); printf("Rise: %d \t Fall:%d", isRiseWorking, isFallWorking); printf("\r\n"); */ } } //////////////////////////////////////////////////For Carafino: End //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() { cs_ADC = 1; Mntr = 0; Mntr2 = 0; pinPwmOutput.period_us(50); pinPWMin_pos.mode(PullNone); timerDutyCycle_pos.start(); timerPeriod_pos.start(); pinPWMin_vel.mode(PullNone); timerDutyCycle_vel.start(); timerPeriod_vel.start(); //calculateTicker.attach(&CalculateDutyCycle,0.1); pinPWMin_pos.rise(&PWMRise_pos); pinPWMin_pos.fall(&PWMFall_pos); pinPWMin_vel.rise(&PWMRise_vel); pinPWMin_vel.fall(&PWMFall_vel); pc.baud(115200); 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; dblLinearPath = dblStartingPos; dblTargetPos = 0.0; dblPos[intPosFilOrder] = dblStartingPos; dblTargetVel = 2.0; dblPosD[intDemPosFilOrder] = dblStartingPos; //dblPosD[intDemPosFilOrder] = 0; 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); } }