Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@19:3e46c457091c, 2014-10-30 (annotated)

Committer:
jessekaiser
Date:
Thu Oct 30 11:45:48 2014 +0000
Revision:
19:3e46c457091c
Parent:
18:fb85c58a4106
Child:
20:25e5916fe7b1
Child:
21:c856b7752d7d
Werkt soort van;

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 19:3e46c457091c 7 #define K_P (0.8)
jessekaiser 19:3e46c457091c 8 #define K_I (0.01 *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 }