Diff: main.cpp

Revision:

0:dc84eed6b8b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 14 17:58:49 2012 +0000
@@ -0,0 +1,214 @@
+/**********************************************************************
+ * @file        i2c_monitor.c
+ * @purpose     MD25 Interface
+ * @version     0.1
+ * @date        25. Jan. 2012
+ * @author      Crispian Poon
+ * @email       pooncg@gmail.com
+ **********************************************************************/
+
+#include "mbed.h"
+
+//Constants declaration
+const int md25Address = 0xB0;
+const char cmdSetMotor1 = 0x00;
+const char cmdSetMotor2 = 0x01;
+const char cmdByte = 0x10;
+const char cmdSetMode = 0x0F;
+const char cmdResetEncoders = 0x20;
+const char cmdGetEncoder1 = 0x02;
+const char cmdGetEncoder2 = 0x06;
+const int encoderRevCount = 1856;
+const int wheelmm = 314;
+const int robotCircumference = 1052;
+
+
+//Global variables declaration
+int tempByte;
+
+//Interface declaration
+I2C i2c(p9, p10);        // sda, scl
+Serial pc(USBTX, USBRX); // tx, rx
+DigitalOut     OBLED1(LED1);
+DigitalOut     OBLED2(LED2);
+
+//Functions declaration
+void resetEncoders();
+long getEncoder1();
+void move(long distance, int speed);
+void turn(int angle, int speed);
+int getSignOfInt(int direction);
+void stop();
+void setSpeed(int speed);
+void setSpeed(int speed1, int speed2);
+void setMode(int mode);
+long encoderToDistance(long encoder);
+long distanceToEncoder(long distance);
+void sendCommand(char command);
+void sendCommand(char command1, char command2 );
+
+//Main loop
+int main() {
+    //TODO declare serial
+    //setSpeed(-127);
+    //delay(5000);
+    //setSpeed(0);
+    //Serial.println(getEncoder1(), DEC);
+    
+    //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
+    while (1) {
+       move(180, 30);
+       wait_ms(3000);
+       stop();
+       turn(-180, 30);
+        OBLED1 = true;
+    }
+}
+
+void resetEncoders() {
+
+    sendCommand(cmdByte, cmdResetEncoders) ;
+
+}
+
+long getEncoder1() {
+    long tempEncoderCount = 0;
+    char cmd[4];
+
+    //i2c request encoder position 1
+    sendCommand(cmdGetEncoder1);
+
+    //TODO create abstract function for read command
+    //i2c read data back
+    i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25
+
+    tempEncoderCount += cmd[0] << 24;
+    tempEncoderCount += cmd[1] << 16;
+    tempEncoderCount += cmd[2] << 8;
+    tempEncoderCount += cmd[3] ;
+
+    return tempEncoderCount;
+
+}
+
+long getEncoder2() {
+    long tempEncoderCount = 0;
+    char cmd[4];
+
+    //i2c request encoder position 1
+    sendCommand(cmdGetEncoder2);
+
+    //i2c read data back
+    i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25
+
+    tempEncoderCount += cmd[0] << 24;
+    tempEncoderCount += cmd[1] << 16;
+    tempEncoderCount += cmd[2] << 8;
+    tempEncoderCount += cmd[3] ;
+
+    return tempEncoderCount;
+
+
+}
+
+void move(long distance, int speed) {
+    resetEncoders();
+    long tempEndEncoder = 0;
+    long startEncoderCount = 0;
+
+    tempEndEncoder = distanceToEncoder(abs(distance));
+    startEncoderCount = getEncoder1();
+
+    setSpeed(getSignOfInt(distance) * speed);
+
+    while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
+        setSpeed(getSignOfInt(distance) * speed);
+    }
+
+
+    resetEncoders();
+    stop();
+}
+
+void turn(int angle, int speed) {
+    resetEncoders();
+    long tempDistance = long((float(angle) / 360) * float(robotCircumference));
+    long tempEndEncoder = 0;
+    long 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();
+}
+
+
+int getSignOfInt(int direction) {
+
+    direction = (direction < 0);
+
+    switch (direction) {
+        case 1:
+            return -1;
+        case 0:
+            return 1;
+    }
+    
+    return 0;
+}
+
+void stop() {
+    sendCommand(cmdSetMotor1, 0);
+    sendCommand(cmdSetMotor2, 0);
+}
+
+void setSpeed(int speed) {
+    setMode(1);
+    ///sendCommand(cmdByte, 0x30);
+    sendCommand(cmdSetMotor1, speed);
+    sendCommand(cmdSetMotor2, speed);
+}
+
+void setSpeed(int speed1, int speed2) {
+    setMode(1), 
+   // sendCommand(cmdByte, 0x30);
+    sendCommand(cmdSetMotor1, speed1);
+    sendCommand(cmdSetMotor2, speed2);
+}
+
+void setMode(int mode) {
+    sendCommand(cmdSetMode, mode);
+}
+
+long encoderToDistance(long encoder) {
+    return long((float(encoder) / float(encoderRevCount)) * wheelmm);
+}
+
+long distanceToEncoder(long distance) {
+    return long((float(distance) / float(wheelmm)) * encoderRevCount);
+}
+
+void sendCommand(char command) {
+    char buffer[1];
+    buffer[0] = command;
+    i2c.write(md25Address, &buffer[0], 1);
+}
+
+void sendCommand(char command1, char command2 ) {
+
+    char buffer[2];
+    buffer[0] = command1;
+    buffer[1] = command2;
+    
+    i2c.write(md25Address, &buffer[0], 2);
+}
+
+
+