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