turning
Dependencies: Rectangle Servo TextLCD mbed
Fork of FollowMeBot by
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; } }