Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@1:97d6b160f708, 2014-10-21 (annotated)

Committer:
jessekaiser
Date:
Tue Oct 21 13:24:23 2014 +0000
Revision:
1:97d6b160f708
Parent:
0:d5d3b731340c
Child:
2:11076f69e0a7
Code voor aansturen motor;

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 0:d5d3b731340c 13 #define M2_PWM PTA5
jessekaiser 0:d5d3b731340c 14 #define M2_DIR PTA4
jessekaiser 0:d5d3b731340c 15
jessekaiser 0:d5d3b731340c 16
jessekaiser 0:d5d3b731340c 17 void clamp(float * in, float min, float max);
jessekaiser 0:d5d3b731340c 18 float pid(float setpoint, float measurement);
jessekaiser 0:d5d3b731340c 19 volatile bool looptimerflag;
jessekaiser 0:d5d3b731340c 20 HIDScope scope(6);
jessekaiser 0:d5d3b731340c 21
jessekaiser 0:d5d3b731340c 22 void setlooptimerflag(void)
jessekaiser 0:d5d3b731340c 23 {
jessekaiser 0:d5d3b731340c 24 looptimerflag = true;
jessekaiser 0:d5d3b731340c 25 }
jessekaiser 0:d5d3b731340c 26
jessekaiser 0:d5d3b731340c 27 int main()
jessekaiser 1:97d6b160f708 28 { //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 1:97d6b160f708 29 //bepaalt of de motor CW of CCW draait.
jessekaiser 0:d5d3b731340c 30 //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter
jessekaiser 0:d5d3b731340c 31 Encoder motor1(PTD3,PTD5); //wit, geel
jessekaiser 0:d5d3b731340c 32 /*PwmOut to motor driver*/
jessekaiser 0:d5d3b731340c 33 PwmOut pwm_motor(M1_PWM); // PTC8, blauw
jessekaiser 0:d5d3b731340c 34 //10kHz PWM frequency
jessekaiser 0:d5d3b731340c 35 pwm_motor.period_us(75);
jessekaiser 0:d5d3b731340c 36 DigitalOut motordir(M1_DIR); //PTC9, groen
jessekaiser 0:d5d3b731340c 37 Ticker looptimer;
jessekaiser 0:d5d3b731340c 38 looptimer.attach(setlooptimerflag,TSAMP);
jessekaiser 0:d5d3b731340c 39 while(1) {
jessekaiser 0:d5d3b731340c 40 int16_t setpoint;
jessekaiser 0:d5d3b731340c 41 float new_pwm;
jessekaiser 0:d5d3b731340c 42 /*wait until timer has elapsed*/
jessekaiser 0:d5d3b731340c 43 while(!looptimerflag);
jessekaiser 0:d5d3b731340c 44 looptimerflag = false; //clear flag
jessekaiser 0:d5d3b731340c 45 /*potmeter value: 0-1*/
jessekaiser 0:d5d3b731340c 46 setpoint = (potmeter.read()-.5)*500;
jessekaiser 0:d5d3b731340c 47 /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
jessekaiser 0:d5d3b731340c 48 new_pwm = pid(setpoint, motor1.getPosition());
jessekaiser 0:d5d3b731340c 49 clamp(&new_pwm, -1,1);
jessekaiser 0:d5d3b731340c 50 scope.set(0, setpoint);
jessekaiser 0:d5d3b731340c 51 scope.set(4, new_pwm);
jessekaiser 0:d5d3b731340c 52 scope.set(5, motor1.getPosition());
jessekaiser 0:d5d3b731340c 53 // ch 1, 2 and 3 set in pid controller */
jessekaiser 0:d5d3b731340c 54 scope.send();
jessekaiser 0:d5d3b731340c 55 if(new_pwm < 0)
jessekaiser 0:d5d3b731340c 56 motordir = 0;
jessekaiser 0:d5d3b731340c 57 else
jessekaiser 0:d5d3b731340c 58 motordir = 1;
jessekaiser 0:d5d3b731340c 59 pwm_motor.write(abs(new_pwm));
jessekaiser 0:d5d3b731340c 60 }
jessekaiser 0:d5d3b731340c 61 }
jessekaiser 0:d5d3b731340c 62
jessekaiser 0:d5d3b731340c 63
jessekaiser 0:d5d3b731340c 64 //clamps value 'in' to min or max when exceeding those values
jessekaiser 0:d5d3b731340c 65 //if you'd like to understand the statement below take a google for
jessekaiser 0:d5d3b731340c 66 //'ternary operators'.
jessekaiser 0:d5d3b731340c 67 void clamp(float * in, float min, float max)
jessekaiser 0:d5d3b731340c 68 {
jessekaiser 0:d5d3b731340c 69 *in > min ? *in < max? : *in = max: *in = min;
jessekaiser 0:d5d3b731340c 70 }
jessekaiser 0:d5d3b731340c 71
jessekaiser 0:d5d3b731340c 72
jessekaiser 0:d5d3b731340c 73 float pid(float setpoint, float measurement)
jessekaiser 0:d5d3b731340c 74 {
jessekaiser 0:d5d3b731340c 75 float error;
jessekaiser 0:d5d3b731340c 76 static float prev_error = 0;
jessekaiser 0:d5d3b731340c 77 float out_p = 0;
jessekaiser 0:d5d3b731340c 78 static float out_i = 0;
jessekaiser 0:d5d3b731340c 79 float out_d = 0;
jessekaiser 0:d5d3b731340c 80 error = setpoint-measurement;
jessekaiser 0:d5d3b731340c 81 out_p = error*K_P;
jessekaiser 0:d5d3b731340c 82 out_i += error*K_I;
jessekaiser 0:d5d3b731340c 83 out_d = (error-prev_error)*K_D;
jessekaiser 0:d5d3b731340c 84 clamp(&out_i,-I_LIMIT,I_LIMIT);
jessekaiser 0:d5d3b731340c 85 prev_error = error;
jessekaiser 0:d5d3b731340c 86 scope.set(1,out_p);
jessekaiser 0:d5d3b731340c 87 scope.set(2,out_i);
jessekaiser 0:d5d3b731340c 88 scope.set(3,out_d);
jessekaiser 0:d5d3b731340c 89 return out_p + out_i + out_d;
jessekaiser 0:d5d3b731340c 90 }