Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@33:0417920456d0, 2014-10-31 (annotated)

Committer:
jessekaiser
Date:
Fri Oct 31 14:15:10 2014 +0000
Revision:
33:0417920456d0
Parent:
32:c6f453a2fd3e
Child:
34:1cea2a17e961
Bijna beide motoren aangesloten

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 28:4f2e15118f13 35
jessekaiser 18:fb85c58a4106 36 float pwm1_percentage = 0;
jessekaiser 18:fb85c58a4106 37 float pwm2_percentage = 0;
jessekaiser 18:fb85c58a4106 38 int cur_pos_motor1;
jessekaiser 18:fb85c58a4106 39 int prev_pos_motor1 = 0;
jessekaiser 18:fb85c58a4106 40 int cur_pos_motor2;
jessekaiser 18:fb85c58a4106 41 int prev_pos_motor2 = 0;
jessekaiser 18:fb85c58a4106 42 float speed1_rad;
jessekaiser 19:3e46c457091c 43 float speed2_rad;
jessekaiser 19:3e46c457091c 44 float pos_motor1_rad;
jessekaiser 19:3e46c457091c 45 float pos_motor2_rad;
jessekaiser 30:6ace856e0ad7 46 int staat1 = 0;
jessekaiser 32:c6f453a2fd3e 47 int staat2 = 0;
jessekaiser 18:fb85c58a4106 48
jessekaiser 21:c856b7752d7d 49 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 50 // maar de locatie van de variabele.
jessekaiser 21:c856b7752d7d 51 {
jessekaiser 21:c856b7752d7d 52 *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 53 // *in = het getal dat staat op locatie van in --> waarde van new_pwm
jessekaiser 7:697293226e5e 54 }
jessekaiser 3:dcc4cebba0d7 55
jessekaiser 25:f45d61cc6dc6 56 float pid1(float setpoint1, float measurement1)
jessekaiser 21:c856b7752d7d 57 {
jessekaiser 25:f45d61cc6dc6 58 float error1;
jessekaiser 25:f45d61cc6dc6 59 float out_p1 = 0;
jessekaiser 25:f45d61cc6dc6 60 static float out_i1 = 0;
jessekaiser 25:f45d61cc6dc6 61 error1 = (setpoint1 - measurement1);
jessekaiser 25:f45d61cc6dc6 62 out_p1 = error1*K_P1;
jessekaiser 25:f45d61cc6dc6 63 out_i1 += error1*K_I1;
jessekaiser 25:f45d61cc6dc6 64 clamp(&out_i1,-I_LIMIT,I_LIMIT);
jessekaiser 25:f45d61cc6dc6 65 return out_p1 + out_i1;
jessekaiser 24:7fbd904d191c 66 }
jessekaiser 24:7fbd904d191c 67
jessekaiser 28:4f2e15118f13 68 float pid2(float setpoint2, float measurement2)
jessekaiser 28:4f2e15118f13 69 {
jessekaiser 28:4f2e15118f13 70 float error2;
jessekaiser 28:4f2e15118f13 71 float out_p2 = 0;
jessekaiser 28:4f2e15118f13 72 static float out_i2 = 0;
jessekaiser 28:4f2e15118f13 73 error2 = (setpoint2 - measurement2);
jessekaiser 28:4f2e15118f13 74 out_p2 = error2*K_P2;
jessekaiser 28:4f2e15118f13 75 out_i2 += error2*K_I2;
jessekaiser 28:4f2e15118f13 76 clamp(&out_i2,-I_LIMIT,I_LIMIT);
jessekaiser 28:4f2e15118f13 77 return out_p2 + out_i2;
jessekaiser 28:4f2e15118f13 78 }
jessekaiser 25:f45d61cc6dc6 79 float prev_setpoint1 = 0;
jessekaiser 25:f45d61cc6dc6 80 float setpoint1 = 0;
jessekaiser 25:f45d61cc6dc6 81 float prev_setpoint2 = 0;
jessekaiser 25:f45d61cc6dc6 82 float setpoint2 = 0;
jessekaiser 25:f45d61cc6dc6 83
jessekaiser 25:f45d61cc6dc6 84 void batje_links ()
jessekaiser 24:7fbd904d191c 85 {
jessekaiser 25:f45d61cc6dc6 86 speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 25:f45d61cc6dc6 87 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 30:6ace856e0ad7 88 if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden
jessekaiser 30:6ace856e0ad7 89 setpoint1 = (180*2.3*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 90 }
jessekaiser 30:6ace856e0ad7 91 if(setpoint1 < -(180*2.3*2.0*PI/360)) {
jessekaiser 30:6ace856e0ad7 92 setpoint1 = -(180*2.3*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 93 }
jessekaiser 25:f45d61cc6dc6 94 prev_setpoint1 = setpoint1;
jessekaiser 24:7fbd904d191c 95 }
jessekaiser 24:7fbd904d191c 96
jessekaiser 24:7fbd904d191c 97 void batje_rechts ()
jessekaiser 24:7fbd904d191c 98 {
jessekaiser 32:c6f453a2fd3e 99 speed1_rad = 3.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 25:f45d61cc6dc6 100 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 32:c6f453a2fd3e 101 if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden
jessekaiser 32:c6f453a2fd3e 102 setpoint1 = (11.3*2.3*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 103 }
jessekaiser 30:6ace856e0ad7 104 if(setpoint1 < -(360*2.3*2.0*PI/360)) {
jessekaiser 30:6ace856e0ad7 105 setpoint1 = -(360*2.3*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 106 }
jessekaiser 25:f45d61cc6dc6 107 pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 25:f45d61cc6dc6 108 prev_setpoint1 = setpoint1;
jessekaiser 32:c6f453a2fd3e 109 if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
jessekaiser 30:6ace856e0ad7 110 staat1 = 1;
jessekaiser 25:f45d61cc6dc6 111 }
jessekaiser 31:0309fbf7b71c 112 }
jessekaiser 25:f45d61cc6dc6 113
jessekaiser 32:c6f453a2fd3e 114
jessekaiser 31:0309fbf7b71c 115 void batje_begin_links ()
jessekaiser 31:0309fbf7b71c 116 {
jessekaiser 31:0309fbf7b71c 117 speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 31:0309fbf7b71c 118 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 31:0309fbf7b71c 119 if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden
jessekaiser 31:0309fbf7b71c 120 setpoint1 = (180*2.3*2.0*PI/360);
jessekaiser 31:0309fbf7b71c 121 }
jessekaiser 31:0309fbf7b71c 122 if(setpoint1 < -(180*2.3*2.0*PI/360)) {
jessekaiser 31:0309fbf7b71c 123 setpoint1 = -(180*2.3*2.0*PI/360);
jessekaiser 31:0309fbf7b71c 124 }
jessekaiser 31:0309fbf7b71c 125 prev_setpoint1 = setpoint1;
jessekaiser 31:0309fbf7b71c 126 }
jessekaiser 25:f45d61cc6dc6 127
jessekaiser 31:0309fbf7b71c 128 void batje_begin_rechts ()
jessekaiser 31:0309fbf7b71c 129 {
jessekaiser 32:c6f453a2fd3e 130 speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 31:0309fbf7b71c 131 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 31:0309fbf7b71c 132 if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden
jessekaiser 31:0309fbf7b71c 133 setpoint1 = (180*2.3*2.0*PI/360);
jessekaiser 31:0309fbf7b71c 134 }
jessekaiser 32:c6f453a2fd3e 135 if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
jessekaiser 32:c6f453a2fd3e 136 setpoint1 = -(0.0*2.3*2.0*PI/360);
jessekaiser 31:0309fbf7b71c 137 }
jessekaiser 31:0309fbf7b71c 138 prev_setpoint1 = setpoint1;
jessekaiser 25:f45d61cc6dc6 139 }
jessekaiser 25:f45d61cc6dc6 140
jessekaiser 25:f45d61cc6dc6 141 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
jessekaiser 25:f45d61cc6dc6 142 {
jessekaiser 32:c6f453a2fd3e 143 speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 32:c6f453a2fd3e 144 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
jessekaiser 33:0417920456d0 145 if(setpoint2 > (360.0*2.0*PI/360)) { //setpoint in graden
jessekaiser 33:0417920456d0 146 setpoint2 = (360.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 147 }
jessekaiser 33:0417920456d0 148 if(setpoint2 < -(360.0*2.0*PI/360)) {
jessekaiser 33:0417920456d0 149 setpoint2 = -(360.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 150 }
jessekaiser 32:c6f453a2fd3e 151 prev_setpoint2 = setpoint2;
jessekaiser 33:0417920456d0 152 if(setpoint2 >= (360.0*2.0*PI/360)-0.1) {
jessekaiser 32:c6f453a2fd3e 153 staat2 = 1;
jessekaiser 26:5981d4b1144e 154 }
jessekaiser 25:f45d61cc6dc6 155 }
jessekaiser 25:f45d61cc6dc6 156
jessekaiser 25:f45d61cc6dc6 157 void arm_mid ()
jessekaiser 25:f45d61cc6dc6 158 {
jessekaiser 32:c6f453a2fd3e 159 speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 32:c6f453a2fd3e 160 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
jessekaiser 32:c6f453a2fd3e 161 if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
jessekaiser 32:c6f453a2fd3e 162 setpoint2 = (155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 163 }
jessekaiser 32:c6f453a2fd3e 164 if(setpoint2 < -(155.0*2.0*PI/360)) {
jessekaiser 32:c6f453a2fd3e 165 setpoint2 = -(155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 166 }
jessekaiser 32:c6f453a2fd3e 167 prev_setpoint2 = setpoint2;
jessekaiser 32:c6f453a2fd3e 168 if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
jessekaiser 32:c6f453a2fd3e 169 staat2 = 1;
jessekaiser 32:c6f453a2fd3e 170 }
jessekaiser 25:f45d61cc6dc6 171 }
jessekaiser 25:f45d61cc6dc6 172
jessekaiser 25:f45d61cc6dc6 173 void arm_laag ()
jessekaiser 25:f45d61cc6dc6 174 {
jessekaiser 32:c6f453a2fd3e 175 speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 32:c6f453a2fd3e 176 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
jessekaiser 32:c6f453a2fd3e 177 if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
jessekaiser 32:c6f453a2fd3e 178 setpoint2 = (155.0*2.0*PI/360);
jessekaiser 28:4f2e15118f13 179 }
jessekaiser 32:c6f453a2fd3e 180 if(setpoint2 < -(155.0*2.0*PI/360)) {
jessekaiser 32:c6f453a2fd3e 181 setpoint2 = -(155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 182 }
jessekaiser 32:c6f453a2fd3e 183 prev_setpoint2 = setpoint2;
jessekaiser 32:c6f453a2fd3e 184 if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
jessekaiser 32:c6f453a2fd3e 185 staat2 = 1;
jessekaiser 32:c6f453a2fd3e 186 }
jessekaiser 28:4f2e15118f13 187 }
jessekaiser 25:f45d61cc6dc6 188
jessekaiser 28:4f2e15118f13 189 void arm_begin ()
jessekaiser 28:4f2e15118f13 190 {
jessekaiser 33:0417920456d0 191 speed2_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 32:c6f453a2fd3e 192 setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
jessekaiser 33:0417920456d0 193 if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden
jessekaiser 33:0417920456d0 194 setpoint2 = (0.0*2.0*PI/360);
jessekaiser 28:4f2e15118f13 195 }
jessekaiser 33:0417920456d0 196 if(setpoint2 < -(0.0*2.0*PI/360)) {
jessekaiser 33:0417920456d0 197 setpoint2 = -(0.0*2.0*PI/360);
jessekaiser 28:4f2e15118f13 198 }
jessekaiser 32:c6f453a2fd3e 199 prev_setpoint2 = setpoint2;
jessekaiser 25:f45d61cc6dc6 200 }
jessekaiser 25:f45d61cc6dc6 201
jessekaiser 0:d5d3b731340c 202 int main()
jessekaiser 3:dcc4cebba0d7 203 {
jessekaiser 33:0417920456d0 204 int wait_iterator1 = 0;
jessekaiser 33:0417920456d0 205 int wait_iterator2 = 0;
jessekaiser 25:f45d61cc6dc6 206 pwm_motor1.period_us(100);
jessekaiser 25:f45d61cc6dc6 207 motor1.setPosition(0);
jessekaiser 25:f45d61cc6dc6 208 pwm_motor2.period_us(100);
jessekaiser 25:f45d61cc6dc6 209 motor2.setPosition(0);
jessekaiser 25:f45d61cc6dc6 210 pc.baud(115200);
jessekaiser 25:f45d61cc6dc6 211
jessekaiser 0:d5d3b731340c 212 while(1) {
jessekaiser 26:5981d4b1144e 213 //MOTOR1
jessekaiser 25:f45d61cc6dc6 214 pc.printf("%d \r\n", motor1.getPosition());
jessekaiser 26:5981d4b1144e 215 cur_pos_motor1 = motor1.getPosition();
jessekaiser 32:c6f453a2fd3e 216 pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //moet 4128
jessekaiser 25:f45d61cc6dc6 217 pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
jessekaiser 22:22cb158bcea4 218 if (pwm1_percentage < -1.0) {
jessekaiser 22:22cb158bcea4 219 pwm1_percentage = -1.0;
jessekaiser 24:7fbd904d191c 220 }
jessekaiser 24:7fbd904d191c 221 if (pwm1_percentage > 1.0) {
jessekaiser 24:7fbd904d191c 222 pwm1_percentage = 1.0;
jessekaiser 22:22cb158bcea4 223 }
jessekaiser 26:5981d4b1144e 224 pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 24:7fbd904d191c 225 if(pwm1_percentage > 0) {
jessekaiser 24:7fbd904d191c 226 motordir1 = 0;
jessekaiser 24:7fbd904d191c 227 } else {
jessekaiser 22:22cb158bcea4 228 motordir1 = 1;
jessekaiser 24:7fbd904d191c 229 }
jessekaiser 32:c6f453a2fd3e 230
jessekaiser 32:c6f453a2fd3e 231 //MOTOR2
jessekaiser 29:4b7f6f15cedf 232 cur_pos_motor2 = motor2.getPosition();
jessekaiser 32:c6f453a2fd3e 233 pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
jessekaiser 32:c6f453a2fd3e 234 pwm2_percentage = pid2(setpoint2, pos_motor2_rad); //
jessekaiser 29:4b7f6f15cedf 235 if (pwm2_percentage < -1.0) {
jessekaiser 29:4b7f6f15cedf 236 pwm2_percentage = -1.0;
jessekaiser 29:4b7f6f15cedf 237 }
jessekaiser 29:4b7f6f15cedf 238 if (pwm2_percentage > 1.0) {
jessekaiser 29:4b7f6f15cedf 239 pwm2_percentage = 1.0;
jessekaiser 29:4b7f6f15cedf 240 }
jessekaiser 29:4b7f6f15cedf 241 pwm_motor2.write(abs(pwm2_percentage));
jessekaiser 29:4b7f6f15cedf 242 if(pwm2_percentage > 0) {
jessekaiser 29:4b7f6f15cedf 243 motordir2 = 0;
jessekaiser 29:4b7f6f15cedf 244 } else {
jessekaiser 29:4b7f6f15cedf 245 motordir2 = 1;
jessekaiser 29:4b7f6f15cedf 246 }
jessekaiser 33:0417920456d0 247
jessekaiser 26:5981d4b1144e 248 wait(TSAMP);
jessekaiser 33:0417920456d0 249
jessekaiser 26:5981d4b1144e 250 //STATES
jessekaiser 30:6ace856e0ad7 251 if(staat1 == 0) {
jessekaiser 32:c6f453a2fd3e 252 batje_rechts();
jessekaiser 33:0417920456d0 253 wait_iterator1 = 0;
jessekaiser 32:c6f453a2fd3e 254 } else if(staat1 ==1) {
jessekaiser 33:0417920456d0 255 wait_iterator1++;
jessekaiser 33:0417920456d0 256 if(wait_iterator1 > 600)
jessekaiser 32:c6f453a2fd3e 257 staat1 = 2;
jessekaiser 32:c6f453a2fd3e 258 } else {
jessekaiser 32:c6f453a2fd3e 259 batje_begin_rechts();
jessekaiser 32:c6f453a2fd3e 260 }
jessekaiser 32:c6f453a2fd3e 261
jessekaiser 32:c6f453a2fd3e 262
jessekaiser 32:c6f453a2fd3e 263 if(staat2 == 0) {
jessekaiser 25:f45d61cc6dc6 264 arm_hoog();
jessekaiser 33:0417920456d0 265 wait_iterator2 = 0;
jessekaiser 33:0417920456d0 266 } else if(staat2 == 1) {
jessekaiser 33:0417920456d0 267 wait_iterator2++;
jessekaiser 33:0417920456d0 268 if(wait_iterator2 > 200)
jessekaiser 32:c6f453a2fd3e 269 staat2 = 2;
jessekaiser 32:c6f453a2fd3e 270 } else {
jessekaiser 28:4f2e15118f13 271 arm_begin();
jessekaiser 24:7fbd904d191c 272 }
jessekaiser 32:c6f453a2fd3e 273 }
jessekaiser 0:d5d3b731340c 274 }