Revision 3:de12b39ad805, committed 2013-11-25

Comitter:

dhamilton31

Date:

Mon Nov 25 19:55:12 2013 +0000

Parent:

2:0b362c662997

Commit message:

Added additional servo control and printing commands to lcd

Changed in this revision

diff -r 0b362c662997 -r de12b39ad805 Rectangle.lib
--- a/Rectangle.lib	Thu Nov 21 00:02:05 2013 +0000
+++ b/Rectangle.lib	Mon Nov 25 19:55:12 2013 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/projetremote/code/Rectangle/#d9c0709ba2ce
+http://mbed.org/users/dhamilton31/code/Rectangle/#087e0b0b526c

diff -r 0b362c662997 -r de12b39ad805 Servo.lib
--- a/Servo.lib	Thu Nov 21 00:02:05 2013 +0000
+++ b/Servo.lib	Mon Nov 25 19:55:12 2013 +0000
@@ -1,1 +1,1 @@
-https://mbed.org/users/simon/code/Servo/#dec99beeafee
+http://mbed.org/users/dhamilton31/code/Servo/#dec99beeafee

diff -r 0b362c662997 -r de12b39ad805 TextLCD.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Mon Nov 25 19:55:12 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37

diff -r 0b362c662997 -r de12b39ad805 iRobot/iRobot.cpp
--- a/iRobot/iRobot.cpp	Thu Nov 21 00:02:05 2013 +0000
+++ b/iRobot/iRobot.cpp	Mon Nov 25 19:55:12 2013 +0000
@@ -28,15 +28,15 @@
 // Start  - send start and safe mode, start streaming sensor data
 void iRobot::start()
 {
-    // device.printf("%c%c", Start, SafeMode);
-    device.putc(Start);
-    device.putc(SafeMode);
+    device.printf("%c%c", Start, SafeMode);
+    //device.putc(Start);
+    //device.putc(SafeMode);
     wait(.5);
-    //  device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
+     /* device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
     device.putc(SensorStream);
     device.putc(1);
     device.putc(BumpsandDrops);
-    wait(.2);
+    wait(.2);*/
     // Setup a serial interrupt function to receive data
     device.attach(this, &iRobot::receive_sensor);
 }
@@ -72,6 +72,33 @@
                   char((speed_left>>8)&0xFF),  char(speed_left&0xFF));
 
 }
