Shuto Naruse
/
Eurobot_2012_Secondary
Eurobot_2012_Secondary
motors.cpp
- Committer:
- narshu
- Date:
- 2012-04-20
- Revision:
- 0:fbfafa6bf5f9
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); }