motor aansturing
Dependencies: Encoder HIDScope MODSERIAL mbed-dsp mbed
main.cpp@1:82a5126adc56, 2014-10-14 (annotated)
- Committer:
- DominiqueC
- Date:
- Tue Oct 14 13:11:03 2014 +0000
- Revision:
- 1:82a5126adc56
- Parent:
- 0:1d02168661ec
voor Daan
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DominiqueC | 0:1d02168661ec | 1 | /***************************************/ |
DominiqueC | 0:1d02168661ec | 2 | /* */ |
DominiqueC | 0:1d02168661ec | 3 | /* BRONCODE GROEP 5, MODULE 9, 2014 */ |
DominiqueC | 0:1d02168661ec | 4 | /* *****-THE SLAP-****** */ |
DominiqueC | 0:1d02168661ec | 5 | /* */ |
DominiqueC | 0:1d02168661ec | 6 | /* -Dominique Clevers */ |
DominiqueC | 0:1d02168661ec | 7 | /* -Rianne van Dommelen */ |
DominiqueC | 0:1d02168661ec | 8 | /* -Daan de Muinck Keizer */ |
DominiqueC | 0:1d02168661ec | 9 | /* -David den Houting */ |
DominiqueC | 0:1d02168661ec | 10 | /* -Marjolein Thijssen */ |
DominiqueC | 0:1d02168661ec | 11 | /***************************************/ |
DominiqueC | 0:1d02168661ec | 12 | #include "mbed.h" |
DominiqueC | 0:1d02168661ec | 13 | #include "encoder.h" |
DominiqueC | 0:1d02168661ec | 14 | #include "MODSERIAL.h" |
DominiqueC | 1:82a5126adc56 | 15 | #include "HIDScope.h" |
DominiqueC | 1:82a5126adc56 | 16 | |
DominiqueC | 1:82a5126adc56 | 17 | |
DominiqueC | 1:82a5126adc56 | 18 | //MAXIMALE UITWIJKING BEPALEN |
DominiqueC | 1:82a5126adc56 | 19 | /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/ |
DominiqueC | 1:82a5126adc56 | 20 | void keep_in_range(float * in, float min, float max); |
DominiqueC | 0:1d02168661ec | 21 | |
DominiqueC | 1:82a5126adc56 | 22 | /** variable to show when a new loop can be started*/ |
DominiqueC | 1:82a5126adc56 | 23 | /** volatile means that it can be changed in an */ |
DominiqueC | 1:82a5126adc56 | 24 | /** interrupt routine, and that that change is vis-*/ |
DominiqueC | 1:82a5126adc56 | 25 | /** ible in the main loop. */ |
DominiqueC | 1:82a5126adc56 | 26 | |
DominiqueC | 1:82a5126adc56 | 27 | volatile bool looptimerflag; |
DominiqueC | 1:82a5126adc56 | 28 | |
DominiqueC | 1:82a5126adc56 | 29 | /** function called by Ticker "looptimer" */ |
DominiqueC | 1:82a5126adc56 | 30 | /** variable 'looptimerflag' is set to 'true' */ |
DominiqueC | 1:82a5126adc56 | 31 | /** each time the looptimer expires. */ |
DominiqueC | 1:82a5126adc56 | 32 | void setlooptimerflag(void) |
DominiqueC | 0:1d02168661ec | 33 | { |
DominiqueC | 1:82a5126adc56 | 34 | looptimerflag = true; |
DominiqueC | 1:82a5126adc56 | 35 | } |
DominiqueC | 1:82a5126adc56 | 36 | |
DominiqueC | 1:82a5126adc56 | 37 | int main() { |
DominiqueC | 1:82a5126adc56 | 38 | //LOCAL VARIABLES |
DominiqueC | 1:82a5126adc56 | 39 | /*Potmeter input*/ |
DominiqueC | 1:82a5126adc56 | 40 | AnalogIn potmeter(PTC2); |
DominiqueC | 1:82a5126adc56 | 41 | /* Encoder, using my encoder library */ |
DominiqueC | 1:82a5126adc56 | 42 | /* First pin should be PTDx or PTAx */ |
DominiqueC | 1:82a5126adc56 | 43 | /* because those pins can be used as */ |
DominiqueC | 1:82a5126adc56 | 44 | /* InterruptIn */ |
DominiqueC | 0:1d02168661ec | 45 | Encoder motor1(PTD0,PTC9); |
DominiqueC | 1:82a5126adc56 | 46 | /* MODSERIAL to get non-blocking Serial*/ |
DominiqueC | 0:1d02168661ec | 47 | MODSERIAL pc(USBTX,USBRX); |
DominiqueC | 1:82a5126adc56 | 48 | /* PWM control to motor */ |
DominiqueC | 1:82a5126adc56 | 49 | PwmOut pwm_motor(PTA12); |
DominiqueC | 1:82a5126adc56 | 50 | /* Direction pin */ |
DominiqueC | 1:82a5126adc56 | 51 | DigitalOut motordir(PTD3); |
DominiqueC | 1:82a5126adc56 | 52 | /* variable to store setpoint in */ |
DominiqueC | 1:82a5126adc56 | 53 | float setpoint; |
DominiqueC | 1:82a5126adc56 | 54 | /* variable to store pwm value in*/ |
DominiqueC | 1:82a5126adc56 | 55 | float pwm_to_motor; |
DominiqueC | 1:82a5126adc56 | 56 | //START OF CODE |
DominiqueC | 1:82a5126adc56 | 57 | |
DominiqueC | 1:82a5126adc56 | 58 | /*Set the baudrate (use this number in RealTerm too! */ |
DominiqueC | 1:82a5126adc56 | 59 | pc.baud(230400); |
DominiqueC | 1:82a5126adc56 | 60 | |
DominiqueC | 1:82a5126adc56 | 61 | /*Create a ticker, and let it call the */ |
DominiqueC | 1:82a5126adc56 | 62 | /*function 'setlooptimerflag' every 0.01s */ |
DominiqueC | 1:82a5126adc56 | 63 | Ticker looptimer; |
DominiqueC | 1:82a5126adc56 | 64 | looptimer.attach(setlooptimerflag,0.01); |
DominiqueC | 1:82a5126adc56 | 65 | |
DominiqueC | 1:82a5126adc56 | 66 | //INFINITE LOOP |
DominiqueC | 1:82a5126adc56 | 67 | while(1) { |
DominiqueC | 1:82a5126adc56 | 68 | /* Wait until looptimer flag is true. */ |
DominiqueC | 1:82a5126adc56 | 69 | /* '!=' means not-is-equal */ |
DominiqueC | 1:82a5126adc56 | 70 | while(looptimerflag != true); |
DominiqueC | 1:82a5126adc56 | 71 | /* Clear the looptimerflag, otherwise */ |
DominiqueC | 1:82a5126adc56 | 72 | /* the program would simply continue */ |
DominiqueC | 1:82a5126adc56 | 73 | /* without waitingin the next iteration*/ |
DominiqueC | 1:82a5126adc56 | 74 | looptimerflag = false; |
DominiqueC | 1:82a5126adc56 | 75 | |
DominiqueC | 1:82a5126adc56 | 76 | /* Read potmeter value, apply some math */ |
DominiqueC | 1:82a5126adc56 | 77 | /* to get useful setpoint value */ |
DominiqueC | 1:82a5126adc56 | 78 | setpoint = (potmeter.read()-0.5)*2000; |
DominiqueC | 1:82a5126adc56 | 79 | |
DominiqueC | 1:82a5126adc56 | 80 | /* Print setpoint and current position to serial terminal*/ |
DominiqueC | 1:82a5126adc56 | 81 | pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition()); |
DominiqueC | 1:82a5126adc56 | 82 | |
DominiqueC | 1:82a5126adc56 | 83 | /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */ |
DominiqueC | 1:82a5126adc56 | 84 | pwm_to_motor = (setpoint - motor1.getPosition())*.001; |
DominiqueC | 1:82a5126adc56 | 85 | /* Coerce pwm value if outside range */ |
DominiqueC | 1:82a5126adc56 | 86 | /* Not strictly needed here, but useful */ |
DominiqueC | 1:82a5126adc56 | 87 | /* if doing other calculations with pwm value */ |
DominiqueC | 1:82a5126adc56 | 88 | keep_in_range(&pwm_to_motor, -1,1); |
DominiqueC | 1:82a5126adc56 | 89 | |
DominiqueC | 1:82a5126adc56 | 90 | /* Control the motor direction pin. based on */ |
DominiqueC | 1:82a5126adc56 | 91 | /* the sign of your pwm value. If your */ |
DominiqueC | 1:82a5126adc56 | 92 | /* motor keeps spinning when running this code */ |
DominiqueC | 1:82a5126adc56 | 93 | /* you probably need to swap the motor wires, */ |
DominiqueC | 1:82a5126adc56 | 94 | /* or swap the 'write(1)' and 'write(0)' below */ |
DominiqueC | 1:82a5126adc56 | 95 | if(pwm_to_motor > 0) |
DominiqueC | 1:82a5126adc56 | 96 | motordir.write(1); |
DominiqueC | 1:82a5126adc56 | 97 | else |
DominiqueC | 1:82a5126adc56 | 98 | motordir.write(0); |
DominiqueC | 1:82a5126adc56 | 99 | //WRITE VALUE TO MOTOR |
DominiqueC | 1:82a5126adc56 | 100 | /* Take the absolute value of the PWM to send */ |
DominiqueC | 1:82a5126adc56 | 101 | /* to the motor. */ |
DominiqueC | 1:82a5126adc56 | 102 | pwm_motor.write(abs(pwm_to_motor)); |
DominiqueC | 0:1d02168661ec | 103 | } |
DominiqueC | 0:1d02168661ec | 104 | } |
DominiqueC | 0:1d02168661ec | 105 | |
DominiqueC | 1:82a5126adc56 | 106 | |
DominiqueC | 1:82a5126adc56 | 107 | //coerces value 'in' to min or max when exceeding those values |
DominiqueC | 1:82a5126adc56 | 108 | //if you'd like to understand the statement below take a google for |
DominiqueC | 1:82a5126adc56 | 109 | //'ternary operators'. |
DominiqueC | 1:82a5126adc56 | 110 | void keep_in_range(float * in, float min, float max) |
DominiqueC | 1:82a5126adc56 | 111 | { |
DominiqueC | 1:82a5126adc56 | 112 | *in > min ? *in < max? : *in = max: *in = min; |
DominiqueC | 1:82a5126adc56 | 113 | } |
DominiqueC | 1:82a5126adc56 | 114 | |
DominiqueC | 1:82a5126adc56 | 115 | |
DominiqueC | 0:1d02168661ec | 116 | //AANSTUREN MOTOR (POSITIE EN SNELHEID) |
DominiqueC | 0:1d02168661ec | 117 | #define TSAMP 0.01 |
DominiqueC | 0:1d02168661ec | 118 | #define K_P (0.1) |
DominiqueC | 0:1d02168661ec | 119 | #define K_I (0.03 *TSAMP) |
DominiqueC | 0:1d02168661ec | 120 | #define K_D (0.001 /TSAMP) |
DominiqueC | 0:1d02168661ec | 121 | #define I_LIMIT 1. |
DominiqueC | 0:1d02168661ec | 122 | |
DominiqueC | 0:1d02168661ec | 123 | #define M1_PWM PTC8 |
DominiqueC | 0:1d02168661ec | 124 | #define M1_DIR PTC9 |
DominiqueC | 0:1d02168661ec | 125 | #define M2_PWM PTA5 |
DominiqueC | 0:1d02168661ec | 126 | #define M2_DIR PTA4 |
DominiqueC | 0:1d02168661ec | 127 | |
DominiqueC | 0:1d02168661ec | 128 | //#define POT_AVG 50 |
DominiqueC | 0:1d02168661ec | 129 | |
DominiqueC | 0:1d02168661ec | 130 | void clamp(float * in, float min, float max); |
DominiqueC | 0:1d02168661ec | 131 | float pid(float setpoint, float measurement); |
DominiqueC | 0:1d02168661ec | 132 | volatile bool looptimerflag; |
DominiqueC | 0:1d02168661ec | 133 | //float potsamples[POT_AVG]; |
DominiqueC | 0:1d02168661ec | 134 | HIDScope scope(6); |
DominiqueC | 0:1d02168661ec | 135 | |
DominiqueC | 0:1d02168661ec | 136 | |
DominiqueC | 0:1d02168661ec | 137 | void setlooptimerflag(void) |
DominiqueC | 0:1d02168661ec | 138 | { |
DominiqueC | 0:1d02168661ec | 139 | looptimerflag = true; |
DominiqueC | 0:1d02168661ec | 140 | } |
DominiqueC | 0:1d02168661ec | 141 | |
DominiqueC | 0:1d02168661ec | 142 | int main() |
DominiqueC | 0:1d02168661ec | 143 | { |
DominiqueC | 0:1d02168661ec | 144 | AnalogIn potmeter(PTC2); |
DominiqueC | 0:1d02168661ec | 145 | //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter |
DominiqueC | 0:1d02168661ec | 146 | Encoder motor1(PTD0,PTA13); |
DominiqueC | 0:1d02168661ec | 147 | /*PwmOut to motor driver*/ |
DominiqueC | 0:1d02168661ec | 148 | PwmOut pwm_motor(M2_PWM); |
DominiqueC | 0:1d02168661ec | 149 | //10kHz PWM frequency |
DominiqueC | 0:1d02168661ec | 150 | pwm_motor.period_us(75); |
DominiqueC | 0:1d02168661ec | 151 | DigitalOut motordir(M2_DIR); |
DominiqueC | 0:1d02168661ec | 152 | Ticker looptimer; |
DominiqueC | 0:1d02168661ec | 153 | looptimer.attach(setlooptimerflag,TSAMP); |
DominiqueC | 0:1d02168661ec | 154 | while(1) { |
DominiqueC | 0:1d02168661ec | 155 | int16_t setpoint; |
DominiqueC | 0:1d02168661ec | 156 | float new_pwm; |
DominiqueC | 0:1d02168661ec | 157 | /*wait until timer has elapsed*/ |
DominiqueC | 0:1d02168661ec | 158 | while(!looptimerflag); |
DominiqueC | 0:1d02168661ec | 159 | looptimerflag = false; //clear flag |
DominiqueC | 0:1d02168661ec | 160 | /*potmeter value: 0-1*/ |
DominiqueC | 0:1d02168661ec | 161 | setpoint = (potmeter.read()-.5)*500; |
DominiqueC | 0:1d02168661ec | 162 | /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/ |
DominiqueC | 0:1d02168661ec | 163 | new_pwm = pid(setpoint, motor1.getPosition()); |
DominiqueC | 0:1d02168661ec | 164 | clamp(&new_pwm, -1,1); |
DominiqueC | 0:1d02168661ec | 165 | scope.set(0, setpoint); |
DominiqueC | 0:1d02168661ec | 166 | scope.set(4, new_pwm); |
DominiqueC | 0:1d02168661ec | 167 | scope.set(5, motor1.getPosition()); |
DominiqueC | 0:1d02168661ec | 168 | // ch 1, 2 and 3 set in pid controller */ |
DominiqueC | 0:1d02168661ec | 169 | scope.send(); |
DominiqueC | 0:1d02168661ec | 170 | if(new_pwm < 0) |
DominiqueC | 0:1d02168661ec | 171 | motordir = 0; |
DominiqueC | 0:1d02168661ec | 172 | else |
DominiqueC | 0:1d02168661ec | 173 | motordir = 1; |
DominiqueC | 0:1d02168661ec | 174 | pwm_motor.write(abs(new_pwm)); |
DominiqueC | 0:1d02168661ec | 175 | } |
DominiqueC | 0:1d02168661ec | 176 | } |
DominiqueC | 0:1d02168661ec | 177 | |
DominiqueC | 0:1d02168661ec | 178 | |
DominiqueC | 0:1d02168661ec | 179 | //clamps value 'in' to min or max when exceeding those values |
DominiqueC | 0:1d02168661ec | 180 | //if you'd like to understand the statement below take a google for |
DominiqueC | 0:1d02168661ec | 181 | //'ternary operators'. |
DominiqueC | 0:1d02168661ec | 182 | void clamp(float * in, float min, float max) |
DominiqueC | 0:1d02168661ec | 183 | { |
DominiqueC | 0:1d02168661ec | 184 | *in > min ? *in < max? : *in = max: *in = min; |
DominiqueC | 0:1d02168661ec | 185 | } |
DominiqueC | 0:1d02168661ec | 186 | |
DominiqueC | 0:1d02168661ec | 187 | |
DominiqueC | 0:1d02168661ec | 188 | float pid(float setpoint, float measurement) |
DominiqueC | 0:1d02168661ec | 189 | { |
DominiqueC | 0:1d02168661ec | 190 | float error; |
DominiqueC | 0:1d02168661ec | 191 | static float prev_error = 0; |
DominiqueC | 0:1d02168661ec | 192 | float out_p = 0; |
DominiqueC | 0:1d02168661ec | 193 | static float out_i = 0; |
DominiqueC | 0:1d02168661ec | 194 | float out_d = 0; |
DominiqueC | 0:1d02168661ec | 195 | error = setpoint-measurement; |
DominiqueC | 0:1d02168661ec | 196 | out_p = error*K_P; |
DominiqueC | 0:1d02168661ec | 197 | out_i += error*K_I; |
DominiqueC | 0:1d02168661ec | 198 | out_d = (error-prev_error)*K_D; |
DominiqueC | 0:1d02168661ec | 199 | clamp(&out_i,-I_LIMIT,I_LIMIT); |
DominiqueC | 0:1d02168661ec | 200 | prev_error = error; |
DominiqueC | 0:1d02168661ec | 201 | scope.set(1,out_p); |
DominiqueC | 0:1d02168661ec | 202 | scope.set(2,out_i); |
DominiqueC | 0:1d02168661ec | 203 | scope.set(3,out_d); |
DominiqueC | 0:1d02168661ec | 204 | return out_p + out_i + out_d; |
DominiqueC | 0:1d02168661ec | 205 | } |
DominiqueC | 0:1d02168661ec | 206 | |
DominiqueC | 1:82a5126adc56 | 207 | |
DominiqueC | 1:82a5126adc56 | 208 | //POSITIE EN SNELHEID UITLEZEN OP PC |
DominiqueC | 1:82a5126adc56 | 209 | int main() |
DominiqueC | 1:82a5126adc56 | 210 | { |
DominiqueC | 1:82a5126adc56 | 211 | while(1) //Loop |
DominiqueC | 1:82a5126adc56 | 212 | { |
DominiqueC | 1:82a5126adc56 | 213 | /** Make encoder object. |
DominiqueC | 1:82a5126adc56 | 214 | * First pin should be on PTAx or PTDx because those pins can be used as InterruptIn |
DominiqueC | 1:82a5126adc56 | 215 | * Second pin can be any digital input |
DominiqueC | 1:82a5126adc56 | 216 | */ |
DominiqueC | 1:82a5126adc56 | 217 | Encoder motor1(PTD0,PTC9); |
DominiqueC | 1:82a5126adc56 | 218 | /*Use USB serial to send values*/ |
DominiqueC | 1:82a5126adc56 | 219 | MODSERIAL pc(USBTX,USBRX); |
DominiqueC | 1:82a5126adc56 | 220 | /*Set baud rate to 115200*/ |
DominiqueC | 1:82a5126adc56 | 221 | pc.baud(115200); |
DominiqueC | 1:82a5126adc56 | 222 | while(1) { //Loop |
DominiqueC | 1:82a5126adc56 | 223 | /**Wait to prevent buffer overflow, and to keep terminal readable (not scrolling too fast)*/ |
DominiqueC | 1:82a5126adc56 | 224 | wait(0.2); |
DominiqueC | 1:82a5126adc56 | 225 | /** print position (integer) and speed (float) to the PC*/ |
DominiqueC | 1:82a5126adc56 | 226 | pc.printf("pos: %d, speed %f \r\n",motor1.getPosition(), motor1.getSpeed()); |
DominiqueC | 1:82a5126adc56 | 227 | } |
DominiqueC | 1:82a5126adc56 | 228 | } |