File content as of revision 0:c8c04928d025:

/**********************************************************************
 * @file        i2c_monitor.c
 * @purpose     Eurobot 2012 - Primary Robot MD25 Interface
 * @version     0.2
 * @date        13. Feb. 2012
 * @author      Crispian Poon
 * @email       pooncg@gmail.com
 ______________________________________________________________________

 Setup information:
 1. Put pull up 2.2k resistors to +3.3v on I2C SCL and SDA
 2. Connect P28 SDA to MD25 yellow cable, P27 SCL to MD25 blue cable

 **********************************************************************/

#include "mbed.h"
#include "PID.h"
//Sonar includes
#include "RF12B.h"
#include "SRF05.h"

//Constants declaration
const int md25Address = 0xB0;
const char cmdSetMotor1 = 0x00;
const char cmdSetMotor2 = 0x01;
const char cmdByte = 0x10;
const char cmdDisableAcceleration = 0x30;
const char cmdSetMode = 0x0F;
const char cmdResetEncoders = 0x20;
const char cmdGetEncoder1 = 0x02;
const char cmdGetEncoder2 = 0x06;
const int encoderRevCount = 1856;
const int wheelmm = 314;
const int robotCircumference = 1052;
#define RATE 0.01
//#include <queue>
#define CODE 0x88


//Global variables declaration
int tempByte;
PID PIDController1(0.9, 2, 0, RATE);

//Interface declaration
I2C i2c(p28, p27);        // sda, scl
Serial pc(USBTX, USBRX); // tx, rx
DigitalOut     OBLED1(LED1);
DigitalOut     OBLED2(LED2);
SRF05 my_srf1(p13,p21);
SRF05 my_srf2(p13,p22);
SRF05 my_srf3(p13,p23);
RF12B RF_Robot(p5,p6,p7,p8,p9);

//Functions declaration
void resetEncoders();
long getEncoder1();
long getEncoder2();
void move(long distance, int speed);
void turn(int angle, int speed);
int getSignOfInt(int direction);
void stop();
void setSpeed(int speed);
void setSpeed(int speed1, int speed2);
void setMode(int mode);
long encoderToDistance(long encoder);
long distanceToEncoder(long distance);
void sendCommand(char command);
void sendCommand(char command1, char command2 );
long get4Bytes(char command);

//Main loop
int main() {
    int Cmd = 30;


    //PID controller initialisation
    PIDController1.setMode(AUTO_MODE);
    PIDController1.setInputLimits(0, 2500);   //arbitarily set limit of 10000, can change!
    PIDController1.setOutputLimits(0, 2500);


    //TODO serial interface code, maybe with interrupt?

    while (1) {

        //Read serial to change variable
        if (pc.readable() == 1) {
            pc.scanf("%i", &Cmd);
        }

        //Tune PID referece
        PIDController1.setTunings(0.4, 2, 0);

        //Debug info
        pc.printf("%i",Cmd);


        move(500, Cmd);
        wait_ms(2000);
        //move(-200, 70);

    }
}

//***************************************************************************************
//Primary robot specific instructions
//***************************************************************************************

float getSonar() {
    float distance[3];
    float result = 0;

    RF_Robot.write(CODE);
    my_srf1.trig();
    wait_ms(50);

    distance[0] = my_srf1.read();
    distance[1] = my_srf2.read();
    distance[2] = my_srf3.read();

    for (int i = 0; i < 3; i++) {
        if ( distance[i] > result) {
            result = distance[i];
        }
    }
    return result;
}

