MorphGI
Low level control for MorphGI Control unit rebuild, using PWM to receive commands from Mid level.
Dependencies: mbed QEI FastAnalogIn mbed-rtos FastPWM
Diff: main.cpp
- Revision:
- 0:20018747657d
- Child:
- 1:cb2859df7a4c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Dec 17 15:11:37 2018 +0000
@@ -0,0 +1,718 @@
+#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
+
+
+
+const float MAX_SPEED_MMPS = 24.3457;
+
+SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel
+
+QEI wheel (PB_5, PB_4, NC, 256, QEI::X4_ENCODING);
+
+DigitalOut Mntr(D3);
+
+ #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 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;
+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);
+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);
+
+ 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;
+
+ 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.001;
+
+
+ //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 dblMaxPos = 41.0; //maximum actuator position position in mm
+ double dblPosConv;
+
+ double dblError;
+ double dblLastError;
+ double dblErrorDot;
+
+ //filter Variables
+ int intPosFilOrder = 1;
+ int intVelFilOrder = 1;
+ int intDemPosFilOrder = 6;
+ int intDemVelFilOrder = 6;
+ int intOutFilOrder = 0;
+
+ //controller variables
+ double omega;
+ double zeta;
+
+ double Kp = 20.0;
+ double Ki = 2.0;
+ double Kd = 1.5;
+ double dblIntTerm;
+ double dblIntLimit = 0.8;
+
+ 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;
+
+ //define custom Functions
+
+ 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)
+{
+ 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;
+
+}
+
+Timer gateTimer;
+
+
+ void PositionControlPID()
+ {
+ while(1)
+ {
+ semPosCtrl.wait();
+
+ Mntr = 1;//!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
+ intFeedBack_pres = intPressureRead & 0xFFE0;
+ intFeedBack_pres = (intFeedBack_pres) | SumDigits(intFeedBack_pres>>5);
+ intFeedBack_pres = (intFeedBack_pres <<1) &0x1;
+ intFeedBack_pres = (intFeedBack_pres <<1) | EvenParityBitGen(intFeedBack_pres);
+
+ 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];
+ }
+
+ //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];
+ }
+
+
+ //printf("%f\r\n",dblPosD[intDemPosFilOrder]);
+
+ intFeedBack_pos = dblPos[intPosFilOrder]/dblMaxPos*511;
+
+ if(intFeedBack_pos>511)
+ {
+ intFeedBack_pos = 511;
+ }
+
+ if(intFeedBack_pos<0)
+ {
+ intFeedBack_pos = 0;
+ }
+
+
+ intFeedBack_pos = (intFeedBack_pos<<5) | SumDigits(intFeedBack_pos);
+ intFeedBack_pos = intFeedBack_pos <<1;
+ intFeedBack_pos = (intFeedBack_pos <<1) | EvenParityBitGen(intFeedBack_pos);
+
+ //intFeedBack = dblSensorDriftError*8191;
+
+
+
+
+
+ ///////////////PATH GENERATION////////////////////////
+ //work out next path point
+ double dblPathDifference;
+ dblPathDifference = dblTargetPos - dblPosD[intDemPosFilOrder];
+ double dblLinearPath;
+ double dblLastLinearPath;
+ //check if target has been reached
+ if (dblPathDifference > 1.05*MAX_SPEED_MMPS*dblSampleTime_s) {
+ //is velocity positive or negative?
+ double dblPathSign = dblPathDifference/fabs(dblPathDifference);
+
+ dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path
+
+ //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;
+
+ //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];
+
+ Mntr = 0;
+
+ //GateControl();
+ gateTimer.reset();
+ pinGate = 1;
+
+ while((gateTimer.read_us() < 600) && pinGate == 1)
+ {
+ if(slave.receive())
+ {
+ //receive first msg
+ slaveReceivePos = slave.read(); //reply with 0xFFFF
+
+ slave.reply(intFeedBack_pos);//prepare position reply
+ slaveReceiveVel = slave.read();//get next message
+
+ //intFeedBack = intPressureRead & 0xFFFF;
+ slave.reply(intFeedBack_pres); //prepare pressure reply
+
+ slave.reply(0xFFFF);
+ pinGate = 0;
+
+ //deal with position
+ //find parity & checkSum
+ intPar_pos = EvenParityBitGen(slaveReceivePos>>1);
+ intChkSum_pos = SumDigits(slaveReceivePos>>7);
+
+ //check if parity, check Sum and mesage type matches
+ if((intPar_pos == (slaveReceivePos&0x1))&&(intChkSum_pos == ((slaveReceivePos>>2)&0x1F))&&( ( (slaveReceivePos>>1) &0x1) ) == 0) {
+ slaveReceivePos = slaveReceivePos>>7;
+ dblTargetPos = (double) dblMaxPos*slaveReceivePos/511;
+
+ //limit demand to ensure safety
+ if(dblTargetPos > dblMaxPos)
+ {
+ dblTargetPos = dblMaxPos;
+ }
+ if(dblTargetPos < 0.0)
+ {
+ dblTargetPos = 0.0;
+ }
+ }
+
+ //deal with velocity
+ //find parity & checkSum
+ intPar_vel = EvenParityBitGen(slaveReceiveVel>>1);
+ intChkSum_vel = SumDigits(slaveReceiveVel>>7);
+
+ //check if parity, check Sum and mesage type matches
+ if((intPar_vel == (slaveReceiveVel&0x1))&&(intChkSum_vel == ((slaveReceiveVel>>2)&0x1F))&&( ( (slaveReceiveVel>>1) &0x1) ) == 0) {
+ slaveReceiveVel = slaveReceiveVel>>7;
+ dblTargetVel = (double) MAX_SPEED_MMPS*slaveReceiveVel/511;
+
+ //limit demand to ensure safety
+ if(dblTargetVel > MAX_SPEED_MMPS)
+ {
+ dblTargetVel = MAX_SPEED_MMPS;
+ }
+ if(dblTargetVel < 0.0)
+ {
+ dblTargetVel = 0.0;
+ }
+ }
+
+
+
+
+// dataFlag = slaveReceive>>13;
+//
+// if(dataFlag == 0)
+// {
+// slaveReceive = slaveReceive>>7 & 0x1FF;
+// //get demand and filter
+// dblPosD[intDemPosFilOrder] = (double) dblMaxPos*slaveReceive/511;
+//
+// //limit demand to ensure safety
+// if(dblPosD[intDemPosFilOrder] > dblMaxPos)
+// {
+// dblPosD[intDemPosFilOrder] = dblMaxPos;
+// }
+// if(dblPosD[intDemPosFilOrder] < 0.0)
+// {
+// dblPosD[intDemPosFilOrder] = 0.0;
+// }
+// }
+
+ }
+ }
+ pinGate = 0;
+
+
+ }
+ }
+
+
+ //configure all control parameters
+ void ControlParameterConfig()
+ {
+ Kp = Kp/dblMotorVoltage;
+ Kd = Kd/dblSampleTime_s/dblMotorVoltage;
+ Ki = Ki*dblSampleTime_s/dblMotorVoltage;
+
+ dblPosConv = dblMaxPos/0.8;
+ }
+
+Ticker debugTicker;
+
+void startPositionControl()
+{
+ semPosCtrl.release();
+}
+
+void startDebug()
+{
+ semDebug.release();
+}
+
+Ticker positionCtrlTicker;
+
+int main() {
+ cs_ADC = 1;
+
+ pinPwmOutput.period_us(50);
+ 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 = (double) POT_2_MM*(intPotRead - POT_OFFSET);
+ dblTargetPos = dblStartingPos;
+ dblPosD[intDemPosFilOrder] = dblStartingPos;
+
+ printf("\r\n%d, %f\r\n", intPotRead, dblStartingPos);
+ //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;
+ PositionControlThread.start(PositionControlPID);
+ DutyCycleThread.start(CalculateDutyCycle);
+ positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s);
+
+ intFeedBack_pos = 0;
+ intFeedBack_pres = 0;
+
+
+ while(1)
+ {
+ Thread::wait(osWaitForever);
+ }
+}
+
+
+