Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@24:7fbd904d191c, 2014-10-30 (annotated)

Committer:
jessekaiser
Date:
Thu Oct 30 20:39:52 2014 +0000
Revision:
24:7fbd904d191c
Parent:
23:3306e3267fe6
Child:
25:f45d61cc6dc6
motor1 kan links - en rechtsom draaien. K_P en K_I waarde zijn nu aardig.

Who changed what in which revision?

UserRevisionLine numberNew 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 24:7fbd904d191c 7 #define K_P (3.5)
jessekaiser 24:7fbd904d191c 8 #define K_I (0.01 *TSAMP)
jessekaiser 0:d5d3b731340c 9 #define I_LIMIT 1.
jessekaiser 18:fb85c58a4106 10 #define PI 3.14159265
jessekaiser 18:fb85c58a4106 11 #define l_arm 0.5
jessekaiser 0:d5d3b731340c 12
jessekaiser 7:697293226e5e 13 #define M1_PWM PTC8
jessekaiser 7:697293226e5e 14 #define M1_DIR PTC9
jessekaiser 7:697293226e5e 15 #define M2_PWM PTA5
jessekaiser 7:697293226e5e 16 #define M2_DIR PTA4
jessekaiser 7:697293226e5e 17
jessekaiser 7:697293226e5e 18 //Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting
jessekaiser 0:d5d3b731340c 19
jessekaiser 3:dcc4cebba0d7 20 Serial pc(USBTX, USBRX);
jessekaiser 4:db3dad7e53e3 21 DigitalOut led1(LED_RED);
jessekaiser 5:e4b9cc904928 22 HIDScope scope(2);
jessekaiser 0:d5d3b731340c 23
jessekaiser 7:697293226e5e 24 //motor 25D
jessekaiser 7:697293226e5e 25 Encoder motor1(PTD3,PTD5); //wit, geel
jessekaiser 7:697293226e5e 26 PwmOut pwm_motor1(M2_PWM);
jessekaiser 7:697293226e5e 27 DigitalOut motordir1(M2_DIR);
jessekaiser 3:dcc4cebba0d7 28
jessekaiser 7:697293226e5e 29 //motor2 37D
jessekaiser 7:697293226e5e 30 Encoder motor2(PTD2, PTD0); //wit, geel
jessekaiser 7:697293226e5e 31 PwmOut pwm_motor2(M1_PWM);
jessekaiser 7:697293226e5e 32 DigitalOut motordir2(M1_DIR);
jessekaiser 18:fb85c58a4106 33 void clamp(float * in, float min, float max);
jessekaiser 18:fb85c58a4106 34 float pid(float setpoint, float measurement);
jessekaiser 18:fb85c58a4106 35 float pwm1_percentage = 0;
jessekaiser 18:fb85c58a4106 36 float pwm2_percentage = 0;
jessekaiser 18:fb85c58a4106 37 int cur_pos_motor1;
jessekaiser 18:fb85c58a4106 38 int prev_pos_motor1 = 0;
jessekaiser 18:fb85c58a4106 39 int cur_pos_motor2;
jessekaiser 18:fb85c58a4106 40 int prev_pos_motor2 = 0;
jessekaiser 18:fb85c58a4106 41 float speed1_rad;
jessekaiser 19:3e46c457091c 42 float speed2_rad;
jessekaiser 19:3e46c457091c 43 float pos_motor1_rad;
jessekaiser 19:3e46c457091c 44 float pos_motor2_rad;
jessekaiser 18:fb85c58a4106 45
jessekaiser 18:fb85c58a4106 46
jessekaiser 18:fb85c58a4106 47
jessekaiser 7:697293226e5e 48
jessekaiser 21:c856b7752d7d 49
jessekaiser 21:c856b7752d7d 50 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 21:c856b7752d7d 51 // maar de locatie van de variabele.
jessekaiser 21:c856b7752d7d 52 {
jessekaiser 21:c856b7752d7d 53 *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 21:c856b7752d7d 54 // *in = het getal dat staat op locatie van in --> waarde van new_pwm
jessekaiser 7:697293226e5e 55 }
jessekaiser 3:dcc4cebba0d7 56
jessekaiser 3:dcc4cebba0d7 57
jessekaiser 21:c856b7752d7d 58 float pid(float setpoint, float cur_pos_motor1)
jessekaiser 21:c856b7752d7d 59 {
jessekaiser 21:c856b7752d7d 60 float error;
jessekaiser 21:c856b7752d7d 61 float out_p = 0;
jessekaiser 21:c856b7752d7d 62 static float out_i = 0;
jessekaiser 21:c856b7752d7d 63 error = (setpoint - cur_pos_motor1);
jessekaiser 21:c856b7752d7d 64 out_p = error*K_P;
jessekaiser 21:c856b7752d7d 65 out_i += error*K_I;
jessekaiser 21:c856b7752d7d 66 clamp(&out_i,-I_LIMIT,I_LIMIT);
jessekaiser 21:c856b7752d7d 67 return out_p + out_i;
jessekaiser 24:7fbd904d191c 68 }
jessekaiser 24:7fbd904d191c 69
jessekaiser 24:7fbd904d191c 70 /*void batje_links ()
jessekaiser 24:7fbd904d191c 71 {
jessekaiser 24:7fbd904d191c 72 speed1_rad = -1.5; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 24:7fbd904d191c 73 cur_pos_motor1 = motor1.getPosition();
jessekaiser 24:7fbd904d191c 74 pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI));
jessekaiser 24:7fbd904d191c 75 setpoint = prev_setpoint + TSAMP * speed1_rad;
jessekaiser 24:7fbd904d191c 76 if(setpoint > (11.3*2.0*PI/360)) { //setpoint in graden
jessekaiser 24:7fbd904d191c 77 setpoint = (11.3*2.0*PI/360);
jessekaiser 24:7fbd904d191c 78 }
jessekaiser 24:7fbd904d191c 79 if(setpoint < -(11.3*2.0*PI/360)) {
jessekaiser 24:7fbd904d191c 80 setpoint = -(11.3*2.0*PI/360);
jessekaiser 24:7fbd904d191c 81 }
jessekaiser 24:7fbd904d191c 82
jessekaiser 24:7fbd904d191c 83 pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 24:7fbd904d191c 84 prev_setpoint = setpoint;
jessekaiser 24:7fbd904d191c 85 wait(TSAMP);
jessekaiser 24:7fbd904d191c 86 }
jessekaiser 24:7fbd904d191c 87
jessekaiser 24:7fbd904d191c 88 void batje_rechts ()
jessekaiser 24:7fbd904d191c 89 {
jessekaiser 24:7fbd904d191c 90 speed1_rad = 1.5; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 24:7fbd904d191c 91 cur_pos_motor1 = motor1.getPosition();
jessekaiser 24:7fbd904d191c 92 pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI));
jessekaiser 24:7fbd904d191c 93 setpoint = prev_setpoint + TSAMP * speed1_rad;
jessekaiser 24:7fbd904d191c 94 if(setpoint > (11.3*2.0*PI/360)) { //setpoint in graden
jessekaiser 24:7fbd904d191c 95 setpoint = (11.3*2.0*PI/360);
jessekaiser 24:7fbd904d191c 96 }
jessekaiser 24:7fbd904d191c 97 if(setpoint < -(11.3*2.0*PI/360)) {
jessekaiser 24:7fbd904d191c 98 setpoint = -(11.3*2.0*PI/360);
jessekaiser 24:7fbd904d191c 99 }
jessekaiser 24:7fbd904d191c 100 pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 24:7fbd904d191c 101 prev_setpoint = setpoint;
jessekaiser 24:7fbd904d191c 102 }*/
jessekaiser 0:d5d3b731340c 103 int main()
jessekaiser 3:dcc4cebba0d7 104 {
jessekaiser 24:7fbd904d191c 105 pwm_motor1.period_us(100);
jessekaiser 24:7fbd904d191c 106 motor1.setPosition(0);
jessekaiser 24:7fbd904d191c 107 float prev_setpoint = 0;
jessekaiser 24:7fbd904d191c 108 float setpoint = 0;
jessekaiser 24:7fbd904d191c 109
jessekaiser 0:d5d3b731340c 110 while(1) {
jessekaiser 6:f260176f704b 111
jessekaiser 19:3e46c457091c 112 pwm1_percentage = pid(setpoint, pos_motor1_rad);
jessekaiser 22:22cb158bcea4 113 if (pwm1_percentage < -1.0) {
jessekaiser 22:22cb158bcea4 114 pwm1_percentage = -1.0;
jessekaiser 24:7fbd904d191c 115 }
jessekaiser 24:7fbd904d191c 116 if (pwm1_percentage > 1.0) {
jessekaiser 24:7fbd904d191c 117 pwm1_percentage = 1.0;
jessekaiser 22:22cb158bcea4 118 }
jessekaiser 19:3e46c457091c 119
jessekaiser 24:7fbd904d191c 120 if(pwm1_percentage > 0) {
jessekaiser 24:7fbd904d191c 121 motordir1 = 0;
jessekaiser 24:7fbd904d191c 122 } else {
jessekaiser 22:22cb158bcea4 123 motordir1 = 1;
jessekaiser 24:7fbd904d191c 124 }
jessekaiser 24:7fbd904d191c 125
jessekaiser 24:7fbd904d191c 126 speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 24:7fbd904d191c 127 cur_pos_motor1 = motor1.getPosition();
jessekaiser 24:7fbd904d191c 128 pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI));
jessekaiser 24:7fbd904d191c 129 setpoint = prev_setpoint + TSAMP * speed1_rad;
jessekaiser 24:7fbd904d191c 130 if(setpoint > (12*2.0*PI/360)) { //setpoint in graden
jessekaiser 24:7fbd904d191c 131 setpoint = (12*2.0*PI/360);
jessekaiser 24:7fbd904d191c 132 }
jessekaiser 24:7fbd904d191c 133 if(setpoint < -(12*2.0*PI/360)) {
jessekaiser 24:7fbd904d191c 134 setpoint = -(12*2.0*PI/360);
jessekaiser 24:7fbd904d191c 135 }
jessekaiser 24:7fbd904d191c 136
jessekaiser 24:7fbd904d191c 137 pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 22:22cb158bcea4 138 prev_setpoint = setpoint;
jessekaiser 22:22cb158bcea4 139 wait(TSAMP);
jessekaiser 6:f260176f704b 140 }
jessekaiser 0:d5d3b731340c 141 }
jessekaiser 18:fb85c58a4106 142
jessekaiser 18:fb85c58a4106 143
jessekaiser 18:fb85c58a4106 144