MorphGI / Mbed 2 deprecated LoCoMoTE_LowLevel_SyringePump_v7

Low level control code for syringe pumps

Dependencies:   mbed QEI FastAnalogIn mbed-rtos FastPWM

main.cpp@6:f4d91041b8af, 2021-06-25 (annotated)

Committer:
dofydoink
Date:
Fri Jun 25 07:38:21 2021 +0000
Revision:
6:f4d91041b8af
Parent:
5:4e710cef655e 
Control Code for Low Level controllers;

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