Control code for the peristaltic pump. Includes current control.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
Diff: main.cpp
- Revision:
- 3:9bd35e5b05ba
- Parent:
- 2:aee7d4724915
- Child:
- 4:3bab17dfae4e
--- a/main.cpp Mon Jan 28 15:30:36 2019 +0000 +++ b/main.cpp Tue Jan 29 14:57:57 2019 +0000 @@ -19,7 +19,7 @@ -const float MAX_SPEED_MMPS = 24.3457; +const double MAX_SPEED_MMPS = 24.3457; int intDummy; SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel @@ -28,14 +28,20 @@ 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 #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 + +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 @@ -81,211 +87,271 @@ 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 +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); - 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 +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; - 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; +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 = 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; - //define custom Functions +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[]) { - int Read14BitADC(int channel, DigitalOut CSpin) + 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) { - 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: + case 1: 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; + break; + + case 2: + message = CHAN_2; + break; + + case 3: + message = CHAN_3; + break; + case 4: + message = CHAN_4; + break; + + default: + message = CHAN_1; } - 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; + 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; - 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; - - } + 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) @@ -302,7 +368,8 @@ return intSumResult; } -int EvenParityBitGen(int var) +//int EvenParityBitGen(int var) +int OddParityBitGen(int var) { unsigned int count = 0, i, b = 1; @@ -310,7 +377,9 @@ if( var & (b << i) ){count++;} } - if( (count % 2) ){return 0;} + if( (count % 2) ){ + return 0; + } return 1; } @@ -321,373 +390,294 @@ } -Timer gateTimer; + - void PositionControlPID() +void PositionControlPID() +{ + while(1) { - while(1) - { - semPosCtrl.wait(); - Mntr = !Mntr; - //Mntr2 = 1; - //Mntr2 = 1 - Mntr2;//!led; - pulsesTest = wheel.getPulses(); + 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; - 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 - 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]; + } - - //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) + //get velocity and filter + dblVel[0] = dblPos[intPosFilOrder] - dblLastPos; + if(intVelFilOrder>0) + { + for (ii = 1; ii<intVelFilOrder+1; ii++) { - for (ii = 1; ii<intPosFilOrder+1; ii++) - { - dblPos[ii] = (double) 0.7*dblPos[ii-1] + 0.3*dblPos[ii]; - } + dblVel[ii] = (double) 0.7*dblVel[ii-1] + 0.3*dblVel[ii]; } - else - { - dblPos[intPosFilOrder] = dblPos[0]; - } + } + 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? - //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]; - } + //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.2*dblLinearPath + 0.8*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; - - //printf("%f\r\n",dblPosD[intDemPosFilOrder]); + } - intFeedBack_pos = (int) dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH*511; - - if(intFeedBack_pos>511) + 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++) { - intFeedBack_pos = 511; - } - - if(intFeedBack_pos<0) - { - intFeedBack_pos = 0; + output[ii] = 0.7*output[ii-1] + 0.3*output[ii]; } - - - 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; - + } + 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) { - ///////////////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); + if(slave.receive()) { + + bool isSPIsuccess = PerformSlaveSPI(&slave,outboundMsgs,inboundMsgsData); - dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path - if(dblLinearPath > dblMaxPos){ - dblLinearPath = dblMaxPos; - } - if (dblLinearPath < 0.0){ - dblLinearPath = 0.0; + 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; } - //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); - - } } + + 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; - } +//configure all control parameters +void ControlParameterConfig() +{ + Kp = Kp/dblMotorVoltage; + Kd = Kd/dblSampleTime_s/dblMotorVoltage; + Ki = Ki*dblSampleTime_s/dblMotorVoltage; +} Ticker debugTicker; @@ -740,25 +730,18 @@ 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); - //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; @@ -769,7 +752,4 @@ { Thread::wait(osWaitForever); } -} - - - +} \ No newline at end of file