Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

Committer:
jessekaiser
Date:
Wed Oct 29 15:06:18 2014 +0000
Revision:
17:3ddab322341a
Parent:
16:f4df06bf58ab
derp

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 17:3ddab322341a 22 HIDScope scope(2);
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 17:3ddab322341a 48 wait(15);
jessekaiser 17:3ddab322341a 49 motordir1=0;
jessekaiser 17:3ddab322341a 50 pwm_motor1.write(1);
jessekaiser 7:697293226e5e 51 scope.set(0, motor1.getPosition());
jessekaiser 17:3ddab322341a 52 scope.set(1, motor2.getPosition());
jessekaiser 17:3ddab322341a 53 scope.send();
jessekaiser 12:795657e66960 54 }
jessekaiser 3:dcc4cebba0d7 55
jessekaiser 0:d5d3b731340c 56 int main()
jessekaiser 12:795657e66960 57 {
jessekaiser 16:f4df06bf58ab 58 Ticker log_timer;
jessekaiser 16:f4df06bf58ab 59 Ticker timer;
jessekaiser 16:f4df06bf58ab 60 log_timer.attach(looper, TSAMP);
jessekaiser 16:f4df06bf58ab 61 timer.attach(&attime, 7);
jessekaiser 16:f4df06bf58ab 62
jessekaiser 14:affdb0636f24 63 //motor1
jessekaiser 15:7d142004b400 64 /*speed1 = 0.7;
jessekaiser 11:ba0a33e6ff0d 65 hoek1 = 0.09; //in seconde
jessekaiser 10:a2a2e7bfdc86 66 wait(1);
jessekaiser 14:affdb0636f24 67 pwm_motor1.write(0.1);
jessekaiser 8:0bbbe93bd1c4 68 pwm_motor1.period_us(100);
jessekaiser 14:affdb0636f24 69 motordir1=0; //aangeven van directie (0 = CCW)
jessekaiser 11:ba0a33e6ff0d 70 pwm_motor1.write(speed1); //snelheid van de motor
jessekaiser 11:ba0a33e6ff0d 71 wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait
jessekaiser 8:0bbbe93bd1c4 72 pwm_motor1.write(0);
jessekaiser 8:0bbbe93bd1c4 73 wait(1);
jessekaiser 8:0bbbe93bd1c4 74 motordir1=1;
jessekaiser 11:ba0a33e6ff0d 75 pwm_motor1.write(speed1);
jessekaiser 11:ba0a33e6ff0d 76 wait(hoek1);
jessekaiser 10:a2a2e7bfdc86 77 pwm_motor1.write(0);
jessekaiser 10:a2a2e7bfdc86 78 wait(1);
jessekaiser 10:a2a2e7bfdc86 79 motordir1=1;
jessekaiser 11:ba0a33e6ff0d 80 pwm_motor1.write(speed1);
jessekaiser 11:ba0a33e6ff0d 81 wait(hoek1);
jessekaiser 11:ba0a33e6ff0d 82 pwm_motor1.write(0);
jessekaiser 11:ba0a33e6ff0d 83 wait(1);
jessekaiser 11:ba0a33e6ff0d 84 motordir1=0;
jessekaiser 11:ba0a33e6ff0d 85 pwm_motor1.write(speed1);
jessekaiser 12:795657e66960 86 wait(hoek1);
jessekaiser 12:795657e66960 87 pwm_motor1.write(0);
jessekaiser 15:7d142004b400 88 */
jessekaiser 12:795657e66960 89
jessekaiser 12:795657e66960 90 //motor2
jessekaiser 17:3ddab322341a 91 /*wait(1);
jessekaiser 12:795657e66960 92 speed2 = 1;
jessekaiser 12:795657e66960 93 motordir1=1;
jessekaiser 15:7d142004b400 94 pwm_motor1.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
jessekaiser 16:f4df06bf58ab 95 wait(0.3); //naar 140 graden
jessekaiser 13:7fe679538649 96 pwm_motor1.write(0); //CCW
jessekaiser 12:795657e66960 97 wait(1);
jessekaiser 12:795657e66960 98 motordir1=0;
jessekaiser 12:795657e66960 99 pwm_motor1.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
jessekaiser 16:f4df06bf58ab 100 wait(0.20); //balletje slaan, 160 graden
jessekaiser 12:795657e66960 101 pwm_motor1.write(0);
jessekaiser 12:795657e66960 102 wait(1);
jessekaiser 13:7fe679538649 103 motordir1=1; //CW
jessekaiser 13:7fe679538649 104 pwm_motor1.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
jessekaiser 16:f4df06bf58ab 105 wait(1); //terug naar begin positie, 20 graden
jessekaiser 17:3ddab322341a 106 pwm_motor1.write(0); */
jessekaiser 9:18e7a0273106 107
jessekaiser 16:f4df06bf58ab 108
jessekaiser 9:18e7a0273106 109 pc.baud(115200);
jessekaiser 0:d5d3b731340c 110 while(1) {
jessekaiser 13:7fe679538649 111 wait(0.2);
jessekaiser 14:affdb0636f24 112 pc.printf("%d\r\n", motor1.getPosition());
jessekaiser 6:f260176f704b 113 }
jessekaiser 0:d5d3b731340c 114 }