Shuto Naruse
/
Eurobot2012_Secondary
Eurobot2012_Secondary
Fork of Eurobot_2012_Secondary by
Diff: motors.cpp
- Revision:
- 1:cc2a9eb0bd55
- Parent:
- 0:fbfafa6bf5f9
--- a/motors.cpp Fri Apr 20 21:32:24 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,179 +0,0 @@ -/********************************************************************** - * @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