File content as of revision 5:a49a77ddf4e3:

#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);
    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;
    if (dt==0)
        correction=0;
    else
        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;
    if (C_speed < 0)
        C_speed = C_speed*-1;
}

void forward()
{
    double f1_speed = i_speed + C_speed;
    double f2_speed = i_speed - C_speed;

    /*pc.printf("C: %f", C_speed);
    if (C_speed < 0)
        pc.printf("-");
    if (C_speed > 0)
        pc.printf("+");
        */

    if(f1_speed >= 0.7) {   //upper bound, can not go faster
        f1_speed = 0.7;
    }
    if (f1_speed <= 0.05)
        f1_speed = 0.05;

    if(f2_speed <= 0.05) {  //lower bound, should not be slower than this
        f2_speed = 0.05;
    }

    pc.printf(" f1: %f", f1_speed);
    pc.printf(" f2: %f", f2_speed);

    //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();
    }
    //
    //debugEncoder();
}