Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@26:5981d4b1144e, 2014-10-31 (annotated)

Committer:
jessekaiser
Date:
Fri Oct 31 12:06:01 2014 +0000
Revision:
26:5981d4b1144e
Parent:
25:f45d61cc6dc6
Child:
27:77bb255753be
Child:
28:4f2e15118f13
Hoera! 1 motor doet 't

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 25:f45d61cc6dc6 7 #define K_P1 (3.5) //voor motor1 is het 3.5
jessekaiser 25:f45d61cc6dc6 8 #define K_I1 (0.01 *TSAMP) //voor motor1 is het 0.01
jessekaiser 25:f45d61cc6dc6 9 #define K_P2 (1.0)
jessekaiser 25:f45d61cc6dc6 10 #define K_I2 (0.01 *TSAMP)
jessekaiser 0:d5d3b731340c 11 #define I_LIMIT 1.
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 25:f45d61cc6dc6 35 //void clamp(float * in, float min, float max);
jessekaiser 25:f45d61cc6dc6 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 25:f45d61cc6dc6 47 int staat = 0;
jessekaiser 18:fb85c58a4106 48
jessekaiser 18:fb85c58a4106 49
jessekaiser 7:697293226e5e 50
jessekaiser 21:c856b7752d7d 51
jessekaiser 21:c856b7752d7d 52 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 53 // maar de locatie van de variabele.
jessekaiser 21:c856b7752d7d 54 {
jessekaiser 21:c856b7752d7d 55 *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 56 // *in = het getal dat staat op locatie van in --> waarde van new_pwm
jessekaiser 7:697293226e5e 57 }
jessekaiser 3:dcc4cebba0d7 58
jessekaiser 3:dcc4cebba0d7 59
jessekaiser 25:f45d61cc6dc6 60 float pid1(float setpoint1, float measurement1)
jessekaiser 21:c856b7752d7d 61 {
jessekaiser 25:f45d61cc6dc6 62 float error1;
jessekaiser 25:f45d61cc6dc6 63 float out_p1 = 0;
jessekaiser 25:f45d61cc6dc6 64 static float out_i1 = 0;
jessekaiser 25:f45d61cc6dc6 65 error1 = (setpoint1 - measurement1);
jessekaiser 25:f45d61cc6dc6 66 out_p1 = error1*K_P1;
jessekaiser 25:f45d61cc6dc6 67 out_i1 += error1*K_I1;
jessekaiser 25:f45d61cc6dc6 68 clamp(&out_i1,-I_LIMIT,I_LIMIT);
jessekaiser 25:f45d61cc6dc6 69 return out_p1 + out_i1;
jessekaiser 24:7fbd904d191c 70 }
jessekaiser 24:7fbd904d191c 71
jessekaiser 25:f45d61cc6dc6 72 float prev_setpoint1 = 0;
jessekaiser 25:f45d61cc6dc6 73 float setpoint1 = 0;
jessekaiser 25:f45d61cc6dc6 74 float prev_setpoint2 = 0;
jessekaiser 25:f45d61cc6dc6 75 float setpoint2 = 0;
jessekaiser 25:f45d61cc6dc6 76
jessekaiser 25:f45d61cc6dc6 77 void batje_links ()
jessekaiser 24:7fbd904d191c 78 {
jessekaiser 25:f45d61cc6dc6 79 speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 25:f45d61cc6dc6 80 cur_pos_motor1 = motor1.getPosition();
jessekaiser 25:f45d61cc6dc6 81 pos_motor1_rad = (float)cur_pos_motor1/(3600.0/(2.0*PI));
jessekaiser 25:f45d61cc6dc6 82 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 25:f45d61cc6dc6 83 if(setpoint1 > (180*2.0*PI/360)) { //setpoint in graden
jessekaiser 25:f45d61cc6dc6 84 setpoint1 = (180*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 85 }
jessekaiser 25:f45d61cc6dc6 86 if(setpoint1 < -(180*2.0*PI/360)) {
jessekaiser 25:f45d61cc6dc6 87 setpoint1 = -(180*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 88 }
jessekaiser 25:f45d61cc6dc6 89
jessekaiser 25:f45d61cc6dc6 90 pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 25:f45d61cc6dc6 91 prev_setpoint1 = setpoint1;
jessekaiser 25:f45d61cc6dc6 92 wait(TSAMP);
jessekaiser 25:f45d61cc6dc6 93
jessekaiser 24:7fbd904d191c 94 }
jessekaiser 24:7fbd904d191c 95
jessekaiser 24:7fbd904d191c 96 void batje_rechts ()
jessekaiser 24:7fbd904d191c 97 {
jessekaiser 25:f45d61cc6dc6 98 speed1_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 25:f45d61cc6dc6 99 cur_pos_motor1 = motor1.getPosition();
jessekaiser 25:f45d61cc6dc6 100 pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
jessekaiser 25:f45d61cc6dc6 101 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 25:f45d61cc6dc6 102 if(setpoint1 > (360*2.0*PI/360)) { //setpoint in graden
jessekaiser 25:f45d61cc6dc6 103 setpoint1 = (360*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 104 }
jessekaiser 25:f45d61cc6dc6 105 if(setpoint1 < -(360*2.0*PI/360)) {
jessekaiser 25:f45d61cc6dc6 106 setpoint1 = -(360*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 107 }
jessekaiser 25:f45d61cc6dc6 108 pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 25:f45d61cc6dc6 109 prev_setpoint1 = setpoint1;
jessekaiser 25:f45d61cc6dc6 110 if(setpoint1 > (360*2.0*PI/360)) {
jessekaiser 25:f45d61cc6dc6 111 staat = 1;
jessekaiser 25:f45d61cc6dc6 112 }
jessekaiser 25:f45d61cc6dc6 113 wait(TSAMP);
jessekaiser 25:f45d61cc6dc6 114
jessekaiser 25:f45d61cc6dc6 115
jessekaiser 25:f45d61cc6dc6 116 }
jessekaiser 25:f45d61cc6dc6 117
jessekaiser 25:f45d61cc6dc6 118 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
jessekaiser 25:f45d61cc6dc6 119 {
jessekaiser 25:f45d61cc6dc6 120 speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 26:5981d4b1144e 121
jessekaiser 25:f45d61cc6dc6 122 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 25:f45d61cc6dc6 123 if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
jessekaiser 25:f45d61cc6dc6 124 setpoint1 = (155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 125 }
jessekaiser 25:f45d61cc6dc6 126 if(setpoint1 < -(155.0*2.0*PI/360)) {
jessekaiser 25:f45d61cc6dc6 127 setpoint1 = -(155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 128 }
jessekaiser 25:f45d61cc6dc6 129 pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 25:f45d61cc6dc6 130 prev_setpoint1 = setpoint1;
jessekaiser 26:5981d4b1144e 131 if(setpoint1 >= (155.0*2.0*PI/360)-0.1) {
jessekaiser 26:5981d4b1144e 132 staat = 1;
jessekaiser 26:5981d4b1144e 133 }
jessekaiser 26:5981d4b1144e 134
jessekaiser 25:f45d61cc6dc6 135
jessekaiser 25:f45d61cc6dc6 136 }
jessekaiser 25:f45d61cc6dc6 137
jessekaiser 25:f45d61cc6dc6 138 void arm_mid ()
jessekaiser 25:f45d61cc6dc6 139 {
jessekaiser 25:f45d61cc6dc6 140 speed1_rad = -3.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 26:5981d4b1144e 141 //cur_pos_motor1 = motor1.getPosition();
jessekaiser 26:5981d4b1144e 142 //os_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
jessekaiser 26:5981d4b1144e 143 setpoint1 = prev_setpoint1 + (TSAMP * speed1_rad);
jessekaiser 25:f45d61cc6dc6 144 if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
jessekaiser 25:f45d61cc6dc6 145 setpoint1 = (155*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 146 }
jessekaiser 25:f45d61cc6dc6 147 if(setpoint1 < -(155*2.0*PI/360)) {
jessekaiser 25:f45d61cc6dc6 148 setpoint1 = -(155*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 149 }
jessekaiser 26:5981d4b1144e 150 //pc.printf("m%f\n\r",setpoint1);
jessekaiser 26:5981d4b1144e 151 //pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 25:f45d61cc6dc6 152 prev_setpoint1 = setpoint1;
jessekaiser 25:f45d61cc6dc6 153
jessekaiser 26:5981d4b1144e 154 //wait(TSAMP);
jessekaiser 25:f45d61cc6dc6 155 }
jessekaiser 25:f45d61cc6dc6 156
jessekaiser 25:f45d61cc6dc6 157 void arm_laag ()
jessekaiser 25:f45d61cc6dc6 158 {
jessekaiser 25:f45d61cc6dc6 159 speed2_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 25:f45d61cc6dc6 160 cur_pos_motor2 = motor2.getPosition();
jessekaiser 25:f45d61cc6dc6 161 pos_motor2_rad = (float)cur_pos_motor2/(3600/(2.0*PI));
jessekaiser 25:f45d61cc6dc6 162 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
jessekaiser 25:f45d61cc6dc6 163 if(setpoint2 > (100*2.0*PI/360)) { //setpoint in graden
jessekaiser 25:f45d61cc6dc6 164 setpoint2 = (100*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 165 }
jessekaiser 25:f45d61cc6dc6 166 if(setpoint2 < -(100*2.0*PI/360)) {
jessekaiser 25:f45d61cc6dc6 167 setpoint2 = -(100*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 168 }
jessekaiser 25:f45d61cc6dc6 169
jessekaiser 25:f45d61cc6dc6 170 pwm_motor2.write(abs(pwm2_percentage));
jessekaiser 25:f45d61cc6dc6 171 prev_setpoint2 = setpoint2;
jessekaiser 25:f45d61cc6dc6 172 wait(TSAMP);
jessekaiser 25:f45d61cc6dc6 173 }
jessekaiser 25:f45d61cc6dc6 174
jessekaiser 0:d5d3b731340c 175 int main()
jessekaiser 3:dcc4cebba0d7 176 {
jessekaiser 26:5981d4b1144e 177 int wait_iterator = 0;
jessekaiser 25:f45d61cc6dc6 178 pwm_motor1.period_us(100);
jessekaiser 25:f45d61cc6dc6 179 motor1.setPosition(0);
jessekaiser 25:f45d61cc6dc6 180 pwm_motor2.period_us(100);
jessekaiser 25:f45d61cc6dc6 181 motor2.setPosition(0);
jessekaiser 25:f45d61cc6dc6 182 pc.baud(115200);
jessekaiser 25:f45d61cc6dc6 183
jessekaiser 0:d5d3b731340c 184 while(1) {
jessekaiser 26:5981d4b1144e 185 //MOTOR1
jessekaiser 25:f45d61cc6dc6 186 pc.printf("%d \r\n", motor1.getPosition());
jessekaiser 26:5981d4b1144e 187 cur_pos_motor1 = motor1.getPosition();
jessekaiser 26:5981d4b1144e 188 pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
jessekaiser 25:f45d61cc6dc6 189 pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
jessekaiser 22:22cb158bcea4 190 if (pwm1_percentage < -1.0) {
jessekaiser 22:22cb158bcea4 191 pwm1_percentage = -1.0;
jessekaiser 24:7fbd904d191c 192 }
jessekaiser 24:7fbd904d191c 193 if (pwm1_percentage > 1.0) {
jessekaiser 24:7fbd904d191c 194 pwm1_percentage = 1.0;
jessekaiser 22:22cb158bcea4 195 }
jessekaiser 26:5981d4b1144e 196 pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 24:7fbd904d191c 197 if(pwm1_percentage > 0) {
jessekaiser 24:7fbd904d191c 198 motordir1 = 0;
jessekaiser 24:7fbd904d191c 199 } else {
jessekaiser 22:22cb158bcea4 200 motordir1 = 1;
jessekaiser 24:7fbd904d191c 201 }
jessekaiser 26:5981d4b1144e 202 wait(TSAMP);
jessekaiser 26:5981d4b1144e 203 //STATES
jessekaiser 25:f45d61cc6dc6 204 if(staat == 0) {
jessekaiser 25:f45d61cc6dc6 205 arm_hoog();
jessekaiser 26:5981d4b1144e 206 wait_iterator = 0;
jessekaiser 25:f45d61cc6dc6 207 }
jessekaiser 26:5981d4b1144e 208 else if(staat ==1)
jessekaiser 26:5981d4b1144e 209 {
jessekaiser 26:5981d4b1144e 210 wait_iterator++;
jessekaiser 26:5981d4b1144e 211 if(wait_iterator > 200)
jessekaiser 26:5981d4b1144e 212 staat = 2;
jessekaiser 26:5981d4b1144e 213 }
jessekaiser 26:5981d4b1144e 214 else
jessekaiser 25:f45d61cc6dc6 215 {
jessekaiser 25:f45d61cc6dc6 216 arm_mid();
jessekaiser 24:7fbd904d191c 217 }
jessekaiser 25:f45d61cc6dc6 218 }
jessekaiser 0:d5d3b731340c 219 }
jessekaiser 18:fb85c58a4106 220
jessekaiser 18:fb85c58a4106 221