MorphGI / Mbed 2 deprecated LoCoMoTE_PeristalticPumpControl_v4

Control code for the peristaltic pump. Includes current control.

Dependencies:   mbed QEI FastAnalogIn mbed-rtos FastPWM

main.cpp@0:20018747657d, 2018-12-17 (annotated)

Committer:
dofydoink
Date:
Mon Dec 17 15:11:37 2018 +0000
Revision:
0:20018747657d
Child:
1:cb2859df7a4c
rgdfgbdfgd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dofydoink 0:20018747657d 1 #include "mbed.h"
dofydoink 0:20018747657d 2 #include "math.h"
dofydoink 0:20018747657d 3
dofydoink 0:20018747657d 4 #include "QEI.h"
dofydoink 0:20018747657d 5
dofydoink 0:20018747657d 6 #include "rtos.h"
dofydoink 0:20018747657d 7 #include "FastPWM.h"
dofydoink 0:20018747657d 8
dofydoink 0:20018747657d 9 #define ERROR_LIMIT_POS 0.008 //limits for when any error measurement is considered to be zero.
dofydoink 0:20018747657d 10 #define ERROR_LIMIT_NEG -0.008
dofydoink 0:20018747657d 11
dofydoink 0:20018747657d 12 #define CHAN_1 0x80
dofydoink 0:20018747657d 13 #define CHAN_2 0x90
dofydoink 0:20018747657d 14 #define CHAN_3 0xA0
dofydoink 0:20018747657d 15 #define CHAN_4 0xB0
dofydoink 0:20018747657d 16
dofydoink 0:20018747657d 17 #define PRESSURE_CHAN 1
dofydoink 0:20018747657d 18 #define POSITION_CHAN 3
dofydoink 0:20018747657d 19
dofydoink 0:20018747657d 20
dofydoink 0:20018747657d 21
dofydoink 0:20018747657d 22 const float MAX_SPEED_MMPS = 24.3457;
dofydoink 0:20018747657d 23
dofydoink 0:20018747657d 24 SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel
dofydoink 0:20018747657d 25
dofydoink 0:20018747657d 26 QEI wheel (PB_5, PB_4, NC, 256, QEI::X4_ENCODING);
dofydoink 0:20018747657d 27
dofydoink 0:20018747657d 28 DigitalOut Mntr(D3);
dofydoink 0:20018747657d 29
dofydoink 0:20018747657d 30 #define EXTERNAL_CLOCK_MODE 0x08
dofydoink 0:20018747657d 31 #define RANGE_CONFIG 0x03
dofydoink 0:20018747657d 32
dofydoink 0:20018747657d 33 #define EXTERNAL_CLOCK_MODE 0x08
dofydoink 0:20018747657d 34 #define RANGE_CONFIG 0x03 //config for 1.5*Vref = 6.144V
dofydoink 0:20018747657d 35
dofydoink 0:20018747657d 36 #define PRESSURE_BIAS_VOLTAGE 0.15151515151515
dofydoink 0:20018747657d 37
dofydoink 0:20018747657d 38 #define POT_2_MM 0.006750412 //convert potentiometer reading to mm (Tested and is right)
dofydoink 0:20018747657d 39 #define POT_OFFSET 7500//6666
dofydoink 0:20018747657d 40
dofydoink 0:20018747657d 41 Serial pc(USBTX, USBRX); // tx, rx
dofydoink 0:20018747657d 42
dofydoink 0:20018747657d 43 Thread PositionControlThread(osPriorityHigh);
dofydoink 0:20018747657d 44 Thread DebugThread(osPriorityNormal);
dofydoink 0:20018747657d 45 Thread GateControlThread(osPriorityNormal);
dofydoink 0:20018747657d 46
dofydoink 0:20018747657d 47 Thread DutyCycleThread(osPriorityRealtime);
dofydoink 0:20018747657d 48 Mutex mutDutyCycle;
dofydoink 0:20018747657d 49 Semaphore semDutyCycle(1);
dofydoink 0:20018747657d 50 Timer timerDutyCycle;
dofydoink 0:20018747657d 51
dofydoink 0:20018747657d 52 volatile unsigned int intT;
dofydoink 0:20018747657d 53 volatile unsigned int intDeltaT;
dofydoink 0:20018747657d 54 double dblDutyCycle;
dofydoink 0:20018747657d 55
dofydoink 0:20018747657d 56 double dblSensorDriftError;
dofydoink 0:20018747657d 57
dofydoink 0:20018747657d 58 short randomvar;
dofydoink 0:20018747657d 59
dofydoink 0:20018747657d 60 volatile int dataRx;
dofydoink 0:20018747657d 61
dofydoink 0:20018747657d 62 void CalculateDutyCycle()
dofydoink 0:20018747657d 63 {
dofydoink 0:20018747657d 64 while(1)
dofydoink 0:20018747657d 65 {
dofydoink 0:20018747657d 66 semDutyCycle.wait();
dofydoink 0:20018747657d 67 mutDutyCycle.lock();
dofydoink 0:20018747657d 68 dblDutyCycle = (double) intDeltaT/intT;
dofydoink 0:20018747657d 69 mutDutyCycle.unlock();
dofydoink 0:20018747657d 70 }
dofydoink 0:20018747657d 71 }
dofydoink 0:20018747657d 72
dofydoink 0:20018747657d 73 Semaphore semPosCtrl(1);
dofydoink 0:20018747657d 74 Semaphore semDebug(1);
dofydoink 0:20018747657d 75 Semaphore semGate(1);
dofydoink 0:20018747657d 76
dofydoink 0:20018747657d 77 Timer timer;
dofydoink 0:20018747657d 78
dofydoink 0:20018747657d 79 long pulsesTest;
dofydoink 0:20018747657d 80
dofydoink 0:20018747657d 81 //define all pins
dofydoink 0:20018747657d 82 SPI spi(PB_15, PB_14, PB_13); // mosi, miso, sclk //DO NOT USE D4 OR D5 !!!
dofydoink 0:20018747657d 83
dofydoink 0:20018747657d 84 DigitalOut cs_ADC(PB_12);
dofydoink 0:20018747657d 85
dofydoink 0:20018747657d 86 DigitalOut pinGate(PA_8);
dofydoink 0:20018747657d 87
dofydoink 0:20018747657d 88 AnalogIn pinDemand1(PA_0);
dofydoink 0:20018747657d 89 AnalogIn pinDemand2(PA_1);
dofydoink 0:20018747657d 90
dofydoink 0:20018747657d 91 AnalogIn pinCurSense(PB_0);
dofydoink 0:20018747657d 92
dofydoink 0:20018747657d 93 FastPWM pinPwmOutput(PC_8);
dofydoink 0:20018747657d 94
dofydoink 0:20018747657d 95 FastPWM pinSigOut(D2);
dofydoink 0:20018747657d 96
dofydoink 0:20018747657d 97 DigitalOut pinDirectionFwd(PA_13);//INB
dofydoink 0:20018747657d 98 DigitalOut pinDirectionRev(PA_14);//INA
dofydoink 0:20018747657d 99
dofydoink 0:20018747657d 100 //define all variables
dofydoink 0:20018747657d 101
dofydoink 0:20018747657d 102 int ii,jj,kk,nn, spiTick; //counting variables
dofydoink 0:20018747657d 103
dofydoink 0:20018747657d 104 volatile int slaveReceivePos;
dofydoink 0:20018747657d 105 volatile int slaveReceiveVel;
dofydoink 0:20018747657d 106
dofydoink 0:20018747657d 107 volatile int dataFlag;
dofydoink 0:20018747657d 108 volatile int intFeedBack_pos;
dofydoink 0:20018747657d 109 volatile int intFeedBack_pres;
dofydoink 0:20018747657d 110
dofydoink 0:20018747657d 111 int intChkSum_pos;
dofydoink 0:20018747657d 112 int intChkSum_vel;
dofydoink 0:20018747657d 113
dofydoink 0:20018747657d 114 int intPar_pos;
dofydoink 0:20018747657d 115 int intPar_vel;
dofydoink 0:20018747657d 116
dofydoink 0:20018747657d 117 //raw Data Readings
dofydoink 0:20018747657d 118 int intPotRead = 0;
dofydoink 0:20018747657d 119 int intPressureRead = 0;
dofydoink 0:20018747657d 120 double analogReadingDemPos;
dofydoink 0:20018747657d 121 double analogReadingDemVel;
dofydoink 0:20018747657d 122 double analogReadingCurSens;
dofydoink 0:20018747657d 123
dofydoink 0:20018747657d 124 char buf[10];
dofydoink 0:20018747657d 125 int dataReceived;
dofydoink 0:20018747657d 126
dofydoink 0:20018747657d 127 //sample time variables
dofydoink 0:20018747657d 128 double dblSampleTime_s = 0.001;
dofydoink 0:20018747657d 129
dofydoink 0:20018747657d 130
dofydoink 0:20018747657d 131 //system state variables
dofydoink 0:20018747657d 132 double dblPos[10];//current position in mm
dofydoink 0:20018747657d 133 double dblPosFil[10];//current position in mm
dofydoink 0:20018747657d 134 double dblLastPos;//previous position in mm
dofydoink 0:20018747657d 135 double dblVel[10];//velocity in mm/s
dofydoink 0:20018747657d 136 double dblAccel; //acceleration in mm/s^2
dofydoink 0:20018747657d 137 double dblIntegral = 0;//integral of position;
dofydoink 0:20018747657d 138
dofydoink 0:20018747657d 139 double dblPotPositionRead;
dofydoink 0:20018747657d 140
dofydoink 0:20018747657d 141 double dblPosBias = 48.0;
dofydoink 0:20018747657d 142
dofydoink 0:20018747657d 143 double dblStartingPos;
dofydoink 0:20018747657d 144
dofydoink 0:20018747657d 145 //demand variables
dofydoink 0:20018747657d 146 double dblPosD[10];
dofydoink 0:20018747657d 147 double dblLastPosD;
dofydoink 0:20018747657d 148 double dblVelD[10];
dofydoink 0:20018747657d 149 double dblAccelD;
dofydoink 0:20018747657d 150
dofydoink 0:20018747657d 151 double dblTargetPos;
dofydoink 0:20018747657d 152 double dblTargetVel;
dofydoink 0:20018747657d 153
dofydoink 0:20018747657d 154 double dblMaxPos = 41.0; //maximum actuator position position in mm
dofydoink 0:20018747657d 155 double dblPosConv;
dofydoink 0:20018747657d 156
dofydoink 0:20018747657d 157 double dblError;
dofydoink 0:20018747657d 158 double dblLastError;
dofydoink 0:20018747657d 159 double dblErrorDot;
dofydoink 0:20018747657d 160
dofydoink 0:20018747657d 161 //filter Variables
dofydoink 0:20018747657d 162 int intPosFilOrder = 1;
dofydoink 0:20018747657d 163 int intVelFilOrder = 1;
dofydoink 0:20018747657d 164 int intDemPosFilOrder = 6;
dofydoink 0:20018747657d 165 int intDemVelFilOrder = 6;
dofydoink 0:20018747657d 166 int intOutFilOrder = 0;
dofydoink 0:20018747657d 167
dofydoink 0:20018747657d 168 //controller variables
dofydoink 0:20018747657d 169 double omega;
dofydoink 0:20018747657d 170 double zeta;
dofydoink 0:20018747657d 171
dofydoink 0:20018747657d 172 double Kp = 20.0;
dofydoink 0:20018747657d 173 double Ki = 2.0;
dofydoink 0:20018747657d 174 double Kd = 1.5;
dofydoink 0:20018747657d 175 double dblIntTerm;
dofydoink 0:20018747657d 176 double dblIntLimit = 0.8;
dofydoink 0:20018747657d 177
dofydoink 0:20018747657d 178 double dblControlBias = 0.0;
dofydoink 0:20018747657d 179
dofydoink 0:20018747657d 180 double dblMotorVoltage = 12.0;
dofydoink 0:20018747657d 181
dofydoink 0:20018747657d 182 double output[10];//controller output
dofydoink 0:20018747657d 183
dofydoink 0:20018747657d 184 //current limit variables
dofydoink 0:20018747657d 185 double CurrentLimitSet;
dofydoink 0:20018747657d 186 double dblCurrentLimitAmps = 3.0;
dofydoink 0:20018747657d 187 double currentBuck;
dofydoink 0:20018747657d 188 double currentBuckGain = 3.0;
dofydoink 0:20018747657d 189
dofydoink 0:20018747657d 190 //define custom Functions
dofydoink 0:20018747657d 191
dofydoink 0:20018747657d 192 int Read14BitADC(int channel, DigitalOut CSpin)
dofydoink 0:20018747657d 193 {
dofydoink 0:20018747657d 194 char message;
dofydoink 0:20018747657d 195 unsigned int outputA;
dofydoink 0:20018747657d 196 unsigned int outputB;
dofydoink 0:20018747657d 197 int output;
dofydoink 0:20018747657d 198
dofydoink 0:20018747657d 199 switch(channel)
dofydoink 0:20018747657d 200 {
dofydoink 0:20018747657d 201 case 1:
dofydoink 0:20018747657d 202 message = CHAN_1;
dofydoink 0:20018747657d 203 break;
dofydoink 0:20018747657d 204
dofydoink 0:20018747657d 205 case 2:
dofydoink 0:20018747657d 206 message = CHAN_2;
dofydoink 0:20018747657d 207 break;
dofydoink 0:20018747657d 208
dofydoink 0:20018747657d 209 case 3:
dofydoink 0:20018747657d 210 message = CHAN_3;
dofydoink 0:20018747657d 211 break;
dofydoink 0:20018747657d 212 case 4:
dofydoink 0:20018747657d 213 message = CHAN_4;
dofydoink 0:20018747657d 214 break;
dofydoink 0:20018747657d 215
dofydoink 0:20018747657d 216 default:
dofydoink 0:20018747657d 217 message = CHAN_1;
dofydoink 0:20018747657d 218 }
dofydoink 0:20018747657d 219
dofydoink 0:20018747657d 220 spi.format(8,0);
dofydoink 0:20018747657d 221 spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
dofydoink 0:20018747657d 222
dofydoink 0:20018747657d 223 CSpin.write(0);
dofydoink 0:20018747657d 224 spi.write(message);
dofydoink 0:20018747657d 225 spi.write(0x00);
dofydoink 0:20018747657d 226 outputA = spi.write(0x00);
dofydoink 0:20018747657d 227 outputB = spi.write(0x00);
dofydoink 0:20018747657d 228 CSpin.write(1);
dofydoink 0:20018747657d 229
dofydoink 0:20018747657d 230 //convert result to sensible value
dofydoink 0:20018747657d 231 outputA = outputA<<8;
dofydoink 0:20018747657d 232 output = outputA | outputB;
dofydoink 0:20018747657d 233 output = output>>2;
dofydoink 0:20018747657d 234
dofydoink 0:20018747657d 235 return output;
dofydoink 0:20018747657d 236 }
dofydoink 0:20018747657d 237
dofydoink 0:20018747657d 238 void ADC_Config()
dofydoink 0:20018747657d 239 {
dofydoink 0:20018747657d 240
dofydoink 0:20018747657d 241 unsigned int msg;
dofydoink 0:20018747657d 242 spi.format(8,0);
dofydoink 0:20018747657d 243 spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
dofydoink 0:20018747657d 244
dofydoink 0:20018747657d 245 msg = CHAN_1 | RANGE_CONFIG; //config channel 1 set Vref as
dofydoink 0:20018747657d 246 cs_ADC = 0;
dofydoink 0:20018747657d 247 spi.write(msg);
dofydoink 0:20018747657d 248 cs_ADC = 1;
dofydoink 0:20018747657d 249
dofydoink 0:20018747657d 250 cs_ADC = 0;
dofydoink 0:20018747657d 251 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 0:20018747657d 252 cs_ADC = 1;
dofydoink 0:20018747657d 253
dofydoink 0:20018747657d 254 msg = CHAN_2 | RANGE_CONFIG; //config channel 2
dofydoink 0:20018747657d 255 cs_ADC = 0;
dofydoink 0:20018747657d 256 spi.write(msg);
dofydoink 0:20018747657d 257 cs_ADC = 1;
dofydoink 0:20018747657d 258
dofydoink 0:20018747657d 259 cs_ADC = 0;
dofydoink 0:20018747657d 260 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 0:20018747657d 261 cs_ADC = 1;
dofydoink 0:20018747657d 262
dofydoink 0:20018747657d 263 msg = CHAN_3 | RANGE_CONFIG; //config channel 3
dofydoink 0:20018747657d 264 cs_ADC = 0;
dofydoink 0:20018747657d 265 spi.write(msg);
dofydoink 0:20018747657d 266 cs_ADC = 1;
dofydoink 0:20018747657d 267
dofydoink 0:20018747657d 268 cs_ADC = 0;
dofydoink 0:20018747657d 269 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 0:20018747657d 270 cs_ADC = 1;
dofydoink 0:20018747657d 271
dofydoink 0:20018747657d 272 msg = CHAN_4 | RANGE_CONFIG; //config channel 4
dofydoink 0:20018747657d 273 cs_ADC = 0;
dofydoink 0:20018747657d 274 spi.write(msg);
dofydoink 0:20018747657d 275 cs_ADC = 1;
dofydoink 0:20018747657d 276
dofydoink 0:20018747657d 277 cs_ADC = 0;
dofydoink 0:20018747657d 278 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 0:20018747657d 279 cs_ADC = 1;
dofydoink 0:20018747657d 280
dofydoink 0:20018747657d 281 }
dofydoink 0:20018747657d 282
dofydoink 0:20018747657d 283
dofydoink 0:20018747657d 284 int SumDigits(int var)
dofydoink 0:20018747657d 285 {
dofydoink 0:20018747657d 286 int intSumResult = 0;
dofydoink 0:20018747657d 287 int intTempVar = var;
dofydoink 0:20018747657d 288 while (intTempVar >0)
dofydoink 0:20018747657d 289 {
dofydoink 0:20018747657d 290 intSumResult += intTempVar%10;
dofydoink 0:20018747657d 291 intTempVar = int(intTempVar/10);
dofydoink 0:20018747657d 292 //intSumResult += int(var/100)%100;
dofydoink 0:20018747657d 293 }
dofydoink 0:20018747657d 294
dofydoink 0:20018747657d 295 return intSumResult;
dofydoink 0:20018747657d 296 }
dofydoink 0:20018747657d 297
dofydoink 0:20018747657d 298 int EvenParityBitGen(int var)
dofydoink 0:20018747657d 299 {
dofydoink 0:20018747657d 300 unsigned int count = 0, i, b = 1;
dofydoink 0:20018747657d 301
dofydoink 0:20018747657d 302 for(i = 0; i < 32; i++){
dofydoink 0:20018747657d 303 if( var & (b << i) ){count++;}
dofydoink 0:20018747657d 304 }
dofydoink 0:20018747657d 305
dofydoink 0:20018747657d 306 if( (count % 2) ){return 0;}
dofydoink 0:20018747657d 307
dofydoink 0:20018747657d 308 return 1;
dofydoink 0:20018747657d 309 }
dofydoink 0:20018747657d 310
dofydoink 0:20018747657d 311 void CloseGate()
dofydoink 0:20018747657d 312 {
dofydoink 0:20018747657d 313 pinGate = 0;
dofydoink 0:20018747657d 314
dofydoink 0:20018747657d 315 }
dofydoink 0:20018747657d 316
dofydoink 0:20018747657d 317 Timer gateTimer;
dofydoink 0:20018747657d 318
dofydoink 0:20018747657d 319
dofydoink 0:20018747657d 320 void PositionControlPID()
dofydoink 0:20018747657d 321 {
dofydoink 0:20018747657d 322 while(1)
dofydoink 0:20018747657d 323 {
dofydoink 0:20018747657d 324 semPosCtrl.wait();
dofydoink 0:20018747657d 325
dofydoink 0:20018747657d 326 Mntr = 1;//!led;
dofydoink 0:20018747657d 327 pulsesTest = wheel.getPulses();
dofydoink 0:20018747657d 328
dofydoink 0:20018747657d 329 double testy = (double) abs(pulsesTest)/2000;
dofydoink 0:20018747657d 330 pinSigOut.write(testy);
dofydoink 0:20018747657d 331
dofydoink 0:20018747657d 332 //take all readings
dofydoink 0:20018747657d 333
dofydoink 0:20018747657d 334 //sensor readings
dofydoink 0:20018747657d 335
dofydoink 0:20018747657d 336 intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure
dofydoink 0:20018747657d 337 intFeedBack_pres = intPressureRead & 0xFFE0;
dofydoink 0:20018747657d 338 intFeedBack_pres = (intFeedBack_pres) | SumDigits(intFeedBack_pres>>5);
dofydoink 0:20018747657d 339 intFeedBack_pres = (intFeedBack_pres <<1) &0x1;
dofydoink 0:20018747657d 340 intFeedBack_pres = (intFeedBack_pres <<1) | EvenParityBitGen(intFeedBack_pres);
dofydoink 0:20018747657d 341
dofydoink 0:20018747657d 342 intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dofydoink 0:20018747657d 343 dblPotPositionRead = (double) POT_2_MM*(intPotRead - POT_OFFSET);
dofydoink 0:20018747657d 344
dofydoink 0:20018747657d 345
dofydoink 0:20018747657d 346 //demand Readings
dofydoink 0:20018747657d 347
dofydoink 0:20018747657d 348
dofydoink 0:20018747657d 349 //current reading
dofydoink 0:20018747657d 350 analogReadingCurSens = (double) 0.3*pinCurSense.read()+0.7*analogReadingCurSens;
dofydoink 0:20018747657d 351
dofydoink 0:20018747657d 352 //convert units and filter
dofydoink 0:20018747657d 353
dofydoink 0:20018747657d 354 //get position and filter
dofydoink 0:20018747657d 355 dblPos[0] = (double) pulsesTest*-0.0078125 + dblStartingPos;
dofydoink 0:20018747657d 356 dblSensorDriftError = dblPotPositionRead - dblPos[0];
dofydoink 0:20018747657d 357
dofydoink 0:20018747657d 358 if(dblSensorDriftError > 2.0)//if encoder reading is seriously wrong
dofydoink 0:20018747657d 359 {
dofydoink 0:20018747657d 360 //dblPos[0] = dblPotPositionRead;
dofydoink 0:20018747657d 361 }
dofydoink 0:20018747657d 362
dofydoink 0:20018747657d 363 //printf("%d, %f, %f\r\n",pulsesTest,dblPos[0],dblPotPositionRead);
dofydoink 0:20018747657d 364 if(intPosFilOrder > 0)
dofydoink 0:20018747657d 365 {
dofydoink 0:20018747657d 366 for (ii = 1; ii<intPosFilOrder+1; ii++)
dofydoink 0:20018747657d 367 {
dofydoink 0:20018747657d 368 dblPos[ii] = (double) 0.7*dblPos[ii-1] + 0.3*dblPos[ii];
dofydoink 0:20018747657d 369 }
dofydoink 0:20018747657d 370 }
dofydoink 0:20018747657d 371 else
dofydoink 0:20018747657d 372 {
dofydoink 0:20018747657d 373 dblPos[intPosFilOrder] = dblPos[0];
dofydoink 0:20018747657d 374 }
dofydoink 0:20018747657d 375
dofydoink 0:20018747657d 376 //get velocity and filter
dofydoink 0:20018747657d 377 dblVel[0] = dblPos[intPosFilOrder] - dblLastPos;
dofydoink 0:20018747657d 378 if(intVelFilOrder>0)
dofydoink 0:20018747657d 379 {
dofydoink 0:20018747657d 380 for (ii = 1; ii<intVelFilOrder+1; ii++)
dofydoink 0:20018747657d 381 {
dofydoink 0:20018747657d 382 dblVel[ii] = (double) 0.7*dblVel[ii-1] + 0.3*dblVel[ii];
dofydoink 0:20018747657d 383 }
dofydoink 0:20018747657d 384 }
dofydoink 0:20018747657d 385 else
dofydoink 0:20018747657d 386 {
dofydoink 0:20018747657d 387 dblVel[intVelFilOrder] = dblVel[0];
dofydoink 0:20018747657d 388 }
dofydoink 0:20018747657d 389
dofydoink 0:20018747657d 390
dofydoink 0:20018747657d 391 //printf("%f\r\n",dblPosD[intDemPosFilOrder]);
dofydoink 0:20018747657d 392
dofydoink 0:20018747657d 393 intFeedBack_pos = dblPos[intPosFilOrder]/dblMaxPos*511;
dofydoink 0:20018747657d 394
dofydoink 0:20018747657d 395 if(intFeedBack_pos>511)
dofydoink 0:20018747657d 396 {
dofydoink 0:20018747657d 397 intFeedBack_pos = 511;
dofydoink 0:20018747657d 398 }
dofydoink 0:20018747657d 399
dofydoink 0:20018747657d 400 if(intFeedBack_pos<0)
dofydoink 0:20018747657d 401 {
dofydoink 0:20018747657d 402 intFeedBack_pos = 0;
dofydoink 0:20018747657d 403 }
dofydoink 0:20018747657d 404
dofydoink 0:20018747657d 405
dofydoink 0:20018747657d 406 intFeedBack_pos = (intFeedBack_pos<<5) | SumDigits(intFeedBack_pos);
dofydoink 0:20018747657d 407 intFeedBack_pos = intFeedBack_pos <<1;
dofydoink 0:20018747657d 408 intFeedBack_pos = (intFeedBack_pos <<1) | EvenParityBitGen(intFeedBack_pos);
dofydoink 0:20018747657d 409
dofydoink 0:20018747657d 410 //intFeedBack = dblSensorDriftError*8191;
dofydoink 0:20018747657d 411
dofydoink 0:20018747657d 412
dofydoink 0:20018747657d 413
dofydoink 0:20018747657d 414
dofydoink 0:20018747657d 415
dofydoink 0:20018747657d 416 ///////////////PATH GENERATION////////////////////////
dofydoink 0:20018747657d 417 //work out next path point
dofydoink 0:20018747657d 418 double dblPathDifference;
dofydoink 0:20018747657d 419 dblPathDifference = dblTargetPos - dblPosD[intDemPosFilOrder];
dofydoink 0:20018747657d 420 double dblLinearPath;
dofydoink 0:20018747657d 421 double dblLastLinearPath;
dofydoink 0:20018747657d 422 //check if target has been reached
dofydoink 0:20018747657d 423 if (dblPathDifference > 1.05*MAX_SPEED_MMPS*dblSampleTime_s) {
dofydoink 0:20018747657d 424 //is velocity positive or negative?
dofydoink 0:20018747657d 425 double dblPathSign = dblPathDifference/fabs(dblPathDifference);
dofydoink 0:20018747657d 426
dofydoink 0:20018747657d 427 dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path
dofydoink 0:20018747657d 428
dofydoink 0:20018747657d 429 //now smooth
dofydoink 0:20018747657d 430 dblPosD[intDemPosFilOrder] = 0.2*dblLinearPath + 0.8*dblPosD[intDemPosFilOrder];
dofydoink 0:20018747657d 431 }
dofydoink 0:20018747657d 432 else {
dofydoink 0:20018747657d 433 //set velocity to zero
dofydoink 0:20018747657d 434 dblTargetVel = 0.0;
dofydoink 0:20018747657d 435 }
dofydoink 0:20018747657d 436
dofydoink 0:20018747657d 437 //make sure path is safe
dofydoink 0:20018747657d 438 if (dblPosD[intDemPosFilOrder] > dblMaxPos) {
dofydoink 0:20018747657d 439 dblPosD[intDemPosFilOrder] = dblMaxPos;
dofydoink 0:20018747657d 440 }
dofydoink 0:20018747657d 441 if (dblPosD[intDemPosFilOrder] < 0.0) {
dofydoink 0:20018747657d 442 dblPosD[intDemPosFilOrder] = 0.0;
dofydoink 0:20018747657d 443 }
dofydoink 0:20018747657d 444
dofydoink 0:20018747657d 445
dofydoink 0:20018747657d 446 dblVelD[0] = dblPosD[intDemPosFilOrder] - dblLastPosD;
dofydoink 0:20018747657d 447
dofydoink 0:20018747657d 448 //run PID calculations
dofydoink 0:20018747657d 449 //get errors
dofydoink 0:20018747657d 450 dblError = dblPosD[intDemPosFilOrder] - dblPos[intPosFilOrder];
dofydoink 0:20018747657d 451 dblErrorDot = dblVelD[intDemVelFilOrder] - dblVel[intVelFilOrder];
dofydoink 0:20018747657d 452 //get integral
dofydoink 0:20018747657d 453 //printf("%f\r\n",dblError);
dofydoink 0:20018747657d 454
dofydoink 0:20018747657d 455
dofydoink 0:20018747657d 456 dblIntTerm = dblIntTerm + Ki*dblError;
dofydoink 0:20018747657d 457
dofydoink 0:20018747657d 458 //limit integral term
dofydoink 0:20018747657d 459 if (dblIntTerm > dblIntLimit)
dofydoink 0:20018747657d 460 {
dofydoink 0:20018747657d 461 dblIntTerm = dblIntLimit;
dofydoink 0:20018747657d 462 }
dofydoink 0:20018747657d 463 if (dblIntTerm < -1.0*dblIntLimit)
dofydoink 0:20018747657d 464 {
dofydoink 0:20018747657d 465 dblIntTerm = (double) -1.0*dblIntLimit;
dofydoink 0:20018747657d 466 }
dofydoink 0:20018747657d 467
dofydoink 0:20018747657d 468 if(fabs(dblError) <0.01)
dofydoink 0:20018747657d 469 {
dofydoink 0:20018747657d 470 dblError = 0.0;
dofydoink 0:20018747657d 471 dblErrorDot = 0.0;
dofydoink 0:20018747657d 472
dofydoink 0:20018747657d 473 }
dofydoink 0:20018747657d 474
dofydoink 0:20018747657d 475 if (fabs(dblErrorDot) < 0.1)
dofydoink 0:20018747657d 476 {
dofydoink 0:20018747657d 477 dblErrorDot = 0.0;
dofydoink 0:20018747657d 478 }
dofydoink 0:20018747657d 479
dofydoink 0:20018747657d 480 //calculate output
dofydoink 0:20018747657d 481 output[0] = Kp*dblError + dblIntTerm + Kd*dblErrorDot;
dofydoink 0:20018747657d 482
dofydoink 0:20018747657d 483 //limit output
dofydoink 0:20018747657d 484 if (output[0] > 0.95)
dofydoink 0:20018747657d 485 {
dofydoink 0:20018747657d 486 output[0] = 0.95;
dofydoink 0:20018747657d 487 }
dofydoink 0:20018747657d 488 if (output[0] < -0.95)
dofydoink 0:20018747657d 489 {
dofydoink 0:20018747657d 490 output[0] = -0.95;
dofydoink 0:20018747657d 491 }
dofydoink 0:20018747657d 492
dofydoink 0:20018747657d 493 if(intOutFilOrder>0)
dofydoink 0:20018747657d 494 {
dofydoink 0:20018747657d 495 for (ii = 1; ii < intOutFilOrder+1; ii++)
dofydoink 0:20018747657d 496 {
dofydoink 0:20018747657d 497 output[ii] = 0.7*output[ii-1] + 0.3*output[ii];
dofydoink 0:20018747657d 498 }
dofydoink 0:20018747657d 499 }
dofydoink 0:20018747657d 500 else
dofydoink 0:20018747657d 501 {
dofydoink 0:20018747657d 502 output[intOutFilOrder] = output[0];
dofydoink 0:20018747657d 503 }
dofydoink 0:20018747657d 504
dofydoink 0:20018747657d 505 //limit current
dofydoink 0:20018747657d 506 if (analogReadingCurSens> CurrentLimitSet)
dofydoink 0:20018747657d 507 {
dofydoink 0:20018747657d 508 currentBuck = CurrentLimitSet / analogReadingCurSens / currentBuckGain;
dofydoink 0:20018747657d 509 }
dofydoink 0:20018747657d 510 else
dofydoink 0:20018747657d 511 {
dofydoink 0:20018747657d 512 currentBuck = 1.0;
dofydoink 0:20018747657d 513 }
dofydoink 0:20018747657d 514
dofydoink 0:20018747657d 515 output[intOutFilOrder] = currentBuck*output[intOutFilOrder];
dofydoink 0:20018747657d 516 //end Current limit
dofydoink 0:20018747657d 517
dofydoink 0:20018747657d 518 //find direction
dofydoink 0:20018747657d 519 if(output[intOutFilOrder] >=0.0)
dofydoink 0:20018747657d 520 {
dofydoink 0:20018747657d 521 pinDirectionFwd = 1;
dofydoink 0:20018747657d 522 pinDirectionRev = 0;
dofydoink 0:20018747657d 523 dblControlBias = 0.0;
dofydoink 0:20018747657d 524 }
dofydoink 0:20018747657d 525 else
dofydoink 0:20018747657d 526 {
dofydoink 0:20018747657d 527 pinDirectionFwd = 0;
dofydoink 0:20018747657d 528 pinDirectionRev = 1;
dofydoink 0:20018747657d 529 dblControlBias = 0.0;
dofydoink 0:20018747657d 530 }
dofydoink 0:20018747657d 531
dofydoink 0:20018747657d 532 pinPwmOutput.write(abs(output[intOutFilOrder])+dblControlBias);
dofydoink 0:20018747657d 533
dofydoink 0:20018747657d 534 //update all past variables
dofydoink 0:20018747657d 535 dblLastPos = dblPos[intPosFilOrder];
dofydoink 0:20018747657d 536 dblLastPosD = dblPosD[intDemPosFilOrder];
dofydoink 0:20018747657d 537
dofydoink 0:20018747657d 538 Mntr = 0;
dofydoink 0:20018747657d 539
dofydoink 0:20018747657d 540 //GateControl();
dofydoink 0:20018747657d 541 gateTimer.reset();
dofydoink 0:20018747657d 542 pinGate = 1;
dofydoink 0:20018747657d 543
dofydoink 0:20018747657d 544 while((gateTimer.read_us() < 600) && pinGate == 1)
dofydoink 0:20018747657d 545 {
dofydoink 0:20018747657d 546 if(slave.receive())
dofydoink 0:20018747657d 547 {
dofydoink 0:20018747657d 548 //receive first msg
dofydoink 0:20018747657d 549 slaveReceivePos = slave.read(); //reply with 0xFFFF
dofydoink 0:20018747657d 550
dofydoink 0:20018747657d 551 slave.reply(intFeedBack_pos);//prepare position reply
dofydoink 0:20018747657d 552 slaveReceiveVel = slave.read();//get next message
dofydoink 0:20018747657d 553
dofydoink 0:20018747657d 554 //intFeedBack = intPressureRead & 0xFFFF;
dofydoink 0:20018747657d 555 slave.reply(intFeedBack_pres); //prepare pressure reply
dofydoink 0:20018747657d 556
dofydoink 0:20018747657d 557 slave.reply(0xFFFF);
dofydoink 0:20018747657d 558 pinGate = 0;
dofydoink 0:20018747657d 559
dofydoink 0:20018747657d 560 //deal with position
dofydoink 0:20018747657d 561 //find parity & checkSum
dofydoink 0:20018747657d 562 intPar_pos = EvenParityBitGen(slaveReceivePos>>1);
dofydoink 0:20018747657d 563 intChkSum_pos = SumDigits(slaveReceivePos>>7);
dofydoink 0:20018747657d 564
dofydoink 0:20018747657d 565 //check if parity, check Sum and mesage type matches
dofydoink 0:20018747657d 566 if((intPar_pos == (slaveReceivePos&0x1))&&(intChkSum_pos == ((slaveReceivePos>>2)&0x1F))&&( ( (slaveReceivePos>>1) &0x1) ) == 0) {
dofydoink 0:20018747657d 567 slaveReceivePos = slaveReceivePos>>7;
dofydoink 0:20018747657d 568 dblTargetPos = (double) dblMaxPos*slaveReceivePos/511;
dofydoink 0:20018747657d 569
dofydoink 0:20018747657d 570 //limit demand to ensure safety
dofydoink 0:20018747657d 571 if(dblTargetPos > dblMaxPos)
dofydoink 0:20018747657d 572 {
dofydoink 0:20018747657d 573 dblTargetPos = dblMaxPos;
dofydoink 0:20018747657d 574 }
dofydoink 0:20018747657d 575 if(dblTargetPos < 0.0)
dofydoink 0:20018747657d 576 {
dofydoink 0:20018747657d 577 dblTargetPos = 0.0;
dofydoink 0:20018747657d 578 }
dofydoink 0:20018747657d 579 }
dofydoink 0:20018747657d 580
dofydoink 0:20018747657d 581 //deal with velocity
dofydoink 0:20018747657d 582 //find parity & checkSum
dofydoink 0:20018747657d 583 intPar_vel = EvenParityBitGen(slaveReceiveVel>>1);
dofydoink 0:20018747657d 584 intChkSum_vel = SumDigits(slaveReceiveVel>>7);
dofydoink 0:20018747657d 585
dofydoink 0:20018747657d 586 //check if parity, check Sum and mesage type matches
dofydoink 0:20018747657d 587 if((intPar_vel == (slaveReceiveVel&0x1))&&(intChkSum_vel == ((slaveReceiveVel>>2)&0x1F))&&( ( (slaveReceiveVel>>1) &0x1) ) == 0) {
dofydoink 0:20018747657d 588 slaveReceiveVel = slaveReceiveVel>>7;
dofydoink 0:20018747657d 589 dblTargetVel = (double) MAX_SPEED_MMPS*slaveReceiveVel/511;
dofydoink 0:20018747657d 590
dofydoink 0:20018747657d 591 //limit demand to ensure safety
dofydoink 0:20018747657d 592 if(dblTargetVel > MAX_SPEED_MMPS)
dofydoink 0:20018747657d 593 {
dofydoink 0:20018747657d 594 dblTargetVel = MAX_SPEED_MMPS;
dofydoink 0:20018747657d 595 }
dofydoink 0:20018747657d 596 if(dblTargetVel < 0.0)
dofydoink 0:20018747657d 597 {
dofydoink 0:20018747657d 598 dblTargetVel = 0.0;
dofydoink 0:20018747657d 599 }
dofydoink 0:20018747657d 600 }
dofydoink 0:20018747657d 601
dofydoink 0:20018747657d 602
dofydoink 0:20018747657d 603
dofydoink 0:20018747657d 604
dofydoink 0:20018747657d 605 // dataFlag = slaveReceive>>13;
dofydoink 0:20018747657d 606 //
dofydoink 0:20018747657d 607 // if(dataFlag == 0)
dofydoink 0:20018747657d 608 // {
dofydoink 0:20018747657d 609 // slaveReceive = slaveReceive>>7 & 0x1FF;
dofydoink 0:20018747657d 610 // //get demand and filter
dofydoink 0:20018747657d 611 // dblPosD[intDemPosFilOrder] = (double) dblMaxPos*slaveReceive/511;
dofydoink 0:20018747657d 612 //
dofydoink 0:20018747657d 613 // //limit demand to ensure safety
dofydoink 0:20018747657d 614 // if(dblPosD[intDemPosFilOrder] > dblMaxPos)
dofydoink 0:20018747657d 615 // {
dofydoink 0:20018747657d 616 // dblPosD[intDemPosFilOrder] = dblMaxPos;
dofydoink 0:20018747657d 617 // }
dofydoink 0:20018747657d 618 // if(dblPosD[intDemPosFilOrder] < 0.0)
dofydoink 0:20018747657d 619 // {
dofydoink 0:20018747657d 620 // dblPosD[intDemPosFilOrder] = 0.0;
dofydoink 0:20018747657d 621 // }
dofydoink 0:20018747657d 622 // }
dofydoink 0:20018747657d 623
dofydoink 0:20018747657d 624 }
dofydoink 0:20018747657d 625 }
dofydoink 0:20018747657d 626 pinGate = 0;
dofydoink 0:20018747657d 627
dofydoink 0:20018747657d 628
dofydoink 0:20018747657d 629 }
dofydoink 0:20018747657d 630 }
dofydoink 0:20018747657d 631
dofydoink 0:20018747657d 632
dofydoink 0:20018747657d 633 //configure all control parameters
dofydoink 0:20018747657d 634 void ControlParameterConfig()
dofydoink 0:20018747657d 635 {
dofydoink 0:20018747657d 636 Kp = Kp/dblMotorVoltage;
dofydoink 0:20018747657d 637 Kd = Kd/dblSampleTime_s/dblMotorVoltage;
dofydoink 0:20018747657d 638 Ki = Ki*dblSampleTime_s/dblMotorVoltage;
dofydoink 0:20018747657d 639
dofydoink 0:20018747657d 640 dblPosConv = dblMaxPos/0.8;
dofydoink 0:20018747657d 641 }
dofydoink 0:20018747657d 642
dofydoink 0:20018747657d 643 Ticker debugTicker;
dofydoink 0:20018747657d 644
dofydoink 0:20018747657d 645 void startPositionControl()
dofydoink 0:20018747657d 646 {
dofydoink 0:20018747657d 647 semPosCtrl.release();
dofydoink 0:20018747657d 648 }
dofydoink 0:20018747657d 649
dofydoink 0:20018747657d 650 void startDebug()
dofydoink 0:20018747657d 651 {
dofydoink 0:20018747657d 652 semDebug.release();
dofydoink 0:20018747657d 653 }
dofydoink 0:20018747657d 654
dofydoink 0:20018747657d 655 Ticker positionCtrlTicker;
dofydoink 0:20018747657d 656
dofydoink 0:20018747657d 657 int main() {
dofydoink 0:20018747657d 658 cs_ADC = 1;
dofydoink 0:20018747657d 659
dofydoink 0:20018747657d 660 pinPwmOutput.period_us(50);
dofydoink 0:20018747657d 661 printf("\r\nYo Yo Yo, Low Level innit\r\n\n\n");
dofydoink 0:20018747657d 662 wait(0.5);
dofydoink 0:20018747657d 663
dofydoink 0:20018747657d 664 timer.start();
dofydoink 0:20018747657d 665 gateTimer.start();
dofydoink 0:20018747657d 666 //cs_ADC = 1;
dofydoink 0:20018747657d 667
dofydoink 0:20018747657d 668 ControlParameterConfig();
dofydoink 0:20018747657d 669 // for (ii = 0; ii< 10; ii++)
dofydoink 0:20018747657d 670 // {
dofydoink 0:20018747657d 671 // uintPotRead = Read14BitADC(3, cs_ADC);//read potentiometer
dofydoink 0:20018747657d 672 // dblStartingPos = (double) POT_2_MM*uintPotRead - dblPosBias;
dofydoink 0:20018747657d 673 // }
dofydoink 0:20018747657d 674 //
dofydoink 0:20018747657d 675 slave.format(16,2);
dofydoink 0:20018747657d 676 slave.frequency(10000000);
dofydoink 0:20018747657d 677 dblPosD[intDemPosFilOrder] = 0.0;
dofydoink 0:20018747657d 678 slaveReceivePos = 0.0;
dofydoink 0:20018747657d 679 slaveReceiveVel = 0.0;
dofydoink 0:20018747657d 680 wait(0.1);
dofydoink 0:20018747657d 681 ADC_Config();
dofydoink 0:20018747657d 682 wait(0.1);
dofydoink 0:20018747657d 683 ADC_Config();
dofydoink 0:20018747657d 684 wait(0.1);
dofydoink 0:20018747657d 685 intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dofydoink 0:20018747657d 686 //intPotRead = (Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dofydoink 0:20018747657d 687 dblStartingPos = (double) POT_2_MM*(intPotRead - POT_OFFSET);
dofydoink 0:20018747657d 688 dblTargetPos = dblStartingPos;
dofydoink 0:20018747657d 689 dblPosD[intDemPosFilOrder] = dblStartingPos;
dofydoink 0:20018747657d 690
dofydoink 0:20018747657d 691 printf("\r\n%d, %f\r\n", intPotRead, dblStartingPos);
dofydoink 0:20018747657d 692 //wait(0.05);
dofydoink 0:20018747657d 693
dofydoink 0:20018747657d 694
dofydoink 0:20018747657d 695 //printf("\n\n\n");
dofydoink 0:20018747657d 696
dofydoink 0:20018747657d 697 //dblStartingPos = (double) POT_2_MM*(uintPotRead - POT_OFFSET);
dofydoink 0:20018747657d 698 timerDutyCycle.start();
dofydoink 0:20018747657d 699
dofydoink 0:20018747657d 700 //calculate/convert variables
dofydoink 0:20018747657d 701
dofydoink 0:20018747657d 702 CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3;
dofydoink 0:20018747657d 703 PositionControlThread.start(PositionControlPID);
dofydoink 0:20018747657d 704 DutyCycleThread.start(CalculateDutyCycle);
dofydoink 0:20018747657d 705 positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s);
dofydoink 0:20018747657d 706
dofydoink 0:20018747657d 707 intFeedBack_pos = 0;
dofydoink 0:20018747657d 708 intFeedBack_pres = 0;
dofydoink 0:20018747657d 709
dofydoink 0:20018747657d 710
dofydoink 0:20018747657d 711 while(1)
dofydoink 0:20018747657d 712 {
dofydoink 0:20018747657d 713 Thread::wait(osWaitForever);
dofydoink 0:20018747657d 714 }
dofydoink 0:20018747657d 715 }
dofydoink 0:20018747657d 716
dofydoink 0:20018747657d 717
dofydoink 0:20018747657d 718