File content as of revision 12:4310fb637705:

#include "PID.h"

//led controlled
const double Kp =.00001;
const double Ki = 0;
const double Kd = 0.1;
const double decay=1;

double total_error=0;
double prev_error=0;

//error based on encoders
double PID()
{
    double error = 0;
    error= signed( counterM1- counterM2);
   /* pc.printf("counter1: %6.6f",counterM1);
    pc.printf("counter2: %6.6f",counterM2);*/
    double P = Kp * error;
    total_error = (total_error+error);
    double I = Ki * total_error;
    total_error /= decay;

    double D = Kd * (error - prev_error);
    prev_error = error;
    return P + I+ D;
}

//error for led
const double Kp_e =.0000001;
const double Ki_e = 0;
const double Kd_e = 0.0000; // 0.0001
const double decay_e=1;

double total_error_e=0;
double prev_error_e=0;


double PID_e()
{
    double error = 0;
    error= signed ( LL_t.read_u16()-RR_t.read_u16());

    // pc.printf("ledL: %d\n",LL_t.read_u16());
//   pc.printf("LedR: %d\n",RR_t.read_u16());
    // pc.printf("kp error: %8.8f\n",(signed ( LL_t.read_u16()-RR_t.read_u16()))*Kp_e);

    double P = Kp_e * error;
    total_error = (total_error+error);
    double I = Ki_e * total_error;
    total_error /= decay_e;
    double D = Kd_e * (error - prev_error);
     // pc.printf("kd error: %8.8f\n", Kd_e * (error - prev_error));
    prev_error = error;
   //pc.printf("total error %8.8f\n",  P + I +D);
    return (P + I+ D);
}
/*
//opening on right side
bool opening_r(double threshold_p, double threshold_d)
{
    return threshold_p < LL_t.read_u16 && prev_error > threshold;
}

bool opening_l(double threshold_p, double threshold_d)
{
    return threshold_p < LL_t.read_u16) && prev_error > threshold;
}

//wall in fron the car
bool wall_front(double threshold_r, double threshold_l)
{
    return threshold_r < R_t.read_u16() && threshold_l < L_t.read_u16();
}*/