turning
Dependencies: Rectangle Servo TextLCD mbed
Fork of FollowMeBot by
main.cpp
- Committer:
- dhamilton31
- Date:
- 2013-11-21
- Revision:
- 2:0b362c662997
- Parent:
- 1:6c399fc35deb
- Child:
- 3:de12b39ad805
File content as of revision 2:0b362c662997:
#include "mbed.h" #include "iRobot.h" #include "Servo.h" #include "Rectangle.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 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); 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 // Function Prototypes void getXYpos(); void moveCamera(); int main() { //followMeBot.start(); while(1) { getXYpos(); wait(.5); } } void 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); } /*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; } }