Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@4:db3dad7e53e3, 2014-10-22 (annotated)

Committer:
jessekaiser
Date:
Wed Oct 22 13:23:30 2014 +0000
Revision:
4:db3dad7e53e3
Parent:
3:dcc4cebba0d7
Child:
5:e4b9cc904928
Motor reageert op pc input

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 0:d5d3b731340c 5 #define TSAMP 0.01
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 0:d5d3b731340c 11 #define M1_PWM PTC8 //blauw
jessekaiser 0:d5d3b731340c 12 #define M1_DIR PTC9 //groen
jessekaiser 2:11076f69e0a7 13 #define M2_PWM PTA5 //blauw
jessekaiser 2:11076f69e0a7 14 #define M2_DIR PTA4 //groen
jessekaiser 0:d5d3b731340c 15
jessekaiser 3:dcc4cebba0d7 16 Serial pc(USBTX, USBRX);
jessekaiser 4:db3dad7e53e3 17 DigitalOut led1(LED_RED);
jessekaiser 3:dcc4cebba0d7 18
jessekaiser 3:dcc4cebba0d7 19
jessekaiser 0:d5d3b731340c 20 void clamp(float * in, float min, float max);
jessekaiser 0:d5d3b731340c 21 float pid(float setpoint, float measurement);
jessekaiser 0:d5d3b731340c 22 volatile bool looptimerflag;
jessekaiser 0:d5d3b731340c 23 HIDScope scope(6);
jessekaiser 0:d5d3b731340c 24
jessekaiser 3:dcc4cebba0d7 25
jessekaiser 0:d5d3b731340c 26 void setlooptimerflag(void)
jessekaiser 0:d5d3b731340c 27 {
jessekaiser 0:d5d3b731340c 28 looptimerflag = true;
jessekaiser 0:d5d3b731340c 29 }
jessekaiser 0:d5d3b731340c 30
jessekaiser 3:dcc4cebba0d7 31
jessekaiser 3:dcc4cebba0d7 32
jessekaiser 0:d5d3b731340c 33 int main()
jessekaiser 3:dcc4cebba0d7 34 {
jessekaiser 3:dcc4cebba0d7 35 //Let op dat de jumpers goed staan als het motortje niet wilt draaien. De E1 jumper moet onder de nummer 7 pin. De locatie van de M1 pin
jessekaiser 3:dcc4cebba0d7 36 //bepaalt of de motor CW of CCW draait.
jessekaiser 0:d5d3b731340c 37 //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter
jessekaiser 3:dcc4cebba0d7 38
jessekaiser 3:dcc4cebba0d7 39 //motor 25D
jessekaiser 0:d5d3b731340c 40 Encoder motor1(PTD3,PTD5); //wit, geel
jessekaiser 3:dcc4cebba0d7 41 PwmOut pwm_motor1(M2_PWM); // PTC8, blauw, /*PwmOut to motor driver*/
jessekaiser 2:11076f69e0a7 42 pwm_motor1.period_us(75); //10kHz PWM frequency
jessekaiser 3:dcc4cebba0d7 43 DigitalOut motordir1(M2_DIR); //PTC9, groen
jessekaiser 3:dcc4cebba0d7 44
jessekaiser 3:dcc4cebba0d7 45 //motor2 37D
jessekaiser 2:11076f69e0a7 46 Encoder motor2(PTD2, PTD0); //wit, geel
jessekaiser 3:dcc4cebba0d7 47 PwmOut pwm_motor2(M1_PWM); //PTA5, blauw
jessekaiser 2:11076f69e0a7 48 pwm_motor2.period_us(75);
jessekaiser 3:dcc4cebba0d7 49 DigitalOut motordir2(M1_DIR); //PTA4, groen
jessekaiser 3:dcc4cebba0d7 50
jessekaiser 4:db3dad7e53e3 51 char c ='0';
jessekaiser 0:d5d3b731340c 52 while(1) {
jessekaiser 4:db3dad7e53e3 53 pwm_motor1.write(1);
jessekaiser 4:db3dad7e53e3 54 motordir1.write(1);
jessekaiser 4:db3dad7e53e3 55 do {
jessekaiser 4:db3dad7e53e3 56
jessekaiser 4:db3dad7e53e3 57 if(pc.readable()) {
jessekaiser 4:db3dad7e53e3 58 c = pc.getc();
jessekaiser 4:db3dad7e53e3 59 }
jessekaiser 4:db3dad7e53e3 60 }
jessekaiser 4:db3dad7e53e3 61 while((c !='1'));
jessekaiser 4:db3dad7e53e3 62 {
jessekaiser 4:db3dad7e53e3 63 c = '0';
jessekaiser 4:db3dad7e53e3 64 pwm_motor1.write(0.5);
jessekaiser 4:db3dad7e53e3 65 motordir1.write(0);
jessekaiser 4:db3dad7e53e3 66 wait(3);
jessekaiser 4:db3dad7e53e3 67 }
jessekaiser 0:d5d3b731340c 68 }
jessekaiser 0:d5d3b731340c 69 }
jessekaiser 0:d5d3b731340c 70
jessekaiser 0:d5d3b731340c 71 //clamps value 'in' to min or max when exceeding those values
jessekaiser 0:d5d3b731340c 72 //if you'd like to understand the statement below take a google for
jessekaiser 0:d5d3b731340c 73 //'ternary operators'.
jessekaiser 0:d5d3b731340c 74 void clamp(float * in, float min, float max)
jessekaiser 0:d5d3b731340c 75 {
jessekaiser 3:dcc4cebba0d7 76 *in > min ? *in < max? : *in = max: *in = min;
jessekaiser 0:d5d3b731340c 77 }
jessekaiser 0:d5d3b731340c 78
jessekaiser 0:d5d3b731340c 79
jessekaiser 0:d5d3b731340c 80 float pid(float setpoint, float measurement)
jessekaiser 0:d5d3b731340c 81 {
jessekaiser 0:d5d3b731340c 82 float error;
jessekaiser 0:d5d3b731340c 83 static float prev_error = 0;
jessekaiser 0:d5d3b731340c 84 float out_p = 0;
jessekaiser 0:d5d3b731340c 85 static float out_i = 0;
jessekaiser 0:d5d3b731340c 86 float out_d = 0;
jessekaiser 0:d5d3b731340c 87 error = setpoint-measurement;
jessekaiser 0:d5d3b731340c 88 out_p = error*K_P;
jessekaiser 0:d5d3b731340c 89 out_i += error*K_I;
jessekaiser 0:d5d3b731340c 90 out_d = (error-prev_error)*K_D;
jessekaiser 0:d5d3b731340c 91 clamp(&out_i,-I_LIMIT,I_LIMIT);
jessekaiser 0:d5d3b731340c 92 prev_error = error;
jessekaiser 0:d5d3b731340c 93 scope.set(1,out_p);
jessekaiser 0:d5d3b731340c 94 scope.set(2,out_i);
jessekaiser 0:d5d3b731340c 95 scope.set(3,out_d);
jessekaiser 0:d5d3b731340c 96 return out_p + out_i + out_d;
jessekaiser 0:d5d3b731340c 97 }