File content as of revision 2:646e3bca12a8:

//****************************************************************************/
// Includes
//****************************************************************************/
#include "PID.h"
#include "QEI.h"
#include "mbed.h"

//****************************************************************************/
// Defines
//****************************************************************************/
#define RATE  0.1
#define Kc   1.0
#define Ti    0.0
#define Td    0.0

//****************************************************************************/
// Globals
//****************************************************************************/
//--------
// Motors
//--------
//Left motor.
PwmOut leftMotor(PB_4);
DigitalOut leftBrake(PA_15);
DigitalOut leftDirection(PB_7);
QEI leftQei(PC_10, PC_12, NC, 624);
PID leftController(Kc, Ti, Td, RATE);

//Right motor.
PwmOut rightMotor(PB_10);
DigitalOut rightBrake(PA_13);
DigitalOut rightDirection(PA_14);
QEI rightQei(PA_5, PA_6, NC, 624);
PID rightController(Kc, Ti, Td, RATE);

//--------
// Serial
//--------
Serial pc(SERIAL_TX,SERIAL_RX);

//--------
// Timers
//--------
Timer endTimer;
//--------------------
// Working variables.
//--------------------
volatile int leftPulses     = 0;
volatile int leftPrevPulses = 0;
volatile float leftPwmDuty  = 1.0;
volatile float leftVelocity = 0.0;

volatile int rightPulses     = 0;
volatile int rightPrevPulses = 0;
volatile float rightPwmDuty  = 1.0;
volatile float rightVelocity = 0.0;

//Velocity to reach.
int goal = 50;

//****************************************************************************/
// Prototypes
//****************************************************************************/
//Set motors to go "forward", brake off, not moving.
void initializeMotors(void);
//Set up PID controllers with appropriate limits and biases.
void initializePidControllers(void);

void initializeMotors(void)
{

    leftMotor.period_us(50);
    leftMotor = 1.0;
    leftBrake = 0.0;
    leftDirection = 0;

    rightMotor.period_us(50);
    rightMotor = 1.0;
    rightBrake = 0.0;
    rightDirection = 0;

}

void initializePidControllers(void)
{

    leftController.setInputLimits(0.0, 10500.0);
    leftController.setOutputLimits(0.0, 1.0);
    leftController.setBias(1.0);
    leftController.setMode(AUTO_MODE);

    rightController.setInputLimits(0.0, 10500.0);
    rightController.setOutputLimits(0.0, 1.0);
    rightController.setBias(1.0);
    rightController.setMode(AUTO_MODE);
}

int main()
{

    int updateTime=0;

    pc.baud(115200);
 pc.printf("start\n\r");
    //Initialization.
    initializeMotors();
    
    wait(1);
    /*
    while(1){
        leftPulses = leftQei.getPulses();
        rightPulses = rightQei.getPulses();
        pc.printf("leftPulses = %d\n\r",leftPulses);
        pc.printf("rightPulses = %d\n\r",rightPulses);
        wait(0.5);
        }
    */
    initializePidControllers();


    endTimer.start();

    //Set velocity set point.
    leftController.setSetPoint(goal);
    rightController.setSetPoint(goal);

    //Run for 3 seconds.
    while (endTimer.read() < 15.0f) {
        
        leftDirection = 1;
        rightDirection = 1;
        
        if( (endTimer.read_ms()-updateTime) > (RATE*1000)) {
            leftPulses = leftQei.getPulses();
            leftVelocity = (leftPulses - leftPrevPulses) / RATE;
            leftPrevPulses = leftPulses;
            leftController.setProcessValue(leftVelocity);
            leftPwmDuty = leftController.compute();
            leftMotor = leftPwmDuty;

            rightPulses = rightQei.getPulses();
            rightVelocity = (rightPulses - rightPrevPulses) / RATE;
            rightPrevPulses = rightPulses;
            rightController.setProcessValue(rightVelocity);
            rightPwmDuty = rightController.compute();
            rightMotor = rightPwmDuty;

            updateTime = endTimer.read_ms();

            pc.printf("leftPulses = %d\n\r",leftPulses);
            pc.printf("rightPulses = %d\n\r",rightPulses);
            pc.printf("leftPwm = %f\n\r",leftPwmDuty);
            pc.printf("rightPwm = %f\n\r",rightPwmDuty);
        }
        //wait(RATE);
    }

    //Stop motors.
    leftMotor  = 1.0;
    rightMotor =1.0;
    
    rightDirection = 0;
    leftDirection = 0;
    endTimer.reset();
    wait(1.0f);

    pc.printf("stop\n]r");

    //Set velocity set point.
    leftController.setSetPoint(goal+1000);
    rightController.setSetPoint(goal);

        leftDirection = 1;
        rightDirection = 1;

    //Run for 2 seconds.
    while (endTimer.read() < 2.0f) {

        if(endTimer.read_ms()-updateTime > RATE*1000) {
            leftPulses = leftQei.getPulses();
            leftVelocity = (leftPulses - leftPrevPulses) / RATE;
            leftPrevPulses = leftPulses;
            leftController.setProcessValue(leftVelocity);
            leftPwmDuty = leftController.compute();
            leftMotor = leftPwmDuty;

            rightPulses = rightQei.getPulses();
            rightVelocity = (rightPulses - rightPrevPulses) / RATE;
            rightPrevPulses = rightPulses;
            rightController.setProcessValue(rightVelocity);
            rightPwmDuty = rightController.compute();
            rightMotor = rightPwmDuty;

            updateTime = endTimer.read_ms();

            pc.printf("leftPulses = %d\n\r",leftPulses);
            pc.printf("rightPulses = %d\n\r",rightPulses);
        }
        //wait(RATE);
    }
    
    rightDirection = 0;
    leftDirection = 0;
    
}