Diff: main.cpp

Revision:

2:0b362c662997

Parent:

1:6c399fc35deb

Child:

3:de12b39ad805

--- a/main.cpp	Mon Nov 18 21:09:13 2013 +0000
+++ b/main.cpp	Thu Nov 21 00:02:05 2013 +0000
@@ -4,13 +4,14 @@
 #include "Rectangle.h"
 
 // Macros/Constants
-#define MAX_VIEW_X 1024 // maximum X value input from camera
-#define MAX_VIEW_Y 1024 // maximum Y value input from camera
-#define CENTER_BOX_TOLLERANCE 15 // Size of our box
-#define TO_SERVO_DIVISOR 100.0 // Value to divide by to get the amount to move the servo by
+#define MAX_VIEW_X 1176 // maximum X value input from camera
+#define MAX_VIEW_Y 711 // 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
 
 // Hardware sensors and devices
 DigitalOut myled(LED1);
+DigitalOut myled2(LED2);
 iRobot followMeBot(p9, p10);
 Servo servoHor(p22);
 Servo servoVer(p21);
@@ -25,26 +26,61 @@
 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
 
+// Function Prototypes
+void getXYpos();
+void moveCamera();
+
 int main()
 {
-    followMeBot.start();
+    //followMeBot.start();
 
     while(1) {
+        getXYpos();
+        wait(.5);
     }
 }
 
 void moveCamera()
 {
-    if(!centerBox.is_touch(xpos, ypos)) {
+    if(xpos == 0) {
+        servoHor = .5;
+    } else if(!centerBox.is_touch(xpos, ypos)) {
         float temp;
-        temp = servoHor.read() + ((float)centerBox.getCenterX() - (float)xpos)/TO_SERVO_DIVISOR;
+        //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);
         }
-        temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
+        /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
         if(temp > 0 && temp <= 1) {
             servoVer = temp;
-        }
+        }*/
+    }
+    pc.puts(serial_rx_buffer);
+}
+
+void getXYpos()
+{
+    //char * temp;
+    //const char del = ';';
+
+    if(pc.readable()) {
+        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();
+    } else {
+        myled = 0;
+    }
+    if(xpos > 500) {
+        myled2 = 1;
+    } else {
+        myled2 = 0;
     }
 }