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