MorphGI
Pressure control for drive segment in rebuild of control unit.
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