File content as of revision 0:fbfafa6bf5f9:

/**********************************************************************
 * @file        motors.cpp
 * @purpose     Eurobot 2012 - Secondary Robot MD25 Interface
 * @version     0.2
 * @date        4th April 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 "motors.h"
#include "globals.h"
#include "TSH.h"

Motors::Motors(TSI2C &i2cin) : i2c(i2cin) {

}

//***************************************************************************************
//Secondary robot specific instructions
//***************************************************************************************

void Motors::move(int distance, int speed) {
    //resetEncoders(); TODO use kalman as feedback instead!

    int tempEndEncoder = 0;
    int startEncoderCount = 0;

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

    setSpeed(getSignOfInt(distance) * speed);

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

    //resetEncoders();
    stop();
}

void Motors::turn(int angle, int speed) {
    //resetEncoders(); TODO use kalman as feedback instead!
    int tempDistance = int((float(angle) / 360) * float(robotCircumference));
    int tempEndEncoder = 0;
    int 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();
}

//***************************************************************************************
//Secondary robot specific helper functions
//***************************************************************************************


int Motors::getSignOfInt(int direction) {

    direction = (direction < 0);

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

    return 0;
}

// returns distance in mm.
float Motors::encoderToDistance(int encoder) {
    return (float(encoder) / float(encoderRevCount)) * wheelmm;
}

int Motors::distanceToEncoder(float distance) {
    return int((distance / float(wheelmm)) * encoderRevCount);
}


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

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

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

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

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

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

int Motors::getEncoder1() {
    return get4Bytes(cmdGetEncoder1);
}

int Motors::getEncoder2() {
    return get4Bytes(cmdGetEncoder2);
}

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



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

int Motors::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;
}

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

void Motors::sendCommand(char command1, char command2 ) {

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

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