+// Angle - The angle in degrees that iRobot Create has turned since the 
+//angle was last requested 
+int iRobot::getAngle()
+{
+    char angleBuff[2];
+    device.printf("%c%c", Sensors, Angle);
+    device.gets(angleBuff, 2);
+    return atoi(angleBuff);
+}
+
+// Turn Angle - The angle in degrees that iRobot Create has turned since the 
+//angle was last requested ccw is pos angle, cw is neg angle
+void iRobot::turnAngle(int angle)
+{
+    char angleTurn[3];
+    angleTurn[0] = waitAngle;
+    angleTurn[1] = angle >> 8;
+    angleTurn[2] = angle & 0xFF;
+    if(angle > 0){
+        device.printf("%c%c%c%c",char(137), char(0), char(200), char(0), char(1));
+    }
+    else if(angle < 0){
+        device.printf("%c%c%c%c",char(137), char(0), char(200), char(255), char(255));
+    }
+    device.printf("%c%c%c", angleTurn[0], angleTurn[1], angleTurn[2]);
+    wait(.05);
+}
 // Charger - search and return to charger using IR beacons (if found)
 void iRobot::charger()
 {
@@ -96,6 +123,7 @@
 void iRobot::receive_sensor()
 {
     char start_character;
+    printf("reading\n");
 // Loop just in case more than one character is in UART's receive FIFO buffer
     while (device.readable()) {
         switch (Sensor_byte_count) {

diff -r 0b362c662997 -r de12b39ad805 iRobot/iRobot.h
--- a/iRobot/iRobot.h	Thu Nov 21 00:02:05 2013 +0000
+++ b/iRobot/iRobot.h	Mon Nov 25 19:55:12 2013 +0000
@@ -20,6 +20,8 @@
     void changeSpeed(int speed);
     void receive_sensor();
     char sensorCode();
+    int getAngle();
+    void turnAngle(int angle);
 
 private:
 
@@ -40,6 +42,7 @@
     static  const char         BumpsandDrops = 7;
     static const char         Distance = 19;
     static const char         Angle = 20;
+    static const char         waitAngle = 157;
     /* Global variables with sensor packet info */
     char Sensor_byte_count;
     char Sensor_Data_Byte;

diff -r 0b362c662997 -r de12b39ad805 main.cpp
--- a/main.cpp	Thu Nov 21 00:02:05 2013 +0000
+++ b/main.cpp	Mon Nov 25 19:55:12 2013 +0000
@@ -2,12 +2,19 @@
 #include "iRobot.h"
 #include "Servo.h"
 #include "Rectangle.h"
+#include "TextLCD.h"
 
 // Macros/Constants
-#define MAX_VIEW_X 1176 // maximum X value input from camera
-#define MAX_VIEW_Y 711 // maximum Y value input from camera
+#define MAX_VIEW_X 1600 // maximum X value input from camera
+#define MAX_VIEW_Y 1200 // maximum Y value input from camera
 #define CENTER_BOX_TOLLERANCE 200 // Size of our box
-#define TO_SERVO_DIVISOR 1176.0 // Value to divide by to get the amount to move the servo by
+#define TO_SERVO_DIVISOR 3000.0 // Value to divide by to get the amount to move the servo by
+#define NO_COLOR_MAX_COUNT 10
+#define COLLISION_DIST .4
+#define BLOB_CLOSE_DIST 200
+#define SERVO_HOME .5
+#define SERVO_HOME_TOL .2
+#define SPEED_CONST 65535
 
 // Hardware sensors and devices
 DigitalOut myled(LED1);
@@ -15,73 +22,123 @@
 iRobot followMeBot(p9, p10);
 Servo servoHor(p22);
 Servo servoVer(p21);
-AnalogIn irSensorFront(p20);
-AnalogIn irSensorLeft(p19);
-AnalogIn irSensorRight(p18);
+AnalogIn irSensorFront(p15);
+//AnalogIn irSensorLeft(p19);
+//AnalogIn irSensorRight(p18);
 Serial pc(USBTX, USBRX); // tx, rx
+TextLCD lcd(p14, p16, p17, p18, p19, p20); // rs, e, d4-d7
 
 // Software variables
-char serial_rx_buffer[128]; // Input buffer for data from the PC
-int xpos, ypos; // x and y positions read from matlab
-Rectangle centerBox((MAX_VIEW_X/2)+CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)+CENTER_BOX_TOLLERANCE,
-                    (MAX_VIEW_X/2)-CENTER_BOX_TOLLERANCE,(MAX_VIEW_Y/2)-CENTER_BOX_TOLLERANCE); // Creates a box to examine if the camera is well enough centered
+char serial_rx_buffer[256]; // Input buffer for data from the PC
+int xpos, ypos, blobArea; // x and y positions read from matlab
+Rectangle centerBox((MAX_VIEW_X/2)-CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)-CENTER_BOX_TOLLERANCE,
+                    (MAX_VIEW_X/2)+CENTER_BOX_TOLLERANCE,(MAX_VIEW_Y/2)+CENTER_BOX_TOLLERANCE); // Creates a box to examine if the camera is well enough centered
+int noColorCounter; // Counts how long it has been since we have seen a color to follow
+bool colorLost; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT)
 
 // Function Prototypes
 void getXYpos();
-void moveCamera();
+float moveCamera();
+void moveBot();
+int servoIsInHome();
 
 int main()
 {
     //followMeBot.start();
-
+    //wait(1);
     while(1) {
         getXYpos();
-        wait(.5);
+        moveBot();
     }
 }
 
-void moveCamera()
+/**
+*   Moves the servo to move the camera based upon where the
+*   color is located on the reported x and y
+*
+*/
+float moveCamera()
 {
-    if(xpos == 0) {
-        servoHor = .5;
-    } else if(!centerBox.is_touch(xpos, ypos)) {
-        float temp;
-        //printf("Servo at: %f\n", servoHor.read());
-        temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR;
-        //printf("move servo by: %f\n", (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR);
-        if(temp > 0 && temp <= 1) {
-            servoHor = temp;
-          sprintf(serial_rx_buffer, "%f\n", temp);
+    float temp = 0;
+    if(xpos == 0) { // If we recieve a 0 for the location
+        if(!colorLost && (++noColorCounter > NO_COLOR_MAX_COUNT)) { // Check to see if we have seen enough to consider the color lost
+            servoHor = .5; // If the color is lost, return servo to home
+            colorLost = true; // Set colorLost to true
+            noColorCounter = 0; // Reset counter
+        }
+    } else if(!centerBox.is_touch(xpos, (MAX_VIEW_Y/2))) { // If we have a location
+        noColorCounter = 0; // Reset counter
+        colorLost = false; // We have found the color!
+        temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR; // Calculate location to move servo to
+        if(temp > 0 && temp <= 1) { // If the value is within the servo range
+            servoHor = temp; // Set the servo equal to the position
+            //sprintf(serial_rx_buffer, "%f\n", temp);
         }
         /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
         if(temp > 0 && temp <= 1) {
             servoVer = temp;
         }*/
     }
-    pc.puts(serial_rx_buffer);
+    //pc.puts(serial_rx_buffer);
+    return temp; // return the servo position
 }
 
+// Will return the number of degrees to turn the irobot by
 void getXYpos()
 {
-    //char * temp;
-    //const char del = ';';
-
-    if(pc.readable()) {
+    char * temp;
+    const char del = ';';
+    if(pc.readable()) { // See if matlab has data for us
         myled = 1;
-        pc.gets(serial_rx_buffer, 128);
-        xpos = atoi(serial_rx_buffer);
-        //sprintf(serial_rx_buffer, "%d\n", xpos);
-        //pc.puts(serial_rx_buffer);
-        //temp = strtok(serial_rx_buffer, &del);
-        moveCamera();
+        pc.gets(serial_rx_buffer, 256); // Get position data
+        pc.puts(serial_rx_buffer);
+        temp = strtok(serial_rx_buffer, &del);
+        xpos = atoi(temp); // Convert data to xposition int
+        temp = strtok(NULL, &del);
+        ypos = atoi(temp);
+        temp = strtok(NULL, &del);
+        blobArea = atoi(temp);
+        lcd.cls();
+        lcd.locate(0,0);
+        lcd.printf("%d;%d", xpos, ypos);
+        moveCamera(); // Move the camera
     } else {
         myled = 0;
     }
-    if(xpos > 500) {
-        myled2 = 1;
-    } else {
-        myled2 = 0;
+}
+
+void moveBot()
+{
+    float irVal = irSensorFront;
+    lcd.locate(0,1);
+    if(!colorLost) {
+        if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) {
+            //followMeBot.stop();
+            lcd.printf("stop");
+        } else if(servoIsInHome() > 0) {
+            //followMeBot.right();
+            lcd.printf("right");
+        } else if(servoIsInHome() < 0) {
+            //followMeBot.left();
+            lcd.printf("left");
+        } else {
+            //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100));
+            //followMeBot.forward();
+            lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));
+        }
+    }
+    else{
+        lcd.printf("Color Lost");
     }
 }
 
-
+int servoIsInHome()
+{
+    if(servoHor.read() > SERVO_HOME + SERVO_HOME_TOL) {
+        return 1;
+    } else if(servoHor.read() < SERVO_HOME - SERVO_HOME_TOL) {
+        return -1;
+    } else {
+        return 0;
+    }
+}