motors.cpp

00001 /**********************************************************************
00002  * @file        motors.cpp
00003  * @purpose     Eurobot 2012 - Secondary Robot MD25 Interface
00004  * @version     0.2
00005  * @date        4th April 2012
00006  * @author      Crispian Poon
00007  * @email       pooncg@gmail.com
00008  ______________________________________________________________________
00009 
00010  Setup information:
00011  1. Put pull up 2.2k resistors to +3.3v on I2C SCL and SDA
00012  2. Connect P28 SDA to MD25 yellow cable, P27 SCL to MD25 blue cable
00013 
00014  **********************************************************************/
00015 
00016 #include "mbed.h"
00017 #include "motors.h"
00018 #include "globals.h"
00019 #include "TSH.h"
00020 
00021 Motors::Motors(TSI2C &i2cin) : i2c(i2cin) {
00022 
00023 }
00024 
00025 //***************************************************************************************
00026 //Secondary robot specific instructions
00027 //***************************************************************************************
00028 
00029 void Motors::move(int distance, int speed) {
00030     //resetEncoders(); TODO use kalman as feedback instead!
00031 
00032     int tempEndEncoder = 0;
00033     int startEncoderCount = 0;
00034 
00035     tempEndEncoder = distanceToEncoder(abs(distance));
00036     startEncoderCount = getEncoder1();
00037 
00038     setSpeed(getSignOfInt(distance) * speed);
00039 
00040     while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
00041         setSpeed(getSignOfInt(distance) * speed);
00042     }
00043 
00044     //resetEncoders();
00045     stop();
00046 }
00047 
00048 void Motors::turn(int angle, int speed) {
00049     //resetEncoders(); TODO use kalman as feedback instead!
00050     int tempDistance = int((float(angle) / 360) * float(robotCircumference));
00051     int tempEndEncoder = 0;
00052     int startEncoderCount = 0;
00053 
00054     tempEndEncoder = distanceToEncoder(abs(tempDistance));
00055     startEncoderCount = getEncoder1();
00056     setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);
00057 
00058     while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
00059         setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);
00060 
00061     }
00062 
00063     //resetEncoders();
00064     stop();
00065 }
00066 
00067 //***************************************************************************************
00068 //Secondary robot specific helper functions
00069 //***************************************************************************************
00070 
00071 
00072 int Motors::getSignOfInt(int direction) {
00073 
00074     direction = (direction < 0);
00075 
00076     switch (direction) {
00077         case 1:
00078             return -1;
00079         case 0:
00080             return 1;
00081     }
00082 
00083     return 0;
00084 }
00085 
00086 // returns distance in mm.
00087 float Motors::encoderToDistance(int encoder) {
00088     return (float(encoder) / float(encoderRevCount)) * wheelmm;
00089 }
00090 
00091 int Motors::distanceToEncoder(float distance) {
00092     return int((distance / float(wheelmm)) * encoderRevCount);
00093 }
00094 
00095 
00096 //***************************************************************************************
00097 //MD25 instructions
00098 //***************************************************************************************
00099 
00100 void Motors::stop() {
00101     sendCommand(cmdSetMotor1, 0);
00102     sendCommand(cmdSetMotor2, 0);
00103 }
00104 
00105 void Motors::setSpeed(int speed) {
00106     setMode(1);
00107     ///sendCommand(cmdByte, 0x30);
00108     sendCommand(cmdSetMotor1, speed);
00109     sendCommand(cmdSetMotor2, speed);
00110 }
00111 
00112 void Motors::setSpeed(int speed1, int speed2) {
00113     setMode(1),
00114     // sendCommand(cmdByte, 0x30);
00115     sendCommand(cmdSetMotor1, speed1);
00116     sendCommand(cmdSetMotor2, speed2);
00117 }
00118 
00119 void Motors::setMode(int mode) {
00120     sendCommand(cmdSetMode, mode);
00121 }
00122 
00123 void Motors::resetEncoders() {
00124     sendCommand(cmdByte, cmdResetEncoders);
00125 }
00126 
00127 int Motors::getEncoder1() {
00128     return get4Bytes(cmdGetEncoder1);
00129 }
00130 
00131 int Motors::getEncoder2() {
00132     return get4Bytes(cmdGetEncoder2);
00133 }
00134 
00135 void Motors::disableAcceleration() {
00136     sendCommand(cmdByte, cmdDisableAcceleration);
00137 }
00138 
00139 
00140 
00141 //***************************************************************************************
00142 //Abstract MD25 communication methods and functions
00143 //***************************************************************************************
00144 
00145 int Motors::get4Bytes(char command) {
00146     long tempWord = 0;
00147     char cmd[4];
00148 
00149     //i2c request
00150     sendCommand(command);
00151 
00152     //i2c read data back
00153     i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25
00154 
00155     //FIXED 22FEB2012 CRISPIAN Taken 0.07 delay off.
00156 
00157     //First byte is largest, shift 4 bytes into tempWord
00158     tempWord += cmd[0] << 24;
00159     tempWord += cmd[1] << 16;
00160     tempWord += cmd[2] << 8;
00161     tempWord += cmd[3] ;
00162 
00163     return tempWord;
00164 }
00165 
00166 void Motors::sendCommand(char command) {
00167     char buffer[1];
00168     buffer[0] = command;
00169     i2c.write(md25Address, &buffer[0], 1);
00170 }
00171 
00172 void Motors::sendCommand(char command1, char command2 ) {
00173 
00174     char buffer[2];
00175     buffer[0] = command1;
00176     buffer[1] = command2;
00177 
00178     i2c.write(md25Address, &buffer[0], 2);
00179 }