Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@23:3306e3267fe6, 2014-10-30 (annotated)

Committer:
jessekaiser
Date:
Thu Oct 30 13:10:42 2014 +0000
Revision:
23:3306e3267fe6
Parent:
22:22cb158bcea4
Child:
24:7fbd904d191c
motor 1 werkt

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 22:22cb158bcea4 7 #define K_P (2.0)
jessekaiser 23:3306e3267fe6 8 #define K_I (0.05 *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 21:c856b7752d7d 68 }
jessekaiser 0:d5d3b731340c 69 int main()
jessekaiser 3:dcc4cebba0d7 70 {
jessekaiser 23:3306e3267fe6 71
jessekaiser 19:3e46c457091c 72 motor1.setPosition(0);
jessekaiser 19:3e46c457091c 73 pwm_motor1.period_us(100);
jessekaiser 19:3e46c457091c 74 float prev_setpoint = 0;
jessekaiser 22:22cb158bcea4 75 float setpoint = 0;
jessekaiser 23:3306e3267fe6 76 speed1_rad = 1.5;
jessekaiser 19:3e46c457091c 77
jessekaiser 18:fb85c58a4106 78
jessekaiser 0:d5d3b731340c 79 while(1) {
jessekaiser 6:f260176f704b 80
jessekaiser 19:3e46c457091c 81 cur_pos_motor1 = motor1.getPosition();
jessekaiser 22:22cb158bcea4 82 pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI));
jessekaiser 22:22cb158bcea4 83
jessekaiser 19:3e46c457091c 84 setpoint = prev_setpoint + TSAMP * speed1_rad;
jessekaiser 22:22cb158bcea4 85 if(setpoint > (2.0*PI)) {
jessekaiser 22:22cb158bcea4 86 setpoint = (2.0*PI);
jessekaiser 22:22cb158bcea4 87 }
jessekaiser 22:22cb158bcea4 88 if(setpoint < -(2.0*PI)){
jessekaiser 22:22cb158bcea4 89 setpoint = -(2.0*PI);
jessekaiser 22:22cb158bcea4 90 }
jessekaiser 22:22cb158bcea4 91
jessekaiser 22:22cb158bcea4 92
jessekaiser 19:3e46c457091c 93 pwm1_percentage = pid(setpoint, pos_motor1_rad);
jessekaiser 22:22cb158bcea4 94 if (pwm1_percentage < -1.0) {
jessekaiser 22:22cb158bcea4 95 pwm1_percentage = -1.0;
jessekaiser 19:3e46c457091c 96 }
jessekaiser 22:22cb158bcea4 97 if (pwm1_percentage >1.0){
jessekaiser 22:22cb158bcea4 98 pwm1_percentage =1.0;
jessekaiser 22:22cb158bcea4 99 }
jessekaiser 19:3e46c457091c 100
jessekaiser 22:22cb158bcea4 101 if(pwm1_percentage > 0) {
jessekaiser 22:22cb158bcea4 102 motordir1 = 0; }
jessekaiser 22:22cb158bcea4 103 else{
jessekaiser 22:22cb158bcea4 104 motordir1 = 1;
jessekaiser 19:3e46c457091c 105 }
jessekaiser 22:22cb158bcea4 106 pwm_motor1.write(abs(pwm1_percentage)) ;//(abs(((1-pwm1_min)/100)*pwm1_percentage + pwm1_min));
jessekaiser 22:22cb158bcea4 107 prev_setpoint = setpoint;
jessekaiser 22:22cb158bcea4 108 wait(TSAMP);
jessekaiser 6:f260176f704b 109 }
jessekaiser 0:d5d3b731340c 110 }
jessekaiser 18:fb85c58a4106 111
jessekaiser 18:fb85c58a4106 112
jessekaiser 18:fb85c58a4106 113