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?
User | Revision | Line number | New 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 | } |