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