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@9:76e4808df4cb, 2017-12-01 (annotated)

Committer:
x58alex41
Date:
Fri Dec 01 21:20:29 2017 +0000
Revision:
9:76e4808df4cb
Parent:
7:edd065946e9b 
motor problems;

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 9:76e4808df4cb 18 /* pc.printf("counter1: %6.6f",counterM1);
x58alex41 9:76e4808df4cb 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 9:76e4808df4cb 31 const double Kp_e =.0000001;
x58alex41 0:9c49bdc7e402 32 const double Ki_e = 0;
x58alex41 9:76e4808df4cb 33 const double Kd_e = 0.0000; // 0.0001
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 9:76e4808df4cb 54 // pc.printf("kd error: %8.8f\n", Kd_e * (error - prev_error));
x58alex41 0:9c49bdc7e402 55 prev_error = error;
x58alex41 9:76e4808df4cb 56 //pc.printf("total error %8.8f\n", P + I +D);
x58alex41 7:edd065946e9b 57 return (P + I+ D);
x58alex41 7:edd065946e9b 58 }
x58alex41 9:76e4808df4cb 59 /*
x58alex41 7:edd065946e9b 60 //opening on right side
x58alex41 7:edd065946e9b 61 bool opening_r(double threshold_p, double threshold_d)
x58alex41 7:edd065946e9b 62 {
x58alex41 7:edd065946e9b 63 return threshold_p < LL_t.read_u16 && prev_error > threshold;
x58alex41 7:edd065946e9b 64 }
x58alex41 7:edd065946e9b 65
x58alex41 7:edd065946e9b 66 bool opening_l(double threshold_p, double threshold_d)
x58alex41 7:edd065946e9b 67 {
x58alex41 7:edd065946e9b 68 return threshold_p < LL_t.read_u16) && prev_error > threshold;
x58alex41 7:edd065946e9b 69 }
x58alex41 7:edd065946e9b 70
x58alex41 7:edd065946e9b 71 //wall in fron the car
x58alex41 7:edd065946e9b 72 bool wall_front(double threshold_r, double threshold_l)
x58alex41 7:edd065946e9b 73 {
x58alex41 9:76e4808df4cb 74 return threshold_r < R_t.read_u16() && threshold_l < L_t.read_u16();
x58alex41 9:76e4808df4cb 75 }*/
x58alex41 7:edd065946e9b 76
x58alex41 7:edd065946e9b 77
x58alex41 7:edd065946e9b 78