Diff: motors/motors.cpp

Revision:

1:cc2a9eb0bd55

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motors/motors.cpp	Wed Oct 17 22:25:31 2012 +0000
@@ -0,0 +1,185 @@
+/**********************************************************************
+ * @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(cmdSetAcceleration, 0x02);
+    ///sendCommand(cmdByte, 0x30);
+    sendCommand(cmdSetMotor1, speed);
+    sendCommand(cmdSetMotor2, speed);
+}
+
+void Motors::setSpeed(int speed1, int speed2) {
+    setMode(1),
+    //sendCommand(cmdSetAcceleration, 0x02);
+    // 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);
+}
+
+void Motors::coastStop(void){
+    stop();
+}
\ No newline at end of file