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(cmdSetAcceleration, 0x02);
00108     ///sendCommand(cmdByte, 0x30);
00109     sendCommand(cmdSetMotor1, speed);
00110     sendCommand(cmdSetMotor2, speed);
00111 }
00112 
00113 void Motors::setSpeed(int speed1, int speed2) {
00114     setMode(1),
00115     //sendCommand(cmdSetAcceleration, 0x02);
00116     // sendCommand(cmdByte, 0x30);
00117     sendCommand(cmdSetMotor1, speed1);
00118     sendCommand(cmdSetMotor2, speed2);
00119 }
00120 
00121 void Motors::setMode(int mode) {
00122     sendCommand(cmdSetMode, mode);
00123 }
00124 
00125 void Motors::resetEncoders() {
00126     sendCommand(cmdByte, cmdResetEncoders);
00127 }
00128 
00129 int Motors::getEncoder1() {
00130     return get4Bytes(cmdGetEncoder1);
00131 }
00132 
00133 int Motors::getEncoder2() {
00134     return get4Bytes(cmdGetEncoder2);
00135 }
00136 
00137 void Motors::disableAcceleration() {
00138     sendCommand(cmdByte, cmdDisableAcceleration);
00139 }
00140 
00141 
00142 
00143 //***************************************************************************************
00144 //Abstract MD25 communication methods and functions
00145 //***************************************************************************************
00146 
00147 int Motors::get4Bytes(char command) {
00148     long tempWord = 0;
00149     char cmd[4];
00150 
00151     //i2c request
00152     sendCommand(command);
00153 
00154     //i2c read data back
00155     i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25
00156 
00157     //FIXED 22FEB2012 CRISPIAN Taken 0.07 delay off.
00158 
00159     //First byte is largest, shift 4 bytes into tempWord
00160     tempWord += cmd[0] << 24;
00161     tempWord += cmd[1] << 16;
00162     tempWord += cmd[2] << 8;
00163     tempWord += cmd[3] ;
00164 
00165     return tempWord;
00166 }
00167 
00168 void Motors::sendCommand(char command) {
00169     char buffer[1];
00170     buffer[0] = command;
00171     i2c.write(md25Address, &buffer[0], 1);
00172 }
00173 
00174 void Motors::sendCommand(char command1, char command2 ) {
00175 
00176     char buffer[2];
00177     buffer[0] = command1;
00178     buffer[1] = command2;
00179 
00180     i2c.write(md25Address, &buffer[0], 2);
00181 }
00182 
00183 void Motors::coastStop(void){
00184     stop();
00185 }