Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@11:ba0a33e6ff0d, 2014-10-28 (annotated)

Committer:
jessekaiser
Date:
Tue Oct 28 00:22:43 2014 +0000
Revision:
11:ba0a33e6ff0d
Parent:
10:a2a2e7bfdc86
Child:
12:795657e66960
Kan hoeken instellen van motor1 met behulp van excel en Putty. Nog geen regelaar ingesteld;

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 11:ba0a33e6ff0d 35 float hoek1;
jessekaiser 11:ba0a33e6ff0d 36
jessekaiser 7:697293226e5e 37 bool flip=false;
jessekaiser 7:697293226e5e 38
jessekaiser 7:697293226e5e 39 void attime()
jessekaiser 0:d5d3b731340c 40 {
jessekaiser 7:697293226e5e 41 flip = !flip;
jessekaiser 0:d5d3b731340c 42 }
jessekaiser 0:d5d3b731340c 43
jessekaiser 7:697293226e5e 44 void looper()
jessekaiser 7:697293226e5e 45 {
jessekaiser 10:a2a2e7bfdc86 46 /*motordir1=0;
jessekaiser 10:a2a2e7bfdc86 47 pwm_motor1.write(0.06);
jessekaiser 7:697293226e5e 48 scope.set(0, motor1.getPosition());
jessekaiser 8:0bbbe93bd1c4 49 scope.set(1, motor1.getSpeed());
jessekaiser 8:0bbbe93bd1c4 50 scope.set(2, motor2.getPosition());
jessekaiser 10:a2a2e7bfdc86 51 scope.set(3, motor2.getSpeed());
jessekaiser 10:a2a2e7bfdc86 52 scope.send();*/
jessekaiser 7:697293226e5e 53 }
jessekaiser 3:dcc4cebba0d7 54
jessekaiser 8:0bbbe93bd1c4 55 /*void clamp(float * in, float min, float max)
jessekaiser 8:0bbbe93bd1c4 56 {
jessekaiser 8:0bbbe93bd1c4 57 *in > min ? *in < max? : *in = max: *in = min;
jessekaiser 8:0bbbe93bd1c4 58 }
jessekaiser 8:0bbbe93bd1c4 59
jessekaiser 8:0bbbe93bd1c4 60 float pi(float setpoint, float measurement)
jessekaiser 8:0bbbe93bd1c4 61 {
jessekaiser 8:0bbbe93bd1c4 62 float error;
jessekaiser 8:0bbbe93bd1c4 63 float out_p = 0;
jessekaiser 8:0bbbe93bd1c4 64 static float out_i = 0;
jessekaiser 8:0bbbe93bd1c4 65 error = setpoint - motor1.getPosition();
jessekaiser 8:0bbbe93bd1c4 66 out_p = error*K_P;
jessekaiser 8:0bbbe93bd1c4 67 out_i += error*K_I;
jessekaiser 8:0bbbe93bd1c4 68 clamp(&out_i,-I_LIMIT,I_LIMIT);
jessekaiser 8:0bbbe93bd1c4 69
jessekaiser 8:0bbbe93bd1c4 70 return out_p + out_i;
jessekaiser 8:0bbbe93bd1c4 71 }*/
jessekaiser 3:dcc4cebba0d7 72
jessekaiser 0:d5d3b731340c 73 int main()
jessekaiser 10:a2a2e7bfdc86 74 {
jessekaiser 11:ba0a33e6ff0d 75 speed1 = 0.7;
jessekaiser 11:ba0a33e6ff0d 76 hoek1 = 0.09; //in seconde
jessekaiser 10:a2a2e7bfdc86 77 wait(1);
jessekaiser 8:0bbbe93bd1c4 78 pwm_motor1.period_us(100);
jessekaiser 11:ba0a33e6ff0d 79 motordir1=0; //aangeven van directie
jessekaiser 11:ba0a33e6ff0d 80 pwm_motor1.write(speed1); //snelheid van de motor
jessekaiser 11:ba0a33e6ff0d 81 wait(hoek1); //Hierdoor kun je het aantal graden bepalen die de as draait
jessekaiser 8:0bbbe93bd1c4 82 pwm_motor1.write(0);
jessekaiser 8:0bbbe93bd1c4 83 wait(1);
jessekaiser 8:0bbbe93bd1c4 84 motordir1=1;
jessekaiser 11:ba0a33e6ff0d 85 pwm_motor1.write(speed1);
jessekaiser 11:ba0a33e6ff0d 86 wait(hoek1);
jessekaiser 10:a2a2e7bfdc86 87 pwm_motor1.write(0);
jessekaiser 10:a2a2e7bfdc86 88 wait(1);
jessekaiser 10:a2a2e7bfdc86 89 motordir1=1;
jessekaiser 11:ba0a33e6ff0d 90 pwm_motor1.write(speed1);
jessekaiser 11:ba0a33e6ff0d 91 wait(hoek1);
jessekaiser 11:ba0a33e6ff0d 92 pwm_motor1.write(0);
jessekaiser 11:ba0a33e6ff0d 93 wait(1);
jessekaiser 11:ba0a33e6ff0d 94 motordir1=0;
jessekaiser 11:ba0a33e6ff0d 95 pwm_motor1.write(speed1);
jessekaiser 11:ba0a33e6ff0d 96 wait(hoek1);
jessekaiser 11:ba0a33e6ff0d 97 pwm_motor1.write(0);
jessekaiser 11:ba0a33e6ff0d 98
jessekaiser 11:ba0a33e6ff0d 99
jessekaiser 10:a2a2e7bfdc86 100
jessekaiser 10:a2a2e7bfdc86 101
jessekaiser 9:18e7a0273106 102
jessekaiser 7:697293226e5e 103 Ticker log_timer;
jessekaiser 7:697293226e5e 104 Ticker timer;
jessekaiser 7:697293226e5e 105 log_timer.attach(looper, TSAMP);
jessekaiser 7:697293226e5e 106 timer.attach(&attime, 7);
jessekaiser 8:0bbbe93bd1c4 107
jessekaiser 9:18e7a0273106 108 pc.baud(115200);
jessekaiser 0:d5d3b731340c 109 while(1) {
jessekaiser 9:18e7a0273106 110 wait(0.2);
jessekaiser 10:a2a2e7bfdc86 111 pc.printf("%d, %f \r\n", motor1.getPosition(), motor1.getSpeed());
jessekaiser 6:f260176f704b 112 }
jessekaiser 0:d5d3b731340c 113 }