Diff: main.cpp

Revision:

1:6c399fc35deb

Parent:

0:3ed56271dd2d

Child:

2:0b362c662997

--- a/main.cpp	Mon Nov 11 19:22:13 2013 +0000
+++ b/main.cpp	Mon Nov 18 21:09:13 2013 +0000
@@ -1,17 +1,51 @@
 #include "mbed.h"
 #include "iRobot.h"
 #include "Servo.h"
+#include "Rectangle.h"
 
-DigitalOut myled(LED1);
+// 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
 
-int main() {
-    iRobot followMeBot(p9, p10);
-    Servo servoHor(p22);
-    Servo servoVer(p21);
+// Hardware sensors and devices
+DigitalOut myled(LED1);
+iRobot followMeBot(p9, p10);
+Servo servoHor(p22);
+Servo servoVer(p21);
+AnalogIn irSensorFront(p20);
+AnalogIn irSensorLeft(p19);
+AnalogIn irSensorRight(p18);
+Serial pc(USBTX, USBRX); // tx, rx
+
+// 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
+
+int main()
+{
+    followMeBot.start();
+
     while(1) {
-        myled = 1;
-        wait(0.2);
-        myled = 0;
-        wait(0.2);
     }
 }
+
+void moveCamera()
+{
+    if(!centerBox.is_touch(xpos, ypos)) {
+        float temp;
+        temp = servoHor.read() + ((float)centerBox.getCenterX() - (float)xpos)/TO_SERVO_DIVISOR;
+        if(temp > 0 && temp <= 1) {
+            servoHor = temp;
+        }
+        temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
+        if(temp > 0 && temp <= 1) {
+            servoVer = temp;
+        }
+    }
+}
+
+