File content as of revision 0:7440a03255a7:

/**
 * Includes.
 */
#include "Motor.h"
#include "QEI.h"
#include "PID.h"
#include "IMU.h"

/**
 * Defines.
 */
#define BUFFER_SIZE 1024 //Used for data logging arrays
#define RATE 0.01        //PID loop timing

//PID tuning constants.
#define L_KC 0.4312 //Forward left motor Kc
#define L_TI 0.1    //Forward left motor Ti
#define L_TD 0.0    //Forward left motor Td
#define R_KC 0.4620 //Forward right motor Kc
#define R_TI 0.1    //Forward right motor Ti
#define R_TD 0.0    //Forward right motor Td

//PID input/output limits.
#define L_INPUT_MIN 0     //Forward left motor minimum input
#define L_INPUT_MAX 3000  //Forward left motor maximum input
#define L_OUTPUT_MIN 0.0  //Forward left motor minimum output
#define L_OUTPUT_MAX 0.9  //Forward left motor maximum output

#define R_INPUT_MIN 0     //Forward right motor input minimum
#define R_INPUT_MAX 3200  //Forward right motor input maximum
#define R_OUTPUT_MIN 0.0  //Forward right motor output minimum
#define R_OUTPUT_MAX 0.9  //Forward right motor output maximum

//Physical dimensions.
#define PULSES_PER_REV        624
#define WHEEL_DIAMETER        58.928 //mm
#define ROTATION_DISTANCE     220.0  //mm
#define REVS_PER_ROTATION     (ROTATION_DISTANCE / WHEEL_DIAMETER)
#define PULSES_PER_ROTATION   (REVS_PER_ROTATION * PULSES_PER_REV)
#define PULSES_PER_MM         (PULSES_PER_REV / WHEEL_DIAMETER)
#define DISTANCE_PER_PULSE    (WHEEL_DIAMETER / PULSES_PER_REV)
#define ENCODING              2 //Use X2 encoding
#define WHEEL_DISTANCE        (ROTATION_DISTANCE / DISTANCE_PER_PULSE)

/**
 * Function Prototypes
 */
void initializeMotors(void);
void initializePid(void);
//cmd = "move", "turn"
//direction = "forward", "backward", "left", "right"
//length = distance in metres or angle in degrees
void processCommand(char* cmd, char* direction, float length);

/**
 * Globals
 */

//Debug.
Serial pc(USBTX, USBRX);

//Motor control.
Motor leftMotor(p21, p20, p19);  //pwm, inB, inA
Motor rightMotor(p22, p17, p18); //pwm, inA, inB
QEI leftQei(p29, p30, NC, 624);  //chanA, chanB, index, ppr
QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr
PID leftPid(L_KC, L_TI, L_TD, RATE);  //Kc, Ti, Td
PID rightPid(R_KC, R_TI, R_TD, RATE); //Kc, Ti, Td
//IMU and peripherals run at a different rate to the main PID loop.
IMU imu(IMU_RATE, GYRO_MEAS_ERROR, ACCELEROMETER_RATE, GYROSCOPE_RATE);

//Filesystem.
LocalFileSystem local("local");

//Left motor working variables.
int leftPulses     = 0;
int leftPrevPulses = 0;
float leftVelocity = 0.0;
float leftVelocityBuffer[BUFFER_SIZE];

//Right motor working variables.
int rightPulses     = 0;
int rightPrevPulses = 0;
float rightVelocity = 0.0;
float rightVelocityBuffer[BUFFER_SIZE];

//General working variables.
float heading       = 0.0;
float prevHeading   = 0.0;
float degreesTurned = 0.0;
float positionSetPoint = 0.0;
float headingSetPoint  = 0.0;

//Store the initial and end heading during a straight line section
//in order to be able to correct turns.
float startHeading = 0.0;
float endHeading   = 0.0;

//Command Parser.
char  cmd0[16]; //{"move", "turn"}
char  cmd1[16]; //{{"forward", "backward"}, {"left", "right"}}
float cmd2 = 0; //{distance in METRES, angle in DEGREES}

void initializeMotors(void) {

    //Set motor PWM periods to 20KHz.
    leftMotor.period(0.00005);
    leftMotor.speed(0.0);

    rightMotor.period(0.00005);
    rightMotor.speed(0.0);

}

void initializePid(void) {

    //Input and output limits have been determined
    //empirically with the specific motors being used.
    //Change appropriately for different motors.
    //Input  units: counts per second.
    //Output units: PwmOut duty cycle as %.
    //Default limits are for moving forward.
    leftPid.setInputLimits(L_INPUT_MIN, L_INPUT_MAX);
    leftPid.setOutputLimits(L_OUTPUT_MIN, L_OUTPUT_MAX);
    leftPid.setMode(AUTO_MODE);
    rightPid.setInputLimits(R_INPUT_MIN, R_INPUT_MAX);
    rightPid.setOutputLimits(R_OUTPUT_MIN, R_OUTPUT_MAX);
    rightPid.setMode(AUTO_MODE);

}

