Pressure control for drive segment in rebuild of control unit.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
Revision 6:6142902e20d1, committed 2021-06-25
- Comitter:
- dofydoink
- Date:
- Fri Jun 25 07:57:57 2021 +0000
- Parent:
- 5:4e710cef655e
- Commit message:
- Working pressure control low level code;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 4e710cef655e -r 6142902e20d1 main.cpp --- a/main.cpp Mon Apr 01 14:15:31 2019 +0000 +++ b/main.cpp Fri Jun 25 07:57:57 2021 +0000 @@ -17,16 +17,17 @@ #define PRESSURE_CHAN 1 #define POSITION_CHAN 3 - +int intDummy; +//SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel -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(PA_6); +InterruptIn pinPWMin_pos(PA_8); +InterruptIn pinPWMin_vel(PA_6); +QEI wheel (PB_5, PB_4, NC, 128, QEI::X2_ENCODING); -QEI wheel (PB_5, PB_4, NC, 256, QEI::X4_ENCODING); - -DigitalOut Mntr(D3); -DigitalOut Mntr2(PB_8); +//DigitalOut Mntr(D3); +//DigitalIn swDem(D2); +//DigitalOut Mntr2(PB_8); #define EXTERNAL_CLOCK_MODE 0x08 #define RANGE_CONFIG 0x03 @@ -38,8 +39,9 @@ const double MAX_ACTUATOR_LENGTH = 52.2; -const double MAX_POSITION_MM = 40.0; //maximum actuator position position in mm - +//const double MAX_POSITION_MM = 40.0; //maximum actuator position position in mm +const double MAX_POSITION_MM = 8.0; //maximum actuator position position in [bar] because this is a total hack. I've replaced position readings with pressure +const double MAX_SPEED_MMPS = 4.0; //sample time variables double dblSampleTime_s = 0.001; @@ -53,30 +55,17 @@ Thread GateControlThread(osPriorityNormal); Thread DutyCycleThread(osPriorityRealtime); -Mutex mutDutyCycle; +Mutex mutDutyCycle_pos; +Mutex mutDutyCycle_vel; 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); @@ -91,16 +80,20 @@ DigitalOut cs_ADC(PB_12); -DigitalOut pinGate(PA_8); +//DigitalOut pinGate(PA_8); -AnalogIn pinDemand1(PA_0); -AnalogIn pinDemand2(PA_1); +//AnalogIn pinDemand1(PA_0); +//AnalogIn pinDemand2(PA_1); AnalogIn pinCurSense(PB_0); +AnalogIn pinPresSens(PC_0); FastPWM pinPwmOutput(PC_8); -FastPWM pinSigOut(D2); +FastPWM pinAltOutB(PA_0); +FastPWM pinAltOutA(PA_1); + +//FastPWM pinSigOut(D2); DigitalOut pinDirectionFwd(PA_13);//INB DigitalOut pinDirectionRev(PA_14);//INA @@ -143,6 +136,15 @@ double dblAccel; //acceleration in mm/s^2 double dblIntegral = 0;//integral of position; +double dblPres; //current pressure in bar +double dblPresFil[10]; //current pressure after filter in bar +double dblPresDot; //time derivative of pressure +double dblPresDotFil[10]; //filtered time derivative of pressure +double dblLastPres; //previous pressure +double dblLastPresDot; +double dblIntPres = 0.0; //time integral of pressure + + double dblPotPositionRead; double dblPosBias = 48.0; @@ -155,10 +157,13 @@ double dblVelD[10]; double dblAccelD; -double dblTargetPos; -double dblTargetVel; -double dblLinearPath; -double dblLastLinearPath; +double dblPresD; +double dblPresDotD; + +double dblTargetPos = 0; +double dblTargetVel =0; +double dblLinearPath = 0; +double dblLastLinearPath = 0; double dblPathSign; @@ -169,26 +174,29 @@ //filter Variables int intPosFilOrder = 0; +int intPresFilOrder = 4; +int intPresDotFilOrder = 4; 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 Kp = 130.0; +double Ki = 0.0; +double Kd = 0.0; double dblIntTerm; -double dblIntLimit = 0.8; +double dblIntLimit = 1.0; int RXFlag; double dblControlBias = 0.0; -double dblMotorVoltage = 12.0; +double dblMotorVoltage = 24.0; double output[10];//controller output @@ -198,11 +206,92 @@ double currentBuck; double currentBuckGain = 3.0; -Timer gateTimer; +//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(); + isRiseWorking = 1; +} + +void PWMFall_pos() { + mutDutyCycle_pos.lock(); + intDeltaT_pos = timerDutyCycle_pos.read_us(); + mutDutyCycle_pos.unlock(); + isFallWorking = 1; +} + +void PWMRise_vel() { + mutDutyCycle_vel.lock(); + intT_vel = timerPeriod_vel.read_us(); + timerPeriod_vel.reset(); + timerDutyCycle_vel.reset(); + mutDutyCycle_vel.unlock(); +} + +void PWMFall_vel() { + mutDutyCycle_vel.lock(); + intDeltaT_vel = timerDutyCycle_vel.read_us(); + mutDutyCycle_vel.unlock(); +} + +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) { +bool CheckMessage(int msg) {//checks if message was corrupted // Find message parity short int count = 0; for(short int i=0; i<32; i++) { @@ -223,44 +312,53 @@ 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; -} +////////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) { @@ -310,8 +408,7 @@ void ADC_Config() { - - unsigned int msg; + unsigned int msg; spi.format(8,0); spi.frequency(3000000);//3MHz clock speed (3.67MHz max) @@ -350,7 +447,6 @@ cs_ADC = 0; spi.write(EXTERNAL_CLOCK_MODE); cs_ADC = 1; - } @@ -364,7 +460,6 @@ intTempVar = int(intTempVar/10); //intSumResult += int(var/100)%100; } - return intSumResult; } @@ -384,42 +479,107 @@ return 1; } -void CloseGate() -{ - pinGate = 0; - -} - - - - +//void CloseGate() +//{ +// pinGate = 0; +// +//} +// + double dblReadDem = 0; + void PositionControlPID() { - while(1) - { + while(1){ semPosCtrl.wait(); - - Mntr2 = !Mntr2; + //slave.reply(0x5555); +//Mntr2 = !Mntr2; //Mntr2 = 1 - Mntr2;//!led; pulsesTest = wheel.getPulses(); + + dblReadDem = (double)pulsesTest/256; + + if (dblReadDem > 1.0){ + dblReadDem = 1.0; + } + if (dblReadDem < 0){ + dblReadDem = 0.0; + } + + dblReadDem = dblReadDem * 4.0; 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); + intPressureRead = pinPresSens.read(); + dblPressureVal_norm = pinPresSens.read(); + dblPressureVal_norm = dblPressureVal_norm*3.3;//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; + } + + + //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; + dblPres = dblPressureVal_bar; + //intFeedBack_pres = (int)(((double)intPressureRead/10667) * 511); + + //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]; + + double alpha = (6.283*dblSampleTime_s*100)/(1 + 6.283*dblSampleTime_s*100); + + //filter pressure reading + if (intPresFilOrder>0) { + dblPresFil[0] = alpha*dblPres + (1-alpha)*dblPresFil[intPresFilOrder];//filter at 100Hz cutoff + if (intPresFilOrder>1) { + for (ii = 1;ii<intPresFilOrder+1; ii++){ + dblPresFil[ii] = alpha*dblPresFil[ii-1]+ (1-alpha)*dblPresFil[ii]; + } + } + } + else{ + dblPresFil[intPresFilOrder] = dblPres; + } + + intFeedBack_pres = (int) (dblPresFil[intPresFilOrder]/12*511);//scale pressure to 12bar + + 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 @@ -427,79 +587,40 @@ 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 + dblPresDot = dblPresFil[intPresFilOrder] - dblLastPres;//calculate difference - //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]; + //filter pressure time derivative + if (intPresDotFilOrder>0){ + dblPresDotFil[0] = alpha*dblPresDot + (1-alpha)*dblPresDotFil[intPresDotFilOrder];//filter at 100Hz cutoff + if (intPresDotFilOrder>1){ + for (ii = 1;ii<intPresDotFilOrder+1; ii++){ + dblPresDotFil[ii] = alpha*dblPresDotFil[ii-1]+ (1-alpha)*dblPresDotFil[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]; + else{ + dblPresDotFil[intPresDotFilOrder] = dblPresDot; } - - //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; - } - - + //feedback stuff, set position equal to pressure + intFeedBack_pos = intFeedBack_pres; 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; - + intFeedBack_pos = (intFeedBack_pos <<1) | OddParityBitGen(intFeedBack_pos);//add parity + + //self pressure demant set +// int intState = swDem.read(); +// if (intState){ +// dblTargetPos = 6.0; +// dblTargetVel = 6.0; +// } +// else { +// dblTargetPos = 1.5; +// dblTargetVel = 6.0; +// } ///////////////PATH GENERATION//////////////////////// //work out next path point @@ -523,7 +644,7 @@ dblLinearPath = 0.0; } - dblPosD[intDemPosFilOrder] = 0.07*dblLinearPath + 0.93*dblPosD[intDemPosFilOrder]; + dblPosD[intDemPosFilOrder] = 0.3*dblLinearPath + 0.7*dblPosD[intDemPosFilOrder]; //make sure path is safe if (dblPosD[intDemPosFilOrder] > MAX_POSITION_MM) { @@ -534,177 +655,261 @@ } dblVelD[0] = dblPosD[intDemPosFilOrder] - dblLastPosD; + dblPresD = dblPosD[intDemPosFilOrder];//set demand pressure to calculated demand + dblPresDotD = dblVelD[0];//set demand rate of change in pressure ///////////////////////////////////////////////////// End of Path Generation - - //run PID calculations //get errors - dblError = dblPosD[intDemPosFilOrder] - dblPos[intPosFilOrder]; - dblErrorDot = dblVelD[intDemVelFilOrder] - dblVel[intVelFilOrder]; + + dblError = dblPresD - dblPresFil[intPresFilOrder]; + dblErrorDot = dblPresDotD - dblPresDotFil[intPresDotFilOrder]; //get integral - //printf("%f\r\n",dblError); - - dblIntTerm = dblIntTerm + Ki*dblError; //limit integral term - if (dblIntTerm > dblIntLimit) - { + if (dblIntTerm > dblIntLimit){ dblIntTerm = dblIntLimit; } - if (dblIntTerm < -1.0*dblIntLimit) - { + if (dblIntTerm < -1.0*dblIntLimit){ dblIntTerm = (double) -1.0*dblIntLimit; } - if(fabs(dblError) <0.01) - { + if(fabs(dblError) <0.0321) { dblError = 0.0; - dblErrorDot = 0.0; + //dblErrorDot = 0.0; } - if (fabs(dblErrorDot) < 0.1) - { + if (fabs(dblErrorDot) < 0.2) { dblErrorDot = 0.0; } //calculate output + + output[0] = Kp*dblError + dblIntTerm + Kd*dblErrorDot; - + //printf("%f\r\n",dblError); //limit output - if (output[0] > 0.95) - { + if (output[0] > 0.95){ output[0] = 0.95; } - if (output[0] < -0.95) - { + if (output[0] < -0.95) { output[0] = -0.95; } - if(intOutFilOrder>0) - { - for (ii = 1; ii < intOutFilOrder+1; ii++) - { + if(intOutFilOrder>0){ + for (ii = 1; ii < intOutFilOrder+1; ii++) { output[ii] = 0.7*output[ii-1] + 0.3*output[ii]; } } - else - { + 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) - { + if (analogReadingCurSens> CurrentLimitSet){ currentBuck = CurrentLimitSet / analogReadingCurSens / currentBuckGain; } - else - { + else{ currentBuck = 1.0; } + + if (currentBuck >1.0){ + currentBuck = 1.0; + } + + if (currentBuck <0.0){ + currentBuck = 0.0; + } output[intOutFilOrder] = currentBuck*output[intOutFilOrder]; + + + //output[intOutFilOrder] = dblPresD; //end Current limit + + double dblBias = 0.08*dblPresFil[intPresFilOrder] + 0.2; + dblBias = 0.0; //find direction if(output[intOutFilOrder] >=0.0) { - pinDirectionFwd = 1; - pinDirectionRev = 0; - dblControlBias = 0.0; + //pinDirectionFwd = 1; + //pinDirectionRev = 0; + //dblControlBias = 0.0; + pinAltOutB.write(abs(output[intOutFilOrder])+dblBias); + pinAltOutA.write(0.0); + pinPwmOutput.write(1.0); + } else { - pinDirectionFwd = 0; - pinDirectionRev = 1; - dblControlBias = 0.0; + //pinDirectionFwd = 0; +// pinDirectionRev = 1; +// dblControlBias = 0.0; + pinAltOutA.write(abs(output[intOutFilOrder])+dblBias); + pinAltOutB.write(0.0); + pinPwmOutput.write(1.0); } - pinPwmOutput.write(abs(output[intOutFilOrder])+dblControlBias); + //printf("O:%f\t\r\n",abs(output[intOutFilOrder])); + //pinPwmOutput.write(abs(output[intOutFilOrder])); //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; - } - - } - + 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; } - 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]); + //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; + } + //intInput = (int)(dblDutyCycle*1000.0); + + //printf("%f\t%f\r\n",dblTargetPos, dblTargetVel); + + //////print debug////// + + //printf("P: %f\tPd: %f\t lin: %f\tTarget: %f\r\n", dblPresFil[intPresFilOrder],dblPresD, dblLinearPath,dblTargetPos); + //prinntf("E: %f\tBias: %f\r\n",output[intOutFilOrder], dblBias); + + ///////////////////Communication (For Carafino: Start)//////////////////////////////// - //printf("Demand Pos: %f\t RXFlag: %d\t parity?%d \r\n",dblTargetPos, RXFlag, parityFail); + //Message handling MCU can only receive messages when gate is 'open'. Gate opens for 500us every 1ms. + //gateTimer.reset();//resets the gate timer +// pinGate = 1;//gate is open. Sets digital output pin to HIGH +// unsigned int outboundMsgs[2] = {intFeedBack_pos, intFeedBack_pres};//prepare feedback messages +// unsigned int inboundMsgsData[2] = {0,0}; + //while(gateTimer.read_us() < 500) {//while gate is open +// +// if(slave.receive()) {//when a message is received +// +// bool isSPIsuccess = PerformSlaveSPI(&slave,outboundMsgs,inboundMsgsData);//process the data and check if it was corrupted +// +// if( isSPIsuccess ) {//if message looks OK +// dblTargetPos = (double)MAX_POSITION_MM*inboundMsgsData[0]/511; //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) +// +// if(dblTargetVel>MAX_SPEED_MMPS) { +// dblTargetVel = MAX_SPEED_MMPS; +// } else if(dblTargetVel<0.0) { +// dblTargetPos = 0.0; +// } +// +// break;//bail out of while loop +// } +// } +// } +// +// pinGate = 0;//close gate + dblLastPres = dblPresFil[intPresFilOrder];//update previous pressure reading + dblLastPresDot = dblPresDotFil[intPresDotFilOrder]; } } - - + //////////////////////////////////////////////////For Carafino: End + + + //configure all control parameters -void ControlParameterConfig() -{ +void ControlParameterConfig(){ Kp = Kp/dblMotorVoltage; Kd = Kd/dblSampleTime_s/dblMotorVoltage; Ki = Ki*dblSampleTime_s/dblMotorVoltage; + + dblReadDem = 0; } Ticker debugTicker; -void startPositionControl() -{ +void startPositionControl(){ semPosCtrl.release(); } -void startDebug() -{ +void startDebug(){ semDebug.release(); } Ticker positionCtrlTicker; int main() { - pinGate = 0; - + //pinGate = 0; + //swDem.mode(PullDown); cs_ADC = 1; - Mntr = 0; - Mntr2 = 0; - pinPwmOutput.period_us(50); + //Mntr = 0; + //Mntr2 = 0; + pinPwmOutput.period_us(1000); + + 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(); + //gateTimer.start(); //cs_ADC = 1; ControlParameterConfig(); @@ -714,8 +919,8 @@ // dblStartingPos = (double) POT_2_MM*uintPotRead - dblPosBias; // } // - slave.format(16,2); - slave.frequency(10000000); +// slave.format(16,2); +// slave.frequency(10000000); dblPosD[intDemPosFilOrder] = 0.0; slaveReceivePos = 0.0; @@ -727,7 +932,7 @@ 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); + dblStartingPos = 0.0;//(double) POT_2_MM*(intPotRead - POT_OFFSET); dblLinearPath = dblStartingPos; dblTargetPos = dblStartingPos; dblPos[intPosFilOrder] = dblStartingPos; @@ -739,7 +944,7 @@ //calculate/convert variables CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3; - slave.reply(0x5555); + //slave.reply(0x5555); PositionControlThread.start(PositionControlPID); positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s); @@ -748,8 +953,7 @@ intFeedBack_pres = 0; - while(1) - { + while(1){ Thread::wait(osWaitForever); } } \ No newline at end of file