Pressure control for drive segment in rebuild of control unit.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
main.cpp
- Committer:
- dofydoink
- Date:
- 2021-06-25
- Revision:
- 6:6142902e20d1
- Parent:
- 5:4e710cef655e
File content as of revision 6:6142902e20d1:
#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 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); //DigitalOut Mntr(D3); //DigitalIn swDem(D2); //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 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; #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); AnalogIn pinPresSens(PC_0); FastPWM pinPwmOutput(PC_8); FastPWM pinAltOutB(PA_0); FastPWM pinAltOutA(PA_1); //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 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; double dblStartingPos; //demand variables double dblPosD[10]; double dblLastPosD; double dblVelD[10]; double dblAccelD; double dblPresD; double dblPresDotD; double dblTargetPos = 0; double dblTargetVel =0; double dblLinearPath = 0; double dblLastLinearPath = 0; double dblPathSign; double dblError; double dblLastError; double dblErrorDot; //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 = 130.0; double Ki = 0.0; double Kd = 0.0; double dblIntTerm; double dblIntLimit = 1.0; int RXFlag; double dblControlBias = 0.0; double dblMotorVoltage = 24.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(); 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) {//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 CloseGate() //{ // pinGate = 0; // //} // double dblReadDem = 0; void PositionControlPID() { while(1){ semPosCtrl.wait(); //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; //take all readings //sensor readings intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure 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 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; dblPresDot = dblPresFil[intPresFilOrder] - dblLastPres;//calculate difference //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{ dblPresDotFil[intPresDotFilOrder] = dblPresDot; } //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 //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 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.3*dblLinearPath + 0.7*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; 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 = dblPresD - dblPresFil[intPresFilOrder]; dblErrorDot = dblPresDotD - dblPresDotFil[intPresDotFilOrder]; //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.0321) { dblError = 0.0; //dblErrorDot = 0.0; } 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){ 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]; //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; pinAltOutB.write(abs(output[intOutFilOrder])+dblBias); pinAltOutA.write(0.0); pinPwmOutput.write(1.0); } else { //pinDirectionFwd = 0; // pinDirectionRev = 1; // dblControlBias = 0.0; pinAltOutA.write(abs(output[intOutFilOrder])+dblBias); pinAltOutB.write(0.0); pinPwmOutput.write(1.0); } //printf("O:%f\t\r\n",abs(output[intOutFilOrder])); //pinPwmOutput.write(abs(output[intOutFilOrder])); //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; } //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)//////////////////////////////// //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(){ Kp = Kp/dblMotorVoltage; Kd = Kd/dblSampleTime_s/dblMotorVoltage; Ki = Ki*dblSampleTime_s/dblMotorVoltage; dblReadDem = 0; } Ticker debugTicker; void startPositionControl(){ semPosCtrl.release(); } void startDebug(){ semDebug.release(); } Ticker positionCtrlTicker; int main() { //pinGate = 0; //swDem.mode(PullDown); cs_ADC = 1; //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(); //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 = 0.0;//(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); } }