File content as of revision 0:dc84eed6b8b6:

/**********************************************************************
 * @file        i2c_monitor.c
 * @purpose     MD25 Interface
 * @version     0.1
 * @date        25. Jan. 2012
 * @author      Crispian Poon
 * @email       pooncg@gmail.com
 **********************************************************************/

#include "mbed.h"

//Constants declaration
const int md25Address = 0xB0;
const char cmdSetMotor1 = 0x00;
const char cmdSetMotor2 = 0x01;
const char cmdByte = 0x10;
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;


//Global variables declaration
int tempByte;

//Interface declaration
I2C i2c(p9, p10);        // sda, scl
Serial pc(USBTX, USBRX); // tx, rx
DigitalOut     OBLED1(LED1);
DigitalOut     OBLED2(LED2);

//Functions declaration
void resetEncoders();
long getEncoder1();
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 );

//Main loop
int main() {
    //TODO declare serial
    //setSpeed(-127);
    //delay(5000);
    //setSpeed(0);
    //Serial.println(getEncoder1(), DEC);
    
    //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
    while (1) {
       move(180, 30);
       wait_ms(3000);
       stop();
       turn(-180, 30);
        OBLED1 = true;
    }
}

void resetEncoders() {

    sendCommand(cmdByte, cmdResetEncoders) ;

}

long getEncoder1() {
    long tempEncoderCount = 0;
    char cmd[4];

    //i2c request encoder position 1
    sendCommand(cmdGetEncoder1);

    //TODO create abstract function for read command
    //i2c read data back
    i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25

    tempEncoderCount += cmd[0] << 24;
    tempEncoderCount += cmd[1] << 16;
    tempEncoderCount += cmd[2] << 8;
    tempEncoderCount += cmd[3] ;

    return tempEncoderCount;

}

long getEncoder2() {
    long tempEncoderCount = 0;
    char cmd[4];

    //i2c request encoder position 1
    sendCommand(cmdGetEncoder2);

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

    tempEncoderCount += cmd[0] << 24;
    tempEncoderCount += cmd[1] << 16;
    tempEncoderCount += cmd[2] << 8;
    tempEncoderCount += cmd[3] ;

    return tempEncoderCount;


}

void move(long distance, int speed) {
    resetEncoders();
    long tempEndEncoder = 0;
    long startEncoderCount = 0;

    tempEndEncoder = distanceToEncoder(abs(distance));
    startEncoderCount = getEncoder1();

    setSpeed(getSignOfInt(distance) * speed);

    while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
        setSpeed(getSignOfInt(distance) * speed);
    }


    resetEncoders();
    stop();
}

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


int getSignOfInt(int direction) {

    direction = (direction < 0);

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

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

void setSpeed(int speed) {
    setMode(1);
    ///sendCommand(cmdByte, 0x30);
    sendCommand(cmdSetMotor1, speed);
    sendCommand(cmdSetMotor2, speed);
}

void setSpeed(int speed1, int speed2) {
    setMode(1), 
   // sendCommand(cmdByte, 0x30);
    sendCommand(cmdSetMotor1, speed1);
    sendCommand(cmdSetMotor2, speed2);
}

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

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

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

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);
}