MorphGI / Mbed 2 deprecated MGI_Rebuild_LowLevel_v7_PressureCtrl_PWM

Pressure control for drive segment in rebuild of control unit.

Dependencies:   mbed QEI FastAnalogIn mbed-rtos FastPWM

main.cpp@2:aee7d4724915, 2019-01-28 (annotated)

Committer:
dofydoink
Date:
Mon Jan 28 15:30:36 2019 +0000
Revision:
2:aee7d4724915
Parent:
1:cb2859df7a4c
Child:
3:9bd35e5b05ba
last working version

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