void processCommand(char* cmd, char* direction, float length) {

    //move command.
    if (strcmp(cmd, "move") == 0) {

        int i = 0; //Data log array index.

        //The PID controller works in the % (unsigned) domain, so we'll
        //need to multiply the output by -1 if our motors need
        //to go "backwards".
        int leftDirection  = 1;
        int rightDirection = 1;

        //Convert from metres into millimetres.
        length *= 1000;
        //Work out how many pulses are required to go that many millimetres.
        length *= PULSES_PER_MM;
        //Make sure we scale the number of pulses according to our encoding method.
        length /= ENCODING;

        positionSetPoint = length;

        if (strcmp(direction, "forward") == 0) {
            //Leave left and rightDirection as +ve.
        } else if (strcmp(cmd1, "backward") == 0) {
            //Change left and rightDirection to -ve.
            leftDirection  = -1;
            rightDirection = -1;
        }

        startHeading = imu.getYaw();

        //With left set point == right set point, the rover veers off to the
        //right - by slowing the right motor down slightly it goes relatively
        //straight.
        leftPid.setSetPoint(1000);
        rightPid.setSetPoint(975);

        //Keep going until we've moved the correct distance defined by the
        //position set point. Take the absolute value of the pulses as we might
        //be moving backwards.
        while ((abs(leftPulses) < positionSetPoint) &&
                (abs(rightPulses) < positionSetPoint)) {

            //Get the current pulse count and subtract the previous one to
            //calculate the current velocity in counts per second.
            leftPulses = leftQei.getPulses();
            leftVelocity = (leftPulses - leftPrevPulses) / RATE;
            leftPrevPulses = leftPulses;
            //Use the absolute value of velocity as the PID controller works
            //in the % (unsigned) domain and will get confused with -ve values.
            leftPid.setProcessValue(fabs(leftVelocity));
            leftMotor.speed(leftPid.compute() * leftDirection);

            rightPulses = rightQei.getPulses();
            rightVelocity = (rightPulses - rightPrevPulses) / RATE;
            rightPrevPulses = rightPulses;
            rightPid.setProcessValue(fabs(rightVelocity));
            rightMotor.speed(rightPid.compute() * rightDirection);

            i++;

            wait(RATE);

        }

        leftMotor.brake();
        rightMotor.brake();

        endHeading = imu.getYaw();

    }
    //turn command.
    else if (strcmp(cmd0, "turn") == 0) {

        //The PID controller works in the % (unsigned) domain, so we'll
        //need to multiply the output by -1 for the motor which needs
        //to go "backwards".
        int leftDirection  = 1;
        int rightDirection = 1;
        headingSetPoint = length + (endHeading - startHeading);

        //In case the rover tries to [pointlessly] turn >360 degrees.
        if (headingSetPoint > 359.8) {

            headingSetPoint -= 359.8;

        }
        
        //Set up the right previous heading for the initial degreesTurned
        //calculation.
        prevHeading = fabs(imu.getYaw());

        if (strcmp(cmd1, "left") == 0) {

            //When turning left, the left motor needs to go backwards
            //while the right motor goes forwards.
            leftDirection  = -1;

        } else if (strcmp(cmd1, "right") == 0) {

            //When turning right, the right motor needs to go backwards
            //while the left motor goes forwards.
            rightDirection = -1;

        }

        //Turn slowly to ensure we don't overshoot the angle by missing
        //the relatively slow readings [in comparison to the PID loop speed]
        //from the IMU.
        leftPid.setSetPoint(500);
        rightPid.setSetPoint(500);

        //Keep turning until we've moved through the correct angle as defined
        //by the heading set point.
        while (degreesTurned < headingSetPoint) {

            //Get the new heading and subtract the previous heading to
            //determine how many more degrees we've moved through.
            //As we're only interested in the relative angle (as opposed to
            //absolute) we'll take the absolute value of the heading to avoid
            //any nastiness when trying to calculate angles as they jump from
            //179.9 to -179.9 degrees.
            heading = fabs(imu.getYaw());
            degreesTurned += fabs(heading - prevHeading);
            prevHeading = heading;

            //Get the current pulse count and subtract the previous one to
            //calculate the current velocity in counts per second.
            leftPulses = leftQei.getPulses();
            leftVelocity = (leftPulses - leftPrevPulses) / RATE;
            leftPrevPulses = leftPulses;
            //Use the absolute value of velocity as the PID controller works
            //in the % (unsigned) domain and will get confused with -ve values.
            leftPid.setProcessValue(fabs(leftVelocity));
            leftMotor.speed(leftPid.compute() * leftDirection);

            rightPulses = rightQei.getPulses();
            rightVelocity = (rightPulses - rightPrevPulses) / RATE;
            rightPrevPulses = rightPulses;
            rightPid.setProcessValue(fabs(rightVelocity));
            rightMotor.speed(rightPid.compute() * rightDirection);

            wait(RATE);

        }

        leftMotor.brake();
        rightMotor.brake();

    }

    //Reset working variables.
    leftQei.reset();
    rightQei.reset();

    leftPulses     = 0;
    leftPrevPulses = 0;
    leftVelocity   = 0.0;
    leftMotor.speed(0.0);

    rightPulses     = 0;
    rightPrevPulses = 0;
    rightVelocity   = 0.0;
    rightMotor.speed(0.0);

    leftPid.setSetPoint(0.0);
    leftPid.setProcessValue(0.0);
    rightPid.setSetPoint(0.0);
    rightPid.setProcessValue(0.0);

    positionSetPoint = 0.0;
    headingSetPoint  = 0.0;
    heading          = 0.0;
    prevHeading      = 0.0;
    degreesTurned    = 0.0;

}

int main() {

    pc.printf("Starting mbed rover test...\n");

    initializeMotors();
    initializePid();

    //Some delay before we start moving.
    wait(5);

    //Open the command script.
    FILE* commands = fopen("/local/commands.txt", "r");

    //Check if we were successful in opening the command script.
    if (commands == NULL) {
        pc.printf("Could not open command file...\n");
        exit(EXIT_FAILURE);
    } else {
        pc.printf("Succesfully opened command file!\n");
    }

    //While there's another line to read from the command script.
    while (fscanf(commands, "%s%s%f", cmd0, cmd1, &cmd2) >= 0) {

        pc.printf("%s %s %f\n", cmd0, cmd1, cmd2);

        processCommand(cmd0, cmd1, cmd2);

        wait(1);

    }

    fclose(commands);

}