File content as of revision 1:b9485bb1ca69:

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

DigitalOut myled(LED1);
DigitalIn mybutton(USER_BUTTON);

PwmOut LMotorForward(PA_7);
PwmOut LMotorReverse(PB_6);

PwmOut RMotorForward(PB_10);
PwmOut RMotorReverse(PB_4);

AVEncoder LeftEncoder(PA_15, PB_3);
AVEncoder RightEncoder(PA_1, PA_2);

Serial pc(USBTX, USBRX);

void pid();
double P_controller(int error);
double D_controller(int error);
double I_controller(int error);
void setRightPwm(double speed); 
void setLeftPwm(double speed);
void stop();

double speed = 0.5;
Ticker interrupt;
int STOP = 0;

int main() 
{
    myled = 1;
    interrupt.attach_us(&pid, 1000);
    while(mybutton == 1);
    
    speed = 0.5;
    
    while(true)
    {
        setLeftPwm(speed);
        setRightPwm(speed);
    }
}

void pid()
{
	unsigned long int error = LeftEncoder.getPulses() - RightEncoder.getPulses();
    speed += P_controller(error) + D_controller(error) + I_controller(error);
    
    if(speed < 0)
    {
        speed = 0;
    }
    else if(speed > 1)
    {
        speed = 1;
    }
	
	LeftEncoder.reset();
	RightEncoder.reset();
}

double P_controller(int error)
{
    return -1;
}

double D_controller(int error)
{
    return -1;
}

double I_controller(int error)
{
    return -1;
}

void setLeftPwm(double speed) 
{
    if(speed == 0)
    {
        LMotorForward = 1.0;
        LMotorReverse = 1.0;
    }
    if(speed > 0)
    {
        LMotorForward = speed;
        LMotorReverse = 0;
    }
    if(speed < 0)
    {
        LMotorForward = 0;
        LMotorReverse = -speed;
    }
}

//todo: make this have ramping
void setRightPwm(double speed) 
{
    if(speed > 0)
    {
        RMotorForward = speed;
        RMotorReverse = STOP;
    }
    if(speed < 0)
    {
        RMotorForward = STOP;
        RMotorReverse = -speed;
    }
}

void stop()
{
    setLeftPwm(0);
    setRightPwm(0);
}