MorphGI
Pressure control for drive segment in rebuild of control unit.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
Diff: main.cpp
- Revision:
- 6:6142902e20d1
- Parent:
- 5:4e710cef655e
--- 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