Shuto Naruse
/
Eurobot2012_Secondary
Eurobot2012_Secondary
Fork of Eurobot_2012_Secondary by
Diff: motors.cpp
- Revision:
- 0:fbfafa6bf5f9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motors.cpp Fri Apr 20 21:32:24 2012 +0000 @@ -0,0 +1,179 @@ +/********************************************************************** + * @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); +} \ No newline at end of file