File content as of revision 4:90303483fd5f:

#include "mbed.h"
#include "QEI.h"

QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
double Kp = .005;
double Ki = 0;//.0000001;
double Kd = 0.001;
PwmOut m_Right_F(PB_10);
PwmOut m_Right_B(PC_7);
PwmOut m_Left_F(PA_7);
PwmOut m_Left_B(PB_6);
double i_speed = 0.1;
double C_speed(0);
int integrator = 0;
int decayFactor = 1;
double Error = 0;
double prevError = 0;
Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
Timer timer;
int counter = 0;

double P_controller(int error)
{
    double correction = (Kp*error);
    //pc.printf("correction is: %d\n", correction);
    return correction;
}

double I_controller(int error)
{
    integrator += error;
    double correction = Ki*integrator;
    integrator /= decayFactor;

    return correction;
}

double D_controller(int error)
{
    int dError = error - prevError;
    int dt = timer.read_us();
    timer.reset();
    prevError = error;
    double correction = Kd*dError/dt;
    return correction;
}

Ticker systicker;
//speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
void systick()
{
    double R_en_count = encoder_Right.getPulses()/100;
    double L_en_count = encoder_Left.getPulses()/100;
    Error = R_en_count - L_en_count;
    double ex = D_controller(Error);
    // if (ex < 0)
    //    ex = -ex;
    C_speed = P_controller(Error) + I_controller(Error) + ex;
}

void forward()
{
    double f1_speed = i_speed + C_speed;
    double f2_speed = i_speed - C_speed;
    
    if (f2_speed > f1_speed)    //if f2>f1, KILL
        {
            while(1) {
                m_Left_F.write(0);
                m_Right_F.write(0);
                }    
        }
        
    if(f1_speed >= 0.7) {
        f1_speed = 0.7;
    }
    if(f2_speed <= 0.2) {
        f2_speed = 0.2;
    }
    //problems when left wheel is held for the + case
    if (Error > 0) { //right wheel is turning more
        m_Left_F.write(f1_speed);
        m_Right_F.write(f2_speed); //f2_speed
    }
    if (Error < 0) { //left wheel is turning more
        m_Right_F.write(f1_speed);
        m_Left_F.write(f2_speed);  //f2_speed
    }
    if (Error == 0)
    {
        m_Right_F.write(i_speed);
        m_Left_F.write(i_speed);    
    }
}

void turnRight()
{

}

void turnLeft()
{

}

void turnAround()
{

}

void debugEncoder()
{
    while(1) {
        wait(1);
        pc.printf("Right: %i", encoder_Right.getPulses());
        pc.printf("  Left: %i", encoder_Left.getPulses(), "\n");
        pc.printf("\n");
    }
}

void debugError()
{
    while(1) {
        pc.printf("Error: %i\n", Error);
    }
}

int main()      //only runs once
{
    systicker.attach_us(&systick, 1000);    //enable interrupt
    while (1) {forward();}
    //
}