Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@8:0bbbe93bd1c4, 2014-10-27 (annotated)

Committer:
jessekaiser
Date:
Mon Oct 27 13:13:01 2014 +0000
Revision:
8:0bbbe93bd1c4
Parent:
7:697293226e5e
Child:
9:18e7a0273106
Snap niet waarom de snelheid niet veranderd;

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 0:d5d3b731340c 4
jessekaiser 8:0bbbe93bd1c4 5 #define TSAMP 0.001
jessekaiser 0:d5d3b731340c 6 #define K_P (0.1)
jessekaiser 0:d5d3b731340c 7 #define K_I (0.03 *TSAMP)
jessekaiser 0:d5d3b731340c 8 #define K_D (0.001 /TSAMP)
jessekaiser 0:d5d3b731340c 9 #define I_LIMIT 1.
jessekaiser 0:d5d3b731340c 10
jessekaiser 7:697293226e5e 11 #define M1_PWM PTC8
jessekaiser 7:697293226e5e 12 #define M1_DIR PTC9
jessekaiser 7:697293226e5e 13 #define M2_PWM PTA5
jessekaiser 7:697293226e5e 14 #define M2_DIR PTA4
jessekaiser 7:697293226e5e 15
jessekaiser 7:697293226e5e 16 //Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting
jessekaiser 0:d5d3b731340c 17
jessekaiser 3:dcc4cebba0d7 18 Serial pc(USBTX, USBRX);
jessekaiser 4:db3dad7e53e3 19 DigitalOut led1(LED_RED);
jessekaiser 8:0bbbe93bd1c4 20 HIDScope scope(3);
jessekaiser 0:d5d3b731340c 21
jessekaiser 7:697293226e5e 22 //motor 25D
jessekaiser 7:697293226e5e 23 Encoder motor1(PTD3,PTD5); //wit, geel
jessekaiser 7:697293226e5e 24 PwmOut pwm_motor1(M2_PWM);
jessekaiser 7:697293226e5e 25 DigitalOut motordir1(M2_DIR);
jessekaiser 3:dcc4cebba0d7 26
jessekaiser 7:697293226e5e 27 //motor2 37D
jessekaiser 7:697293226e5e 28 Encoder motor2(PTD2, PTD0); //wit, geel
jessekaiser 7:697293226e5e 29 PwmOut pwm_motor2(M1_PWM);
jessekaiser 7:697293226e5e 30 DigitalOut motordir2(M1_DIR);
jessekaiser 7:697293226e5e 31
jessekaiser 7:697293226e5e 32 bool flip=false;
jessekaiser 7:697293226e5e 33
jessekaiser 7:697293226e5e 34 void attime()
jessekaiser 0:d5d3b731340c 35 {
jessekaiser 7:697293226e5e 36 flip = !flip;
jessekaiser 0:d5d3b731340c 37 }
jessekaiser 0:d5d3b731340c 38
jessekaiser 7:697293226e5e 39 void looper()
jessekaiser 7:697293226e5e 40 {
jessekaiser 7:697293226e5e 41 motordir1=0;
jessekaiser 8:0bbbe93bd1c4 42 pwm_motor1.write(0.06);
jessekaiser 7:697293226e5e 43 scope.set(0, motor1.getPosition());
jessekaiser 8:0bbbe93bd1c4 44 scope.set(1, motor1.getSpeed());
jessekaiser 8:0bbbe93bd1c4 45 scope.set(2, motor2.getPosition());
jessekaiser 7:697293226e5e 46 scope.send();
jessekaiser 7:697293226e5e 47 }
jessekaiser 3:dcc4cebba0d7 48
jessekaiser 8:0bbbe93bd1c4 49 /*void clamp(float * in, float min, float max)
jessekaiser 8:0bbbe93bd1c4 50 {
jessekaiser 8:0bbbe93bd1c4 51 *in > min ? *in < max? : *in = max: *in = min;
jessekaiser 8:0bbbe93bd1c4 52 }
jessekaiser 8:0bbbe93bd1c4 53
jessekaiser 8:0bbbe93bd1c4 54 float pi(float setpoint, float measurement)
jessekaiser 8:0bbbe93bd1c4 55 {
jessekaiser 8:0bbbe93bd1c4 56 float error;
jessekaiser 8:0bbbe93bd1c4 57 float out_p = 0;
jessekaiser 8:0bbbe93bd1c4 58 static float out_i = 0;
jessekaiser 8:0bbbe93bd1c4 59 error = setpoint - motor1.getPosition();
jessekaiser 8:0bbbe93bd1c4 60 out_p = error*K_P;
jessekaiser 8:0bbbe93bd1c4 61 out_i += error*K_I;
jessekaiser 8:0bbbe93bd1c4 62 clamp(&out_i,-I_LIMIT,I_LIMIT);
jessekaiser 8:0bbbe93bd1c4 63
jessekaiser 8:0bbbe93bd1c4 64 return out_p + out_i;
jessekaiser 8:0bbbe93bd1c4 65 }*/
jessekaiser 3:dcc4cebba0d7 66
jessekaiser 0:d5d3b731340c 67 int main()
jessekaiser 3:dcc4cebba0d7 68 {
jessekaiser 8:0bbbe93bd1c4 69 wait(1);
jessekaiser 8:0bbbe93bd1c4 70 /*motordir1=0;
jessekaiser 8:0bbbe93bd1c4 71 pwm_motor1.period_us(100);
jessekaiser 8:0bbbe93bd1c4 72 pwm_motor1.write(1);
jessekaiser 8:0bbbe93bd1c4 73 wait(0.034);
jessekaiser 8:0bbbe93bd1c4 74 pwm_motor1.write(0);
jessekaiser 8:0bbbe93bd1c4 75 wait(1);
jessekaiser 8:0bbbe93bd1c4 76 motordir1=1;
jessekaiser 8:0bbbe93bd1c4 77 pwm_motor1.write(1);
jessekaiser 8:0bbbe93bd1c4 78 wait(0.034);
jessekaiser 8:0bbbe93bd1c4 79 pwm_motor1.write(0);*/
jessekaiser 7:697293226e5e 80 Ticker log_timer;
jessekaiser 7:697293226e5e 81 Ticker timer;
jessekaiser 7:697293226e5e 82 log_timer.attach(looper, TSAMP);
jessekaiser 7:697293226e5e 83 timer.attach(&attime, 7);
jessekaiser 8:0bbbe93bd1c4 84
jessekaiser 0:d5d3b731340c 85 while(1) {
jessekaiser 6:f260176f704b 86
jessekaiser 6:f260176f704b 87 }
jessekaiser 0:d5d3b731340c 88 }