//*********************************************
//Parameter @distance in mm, @speed is 0 to 127
//Description: moves robot by @distance mm at @speed in straight line
//*********************************************
void move(long distance, int speed) {

    long tempEndEncoder = 0;
    long startEncoderCount = 0;
    long previousEncoder1Count = 0;
    long previousEncoder2Count = 0;
    long tempEncoder1Speed = 0;
    long tempEncoder2Speed = 0;
    int directionFlag = 0;
    float encoderSpeedConstant = 0;

    //Variables initialisation
    resetEncoders();    //reset MD25 encoder TODO too large distance may cause overflow
    tempEndEncoder = distanceToEncoder(abs(distance));     //Convert distance to end encoder value
    directionFlag = getSignOfInt(distance);     //get direction from distance e.g. -1mm, +1mm converter to - and + operator

    //setSpeed(speed);
    //wait_ms(100);

    while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {

        //TODO loop counter

        //calculate speed of encoder1 and encoder 2 change
        tempEncoder1Speed = abs(getEncoder1() - previousEncoder1Count);
        tempEncoder2Speed = abs(getEncoder2() - previousEncoder2Count);


        //TODO optimize so only 1 calc is done per function call
        //get encoder speed to @speed conversion ratio
        encoderSpeedConstant = double(speed) / double(tempEncoder1Speed);

        //set previous loop encoder values
        previousEncoder1Count = getEncoder1();
        previousEncoder2Count = getEncoder2();

        //Set PID input value, tries to adjust encoder2 speed to encoder1 speed
        PIDController1.setSetPoint(tempEncoder1Speed);
        PIDController1.setProcessValue(tempEncoder2Speed);

        float tempVal = PIDController1.compute();
        //Debug Info
        //pc.printf("%f \n", tempVal);
        //PID control of real motor speeds, Motor2 actual speed mirros Motor1 actual speed
        setSpeed(directionFlag*speed, directionFlag * tempVal * encoderSpeedConstant);

        //delay to increase significant encoder counts
        wait(RATE);

    }

    //Stop robot
    stop();

    //Debug info - see if loop is completed.
    OBLED1 = true;

    //Reset encoder values
    resetEncoders();
}

void turn(int angle, int speed) {
    resetEncoders();
    long tempDistance = long((float(angle) / 360) * float(robotCircumference));
    long tempEndEncoder = 0;
    long startEncoderCount = 0;

    tempEndEncoder = distanceToEncoder(abs(tempDistance));
    startEncoderCount = getEncoder1();
    setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);

    while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
        setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);

    }

    resetEncoders();
    stop();
}


//***************************************************************************************
//Primary robot specific helper functions
//***************************************************************************************

int getSignOfInt(int direction) {

    direction = (direction < 0);

    switch (direction) {
        case 1:
            return -1;
        case 0:
            return 1;
    }

    return 0;
}

long encoderToDistance(long encoder) {
    return long((float(encoder) / float(encoderRevCount)) * wheelmm);
}

long distanceToEncoder(long distance) {
    return long((float(distance) / float(wheelmm)) * encoderRevCount);
}


//***************************************************************************************
//MD25 instructions
//***************************************************************************************

void stop() {
    sendCommand(cmdSetMotor1, 0);
    sendCommand(cmdSetMotor2, 0);
}

void disableAcceleration() {
    sendCommand(cmdByte, cmdDisableAcceleration);
}

void setSpeed(int speed) {
    setMode(1);
    disableAcceleration();
    sendCommand(cmdSetMotor1, speed);
    sendCommand(cmdSetMotor2, speed);
}

void setSpeed(int speed1, int speed2) {
    setMode(1),
    disableAcceleration();
    sendCommand(cmdSetMotor1, speed1);
    sendCommand(cmdSetMotor2, speed2);
}

void setMode(int mode) {
    sendCommand(cmdSetMode, mode);
}

void resetEncoders() {
    sendCommand(cmdByte, cmdResetEncoders) ;
}

long getEncoder1() {
    return get4Bytes(cmdGetEncoder1);
}

long getEncoder2() {
    return get4Bytes(cmdGetEncoder2);
}


//***************************************************************************************
//Abstract MD25 communication methods and functions
//***************************************************************************************

void sendCommand(char command) {
    char buffer[1];
    buffer[0] = command;
    i2c.write(md25Address, &buffer[0], 1);
}

void sendCommand(char command1, char command2 ) {

    char buffer[2];
    buffer[0] = command1;
    buffer[1] = command2;

    i2c.write(md25Address, &buffer[0], 2);
}

long get4Bytes(char command) {
    long tempWord = 0;
    char cmd[4];

    //i2c request
    sendCommand(command);

    //i2c read data back
    i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25

    //FIXED 22FEB2012 CRISPIAN Taken 0.07 delay off.

    //First byte is largest, shift 4 bytes into tempWord
    tempWord += cmd[0] << 24;
    tempWord += cmd[1] << 16;
    tempWord += cmd[2] << 8;
    tempWord += cmd[3] ;

    return tempWord;
}