Fabien Lepoutre
/
PID_2
main.cpp@0:e837a6fef44b, 2012-02-27 (annotated)
- Committer:
- fabienlepoutre
- Date:
- Mon Feb 27 16:22:19 2012 +0000
- Revision:
- 0:e837a6fef44b
Test du 27 Fevrier
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fabienlepoutre | 0:e837a6fef44b | 1 | #include "mbed.h" |
fabienlepoutre | 0:e837a6fef44b | 2 | |
fabienlepoutre | 0:e837a6fef44b | 3 | DigitalOut myled(LED1); |
fabienlepoutre | 0:e837a6fef44b | 4 | AnalogIn entree(p20); |
fabienlepoutre | 0:e837a6fef44b | 5 | AnalogOut sortie(p18); |
fabienlepoutre | 0:e837a6fef44b | 6 | |
fabienlepoutre | 0:e837a6fef44b | 7 | int main() { |
fabienlepoutre | 0:e837a6fef44b | 8 | float integral =0; |
fabienlepoutre | 0:e837a6fef44b | 9 | float previous_error =0; |
fabienlepoutre | 0:e837a6fef44b | 10 | float error =0; |
fabienlepoutre | 0:e837a6fef44b | 11 | float prev_sortie =0; |
fabienlepoutre | 0:e837a6fef44b | 12 | float Kp = 2.72, Ki=2.61, Kd=0; |
fabienlepoutre | 0:e837a6fef44b | 13 | int interval=2; |
fabienlepoutre | 0:e837a6fef44b | 14 | float derivative = 0; |
fabienlepoutre | 0:e837a6fef44b | 15 | float sortie_ns; |
fabienlepoutre | 0:e837a6fef44b | 16 | myled = 0; |
fabienlepoutre | 0:e837a6fef44b | 17 | /* printf("Entrer les parametres Kp : \r\n"); |
fabienlepoutre | 0:e837a6fef44b | 18 | scanf("%f", &Kp); |
fabienlepoutre | 0:e837a6fef44b | 19 | printf("Ki : \r\n"); |
fabienlepoutre | 0:e837a6fef44b | 20 | scanf("%f", &Ki); |
fabienlepoutre | 0:e837a6fef44b | 21 | printf("Kd: \r\n"); |
fabienlepoutre | 0:e837a6fef44b | 22 | scanf("%f", &Kd); |
fabienlepoutre | 0:e837a6fef44b | 23 | printf("Periode d'echantillonnage (en ms) :\r\n"); |
fabienlepoutre | 0:e837a6fef44b | 24 | scanf("%d", &interval);*/ |
fabienlepoutre | 0:e837a6fef44b | 25 | printf("Debut du programme avec Ki = %f et Kp = %f \n\r", Ki, Kp); |
fabienlepoutre | 0:e837a6fef44b | 26 | float ana_test = entree.read()*3.3; |
fabienlepoutre | 0:e837a6fef44b | 27 | printf("p20 = %f \n\r", ana_test); |
fabienlepoutre | 0:e837a6fef44b | 28 | int i=0; |
fabienlepoutre | 0:e837a6fef44b | 29 | while(1){ |
fabienlepoutre | 0:e837a6fef44b | 30 | // printf("In : \r\n"); |
fabienlepoutre | 0:e837a6fef44b | 31 | // scanf("%f", &error); |
fabienlepoutre | 0:e837a6fef44b | 32 | /* while(i<20){ |
fabienlepoutre | 0:e837a6fef44b | 33 | myled = 1; |
fabienlepoutre | 0:e837a6fef44b | 34 | error = error + entree.read(); |
fabienlepoutre | 0:e837a6fef44b | 35 | i++; |
fabienlepoutre | 0:e837a6fef44b | 36 | } |
fabienlepoutre | 0:e837a6fef44b | 37 | i=0; |
fabienlepoutre | 0:e837a6fef44b | 38 | myled=0; |
fabienlepoutre | 0:e837a6fef44b | 39 | error = error/20.0;*/ |
fabienlepoutre | 0:e837a6fef44b | 40 | error = entree.read(); |
fabienlepoutre | 0:e837a6fef44b | 41 | |
fabienlepoutre | 0:e837a6fef44b | 42 | integral = integral + (previous_error*interval); |
fabienlepoutre | 0:e837a6fef44b | 43 | derivative = (error - previous_error)/interval; |
fabienlepoutre | 0:e837a6fef44b | 44 | sortie_ns = prev_sortie + (Kp*error)-(Ki*previous_error)+(Kd*derivative); |
fabienlepoutre | 0:e837a6fef44b | 45 | if(sortie_ns >1) sortie_ns =1; |
fabienlepoutre | 0:e837a6fef44b | 46 | if(sortie_ns<0) sortie_ns=0; |
fabienlepoutre | 0:e837a6fef44b | 47 | sortie = sortie_ns; |
fabienlepoutre | 0:e837a6fef44b | 48 | prev_sortie = sortie_ns;; |
fabienlepoutre | 0:e837a6fef44b | 49 | previous_error = error; |
fabienlepoutre | 0:e837a6fef44b | 50 | wait_ms(interval); |
fabienlepoutre | 0:e837a6fef44b | 51 | } |
fabienlepoutre | 0:e837a6fef44b | 52 | |
fabienlepoutre | 0:e837a6fef44b | 53 | |
fabienlepoutre | 0:e837a6fef44b | 54 | |
fabienlepoutre | 0:e837a6fef44b | 55 | } |