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@6:6142902e20d1, 2021-06-25 (annotated)

Committer:
dofydoink
Date:
Fri Jun 25 07:57:57 2021 +0000
Revision:
6:6142902e20d1
Parent:
5:4e710cef655e 
Working pressure control low level code;

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 6:6142902e20d1 20 int intDummy;
dofydoink 6:6142902e20d1 21 //SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel
dofydoink 0:20018747657d 22
dofydoink 6:6142902e20d1 23 //InterruptIn pinPWMin(PA_6);
dofydoink 6:6142902e20d1 24 InterruptIn pinPWMin_pos(PA_8);
dofydoink 6:6142902e20d1 25 InterruptIn pinPWMin_vel(PA_6);
dofydoink 6:6142902e20d1 26 QEI wheel (PB_5, PB_4, NC, 128, QEI::X2_ENCODING);
dofydoink 0:20018747657d 27
dofydoink 6:6142902e20d1 28 //DigitalOut Mntr(D3);
dofydoink 6:6142902e20d1 29 //DigitalIn swDem(D2);
dofydoink 6:6142902e20d1 30 //DigitalOut Mntr2(PB_8);
dofydoink 0:20018747657d 31
dofydoink 3:9bd35e5b05ba 32 #define EXTERNAL_CLOCK_MODE 0x08
dofydoink 3:9bd35e5b05ba 33 #define RANGE_CONFIG 0x03
dofydoink 0:20018747657d 34
dofydoink 0:20018747657d 35 #define EXTERNAL_CLOCK_MODE 0x08
dofydoink 0:20018747657d 36 #define RANGE_CONFIG 0x03 //config for 1.5*Vref = 6.144V
dofydoink 0:20018747657d 37
dofydoink 0:20018747657d 38 #define PRESSURE_BIAS_VOLTAGE 0.15151515151515
dofydoink 3:9bd35e5b05ba 39
dofydoink 3:9bd35e5b05ba 40 const double MAX_ACTUATOR_LENGTH = 52.2;
dofydoink 3:9bd35e5b05ba 41
dofydoink 6:6142902e20d1 42 //const double MAX_POSITION_MM = 40.0; //maximum actuator position position in mm
dofydoink 6:6142902e20d1 43 const double MAX_POSITION_MM = 8.0; //maximum actuator position position in [bar] because this is a total hack. I've replaced position readings with pressure
dofydoink 6:6142902e20d1 44 const double MAX_SPEED_MMPS = 4.0;
dofydoink 3:9bd35e5b05ba 45 //sample time variables
dofydoink 3:9bd35e5b05ba 46 double dblSampleTime_s = 0.001;
dofydoink 0:20018747657d 47
dofydoink 0:20018747657d 48 #define POT_2_MM 0.006750412 //convert potentiometer reading to mm (Tested and is right)
dofydoink 0:20018747657d 49 #define POT_OFFSET 7500//6666
dofydoink 0:20018747657d 50
dofydoink 0:20018747657d 51 Serial pc(USBTX, USBRX); // tx, rx
dofydoink 0:20018747657d 52
dofydoink 0:20018747657d 53 Thread PositionControlThread(osPriorityHigh);
dofydoink 0:20018747657d 54 Thread DebugThread(osPriorityNormal);
dofydoink 0:20018747657d 55 Thread GateControlThread(osPriorityNormal);
dofydoink 0:20018747657d 56
dofydoink 0:20018747657d 57 Thread DutyCycleThread(osPriorityRealtime);
dofydoink 6:6142902e20d1 58 Mutex mutDutyCycle_pos;
dofydoink 6:6142902e20d1 59 Mutex mutDutyCycle_vel;
dofydoink 0:20018747657d 60 Semaphore semDutyCycle(1);
dofydoink 0:20018747657d 61 Timer timerDutyCycle;
dofydoink 0:20018747657d 62
dofydoink 0:20018747657d 63 double dblSensorDriftError;
dofydoink 0:20018747657d 64
dofydoink 0:20018747657d 65 short randomvar;
dofydoink 0:20018747657d 66
dofydoink 0:20018747657d 67 volatile int dataRx;
dofydoink 0:20018747657d 68
dofydoink 0:20018747657d 69
dofydoink 0:20018747657d 70 Semaphore semPosCtrl(1);
dofydoink 0:20018747657d 71 Semaphore semDebug(1);
dofydoink 0:20018747657d 72 Semaphore semGate(1);
dofydoink 0:20018747657d 73
dofydoink 0:20018747657d 74 Timer timer;
dofydoink 0:20018747657d 75
dofydoink 0:20018747657d 76 long pulsesTest;
dofydoink 0:20018747657d 77
dofydoink 0:20018747657d 78 //define all pins
dofydoink 3:9bd35e5b05ba 79 SPI spi(PB_15, PB_14, PB_13); // mosi, miso, sclk //DO NOT USE D4 OR D5 !!!
dofydoink 3:9bd35e5b05ba 80
dofydoink 3:9bd35e5b05ba 81 DigitalOut cs_ADC(PB_12);
dofydoink 3:9bd35e5b05ba 82
dofydoink 6:6142902e20d1 83 //DigitalOut pinGate(PA_8);
dofydoink 3:9bd35e5b05ba 84
dofydoink 6:6142902e20d1 85 //AnalogIn pinDemand1(PA_0);
dofydoink 6:6142902e20d1 86 //AnalogIn pinDemand2(PA_1);
dofydoink 3:9bd35e5b05ba 87
dofydoink 3:9bd35e5b05ba 88 AnalogIn pinCurSense(PB_0);
dofydoink 6:6142902e20d1 89 AnalogIn pinPresSens(PC_0);
dofydoink 0:20018747657d 90
dofydoink 3:9bd35e5b05ba 91 FastPWM pinPwmOutput(PC_8);
dofydoink 3:9bd35e5b05ba 92
dofydoink 6:6142902e20d1 93 FastPWM pinAltOutB(PA_0);
dofydoink 6:6142902e20d1 94 FastPWM pinAltOutA(PA_1);
dofydoink 6:6142902e20d1 95
dofydoink 6:6142902e20d1 96 //FastPWM pinSigOut(D2);
dofydoink 3:9bd35e5b05ba 97
dofydoink 3:9bd35e5b05ba 98 DigitalOut pinDirectionFwd(PA_13);//INB
dofydoink 3:9bd35e5b05ba 99 DigitalOut pinDirectionRev(PA_14);//INA
dofydoink 1:cb2859df7a4c 100
dofydoink 3:9bd35e5b05ba 101 //define all variables
dofydoink 3:9bd35e5b05ba 102
dofydoink 3:9bd35e5b05ba 103 int ii,jj,kk,nn, spiTick; //counting variables
dofydoink 3:9bd35e5b05ba 104
dofydoink 3:9bd35e5b05ba 105 volatile int slaveReceivePos;
dofydoink 3:9bd35e5b05ba 106 volatile int slaveReceiveVel;
dofydoink 0:20018747657d 107
dofydoink 3:9bd35e5b05ba 108 volatile int dataFlag;
dofydoink 3:9bd35e5b05ba 109 volatile int intFeedBack_pos;
dofydoink 3:9bd35e5b05ba 110 volatile int intFeedBack_pres;
dofydoink 3:9bd35e5b05ba 111
dofydoink 3:9bd35e5b05ba 112 int intChkSum_pos;
dofydoink 3:9bd35e5b05ba 113 int intChkSum_vel;
dofydoink 3:9bd35e5b05ba 114
dofydoink 3:9bd35e5b05ba 115 int intPar_pos;
dofydoink 3:9bd35e5b05ba 116 int intPar_vel;
dofydoink 3:9bd35e5b05ba 117
dofydoink 3:9bd35e5b05ba 118 //raw Data Readings
dofydoink 3:9bd35e5b05ba 119 int intPotRead = 0;
dofydoink 3:9bd35e5b05ba 120 int intPressureRead = 0;
dofydoink 3:9bd35e5b05ba 121 double analogReadingDemPos;
dofydoink 3:9bd35e5b05ba 122 double analogReadingDemVel;
dofydoink 3:9bd35e5b05ba 123 double analogReadingCurSens;
dofydoink 3:9bd35e5b05ba 124
dofydoink 3:9bd35e5b05ba 125 char buf[10];
dofydoink 3:9bd35e5b05ba 126 int dataReceived;
dofydoink 3:9bd35e5b05ba 127
dofydoink 3:9bd35e5b05ba 128
dofydoink 3:9bd35e5b05ba 129
dofydoink 3:9bd35e5b05ba 130
dofydoink 3:9bd35e5b05ba 131 //system state variables
dofydoink 3:9bd35e5b05ba 132 double dblPos[10];//current position in mm
dofydoink 3:9bd35e5b05ba 133 double dblPosFil[10];//current position in mm
dofydoink 3:9bd35e5b05ba 134 double dblLastPos;//previous position in mm
dofydoink 3:9bd35e5b05ba 135 double dblVel[10];//velocity in mm/s
dofydoink 3:9bd35e5b05ba 136 double dblAccel; //acceleration in mm/s^2
dofydoink 3:9bd35e5b05ba 137 double dblIntegral = 0;//integral of position;
dofydoink 3:9bd35e5b05ba 138
dofydoink 6:6142902e20d1 139 double dblPres; //current pressure in bar
dofydoink 6:6142902e20d1 140 double dblPresFil[10]; //current pressure after filter in bar
dofydoink 6:6142902e20d1 141 double dblPresDot; //time derivative of pressure
dofydoink 6:6142902e20d1 142 double dblPresDotFil[10]; //filtered time derivative of pressure
dofydoink 6:6142902e20d1 143 double dblLastPres; //previous pressure
dofydoink 6:6142902e20d1 144 double dblLastPresDot;
dofydoink 6:6142902e20d1 145 double dblIntPres = 0.0; //time integral of pressure
dofydoink 6:6142902e20d1 146
dofydoink 6:6142902e20d1 147
dofydoink 3:9bd35e5b05ba 148 double dblPotPositionRead;
dofydoink 3:9bd35e5b05ba 149
dofydoink 3:9bd35e5b05ba 150 double dblPosBias = 48.0;
dofydoink 3:9bd35e5b05ba 151
dofydoink 3:9bd35e5b05ba 152 double dblStartingPos;
dofydoink 3:9bd35e5b05ba 153
dofydoink 3:9bd35e5b05ba 154 //demand variables
dofydoink 3:9bd35e5b05ba 155 double dblPosD[10];
dofydoink 3:9bd35e5b05ba 156 double dblLastPosD;
dofydoink 3:9bd35e5b05ba 157 double dblVelD[10];
dofydoink 3:9bd35e5b05ba 158 double dblAccelD;
dofydoink 3:9bd35e5b05ba 159
dofydoink 6:6142902e20d1 160 double dblPresD;
dofydoink 6:6142902e20d1 161 double dblPresDotD;
dofydoink 6:6142902e20d1 162
dofydoink 6:6142902e20d1 163 double dblTargetPos = 0;
dofydoink 6:6142902e20d1 164 double dblTargetVel =0;
dofydoink 6:6142902e20d1 165 double dblLinearPath = 0;
dofydoink 6:6142902e20d1 166 double dblLastLinearPath = 0;
dofydoink 3:9bd35e5b05ba 167 double dblPathSign;
dofydoink 3:9bd35e5b05ba 168
dofydoink 3:9bd35e5b05ba 169
dofydoink 3:9bd35e5b05ba 170
dofydoink 3:9bd35e5b05ba 171 double dblError;
dofydoink 3:9bd35e5b05ba 172 double dblLastError;
dofydoink 3:9bd35e5b05ba 173 double dblErrorDot;
dofydoink 3:9bd35e5b05ba 174
dofydoink 3:9bd35e5b05ba 175 //filter Variables
dofydoink 4:3bab17dfae4e 176 int intPosFilOrder = 0;
dofydoink 6:6142902e20d1 177 int intPresFilOrder = 4;
dofydoink 6:6142902e20d1 178 int intPresDotFilOrder = 4;
dofydoink 3:9bd35e5b05ba 179 int intVelFilOrder = 1;
dofydoink 3:9bd35e5b05ba 180 int intDemPosFilOrder = 6;
dofydoink 3:9bd35e5b05ba 181 int intDemVelFilOrder = 6;
dofydoink 3:9bd35e5b05ba 182 int intOutFilOrder = 0;
dofydoink 3:9bd35e5b05ba 183
dofydoink 6:6142902e20d1 184 double dblPressureVal_norm;
dofydoink 3:9bd35e5b05ba 185 double dblPressureVal_bar;
dofydoink 3:9bd35e5b05ba 186
dofydoink 3:9bd35e5b05ba 187 //controller variables
dofydoink 3:9bd35e5b05ba 188 double omega;
dofydoink 3:9bd35e5b05ba 189 double zeta;
dofydoink 3:9bd35e5b05ba 190
dofydoink 6:6142902e20d1 191 double Kp = 130.0;
dofydoink 6:6142902e20d1 192 double Ki = 0.0;
dofydoink 6:6142902e20d1 193 double Kd = 0.0;
dofydoink 3:9bd35e5b05ba 194 double dblIntTerm;
dofydoink 6:6142902e20d1 195 double dblIntLimit = 1.0;
dofydoink 3:9bd35e5b05ba 196 int RXFlag;
dofydoink 3:9bd35e5b05ba 197 double dblControlBias = 0.0;
dofydoink 3:9bd35e5b05ba 198
dofydoink 6:6142902e20d1 199 double dblMotorVoltage = 24.0;
dofydoink 0:20018747657d 200
dofydoink 3:9bd35e5b05ba 201 double output[10];//controller output
dofydoink 3:9bd35e5b05ba 202
dofydoink 3:9bd35e5b05ba 203 //current limit variables
dofydoink 3:9bd35e5b05ba 204 double CurrentLimitSet;
dofydoink 3:9bd35e5b05ba 205 double dblCurrentLimitAmps = 3.0;
dofydoink 3:9bd35e5b05ba 206 double currentBuck;
dofydoink 3:9bd35e5b05ba 207 double currentBuckGain = 3.0;
dofydoink 3:9bd35e5b05ba 208
dofydoink 6:6142902e20d1 209 //Pressure Limitation
dofydoink 6:6142902e20d1 210 const double dblPressureLimitBar = 10.0;
dofydoink 6:6142902e20d1 211 double dblPressureLimitGain = 2.0;
dofydoink 6:6142902e20d1 212 double dblPressureLimitBuck;
dofydoink 6:6142902e20d1 213 double dblPressureLimitDerivativeGain = 0.1;
dofydoink 6:6142902e20d1 214 double dblLastErrorPressure;
dofydoink 6:6142902e20d1 215 double dblErrorPressure;
dofydoink 6:6142902e20d1 216 double dblDeltaErrorPressure;
dofydoink 6:6142902e20d1 217
dofydoink 6:6142902e20d1 218 int intT_pos= 100;
dofydoink 6:6142902e20d1 219 int intDeltaT_pos =0;
dofydoink 6:6142902e20d1 220 int intT_vel= 100;
dofydoink 6:6142902e20d1 221 int intDeltaT_vel =0;
dofydoink 6:6142902e20d1 222
dofydoink 6:6142902e20d1 223 Timer timerPeriod_pos;
dofydoink 6:6142902e20d1 224 Timer timerDutyCycle_pos;
dofydoink 6:6142902e20d1 225 Timer timerPeriod_vel;
dofydoink 6:6142902e20d1 226 Timer timerDutyCycle_vel;
dofydoink 6:6142902e20d1 227
dofydoink 6:6142902e20d1 228 bool isFallWorking = 0;
dofydoink 6:6142902e20d1 229 bool isRiseWorking = 0;
dofydoink 6:6142902e20d1 230
dofydoink 6:6142902e20d1 231 const int intPeriod_us = 2000;
dofydoink 6:6142902e20d1 232
dofydoink 6:6142902e20d1 233 void PWMRise_pos() {
dofydoink 6:6142902e20d1 234 mutDutyCycle_pos.lock();
dofydoink 6:6142902e20d1 235 intT_pos = timerPeriod_pos.read_us();
dofydoink 6:6142902e20d1 236 timerPeriod_pos.reset();
dofydoink 6:6142902e20d1 237 timerDutyCycle_pos.reset();
dofydoink 6:6142902e20d1 238 mutDutyCycle_pos.unlock();
dofydoink 6:6142902e20d1 239 isRiseWorking = 1;
dofydoink 6:6142902e20d1 240 }
dofydoink 6:6142902e20d1 241
dofydoink 6:6142902e20d1 242 void PWMFall_pos() {
dofydoink 6:6142902e20d1 243 mutDutyCycle_pos.lock();
dofydoink 6:6142902e20d1 244 intDeltaT_pos = timerDutyCycle_pos.read_us();
dofydoink 6:6142902e20d1 245 mutDutyCycle_pos.unlock();
dofydoink 6:6142902e20d1 246 isFallWorking = 1;
dofydoink 6:6142902e20d1 247 }
dofydoink 6:6142902e20d1 248
dofydoink 6:6142902e20d1 249 void PWMRise_vel() {
dofydoink 6:6142902e20d1 250 mutDutyCycle_vel.lock();
dofydoink 6:6142902e20d1 251 intT_vel = timerPeriod_vel.read_us();
dofydoink 6:6142902e20d1 252 timerPeriod_vel.reset();
dofydoink 6:6142902e20d1 253 timerDutyCycle_vel.reset();
dofydoink 6:6142902e20d1 254 mutDutyCycle_vel.unlock();
dofydoink 6:6142902e20d1 255 }
dofydoink 6:6142902e20d1 256
dofydoink 6:6142902e20d1 257 void PWMFall_vel() {
dofydoink 6:6142902e20d1 258 mutDutyCycle_vel.lock();
dofydoink 6:6142902e20d1 259 intDeltaT_vel = timerDutyCycle_vel.read_us();
dofydoink 6:6142902e20d1 260 mutDutyCycle_vel.unlock();
dofydoink 6:6142902e20d1 261 }
dofydoink 6:6142902e20d1 262
dofydoink 6:6142902e20d1 263 double CalculateDemand(Mutex mutex, int intT, int intDeltaT, int intPeriod) {
dofydoink 6:6142902e20d1 264 mutex.lock();
dofydoink 6:6142902e20d1 265 int _intT = intT;
dofydoink 6:6142902e20d1 266 int _intDeltaT = intDeltaT;
dofydoink 6:6142902e20d1 267 mutex.unlock();
dofydoink 6:6142902e20d1 268 double dblOutput, dblDutyCycle;
dofydoink 6:6142902e20d1 269 if(_intT > 0){
dofydoink 6:6142902e20d1 270 if(_intT>(intPeriod_us-10) && _intT <(intPeriod_us+10)){
dofydoink 6:6142902e20d1 271 dblDutyCycle = (double)_intDeltaT/_intT;
dofydoink 6:6142902e20d1 272
dofydoink 6:6142902e20d1 273 dblDutyCycle -= 0.1;
dofydoink 6:6142902e20d1 274 dblDutyCycle /=0.8;
dofydoink 6:6142902e20d1 275 if(dblDutyCycle >1.0){
dofydoink 6:6142902e20d1 276 dblDutyCycle = 1.0;
dofydoink 6:6142902e20d1 277 }
dofydoink 6:6142902e20d1 278 if(dblDutyCycle <0.0){
dofydoink 6:6142902e20d1 279 dblDutyCycle = 0.0;
dofydoink 6:6142902e20d1 280 }
dofydoink 6:6142902e20d1 281 }
dofydoink 6:6142902e20d1 282 }
dofydoink 6:6142902e20d1 283
dofydoink 6:6142902e20d1 284 int intInput = (int)(dblDutyCycle*400.0);
dofydoink 6:6142902e20d1 285 //dblTargetPos = (double)MAX_POSITION_MM*dblDutyCycle; //set target position (9-bit value)
dofydoink 6:6142902e20d1 286 dblOutput = (double)intInput/400.0; //set target position (9-bit value)
dofydoink 6:6142902e20d1 287 return dblOutput;
dofydoink 6:6142902e20d1 288 }
dofydoink 6:6142902e20d1 289
dofydoink 6:6142902e20d1 290 //Timer gateTimer;
dofydoink 3:9bd35e5b05ba 291
dofydoink 3:9bd35e5b05ba 292 //define custom Functions
dofydoink 3:9bd35e5b05ba 293
dofydoink 6:6142902e20d1 294 bool CheckMessage(int msg) {//checks if message was corrupted
dofydoink 3:9bd35e5b05ba 295 // Find message parity
dofydoink 3:9bd35e5b05ba 296 short int count = 0;
dofydoink 3:9bd35e5b05ba 297 for(short int i=0; i<32; i++) {
dofydoink 3:9bd35e5b05ba 298 if( msg>>1 & (1<<i) ) count++;
dofydoink 3:9bd35e5b05ba 299 }
dofydoink 3:9bd35e5b05ba 300 int intParity = !(count%2);
dofydoink 3:9bd35e5b05ba 301 // Find message CheckSum
dofydoink 3:9bd35e5b05ba 302 int intChkSum = 0;
dofydoink 3:9bd35e5b05ba 303 int intTempVar = msg>>7;
dofydoink 3:9bd35e5b05ba 304 while(intTempVar > 0) {
dofydoink 3:9bd35e5b05ba 305 intChkSum += intTempVar%10;
dofydoink 3:9bd35e5b05ba 306 intTempVar = int(intTempVar/10);
dofydoink 3:9bd35e5b05ba 307 }
dofydoink 3:9bd35e5b05ba 308 // Check if parity, CheckSum and mesage type match
dofydoink 3:9bd35e5b05ba 309 bool isParityCorrect = (intParity == (msg&0x1));
dofydoink 3:9bd35e5b05ba 310 bool isChkSumCorrect = (intChkSum == ((msg>>2)&0x1F));
dofydoink 3:9bd35e5b05ba 311 bool isCheckPassed = (isParityCorrect && isChkSumCorrect);
dofydoink 3:9bd35e5b05ba 312 return isCheckPassed;
dofydoink 3:9bd35e5b05ba 313 }
dofydoink 3:9bd35e5b05ba 314
dofydoink 6:6142902e20d1 315 ////////For Carafino: Start/////////
dofydoink 6:6142902e20d1 316
dofydoink 6:6142902e20d1 317
dofydoink 6:6142902e20d1 318
dofydoink 6:6142902e20d1 319
dofydoink 6:6142902e20d1 320
dofydoink 6:6142902e20d1 321 //Message checking (For Carafino)
dofydoink 6:6142902e20d1 322 //bool PerformSlaveSPI(SPISlave *spiSlave, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]) {//performs the SPI transaction
dofydoink 6:6142902e20d1 323 //
dofydoink 6:6142902e20d1 324 // unsigned int dummyMsg = 0x5555;
dofydoink 6:6142902e20d1 325 // bool isSuccess = true;
dofydoink 6:6142902e20d1 326 // unsigned int inboundMsg, typeBit;
dofydoink 6:6142902e20d1 327 // short int numPacketsReceived = 0;
dofydoink 6:6142902e20d1 328 // for(short int i=0; i<3; i++) { // Loop 3 times for 3 SPI messages
dofydoink 6:6142902e20d1 329 // while( gateTimer.read_us() < 500 ) {
dofydoink 6:6142902e20d1 330 // if( spiSlave->receive() ) {
dofydoink 6:6142902e20d1 331 // numPacketsReceived++;
dofydoink 6:6142902e20d1 332 //
dofydoink 6:6142902e20d1 333 // inboundMsg = spiSlave->read();
dofydoink 6:6142902e20d1 334 // //Mntr = 1;//dummy variable used to check function
dofydoink 6:6142902e20d1 335 // if(i==0) {
dofydoink 6:6142902e20d1 336 // spiSlave->reply(outboundMsgs[0]);
dofydoink 6:6142902e20d1 337 // } else if(i==1) {
dofydoink 6:6142902e20d1 338 // spiSlave->reply(outboundMsgs[1]);
dofydoink 6:6142902e20d1 339 // } else {
dofydoink 6:6142902e20d1 340 // spiSlave->reply(dummyMsg);
dofydoink 6:6142902e20d1 341 // }
dofydoink 6:6142902e20d1 342 // //Mntr = 0;
dofydoink 6:6142902e20d1 343 //
dofydoink 6:6142902e20d1 344 // if((unsigned int)inboundMsg != dummyMsg) { // Message is not dummy which is only used for reply
dofydoink 6:6142902e20d1 345 // typeBit = inboundMsg>>1 & 0x1;//extracts type bit from message, 0 = target Position; 1 = target Velocity
dofydoink 6:6142902e20d1 346 // inboundMsgsData[typeBit] = inboundMsg>>7 & 0x1FF;//this contains the data recieved from master
dofydoink 6:6142902e20d1 347 // if( !CheckMessage(inboundMsg) ) {
dofydoink 6:6142902e20d1 348 // isSuccess = false;
dofydoink 6:6142902e20d1 349 // }
dofydoink 6:6142902e20d1 350 // }
dofydoink 6:6142902e20d1 351 // break;
dofydoink 6:6142902e20d1 352 // }
dofydoink 6:6142902e20d1 353 // }
dofydoink 6:6142902e20d1 354 // }
dofydoink 6:6142902e20d1 355 // if( numPacketsReceived != 3 ) {//if it hasn't received three messages, it failed.
dofydoink 6:6142902e20d1 356 // isSuccess = false;
dofydoink 6:6142902e20d1 357 // }
dofydoink 6:6142902e20d1 358 // return isSuccess;
dofydoink 6:6142902e20d1 359 //}
dofydoink 6:6142902e20d1 360
dofydoink 6:6142902e20d1 361 ////////For Carafino: End/////////
dofydoink 3:9bd35e5b05ba 362
dofydoink 3:9bd35e5b05ba 363 int Read14BitADC(int channel, DigitalOut CSpin)
dofydoink 3:9bd35e5b05ba 364 {
dofydoink 3:9bd35e5b05ba 365 char message;
dofydoink 3:9bd35e5b05ba 366 unsigned int outputA;
dofydoink 3:9bd35e5b05ba 367 unsigned int outputB;
dofydoink 3:9bd35e5b05ba 368 int output;
dofydoink 3:9bd35e5b05ba 369
dofydoink 3:9bd35e5b05ba 370 switch(channel)
dofydoink 0:20018747657d 371 {
dofydoink 3:9bd35e5b05ba 372 case 1:
dofydoink 0:20018747657d 373 message = CHAN_1;
dofydoink 3:9bd35e5b05ba 374 break;
dofydoink 3:9bd35e5b05ba 375
dofydoink 3:9bd35e5b05ba 376 case 2:
dofydoink 3:9bd35e5b05ba 377 message = CHAN_2;
dofydoink 3:9bd35e5b05ba 378 break;
dofydoink 3:9bd35e5b05ba 379
dofydoink 3:9bd35e5b05ba 380 case 3:
dofydoink 3:9bd35e5b05ba 381 message = CHAN_3;
dofydoink 3:9bd35e5b05ba 382 break;
dofydoink 3:9bd35e5b05ba 383 case 4:
dofydoink 3:9bd35e5b05ba 384 message = CHAN_4;
dofydoink 3:9bd35e5b05ba 385 break;
dofydoink 3:9bd35e5b05ba 386
dofydoink 3:9bd35e5b05ba 387 default:
dofydoink 3:9bd35e5b05ba 388 message = CHAN_1;
dofydoink 0:20018747657d 389 }
dofydoink 0:20018747657d 390
dofydoink 3:9bd35e5b05ba 391 spi.format(8,0);
dofydoink 3:9bd35e5b05ba 392 spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
dofydoink 3:9bd35e5b05ba 393
dofydoink 3:9bd35e5b05ba 394 CSpin.write(0);
dofydoink 3:9bd35e5b05ba 395 spi.write(message);
dofydoink 3:9bd35e5b05ba 396 spi.write(0x00);
dofydoink 3:9bd35e5b05ba 397 outputA = spi.write(0x00);
dofydoink 3:9bd35e5b05ba 398 outputB = spi.write(0x00);
dofydoink 3:9bd35e5b05ba 399 CSpin.write(1);
dofydoink 3:9bd35e5b05ba 400
dofydoink 3:9bd35e5b05ba 401 //convert result to sensible value
dofydoink 3:9bd35e5b05ba 402 outputA = outputA<<8;
dofydoink 3:9bd35e5b05ba 403 output = outputA | outputB;
dofydoink 3:9bd35e5b05ba 404 output = output>>2;
dofydoink 3:9bd35e5b05ba 405
dofydoink 3:9bd35e5b05ba 406 return output;
dofydoink 3:9bd35e5b05ba 407 }
dofydoink 3:9bd35e5b05ba 408
dofydoink 3:9bd35e5b05ba 409 void ADC_Config()
dofydoink 3:9bd35e5b05ba 410 {
dofydoink 6:6142902e20d1 411 unsigned int msg;
dofydoink 3:9bd35e5b05ba 412 spi.format(8,0);
dofydoink 3:9bd35e5b05ba 413 spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
dofydoink 3:9bd35e5b05ba 414
dofydoink 3:9bd35e5b05ba 415 msg = CHAN_1 | RANGE_CONFIG; //config channel 1 set Vref as
dofydoink 3:9bd35e5b05ba 416 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 417 spi.write(msg);
dofydoink 3:9bd35e5b05ba 418 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 419
dofydoink 3:9bd35e5b05ba 420 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 421 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 3:9bd35e5b05ba 422 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 423
dofydoink 3:9bd35e5b05ba 424 msg = CHAN_2 | RANGE_CONFIG; //config channel 2
dofydoink 3:9bd35e5b05ba 425 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 426 spi.write(msg);
dofydoink 3:9bd35e5b05ba 427 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 428
dofydoink 3:9bd35e5b05ba 429 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 430 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 3:9bd35e5b05ba 431 cs_ADC = 1;
dofydoink 0:20018747657d 432
dofydoink 3:9bd35e5b05ba 433 msg = CHAN_3 | RANGE_CONFIG; //config channel 3
dofydoink 3:9bd35e5b05ba 434 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 435 spi.write(msg);
dofydoink 3:9bd35e5b05ba 436 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 437
dofydoink 3:9bd35e5b05ba 438 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 439 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 3:9bd35e5b05ba 440 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 441
dofydoink 3:9bd35e5b05ba 442 msg = CHAN_4 | RANGE_CONFIG; //config channel 4
dofydoink 3:9bd35e5b05ba 443 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 444 spi.write(msg);
dofydoink 3:9bd35e5b05ba 445 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 446
dofydoink 3:9bd35e5b05ba 447 cs_ADC = 0;
dofydoink 3:9bd35e5b05ba 448 spi.write(EXTERNAL_CLOCK_MODE);
dofydoink 3:9bd35e5b05ba 449 cs_ADC = 1;
dofydoink 3:9bd35e5b05ba 450 }
dofydoink 0:20018747657d 451
dofydoink 0:20018747657d 452
dofydoink 0:20018747657d 453 int SumDigits(int var)
dofydoink 0:20018747657d 454 {
dofydoink 0:20018747657d 455 int intSumResult = 0;
dofydoink 0:20018747657d 456 int intTempVar = var;
dofydoink 0:20018747657d 457 while (intTempVar >0)
dofydoink 0:20018747657d 458 {
dofydoink 0:20018747657d 459 intSumResult += intTempVar%10;
dofydoink 0:20018747657d 460 intTempVar = int(intTempVar/10);
dofydoink 0:20018747657d 461 //intSumResult += int(var/100)%100;
dofydoink 0:20018747657d 462 }
dofydoink 0:20018747657d 463 return intSumResult;
dofydoink 0:20018747657d 464 }
dofydoink 0:20018747657d 465
dofydoink 3:9bd35e5b05ba 466 //int EvenParityBitGen(int var)
dofydoink 3:9bd35e5b05ba 467 int OddParityBitGen(int var)
dofydoink 0:20018747657d 468 {
dofydoink 0:20018747657d 469 unsigned int count = 0, i, b = 1;
dofydoink 0:20018747657d 470
dofydoink 0:20018747657d 471 for(i = 0; i < 32; i++){
dofydoink 0:20018747657d 472 if( var & (b << i) ){count++;}
dofydoink 0:20018747657d 473 }
dofydoink 0:20018747657d 474
dofydoink 3:9bd35e5b05ba 475 if( (count % 2) ){
dofydoink 3:9bd35e5b05ba 476 return 0;
dofydoink 3:9bd35e5b05ba 477 }
dofydoink 0:20018747657d 478
dofydoink 0:20018747657d 479 return 1;
dofydoink 0:20018747657d 480 }
dofydoink 0:20018747657d 481
dofydoink 6:6142902e20d1 482 //void CloseGate()
dofydoink 6:6142902e20d1 483 //{
dofydoink 6:6142902e20d1 484 // pinGate = 0;
dofydoink 6:6142902e20d1 485 //
dofydoink 6:6142902e20d1 486 //}
dofydoink 6:6142902e20d1 487 //
dofydoink 6:6142902e20d1 488 double dblReadDem = 0;
dofydoink 6:6142902e20d1 489
dofydoink 3:9bd35e5b05ba 490 void PositionControlPID()
dofydoink 3:9bd35e5b05ba 491 {
dofydoink 6:6142902e20d1 492 while(1){
dofydoink 3:9bd35e5b05ba 493 semPosCtrl.wait();
dofydoink 6:6142902e20d1 494 //slave.reply(0x5555);
dofydoink 6:6142902e20d1 495 //Mntr2 = !Mntr2;
dofydoink 3:9bd35e5b05ba 496 //Mntr2 = 1 - Mntr2;//!led;
dofydoink 3:9bd35e5b05ba 497 pulsesTest = wheel.getPulses();
dofydoink 6:6142902e20d1 498
dofydoink 6:6142902e20d1 499 dblReadDem = (double)pulsesTest/256;
dofydoink 6:6142902e20d1 500
dofydoink 6:6142902e20d1 501 if (dblReadDem > 1.0){
dofydoink 6:6142902e20d1 502 dblReadDem = 1.0;
dofydoink 6:6142902e20d1 503 }
dofydoink 6:6142902e20d1 504 if (dblReadDem < 0){
dofydoink 6:6142902e20d1 505 dblReadDem = 0.0;
dofydoink 6:6142902e20d1 506 }
dofydoink 6:6142902e20d1 507
dofydoink 6:6142902e20d1 508 dblReadDem = dblReadDem * 4.0;
dofydoink 3:9bd35e5b05ba 509
dofydoink 3:9bd35e5b05ba 510 double testy = (double) abs(pulsesTest)/2000;
dofydoink 3:9bd35e5b05ba 511 //take all readings
dofydoink 3:9bd35e5b05ba 512 //sensor readings
dofydoink 3:9bd35e5b05ba 513
dofydoink 3:9bd35e5b05ba 514 intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure
dofydoink 6:6142902e20d1 515 intPressureRead = pinPresSens.read();
dofydoink 3:9bd35e5b05ba 516
dofydoink 3:9bd35e5b05ba 517
dofydoink 6:6142902e20d1 518 dblPressureVal_norm = pinPresSens.read();
dofydoink 6:6142902e20d1 519 dblPressureVal_norm = dblPressureVal_norm*3.3;//convert to voltage
dofydoink 6:6142902e20d1 520 dblPressureVal_norm = dblPressureVal_norm - 0.5;//subtract offset
dofydoink 6:6142902e20d1 521 dblPressureVal_norm = dblPressureVal_norm/4.0;//calculate normalised pressure
dofydoink 6:6142902e20d1 522
dofydoink 6:6142902e20d1 523 if (dblPressureVal_norm >1.0){
dofydoink 6:6142902e20d1 524 dblPressureVal_norm = 1.0;
dofydoink 6:6142902e20d1 525 }
dofydoink 6:6142902e20d1 526 if (dblPressureVal_norm < 0.0){
dofydoink 6:6142902e20d1 527 dblPressureVal_norm = 0.0;
dofydoink 6:6142902e20d1 528 }
dofydoink 6:6142902e20d1 529
dofydoink 6:6142902e20d1 530
dofydoink 6:6142902e20d1 531 //intPressureRead = intPressureRead-1334;
dofydoink 6:6142902e20d1 532 //intPressureRead = intPressureRead-1679;
dofydoink 6:6142902e20d1 533
dofydoink 6:6142902e20d1 534 //dblPressureVal_bar = ( (double) intPressureRead/10667)*10.0;
dofydoink 6:6142902e20d1 535 //pressureCheck = ( (double) intPressureRead/10667)*10.0;
dofydoink 6:6142902e20d1 536 dblPressureVal_bar = dblPressureVal_norm * 25.0;
dofydoink 6:6142902e20d1 537 dblPres = dblPressureVal_bar;
dofydoink 6:6142902e20d1 538 //intFeedBack_pres = (int)(((double)intPressureRead/10667) * 511);
dofydoink 6:6142902e20d1 539
dofydoink 6:6142902e20d1 540 //unsigned short garb = 0x01;
dofydoink 6:6142902e20d1 541 //
dofydoink 6:6142902e20d1 542 // intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dofydoink 6:6142902e20d1 543 // dblPotPositionRead = (double) POT_2_MM*(intPotRead - POT_OFFSET);
dofydoink 6:6142902e20d1 544
dofydoink 6:6142902e20d1 545 //demand Readings
dofydoink 6:6142902e20d1 546
dofydoink 6:6142902e20d1 547 //current reading
dofydoink 6:6142902e20d1 548 analogReadingCurSens = (double) 0.3*pinCurSense.read()+0.7*analogReadingCurSens;
dofydoink 6:6142902e20d1 549
dofydoink 6:6142902e20d1 550 //convert units and filter
dofydoink 6:6142902e20d1 551
dofydoink 6:6142902e20d1 552 //get position and filter
dofydoink 6:6142902e20d1 553 //dblPos[0] = (double) pulsesTest*-0.0078125 + dblStartingPos;
dofydoink 6:6142902e20d1 554 // dblSensorDriftError = dblPotPositionRead - dblPos[0];
dofydoink 6:6142902e20d1 555
dofydoink 6:6142902e20d1 556 double alpha = (6.283*dblSampleTime_s*100)/(1 + 6.283*dblSampleTime_s*100);
dofydoink 6:6142902e20d1 557
dofydoink 6:6142902e20d1 558 //filter pressure reading
dofydoink 6:6142902e20d1 559 if (intPresFilOrder>0) {
dofydoink 6:6142902e20d1 560 dblPresFil[0] = alpha*dblPres + (1-alpha)*dblPresFil[intPresFilOrder];//filter at 100Hz cutoff
dofydoink 6:6142902e20d1 561 if (intPresFilOrder>1) {
dofydoink 6:6142902e20d1 562 for (ii = 1;ii<intPresFilOrder+1; ii++){
dofydoink 6:6142902e20d1 563 dblPresFil[ii] = alpha*dblPresFil[ii-1]+ (1-alpha)*dblPresFil[ii];
dofydoink 6:6142902e20d1 564 }
dofydoink 6:6142902e20d1 565 }
dofydoink 6:6142902e20d1 566 }
dofydoink 6:6142902e20d1 567 else{
dofydoink 6:6142902e20d1 568 dblPresFil[intPresFilOrder] = dblPres;
dofydoink 6:6142902e20d1 569 }
dofydoink 6:6142902e20d1 570
dofydoink 6:6142902e20d1 571 intFeedBack_pres = (int) (dblPresFil[intPresFilOrder]/12*511);//scale pressure to 12bar
dofydoink 6:6142902e20d1 572
dofydoink 6:6142902e20d1 573 if(intFeedBack_pres > 511){
dofydoink 6:6142902e20d1 574 intFeedBack_pres = 511;
dofydoink 6:6142902e20d1 575 }
dofydoink 6:6142902e20d1 576 if(intFeedBack_pres < 0){
dofydoink 6:6142902e20d1 577 intFeedBack_pres = 0;
dofydoink 6:6142902e20d1 578 }
dofydoink 6:6142902e20d1 579
dofydoink 6:6142902e20d1 580 //printf("%f\t",dblPos[0]);
dofydoink 6:6142902e20d1 581 //printf("%d\t",intPressureRead);
dofydoink 6:6142902e20d1 582 //printf("\r\n");
dofydoink 3:9bd35e5b05ba 583 //intFeedBack_pres = intFeedBack_pres>>5;
dofydoink 3:9bd35e5b05ba 584
dofydoink 3:9bd35e5b05ba 585 intFeedBack_pres = (intFeedBack_pres<<5) | SumDigits(intFeedBack_pres);//add checksum
dofydoink 3:9bd35e5b05ba 586 intFeedBack_pres = (intFeedBack_pres<<1);
dofydoink 3:9bd35e5b05ba 587 intFeedBack_pres = intFeedBack_pres | 0x0001; //add type (1 for pressure)
dofydoink 3:9bd35e5b05ba 588 intFeedBack_pres = (intFeedBack_pres <<1) | OddParityBitGen(intFeedBack_pres);//add parity
dofydoink 3:9bd35e5b05ba 589 intFeedBack_pres = intFeedBack_pres & 0xFFFF;
dofydoink 3:9bd35e5b05ba 590
dofydoink 3:9bd35e5b05ba 591
dofydoink 6:6142902e20d1 592 dblPresDot = dblPresFil[intPresFilOrder] - dblLastPres;//calculate difference
dofydoink 2:aee7d4724915 593
dofydoink 3:9bd35e5b05ba 594
dofydoink 6:6142902e20d1 595 //filter pressure time derivative
dofydoink 6:6142902e20d1 596 if (intPresDotFilOrder>0){
dofydoink 6:6142902e20d1 597 dblPresDotFil[0] = alpha*dblPresDot + (1-alpha)*dblPresDotFil[intPresDotFilOrder];//filter at 100Hz cutoff
dofydoink 6:6142902e20d1 598 if (intPresDotFilOrder>1){
dofydoink 6:6142902e20d1 599 for (ii = 1;ii<intPresDotFilOrder+1; ii++){
dofydoink 6:6142902e20d1 600 dblPresDotFil[ii] = alpha*dblPresDotFil[ii-1]+ (1-alpha)*dblPresDotFil[ii];
dofydoink 6:6142902e20d1 601 }
dofydoink 3:9bd35e5b05ba 602 }
dofydoink 3:9bd35e5b05ba 603 }
dofydoink 6:6142902e20d1 604 else{
dofydoink 6:6142902e20d1 605 dblPresDotFil[intPresDotFilOrder] = dblPresDot;
dofydoink 3:9bd35e5b05ba 606 }
dofydoink 3:9bd35e5b05ba 607
dofydoink 6:6142902e20d1 608 //feedback stuff, set position equal to pressure
dofydoink 6:6142902e20d1 609 intFeedBack_pos = intFeedBack_pres;
dofydoink 3:9bd35e5b05ba 610 intFeedBack_pos = (intFeedBack_pos<<5) | SumDigits(intFeedBack_pos);//add checkSum
dofydoink 3:9bd35e5b05ba 611 intFeedBack_pos = intFeedBack_pos <<1; // add type (0 for position)
dofydoink 6:6142902e20d1 612 intFeedBack_pos = (intFeedBack_pos <<1) | OddParityBitGen(intFeedBack_pos);//add parity
dofydoink 6:6142902e20d1 613
dofydoink 6:6142902e20d1 614 //self pressure demant set
dofydoink 6:6142902e20d1 615 // int intState = swDem.read();
dofydoink 6:6142902e20d1 616 // if (intState){
dofydoink 6:6142902e20d1 617 // dblTargetPos = 6.0;
dofydoink 6:6142902e20d1 618 // dblTargetVel = 6.0;
dofydoink 6:6142902e20d1 619 // }
dofydoink 6:6142902e20d1 620 // else {
dofydoink 6:6142902e20d1 621 // dblTargetPos = 1.5;
dofydoink 6:6142902e20d1 622 // dblTargetVel = 6.0;
dofydoink 6:6142902e20d1 623 // }
dofydoink 3:9bd35e5b05ba 624
dofydoink 3:9bd35e5b05ba 625 ///////////////PATH GENERATION////////////////////////
dofydoink 3:9bd35e5b05ba 626 //work out next path point
dofydoink 3:9bd35e5b05ba 627 double dblPathDifference;
dofydoink 3:9bd35e5b05ba 628 dblPathDifference = dblTargetPos - dblLinearPath;
dofydoink 3:9bd35e5b05ba 629 dblPathSign = dblPathDifference/fabs(dblPathDifference); //is velocity positive or negative?
dofydoink 0:20018747657d 630
dofydoink 3:9bd35e5b05ba 631 //check if target has not been reached (with additional 1% of step to be sure)
dofydoink 3:9bd35e5b05ba 632 if (fabs(dblPathDifference) > 1.01*dblTargetVel*dblSampleTime_s) {
dofydoink 3:9bd35e5b05ba 633 dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path
dofydoink 3:9bd35e5b05ba 634 }
dofydoink 3:9bd35e5b05ba 635 else { //if path is very close to target position
dofydoink 3:9bd35e5b05ba 636 dblLinearPath = dblTargetPos;
dofydoink 3:9bd35e5b05ba 637 }
dofydoink 3:9bd35e5b05ba 638
dofydoink 3:9bd35e5b05ba 639 //limit position
dofydoink 3:9bd35e5b05ba 640 if(dblLinearPath > MAX_POSITION_MM){
dofydoink 3:9bd35e5b05ba 641 dblLinearPath = MAX_POSITION_MM;
dofydoink 3:9bd35e5b05ba 642 }
dofydoink 3:9bd35e5b05ba 643 if (dblLinearPath < 0.0){
dofydoink 3:9bd35e5b05ba 644 dblLinearPath = 0.0;
dofydoink 3:9bd35e5b05ba 645 }
dofydoink 3:9bd35e5b05ba 646
dofydoink 6:6142902e20d1 647 dblPosD[intDemPosFilOrder] = 0.3*dblLinearPath + 0.7*dblPosD[intDemPosFilOrder];
dofydoink 3:9bd35e5b05ba 648
dofydoink 3:9bd35e5b05ba 649 //make sure path is safe
dofydoink 3:9bd35e5b05ba 650 if (dblPosD[intDemPosFilOrder] > MAX_POSITION_MM) {
dofydoink 3:9bd35e5b05ba 651 dblPosD[intDemPosFilOrder] = MAX_POSITION_MM;
dofydoink 3:9bd35e5b05ba 652 }
dofydoink 3:9bd35e5b05ba 653 if (dblPosD[intDemPosFilOrder] < 0.0) {
dofydoink 3:9bd35e5b05ba 654 dblPosD[intDemPosFilOrder] = 0.0;
dofydoink 3:9bd35e5b05ba 655 }
dofydoink 3:9bd35e5b05ba 656
dofydoink 3:9bd35e5b05ba 657 dblVelD[0] = dblPosD[intDemPosFilOrder] - dblLastPosD;
dofydoink 6:6142902e20d1 658 dblPresD = dblPosD[intDemPosFilOrder];//set demand pressure to calculated demand
dofydoink 6:6142902e20d1 659 dblPresDotD = dblVelD[0];//set demand rate of change in pressure
dofydoink 3:9bd35e5b05ba 660
dofydoink 3:9bd35e5b05ba 661 ///////////////////////////////////////////////////// End of Path Generation
dofydoink 3:9bd35e5b05ba 662 //run PID calculations
dofydoink 3:9bd35e5b05ba 663 //get errors
dofydoink 6:6142902e20d1 664
dofydoink 6:6142902e20d1 665 dblError = dblPresD - dblPresFil[intPresFilOrder];
dofydoink 6:6142902e20d1 666 dblErrorDot = dblPresDotD - dblPresDotFil[intPresDotFilOrder];
dofydoink 3:9bd35e5b05ba 667 //get integral
dofydoink 3:9bd35e5b05ba 668 dblIntTerm = dblIntTerm + Ki*dblError;
dofydoink 3:9bd35e5b05ba 669
dofydoink 3:9bd35e5b05ba 670 //limit integral term
dofydoink 6:6142902e20d1 671 if (dblIntTerm > dblIntLimit){
dofydoink 3:9bd35e5b05ba 672 dblIntTerm = dblIntLimit;
dofydoink 3:9bd35e5b05ba 673 }
dofydoink 6:6142902e20d1 674 if (dblIntTerm < -1.0*dblIntLimit){
dofydoink 3:9bd35e5b05ba 675 dblIntTerm = (double) -1.0*dblIntLimit;
dofydoink 3:9bd35e5b05ba 676 }
dofydoink 3:9bd35e5b05ba 677
dofydoink 6:6142902e20d1 678 if(fabs(dblError) <0.0321) {
dofydoink 3:9bd35e5b05ba 679 dblError = 0.0;
dofydoink 6:6142902e20d1 680 //dblErrorDot = 0.0;
dofydoink 0:20018747657d 681
dofydoink 3:9bd35e5b05ba 682 }
dofydoink 0:20018747657d 683
dofydoink 6:6142902e20d1 684 if (fabs(dblErrorDot) < 0.2) {
dofydoink 3:9bd35e5b05ba 685 dblErrorDot = 0.0;
dofydoink 3:9bd35e5b05ba 686 }
dofydoink 3:9bd35e5b05ba 687
dofydoink 3:9bd35e5b05ba 688 //calculate output
dofydoink 6:6142902e20d1 689
dofydoink 6:6142902e20d1 690
dofydoink 3:9bd35e5b05ba 691 output[0] = Kp*dblError + dblIntTerm + Kd*dblErrorDot;
dofydoink 6:6142902e20d1 692 //printf("%f\r\n",dblError);
dofydoink 3:9bd35e5b05ba 693 //limit output
dofydoink 6:6142902e20d1 694 if (output[0] > 0.95){
dofydoink 3:9bd35e5b05ba 695 output[0] = 0.95;
dofydoink 3:9bd35e5b05ba 696 }
dofydoink 6:6142902e20d1 697 if (output[0] < -0.95) {
dofydoink 3:9bd35e5b05ba 698 output[0] = -0.95;
dofydoink 3:9bd35e5b05ba 699 }
dofydoink 3:9bd35e5b05ba 700
dofydoink 6:6142902e20d1 701 if(intOutFilOrder>0){
dofydoink 6:6142902e20d1 702 for (ii = 1; ii < intOutFilOrder+1; ii++) {
dofydoink 3:9bd35e5b05ba 703 output[ii] = 0.7*output[ii-1] + 0.3*output[ii];
dofydoink 0:20018747657d 704 }
dofydoink 3:9bd35e5b05ba 705 }
dofydoink 6:6142902e20d1 706 else{
dofydoink 3:9bd35e5b05ba 707 output[intOutFilOrder] = output[0];
dofydoink 3:9bd35e5b05ba 708 }
dofydoink 6:6142902e20d1 709
dofydoink 6:6142902e20d1 710
dofydoink 6:6142902e20d1 711 //limit pressure
dofydoink 6:6142902e20d1 712 if (dblPressureVal_bar >dblPressureLimitBar){
dofydoink 6:6142902e20d1 713 dblErrorPressure = dblPressureVal_bar - dblPressureLimitBar;
dofydoink 6:6142902e20d1 714 dblDeltaErrorPressure = dblErrorPressure - dblLastErrorPressure;
dofydoink 6:6142902e20d1 715
dofydoink 6:6142902e20d1 716 dblPressureLimitBuck = dblPressureLimitGain * dblErrorPressure;
dofydoink 6:6142902e20d1 717 dblPressureLimitBuck = dblPressureLimitBuck + (dblPressureLimitDerivativeGain*dblDeltaErrorPressure);
dofydoink 6:6142902e20d1 718 dblPressureLimitBuck = dblPressureLimitBuck *1.9/2.0;
dofydoink 6:6142902e20d1 719 dblLastErrorPressure = dblErrorPressure;
dofydoink 6:6142902e20d1 720 }
dofydoink 6:6142902e20d1 721 else {
dofydoink 6:6142902e20d1 722 dblPressureLimitBuck = 0.0;
dofydoink 6:6142902e20d1 723 }
dofydoink 6:6142902e20d1 724
dofydoink 6:6142902e20d1 725 if (dblPressureLimitBuck < 0.0){
dofydoink 6:6142902e20d1 726 dblPressureLimitBuck = 0.0;
dofydoink 6:6142902e20d1 727 }
dofydoink 6:6142902e20d1 728 if (dblPressureLimitBuck > 1.9){
dofydoink 6:6142902e20d1 729 dblPressureLimitBuck = 1.9;
dofydoink 6:6142902e20d1 730 }
dofydoink 6:6142902e20d1 731
dofydoink 6:6142902e20d1 732 output[intOutFilOrder] = output[intOutFilOrder] - dblPressureLimitBuck;
dofydoink 6:6142902e20d1 733
dofydoink 6:6142902e20d1 734 //limit output
dofydoink 6:6142902e20d1 735 if (output[intOutFilOrder] > 0.95){
dofydoink 6:6142902e20d1 736 output[intOutFilOrder] = 0.95;
dofydoink 6:6142902e20d1 737 }
dofydoink 6:6142902e20d1 738 if (output[intOutFilOrder] < -0.95){
dofydoink 6:6142902e20d1 739 output[intOutFilOrder] = -0.95;
dofydoink 6:6142902e20d1 740 }
dofydoink 6:6142902e20d1 741
dofydoink 3:9bd35e5b05ba 742 //limit current
dofydoink 6:6142902e20d1 743 if (analogReadingCurSens> CurrentLimitSet){
dofydoink 3:9bd35e5b05ba 744 currentBuck = CurrentLimitSet / analogReadingCurSens / currentBuckGain;
dofydoink 3:9bd35e5b05ba 745 }
dofydoink 6:6142902e20d1 746 else{
dofydoink 3:9bd35e5b05ba 747 currentBuck = 1.0;
dofydoink 3:9bd35e5b05ba 748 }
dofydoink 6:6142902e20d1 749
dofydoink 6:6142902e20d1 750 if (currentBuck >1.0){
dofydoink 6:6142902e20d1 751 currentBuck = 1.0;
dofydoink 6:6142902e20d1 752 }
dofydoink 6:6142902e20d1 753
dofydoink 6:6142902e20d1 754 if (currentBuck <0.0){
dofydoink 6:6142902e20d1 755 currentBuck = 0.0;
dofydoink 6:6142902e20d1 756 }
dofydoink 3:9bd35e5b05ba 757
dofydoink 3:9bd35e5b05ba 758 output[intOutFilOrder] = currentBuck*output[intOutFilOrder];
dofydoink 6:6142902e20d1 759
dofydoink 6:6142902e20d1 760
dofydoink 6:6142902e20d1 761 //output[intOutFilOrder] = dblPresD;
dofydoink 3:9bd35e5b05ba 762 //end Current limit
dofydoink 6:6142902e20d1 763
dofydoink 6:6142902e20d1 764 double dblBias = 0.08*dblPresFil[intPresFilOrder] + 0.2;
dofydoink 6:6142902e20d1 765 dblBias = 0.0;
dofydoink 3:9bd35e5b05ba 766
dofydoink 3:9bd35e5b05ba 767 //find direction
dofydoink 3:9bd35e5b05ba 768 if(output[intOutFilOrder] >=0.0)
dofydoink 3:9bd35e5b05ba 769 {
dofydoink 6:6142902e20d1 770 //pinDirectionFwd = 1;
dofydoink 6:6142902e20d1 771 //pinDirectionRev = 0;
dofydoink 6:6142902e20d1 772 //dblControlBias = 0.0;
dofydoink 6:6142902e20d1 773 pinAltOutB.write(abs(output[intOutFilOrder])+dblBias);
dofydoink 6:6142902e20d1 774 pinAltOutA.write(0.0);
dofydoink 6:6142902e20d1 775 pinPwmOutput.write(1.0);
dofydoink 6:6142902e20d1 776
dofydoink 3:9bd35e5b05ba 777 }
dofydoink 3:9bd35e5b05ba 778 else
dofydoink 3:9bd35e5b05ba 779 {
dofydoink 6:6142902e20d1 780 //pinDirectionFwd = 0;
dofydoink 6:6142902e20d1 781 // pinDirectionRev = 1;
dofydoink 6:6142902e20d1 782 // dblControlBias = 0.0;
dofydoink 6:6142902e20d1 783 pinAltOutA.write(abs(output[intOutFilOrder])+dblBias);
dofydoink 6:6142902e20d1 784 pinAltOutB.write(0.0);
dofydoink 6:6142902e20d1 785 pinPwmOutput.write(1.0);
dofydoink 3:9bd35e5b05ba 786 }
dofydoink 3:9bd35e5b05ba 787
dofydoink 6:6142902e20d1 788 //printf("O:%f\t\r\n",abs(output[intOutFilOrder]));
dofydoink 6:6142902e20d1 789 //pinPwmOutput.write(abs(output[intOutFilOrder]));
dofydoink 3:9bd35e5b05ba 790
dofydoink 3:9bd35e5b05ba 791 //update all past variables
dofydoink 3:9bd35e5b05ba 792 dblLastPos = dblPos[intPosFilOrder];
dofydoink 3:9bd35e5b05ba 793 dblLastPosD = dblPosD[intDemPosFilOrder];
dofydoink 3:9bd35e5b05ba 794
dofydoink 6:6142902e20d1 795 dblTargetPos = CalculateDemand(mutDutyCycle_pos, intT_pos, intDeltaT_pos, intPeriod_us);
dofydoink 6:6142902e20d1 796 dblTargetPos = (double)MAX_POSITION_MM*dblTargetPos; //set target position (9-bit value)
dofydoink 6:6142902e20d1 797 if(dblTargetPos>MAX_POSITION_MM) { // Limit demand to ensure safety
dofydoink 6:6142902e20d1 798 dblTargetPos = MAX_POSITION_MM;
dofydoink 6:6142902e20d1 799 } else if(dblTargetPos<0.0) {
dofydoink 6:6142902e20d1 800 dblTargetPos = 0.0;
dofydoink 0:20018747657d 801 }
dofydoink 3:9bd35e5b05ba 802
dofydoink 6:6142902e20d1 803 //dblTargetVel = (double)MAX_SPEED_MMPS*inboundMsgsData[1]/511;//set target velocity (9-bit value)
dofydoink 6:6142902e20d1 804 dblTargetVel = CalculateDemand(mutDutyCycle_vel, intT_vel, intDeltaT_vel, intPeriod_us);
dofydoink 6:6142902e20d1 805 dblTargetVel = (double)MAX_SPEED_MMPS*dblTargetVel; //set target position (9-bit value)
dofydoink 6:6142902e20d1 806 if(dblTargetVel>MAX_SPEED_MMPS) {
dofydoink 6:6142902e20d1 807 dblTargetVel = MAX_SPEED_MMPS;
dofydoink 6:6142902e20d1 808 } else if(dblTargetVel<0.0) {
dofydoink 6:6142902e20d1 809 dblTargetVel = 0.0;
dofydoink 6:6142902e20d1 810 }
dofydoink 6:6142902e20d1 811 //intInput = (int)(dblDutyCycle*1000.0);
dofydoink 6:6142902e20d1 812
dofydoink 6:6142902e20d1 813 //printf("%f\t%f\r\n",dblTargetPos, dblTargetVel);
dofydoink 6:6142902e20d1 814
dofydoink 6:6142902e20d1 815 //////print debug//////
dofydoink 6:6142902e20d1 816
dofydoink 6:6142902e20d1 817 //printf("P: %f\tPd: %f\t lin: %f\tTarget: %f\r\n", dblPresFil[intPresFilOrder],dblPresD, dblLinearPath,dblTargetPos);
dofydoink 6:6142902e20d1 818 //prinntf("E: %f\tBias: %f\r\n",output[intOutFilOrder], dblBias);
dofydoink 6:6142902e20d1 819
dofydoink 6:6142902e20d1 820 ///////////////////Communication (For Carafino: Start)////////////////////////////////
dofydoink 3:9bd35e5b05ba 821
dofydoink 6:6142902e20d1 822 //Message handling MCU can only receive messages when gate is 'open'. Gate opens for 500us every 1ms.
dofydoink 6:6142902e20d1 823 //gateTimer.reset();//resets the gate timer
dofydoink 6:6142902e20d1 824 // pinGate = 1;//gate is open. Sets digital output pin to HIGH
dofydoink 6:6142902e20d1 825 // unsigned int outboundMsgs[2] = {intFeedBack_pos, intFeedBack_pres};//prepare feedback messages
dofydoink 6:6142902e20d1 826 // unsigned int inboundMsgsData[2] = {0,0};
dofydoink 6:6142902e20d1 827 //while(gateTimer.read_us() < 500) {//while gate is open
dofydoink 6:6142902e20d1 828 //
dofydoink 6:6142902e20d1 829 // if(slave.receive()) {//when a message is received
dofydoink 6:6142902e20d1 830 //
dofydoink 6:6142902e20d1 831 // bool isSPIsuccess = PerformSlaveSPI(&slave,outboundMsgs,inboundMsgsData);//process the data and check if it was corrupted
dofydoink 6:6142902e20d1 832 //
dofydoink 6:6142902e20d1 833 // if( isSPIsuccess ) {//if message looks OK
dofydoink 6:6142902e20d1 834 // dblTargetPos = (double)MAX_POSITION_MM*inboundMsgsData[0]/511; //set target position (9-bit value)
dofydoink 6:6142902e20d1 835 // if(dblTargetPos>MAX_POSITION_MM) { // Limit demand to ensure safety
dofydoink 6:6142902e20d1 836 // dblTargetPos = MAX_POSITION_MM;
dofydoink 6:6142902e20d1 837 // } else if(dblTargetPos<0.0) {
dofydoink 6:6142902e20d1 838 // dblTargetPos = 0.0;
dofydoink 6:6142902e20d1 839 // }
dofydoink 6:6142902e20d1 840 //
dofydoink 6:6142902e20d1 841 // dblTargetVel = (double)MAX_SPEED_MMPS*inboundMsgsData[1]/511;//set target velocity (9-bit value)
dofydoink 6:6142902e20d1 842 //
dofydoink 6:6142902e20d1 843 // if(dblTargetVel>MAX_SPEED_MMPS) {
dofydoink 6:6142902e20d1 844 // dblTargetVel = MAX_SPEED_MMPS;
dofydoink 6:6142902e20d1 845 // } else if(dblTargetVel<0.0) {
dofydoink 6:6142902e20d1 846 // dblTargetPos = 0.0;
dofydoink 6:6142902e20d1 847 // }
dofydoink 6:6142902e20d1 848 //
dofydoink 6:6142902e20d1 849 // break;//bail out of while loop
dofydoink 6:6142902e20d1 850 // }
dofydoink 6:6142902e20d1 851 // }
dofydoink 6:6142902e20d1 852 // }
dofydoink 6:6142902e20d1 853 //
dofydoink 6:6142902e20d1 854 // pinGate = 0;//close gate
dofydoink 3:9bd35e5b05ba 855
dofydoink 6:6142902e20d1 856 dblLastPres = dblPresFil[intPresFilOrder];//update previous pressure reading
dofydoink 6:6142902e20d1 857 dblLastPresDot = dblPresDotFil[intPresDotFilOrder];
dofydoink 3:9bd35e5b05ba 858 }
dofydoink 3:9bd35e5b05ba 859 }
dofydoink 6:6142902e20d1 860 //////////////////////////////////////////////////For Carafino: End
dofydoink 6:6142902e20d1 861
dofydoink 6:6142902e20d1 862
dofydoink 6:6142902e20d1 863
dofydoink 3:9bd35e5b05ba 864 //configure all control parameters
dofydoink 6:6142902e20d1 865 void ControlParameterConfig(){
dofydoink 3:9bd35e5b05ba 866 Kp = Kp/dblMotorVoltage;
dofydoink 3:9bd35e5b05ba 867 Kd = Kd/dblSampleTime_s/dblMotorVoltage;
dofydoink 3:9bd35e5b05ba 868 Ki = Ki*dblSampleTime_s/dblMotorVoltage;
dofydoink 6:6142902e20d1 869
dofydoink 6:6142902e20d1 870 dblReadDem = 0;
dofydoink 3:9bd35e5b05ba 871 }
dofydoink 0:20018747657d 872
dofydoink 0:20018747657d 873 Ticker debugTicker;
dofydoink 0:20018747657d 874
dofydoink 6:6142902e20d1 875 void startPositionControl(){
dofydoink 0:20018747657d 876 semPosCtrl.release();
dofydoink 0:20018747657d 877 }
dofydoink 0:20018747657d 878
dofydoink 6:6142902e20d1 879 void startDebug(){
dofydoink 0:20018747657d 880 semDebug.release();
dofydoink 0:20018747657d 881 }
dofydoink 0:20018747657d 882
dofydoink 0:20018747657d 883 Ticker positionCtrlTicker;
dofydoink 0:20018747657d 884
dofydoink 0:20018747657d 885 int main() {
dofydoink 6:6142902e20d1 886 //pinGate = 0;
dofydoink 6:6142902e20d1 887 //swDem.mode(PullDown);
dofydoink 0:20018747657d 888 cs_ADC = 1;
dofydoink 6:6142902e20d1 889 //Mntr = 0;
dofydoink 6:6142902e20d1 890 //Mntr2 = 0;
dofydoink 6:6142902e20d1 891 pinPwmOutput.period_us(1000);
dofydoink 6:6142902e20d1 892
dofydoink 6:6142902e20d1 893 pinPWMin_pos.mode(PullNone);
dofydoink 6:6142902e20d1 894 timerDutyCycle_pos.start();
dofydoink 6:6142902e20d1 895 timerPeriod_pos.start();
dofydoink 6:6142902e20d1 896
dofydoink 6:6142902e20d1 897 pinPWMin_vel.mode(PullNone);
dofydoink 6:6142902e20d1 898 timerDutyCycle_vel.start();
dofydoink 6:6142902e20d1 899 timerPeriod_vel.start();
dofydoink 6:6142902e20d1 900 //calculateTicker.attach(&CalculateDutyCycle,0.1);
dofydoink 6:6142902e20d1 901
dofydoink 6:6142902e20d1 902 pinPWMin_pos.rise(&PWMRise_pos);
dofydoink 6:6142902e20d1 903 pinPWMin_pos.fall(&PWMFall_pos);
dofydoink 6:6142902e20d1 904 pinPWMin_vel.rise(&PWMRise_vel);
dofydoink 6:6142902e20d1 905 pinPWMin_vel.fall(&PWMFall_vel);
dofydoink 6:6142902e20d1 906
dofydoink 6:6142902e20d1 907 pc.baud(115200);
dofydoink 0:20018747657d 908 printf("\r\nYo Yo Yo, Low Level innit\r\n\n\n");
dofydoink 0:20018747657d 909 wait(0.5);
dofydoink 0:20018747657d 910
dofydoink 0:20018747657d 911 timer.start();
dofydoink 6:6142902e20d1 912 //gateTimer.start();
dofydoink 0:20018747657d 913 //cs_ADC = 1;
dofydoink 0:20018747657d 914
dofydoink 0:20018747657d 915 ControlParameterConfig();
dofydoink 0:20018747657d 916 // for (ii = 0; ii< 10; ii++)
dofydoink 0:20018747657d 917 // {
dofydoink 0:20018747657d 918 // uintPotRead = Read14BitADC(3, cs_ADC);//read potentiometer
dofydoink 0:20018747657d 919 // dblStartingPos = (double) POT_2_MM*uintPotRead - dblPosBias;
dofydoink 0:20018747657d 920 // }
dofydoink 0:20018747657d 921 //
dofydoink 6:6142902e20d1 922 // slave.format(16,2);
dofydoink 6:6142902e20d1 923 // slave.frequency(10000000);
dofydoink 2:aee7d4724915 924
dofydoink 0:20018747657d 925 dblPosD[intDemPosFilOrder] = 0.0;
dofydoink 0:20018747657d 926 slaveReceivePos = 0.0;
dofydoink 0:20018747657d 927 slaveReceiveVel = 0.0;
dofydoink 0:20018747657d 928 wait(0.1);
dofydoink 0:20018747657d 929 ADC_Config();
dofydoink 0:20018747657d 930 wait(0.1);
dofydoink 0:20018747657d 931 ADC_Config();
dofydoink 0:20018747657d 932 wait(0.1);
dofydoink 0:20018747657d 933 intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dofydoink 0:20018747657d 934 //intPotRead = (Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
dofydoink 6:6142902e20d1 935 dblStartingPos = 0.0;//(double) POT_2_MM*(intPotRead - POT_OFFSET);
dofydoink 2:aee7d4724915 936 dblLinearPath = dblStartingPos;
dofydoink 0:20018747657d 937 dblTargetPos = dblStartingPos;
dofydoink 3:9bd35e5b05ba 938 dblPos[intPosFilOrder] = dblStartingPos;
dofydoink 2:aee7d4724915 939 dblTargetVel = 0.0;
dofydoink 0:20018747657d 940 dblPosD[intDemPosFilOrder] = dblStartingPos;
dofydoink 0:20018747657d 941
dofydoink 2:aee7d4724915 942 printf("\r\nPotRead: %d, Current Pos: %f, Target Pos: %f\r\n", intPotRead, dblStartingPos, dblTargetPos);
dofydoink 0:20018747657d 943
dofydoink 0:20018747657d 944 //calculate/convert variables
dofydoink 0:20018747657d 945
dofydoink 0:20018747657d 946 CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3;
dofydoink 6:6142902e20d1 947 //slave.reply(0x5555);
dofydoink 0:20018747657d 948 PositionControlThread.start(PositionControlPID);
dofydoink 3:9bd35e5b05ba 949
dofydoink 0:20018747657d 950 positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s);
dofydoink 0:20018747657d 951
dofydoink 0:20018747657d 952 intFeedBack_pos = 0;
dofydoink 0:20018747657d 953 intFeedBack_pres = 0;
dofydoink 0:20018747657d 954
dofydoink 0:20018747657d 955
dofydoink 6:6142902e20d1 956 while(1){
dofydoink 0:20018747657d 957 Thread::wait(osWaitForever);
dofydoink 0:20018747657d 958 }
dofydoink 3:9bd35e5b05ba 959 }