MorphGI / Mbed 2 deprecated LoCoMoTE_LowLevel_v7_PWM

Low level control for MorphGI Control unit rebuild, using PWM to receive commands from Mid level.

Dependencies:   mbed QEI FastAnalogIn mbed-rtos FastPWM

main.cpp@6:042db7596e55, 2021-06-24 (annotated)

Committer:
dofydoink
Date:
Thu Jun 24 20:45:25 2021 +0000
Revision:
6:042db7596e55
Parent:
5:4e710cef655e 
PWM low level;

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