Micromouse / Mbed 2 deprecated Main-codetest

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

PID.cpp@7:edd065946e9b, 2017-11-22 (annotated)

Committer:
x58alex41
Date:
Wed Nov 22 06:58:41 2017 +0000
Revision:
7:edd065946e9b
Parent:
6:688449345fff
Child:
8:45797dcd8b66
Child:
9:76e4808df4cb
added some unfinished controls

Who changed what in which revision?

UserRevisionLine numberNew contents of line
x58alex41 0:9c49bdc7e402 1
x58alex41 0:9c49bdc7e402 2 #include "PID.h"
x58alex41 0:9c49bdc7e402 3
x58alex41 0:9c49bdc7e402 4 //led controlled
x58alex41 0:9c49bdc7e402 5 const double Kp =.00001;
x58alex41 0:9c49bdc7e402 6 const double Ki = 0;
x58alex41 0:9c49bdc7e402 7 const double Kd = 0.1;
x58alex41 0:9c49bdc7e402 8 const double decay=1;
x58alex41 0:9c49bdc7e402 9
x58alex41 0:9c49bdc7e402 10 double total_error=0;
x58alex41 0:9c49bdc7e402 11 double prev_error=0;
x58alex41 0:9c49bdc7e402 12
x58alex41 7:edd065946e9b 13 //error based on encoders
x58alex41 0:9c49bdc7e402 14 double PID()
x58alex41 0:9c49bdc7e402 15 {
x58alex41 0:9c49bdc7e402 16 double error = 0;
x58alex41 0:9c49bdc7e402 17 error= signed( counterM1- counterM2);
x58alex41 6:688449345fff 18 pc.printf("counter1: %6.6f",counterM1);
x58alex41 7:edd065946e9b 19 pc.printf("counter2: %6.6f",counterM2);
x58alex41 0:9c49bdc7e402 20 double P = Kp * error;
x58alex41 0:9c49bdc7e402 21 total_error = (total_error+error);
x58alex41 0:9c49bdc7e402 22 double I = Ki * total_error;
x58alex41 0:9c49bdc7e402 23 total_error /= decay;
x58alex41 0:9c49bdc7e402 24
x58alex41 0:9c49bdc7e402 25 double D = Kd * (error - prev_error);
x58alex41 0:9c49bdc7e402 26 prev_error = error;
x58alex41 0:9c49bdc7e402 27 return P + I+ D;
x58alex41 0:9c49bdc7e402 28 }
x58alex41 0:9c49bdc7e402 29
x58alex41 7:edd065946e9b 30 //error for led
x58alex41 6:688449345fff 31 const double Kp_e =.00001;
x58alex41 0:9c49bdc7e402 32 const double Ki_e = 0;
x58alex41 6:688449345fff 33 const double Kd_e = 0;
x58alex41 0:9c49bdc7e402 34 const double decay_e=1;
x58alex41 0:9c49bdc7e402 35
x58alex41 0:9c49bdc7e402 36 double total_error_e=0;
x58alex41 0:9c49bdc7e402 37 double prev_error_e=0;
x58alex41 0:9c49bdc7e402 38
x58alex41 0:9c49bdc7e402 39
x58alex41 7:edd065946e9b 40 double PID_e()
x58alex41 7:edd065946e9b 41 {
x58alex41 0:9c49bdc7e402 42 double error = 0;
x58alex41 0:9c49bdc7e402 43 error= signed ( LL_t.read_u16()-RR_t.read_u16());
x58alex41 7:edd065946e9b 44
x58alex41 7:edd065946e9b 45 // pc.printf("ledL: %d\n",LL_t.read_u16());
x58alex41 7:edd065946e9b 46 // pc.printf("LedR: %d\n",RR_t.read_u16());
x58alex41 6:688449345fff 47 // pc.printf("kp error: %8.8f\n",(signed ( LL_t.read_u16()-RR_t.read_u16()))*Kp_e);
x58alex41 6:688449345fff 48
x58alex41 0:9c49bdc7e402 49 double P = Kp_e * error;
x58alex41 0:9c49bdc7e402 50 total_error = (total_error+error);
x58alex41 0:9c49bdc7e402 51 double I = Ki_e * total_error;
x58alex41 0:9c49bdc7e402 52 total_error /= decay_e;
x58alex41 0:9c49bdc7e402 53 double D = Kd_e * (error - prev_error);
x58alex41 0:9c49bdc7e402 54 prev_error = error;
x58alex41 7:edd065946e9b 55 return (P + I+ D);
x58alex41 7:edd065946e9b 56 }
x58alex41 7:edd065946e9b 57
x58alex41 7:edd065946e9b 58 //opening on right side
x58alex41 7:edd065946e9b 59 bool opening_r(double threshold_p, double threshold_d)
x58alex41 7:edd065946e9b 60 {
x58alex41 7:edd065946e9b 61 return threshold_p < LL_t.read_u16 && prev_error > threshold;
x58alex41 7:edd065946e9b 62 }
x58alex41 7:edd065946e9b 63
x58alex41 7:edd065946e9b 64 bool opening_l(double threshold_p, double threshold_d)
x58alex41 7:edd065946e9b 65 {
x58alex41 7:edd065946e9b 66 return threshold_p < LL_t.read_u16) && prev_error > threshold;
x58alex41 7:edd065946e9b 67 }
x58alex41 7:edd065946e9b 68
x58alex41 7:edd065946e9b 69 //wall in fron the car
x58alex41 7:edd065946e9b 70 bool wall_front(double threshold_r, double threshold_l)
x58alex41 7:edd065946e9b 71 {
x58alex41 7:edd065946e9b 72 return threshold_r < R_t.read_u(16) && threshold_l < L_t.read_u(16);
x58alex41 7:edd065946e9b 73 }
x58alex41 7:edd065946e9b 74
x58alex41 7:edd065946e9b 75
x58alex41 7:edd065946e9b 76