Script voor aansturen motoren
Dependencies: Encoder HIDScope MODSERIAL mbed-dsp mbed
main.cpp@20:25e5916fe7b1, 2014-10-30 (annotated)
- Committer:
- jessekaiser
- Date:
- Thu Oct 30 11:57:32 2014 +0000
- Revision:
- 20:25e5916fe7b1
- Parent:
- 19:3e46c457091c
werken aan PI regelaar;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jessekaiser | 0:d5d3b731340c | 1 | #include "mbed.h" |
jessekaiser | 0:d5d3b731340c | 2 | #include "encoder.h" |
jessekaiser | 0:d5d3b731340c | 3 | #include "HIDScope.h" |
jessekaiser | 19:3e46c457091c | 4 | #include "PwmOut.h" |
jessekaiser | 0:d5d3b731340c | 5 | |
jessekaiser | 6:f260176f704b | 6 | #define TSAMP 0.005 |
jessekaiser | 19:3e46c457091c | 7 | #define K_P (0.8) |
jessekaiser | 20:25e5916fe7b1 | 8 | #define K_I (0.1 *TSAMP) |
jessekaiser | 0:d5d3b731340c | 9 | #define I_LIMIT 1. |
jessekaiser | 19:3e46c457091c | 10 | #define pwm1_min 0.06 |
jessekaiser | 18:fb85c58a4106 | 11 | #define pwm2_min 0 |
jessekaiser | 18:fb85c58a4106 | 12 | #define PI 3.14159265 |
jessekaiser | 18:fb85c58a4106 | 13 | #define l_arm 0.5 |
jessekaiser | 0:d5d3b731340c | 14 | |
jessekaiser | 7:697293226e5e | 15 | #define M1_PWM PTC8 |
jessekaiser | 7:697293226e5e | 16 | #define M1_DIR PTC9 |
jessekaiser | 7:697293226e5e | 17 | #define M2_PWM PTA5 |
jessekaiser | 7:697293226e5e | 18 | #define M2_DIR PTA4 |
jessekaiser | 7:697293226e5e | 19 | |
jessekaiser | 7:697293226e5e | 20 | //Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting |
jessekaiser | 0:d5d3b731340c | 21 | |
jessekaiser | 3:dcc4cebba0d7 | 22 | Serial pc(USBTX, USBRX); |
jessekaiser | 4:db3dad7e53e3 | 23 | DigitalOut led1(LED_RED); |
jessekaiser | 5:e4b9cc904928 | 24 | HIDScope scope(2); |
jessekaiser | 0:d5d3b731340c | 25 | |
jessekaiser | 7:697293226e5e | 26 | //motor 25D |
jessekaiser | 7:697293226e5e | 27 | Encoder motor1(PTD3,PTD5); //wit, geel |
jessekaiser | 7:697293226e5e | 28 | PwmOut pwm_motor1(M2_PWM); |
jessekaiser | 7:697293226e5e | 29 | DigitalOut motordir1(M2_DIR); |
jessekaiser | 3:dcc4cebba0d7 | 30 | |
jessekaiser | 7:697293226e5e | 31 | //motor2 37D |
jessekaiser | 7:697293226e5e | 32 | Encoder motor2(PTD2, PTD0); //wit, geel |
jessekaiser | 7:697293226e5e | 33 | PwmOut pwm_motor2(M1_PWM); |
jessekaiser | 7:697293226e5e | 34 | DigitalOut motordir2(M1_DIR); |
jessekaiser | 18:fb85c58a4106 | 35 | void clamp(float * in, float min, float max); |
jessekaiser | 18:fb85c58a4106 | 36 | float pid(float setpoint, float measurement); |
jessekaiser | 18:fb85c58a4106 | 37 | float pwm1_percentage = 0; |
jessekaiser | 18:fb85c58a4106 | 38 | float pwm2_percentage = 0; |
jessekaiser | 18:fb85c58a4106 | 39 | int cur_pos_motor1; |
jessekaiser | 18:fb85c58a4106 | 40 | int prev_pos_motor1 = 0; |
jessekaiser | 18:fb85c58a4106 | 41 | int cur_pos_motor2; |
jessekaiser | 18:fb85c58a4106 | 42 | int prev_pos_motor2 = 0; |
jessekaiser | 18:fb85c58a4106 | 43 | float speed1_rad; |
jessekaiser | 19:3e46c457091c | 44 | float speed2_rad; |
jessekaiser | 19:3e46c457091c | 45 | float pos_motor1_rad; |
jessekaiser | 19:3e46c457091c | 46 | float pos_motor2_rad; |
jessekaiser | 18:fb85c58a4106 | 47 | |
jessekaiser | 18:fb85c58a4106 | 48 | |
jessekaiser | 18:fb85c58a4106 | 49 | |
jessekaiser | 18:fb85c58a4106 | 50 | |
jessekaiser | 18:fb85c58a4106 | 51 | volatile bool looptimerflag; |
jessekaiser | 7:697293226e5e | 52 | |
jessekaiser | 7:697293226e5e | 53 | bool flip=false; |
jessekaiser | 7:697293226e5e | 54 | |
jessekaiser | 7:697293226e5e | 55 | void attime() |
jessekaiser | 0:d5d3b731340c | 56 | { |
jessekaiser | 7:697293226e5e | 57 | flip = !flip; |
jessekaiser | 0:d5d3b731340c | 58 | } |
jessekaiser | 0:d5d3b731340c | 59 | |
jessekaiser | 7:697293226e5e | 60 | void looper() |
jessekaiser | 7:697293226e5e | 61 | { |
jessekaiser | 7:697293226e5e | 62 | motordir1=0; |
jessekaiser | 7:697293226e5e | 63 | pwm_motor1.write(1); |
jessekaiser | 7:697293226e5e | 64 | scope.set(0, motor1.getPosition()); |
jessekaiser | 7:697293226e5e | 65 | scope.set(1, motor2.getPosition()); |
jessekaiser | 7:697293226e5e | 66 | scope.send(); |
jessekaiser | 7:697293226e5e | 67 | } |
jessekaiser | 3:dcc4cebba0d7 | 68 | |
jessekaiser | 3:dcc4cebba0d7 | 69 | |
jessekaiser | 0:d5d3b731340c | 70 | int main() |
jessekaiser | 3:dcc4cebba0d7 | 71 | { |
jessekaiser | 19:3e46c457091c | 72 | //Ticker log_timer; |
jessekaiser | 19:3e46c457091c | 73 | //log_timer.attach(looper, TSAMP); |
jessekaiser | 19:3e46c457091c | 74 | motor1.setPosition(0); |
jessekaiser | 19:3e46c457091c | 75 | pwm_motor1.period_us(100); |
jessekaiser | 19:3e46c457091c | 76 | float prev_setpoint = 0; |
jessekaiser | 19:3e46c457091c | 77 | float setpoint; |
jessekaiser | 19:3e46c457091c | 78 | speed1_rad = 0.29; |
jessekaiser | 19:3e46c457091c | 79 | |
jessekaiser | 18:fb85c58a4106 | 80 | |
jessekaiser | 0:d5d3b731340c | 81 | while(1) { |
jessekaiser | 6:f260176f704b | 82 | |
jessekaiser | 19:3e46c457091c | 83 | cur_pos_motor1 = motor1.getPosition(); |
jessekaiser | 19:3e46c457091c | 84 | pos_motor1_rad = cur_pos_motor1/(4128/(2*PI)); |
jessekaiser | 19:3e46c457091c | 85 | |
jessekaiser | 19:3e46c457091c | 86 | setpoint = prev_setpoint + TSAMP * speed1_rad; |
jessekaiser | 19:3e46c457091c | 87 | pwm1_percentage = pid(setpoint, pos_motor1_rad); |
jessekaiser | 19:3e46c457091c | 88 | |
jessekaiser | 19:3e46c457091c | 89 | if (pwm1_percentage < -100) { |
jessekaiser | 19:3e46c457091c | 90 | pwm1_percentage = -100; |
jessekaiser | 19:3e46c457091c | 91 | } |
jessekaiser | 19:3e46c457091c | 92 | else if (pwm1_percentage >100){ |
jessekaiser | 19:3e46c457091c | 93 | pwm1_percentage =100; |
jessekaiser | 19:3e46c457091c | 94 | } |
jessekaiser | 19:3e46c457091c | 95 | |
jessekaiser | 19:3e46c457091c | 96 | if(pwm1_percentage < 0) { |
jessekaiser | 19:3e46c457091c | 97 | motordir1 = 1; |
jessekaiser | 19:3e46c457091c | 98 | } |
jessekaiser | 19:3e46c457091c | 99 | else { |
jessekaiser | 19:3e46c457091c | 100 | motordir1 = 0; |
jessekaiser | 19:3e46c457091c | 101 | } |
jessekaiser | 19:3e46c457091c | 102 | |
jessekaiser | 19:3e46c457091c | 103 | if(cur_pos_motor1 < 1032) { |
jessekaiser | 19:3e46c457091c | 104 | pwm_motor1.write(pwm1_percentage/100);//(abs(((1-pwm1_min)/100)*pwm1_percentage + pwm1_min)); |
jessekaiser | 19:3e46c457091c | 105 | prev_setpoint = setpoint; |
jessekaiser | 19:3e46c457091c | 106 | } |
jessekaiser | 19:3e46c457091c | 107 | else { |
jessekaiser | 19:3e46c457091c | 108 | pwm_motor1.write(0); |
jessekaiser | 19:3e46c457091c | 109 | } |
jessekaiser | 6:f260176f704b | 110 | } |
jessekaiser | 0:d5d3b731340c | 111 | } |
jessekaiser | 18:fb85c58a4106 | 112 | |
jessekaiser | 18:fb85c58a4106 | 113 | void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op |
jessekaiser | 19:3e46c457091c | 114 | // maar de locatie van de variabele. |
jessekaiser | 18:fb85c58a4106 | 115 | { |
jessekaiser | 19:3e46c457091c | 116 | *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c |
jessekaiser | 18:fb85c58a4106 | 117 | // *in = het getal dat staat op locatie van in --> waarde van new_pwm |
jessekaiser | 18:fb85c58a4106 | 118 | } |
jessekaiser | 18:fb85c58a4106 | 119 | |
jessekaiser | 18:fb85c58a4106 | 120 | |
jessekaiser | 18:fb85c58a4106 | 121 | float pid(float setpoint, float measurement) |
jessekaiser | 18:fb85c58a4106 | 122 | { |
jessekaiser | 18:fb85c58a4106 | 123 | float error; |
jessekaiser | 18:fb85c58a4106 | 124 | float out_p = 0; |
jessekaiser | 18:fb85c58a4106 | 125 | static float out_i = 0; |
jessekaiser | 18:fb85c58a4106 | 126 | error = (setpoint-measurement); |
jessekaiser | 19:3e46c457091c | 127 | out_p = error*K_P; |
jessekaiser | 18:fb85c58a4106 | 128 | out_i += error*K_I; |
jessekaiser | 18:fb85c58a4106 | 129 | clamp(&out_i,-I_LIMIT,I_LIMIT); |
jessekaiser | 19:3e46c457091c | 130 | return out_p + out_i; |
jessekaiser | 18:fb85c58a4106 | 131 | } |