Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@14:affdb0636f24, 2014-10-28 (annotated)

Committer:
jessekaiser
Date:
Tue Oct 28 13:16:00 2014 +0000
Revision:
14:affdb0636f24
Parent:
13:7fe679538649
Child:
15:7d142004b400
Afstanden motor1 en motor2 afgesteld. Zodra de arm erop zit zal dit nog wel wat aangepast moeten worden. PI regelaar niet ingesteld (weet nog niet hoe).

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 9:18e7a0273106 4 #include "MODSERIAL.h"
jessekaiser 0:d5d3b731340c 5
jessekaiser 9:18e7a0273106 6
jessekaiser 9:18e7a0273106 7 #define TSAMP 0.01
jessekaiser 0:d5d3b731340c 8 #define K_P (0.1)
jessekaiser 0:d5d3b731340c 9 #define K_I (0.03 *TSAMP)
jessekaiser 0:d5d3b731340c 10 #define K_D (0.001 /TSAMP)
jessekaiser 0:d5d3b731340c 11 #define I_LIMIT 1.
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 9:18e7a0273106 22 HIDScope scope(4);
jessekaiser 0:d5d3b731340c 23
jessekaiser 7:697293226e5e 24 //motor 25D
jessekaiser 9:18e7a0273106 25 Encoder motor1(PTD3,PTD5,true); //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 9:18e7a0273106 30 Encoder motor2(PTD2, PTD0,true); //wit, geel
jessekaiser 7:697293226e5e 31 PwmOut pwm_motor2(M1_PWM);
jessekaiser 7:697293226e5e 32 DigitalOut motordir2(M1_DIR);
jessekaiser 7:697293226e5e 33
jessekaiser 11:ba0a33e6ff0d 34 float speed1;
jessekaiser 12:795657e66960 35 float hoek1;
jessekaiser 12:795657e66960 36 float speed2;
jessekaiser 12:795657e66960 37 float hoek2;
jessekaiser 11:ba0a33e6ff0d 38
jessekaiser 7:697293226e5e 39 bool flip=false;
jessekaiser 7:697293226e5e 40
jessekaiser 7:697293226e5e 41 void attime()
jessekaiser 0:d5d3b731340c 42 {
jessekaiser 7:697293226e5e 43 flip = !flip;
jessekaiser 0:d5d3b731340c 44 }
jessekaiser 0:d5d3b731340c 45
jessekaiser 7:697293226e5e 46 void looper()
jessekaiser 7:697293226e5e 47 {
jessekaiser 10:a2a2e7bfdc86 48 /*motordir1=0;
jessekaiser 10:a2a2e7bfdc86 49 pwm_motor1.write(0.06);
jessekaiser 7:697293226e5e 50 scope.set(0, motor1.getPosition());
jessekaiser 8:0bbbe93bd1c4 51 scope.set(1, motor1.getSpeed());
jessekaiser 8:0bbbe93bd1c4 52 scope.set(2, motor2.getPosition());
jessekaiser 10:a2a2e7bfdc86 53 scope.set(3, motor2.getSpeed());
jessekaiser 10:a2a2e7bfdc86 54 scope.send();*/
jessekaiser 7:697293226e5e 55 }
jessekaiser 3:dcc4cebba0d7 56
jessekaiser 12:795657e66960 57 void clamp(float * in, float min, float max)
jessekaiser 8:0bbbe93bd1c4 58 {
jessekaiser 8:0bbbe93bd1c4 59 *in > min ? *in < max? : *in = max: *in = min;
jessekaiser 8:0bbbe93bd1c4 60 }
jessekaiser 8:0bbbe93bd1c4 61
jessekaiser 8:0bbbe93bd1c4 62 float pi(float setpoint, float measurement)
jessekaiser 8:0bbbe93bd1c4 63 {
jessekaiser 8:0bbbe93bd1c4 64 float error;
jessekaiser 8:0bbbe93bd1c4 65 float out_p = 0;
jessekaiser 8:0bbbe93bd1c4 66 static float out_i = 0;
jessekaiser 8:0bbbe93bd1c4 67 error = setpoint - motor1.getPosition();
jessekaiser 8:0bbbe93bd1c4 68 out_p = error*K_P;
jessekaiser 8:0bbbe93bd1c4 69 out_i += error*K_I;
jessekaiser 8:0bbbe93bd1c4 70 clamp(&out_i,-I_LIMIT,I_LIMIT);
jessekaiser 8:0bbbe93bd1c4 71
jessekaiser 8:0bbbe93bd1c4 72 return out_p + out_i;
jessekaiser 12:795657e66960 73 }
jessekaiser 3:dcc4cebba0d7 74
jessekaiser 0:d5d3b731340c 75 int main()
jessekaiser 12:795657e66960 76 {
jessekaiser 14:affdb0636f24 77 //motor1
jessekaiser 12:795657e66960 78 speed1 = 0.7;
jessekaiser 11:ba0a33e6ff0d 79 hoek1 = 0.09; //in seconde
jessekaiser 10:a2a2e7bfdc86 80 wait(1);
jessekaiser 14:affdb0636f24 81 pwm_motor1.write(0.1);
jessekaiser 8:0bbbe93bd1c4 82 pwm_motor1.period_us(100);
jessekaiser 14:affdb0636f24 83 motordir1=0; //aangeven van directie (0 = CCW)
jessekaiser 11:ba0a33e6ff0d 84 pwm_motor1.write(speed1); //snelheid van de motor
jessekaiser 11:ba0a33e6ff0d 85 wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait
jessekaiser 8:0bbbe93bd1c4 86 pwm_motor1.write(0);
jessekaiser 8:0bbbe93bd1c4 87 wait(1);
jessekaiser 8:0bbbe93bd1c4 88 motordir1=1;
jessekaiser 11:ba0a33e6ff0d 89 pwm_motor1.write(speed1);
jessekaiser 11:ba0a33e6ff0d 90 wait(hoek1);
jessekaiser 10:a2a2e7bfdc86 91 pwm_motor1.write(0);
jessekaiser 10:a2a2e7bfdc86 92 wait(1);
jessekaiser 10:a2a2e7bfdc86 93 motordir1=1;
jessekaiser 11:ba0a33e6ff0d 94 pwm_motor1.write(speed1);
jessekaiser 11:ba0a33e6ff0d 95 wait(hoek1);
jessekaiser 11:ba0a33e6ff0d 96 pwm_motor1.write(0);
jessekaiser 11:ba0a33e6ff0d 97 wait(1);
jessekaiser 11:ba0a33e6ff0d 98 motordir1=0;
jessekaiser 11:ba0a33e6ff0d 99 pwm_motor1.write(speed1);
jessekaiser 12:795657e66960 100 wait(hoek1);
jessekaiser 12:795657e66960 101 pwm_motor1.write(0);
jessekaiser 14:affdb0636f24 102
jessekaiser 12:795657e66960 103
jessekaiser 12:795657e66960 104 //motor2
jessekaiser 14:affdb0636f24 105 /*wait(1);
jessekaiser 12:795657e66960 106 speed2 = 1;
jessekaiser 12:795657e66960 107 motordir1=1;
jessekaiser 13:7fe679538649 108 pwm_motor1.write(0.1); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
jessekaiser 13:7fe679538649 109 wait(2.7); //naar 140 graden
jessekaiser 13:7fe679538649 110 pwm_motor1.write(0); //CCW
jessekaiser 12:795657e66960 111 wait(1);
jessekaiser 12:795657e66960 112 motordir1=0;
jessekaiser 12:795657e66960 113 pwm_motor1.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
jessekaiser 13:7fe679538649 114 wait(0.2); //balletje slaan, 160 graden
jessekaiser 12:795657e66960 115 pwm_motor1.write(0);
jessekaiser 12:795657e66960 116 wait(1);
jessekaiser 13:7fe679538649 117 motordir1=1; //CW
jessekaiser 13:7fe679538649 118 pwm_motor1.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
jessekaiser 13:7fe679538649 119 wait(0.5); //terug naar begin positie, 20 graden
jessekaiser 14:affdb0636f24 120 pwm_motor1.write(0); */
jessekaiser 9:18e7a0273106 121
jessekaiser 7:697293226e5e 122 Ticker log_timer;
jessekaiser 7:697293226e5e 123 Ticker timer;
jessekaiser 7:697293226e5e 124 log_timer.attach(looper, TSAMP);
jessekaiser 7:697293226e5e 125 timer.attach(&attime, 7);
jessekaiser 8:0bbbe93bd1c4 126
jessekaiser 9:18e7a0273106 127 pc.baud(115200);
jessekaiser 0:d5d3b731340c 128 while(1) {
jessekaiser 13:7fe679538649 129 wait(0.2);
jessekaiser 14:affdb0636f24 130 pc.printf("%d\r\n", motor1.getPosition());
jessekaiser 6:f260176f704b 131 }
jessekaiser 0:d5d3b731340c 132 }