Marijke Abma / Mbed 2 deprecated motor1aansturing201014

Programma om motor 1 aan te sturen

Dependencies:   Encoder MODSERIAL mbed-dsp mbed

Fork of motor1aansturing by BMT M9 Groep01

main.cpp@28:fa85793c50bb, 2014-10-20 (annotated)

Committer:
MarijkeAbma
Date:
Mon Oct 20 12:13:50 2014 +0000
Revision:
28:fa85793c50bb
Parent:
27:fb73c8b5321a 
Ready for publishing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jolein 0:eb00992c1597 1 #include "mbed.h"
Jolein 0:eb00992c1597 2 #include "encoder.h"
Jolein 15:3a6074f3ceaf 3
Jolein 26:02170c78ac2f 4 #define K_P (0.01)
Jolein 0:eb00992c1597 5 #define K_I (0 *TSAMP)
Jolein 2:ef0fa691e77e 6 #define K_D (0 /TSAMP)
Jolein 0:eb00992c1597 7 #define I_LIMIT 1.
Jolein 26:02170c78ac2f 8
Jolein 26:02170c78ac2f 9 #define TSAMP 0.01
Jolein 16:bf4c5affd3e9 10 #define WACHTEN 1
Jolein 15:3a6074f3ceaf 11 #define SLAAN 2
Jolein 15:3a6074f3ceaf 12 #define TERUGKEREN 3
Jolein 26:02170c78ac2f 13 #define ANGLEMAX 1000
Jolein 18:1188cdb7ccbe 14 #define ANGLEMIN 0
Jolein 0:eb00992c1597 15
Jolein 26:02170c78ac2f 16 float pid(float setspeed, float measurement);
Jolein 26:02170c78ac2f 17 float new_pwm;
Jolein 26:02170c78ac2f 18 DigitalOut motordir2(PTA4);
Jolein 26:02170c78ac2f 19 PwmOut pwm_motor2(PTA5);
Jolein 26:02170c78ac2f 20 Encoder motor2(PTD0,PTD2);
Jolein 26:02170c78ac2f 21
Jolein 0:eb00992c1597 22 volatile bool looptimerflag;
Jolein 0:eb00992c1597 23
Jolein 0:eb00992c1597 24 void setlooptimerflag(void)
Jolein 0:eb00992c1597 25 {
Jolein 0:eb00992c1597 26 looptimerflag = true;
Jolein 0:eb00992c1597 27 }
Jolein 0:eb00992c1597 28
Jolein 26:02170c78ac2f 29 int main ()
Jolein 0:eb00992c1597 30 {
Jolein 26:02170c78ac2f 31 pwm_motor2.write(0.3);
Jolein 26:02170c78ac2f 32 motordir2 = 0;
Jolein 16:bf4c5affd3e9 33
Jolein 26:02170c78ac2f 34 int toestand = TERUGKEREN; //initial condition
Jolein 26:02170c78ac2f 35 int EMG = 1;
Jolein 27:fb73c8b5321a 36 float setspeed = 0, V3=40, V2=30, V1 =20, Vreturn= 15;//V in counts/s
Jolein 16:bf4c5affd3e9 37
Jolein 0:eb00992c1597 38 Ticker looptimer;
Jolein 0:eb00992c1597 39 looptimer.attach(setlooptimerflag,TSAMP);
Jolein 25:9ab3123c2b15 40 while(1)
Jolein 25:9ab3123c2b15 41 {
Jolein 0:eb00992c1597 42 while(!looptimerflag);
Jolein 2:ef0fa691e77e 43 looptimerflag = false;
Jolein 16:bf4c5affd3e9 44
Jolein 26:02170c78ac2f 45 if (motor2.getPosition()<= ANGLEMIN && toestand != SLAAN)
Jolein 16:bf4c5affd3e9 46 {
Jolein 22:93d32c57bac8 47 toestand = WACHTEN;
Jolein 22:93d32c57bac8 48 }
Jolein 22:93d32c57bac8 49
Jolein 26:02170c78ac2f 50 if (EMG != 0 && toestand == WACHTEN) // iets vinden om te testen... ipv 1
Jolein 22:93d32c57bac8 51 {
Jolein 22:93d32c57bac8 52 toestand = SLAAN;
Jolein 26:02170c78ac2f 53 if ( EMG==3)
Jolein 22:93d32c57bac8 54 {
Jolein 26:02170c78ac2f 55 setspeed = V3;
Jolein 22:93d32c57bac8 56 }
Jolein 26:02170c78ac2f 57 if ( EMG==2)
Jolein 22:93d32c57bac8 58 {
Jolein 26:02170c78ac2f 59 setspeed = V2;
Jolein 22:93d32c57bac8 60 }
Jolein 26:02170c78ac2f 61 if ( EMG==1)
Jolein 22:93d32c57bac8 62 {
Jolein 26:02170c78ac2f 63 setspeed = V1;
Jolein 17:0116e2f17d0d 64 }
Jolein 16:bf4c5affd3e9 65 }
Jolein 16:bf4c5affd3e9 66
Jolein 26:02170c78ac2f 67 if (toestand == SLAAN && motor2.getPosition() >= ANGLEMAX)
Jolein 22:93d32c57bac8 68 {
Jolein 26:02170c78ac2f 69 toestand = TERUGKEREN;
Jolein 26:02170c78ac2f 70 }
Jolein 26:02170c78ac2f 71 if (toestand == TERUGKEREN)
Jolein 26:02170c78ac2f 72 {
Jolein 26:02170c78ac2f 73 new_pwm = pid(Vreturn, motor2.getSpeed());
Jolein 26:02170c78ac2f 74 pwm_motor2.write(new_pwm);
Jolein 26:02170c78ac2f 75 motordir2 = 1;
Jolein 22:93d32c57bac8 76 }
Jolein 26:02170c78ac2f 77 if (toestand == WACHTEN)
Jolein 26:02170c78ac2f 78 {
Jolein 26:02170c78ac2f 79 pwm_motor2.write(0);
Jolein 14:6ec84bfda0f3 80 }
Jolein 26:02170c78ac2f 81 if (toestand == SLAAN)
Jolein 26:02170c78ac2f 82 {
Jolein 26:02170c78ac2f 83 new_pwm = pid(setspeed, motor2.getSpeed());
Jolein 26:02170c78ac2f 84 motordir2 = 0;
Jolein 26:02170c78ac2f 85 pwm_motor2.write(new_pwm);
Jolein 26:02170c78ac2f 86 }
Jolein 26:02170c78ac2f 87
Jolein 26:02170c78ac2f 88 }//end while
Jolein 26:02170c78ac2f 89 }//end main
Jolein 0:eb00992c1597 90
Jolein 26:02170c78ac2f 91
Jolein 26:02170c78ac2f 92 void clamp(float * in, float min, float max)//nodig??
Jolein 0:eb00992c1597 93 {
Jolein 0:eb00992c1597 94 *in > min ? *in < max? : *in = max: *in = min;
Jolein 0:eb00992c1597 95 }
Jolein 0:eb00992c1597 96
Jolein 26:02170c78ac2f 97 float pid(float setspeed, float measurement)
Jolein 0:eb00992c1597 98 {
Jolein 0:eb00992c1597 99 float error;
Jolein 0:eb00992c1597 100 static float prev_error = 0;
Jolein 0:eb00992c1597 101 float out_p = 0;
Jolein 0:eb00992c1597 102 static float out_i = 0;
Jolein 0:eb00992c1597 103 float out_d = 0;
Jolein 26:02170c78ac2f 104 error = setspeed-measurement;
Jolein 0:eb00992c1597 105 out_p = error*K_P;
Jolein 0:eb00992c1597 106 out_i += error*K_I;
Jolein 0:eb00992c1597 107 out_d = (error-prev_error)*K_D;
Jolein 0:eb00992c1597 108 clamp(&out_i,-I_LIMIT,I_LIMIT);
Jolein 0:eb00992c1597 109 prev_error = error;
Jolein 0:eb00992c1597 110 return out_p + out_i + out_d;
Jolein 26:02170c78ac2f 111 }