![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
turning
Dependencies: Rectangle Servo TextLCD mbed
Fork of FollowMeBot by
Revision 4:aa21c6609858, committed 2013-12-02
- Comitter:
- rchoy3
- Date:
- Mon Dec 02 17:20:53 2013 +0000
- Parent:
- 3:de12b39ad805
- Commit message:
- turning
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Nov 25 19:55:12 2013 +0000 +++ b/main.cpp Mon Dec 02 17:20:53 2013 +0000 @@ -8,7 +8,7 @@ #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 3000.0 // Value to divide by to get the amount to move the servo by +#define TO_SERVO_DIVISOR 5000.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 @@ -34,7 +34,7 @@ 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) +bool colorLost = true; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT) // Function Prototypes void getXYpos(); @@ -44,8 +44,9 @@ int main() { - //followMeBot.start(); - //wait(1); + followMeBot.start(); + servoHor = .5; + while(1) { getXYpos(); moveBot(); @@ -62,7 +63,7 @@ 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 +// servoHor = .5; // If the color is lost, return servo to home colorLost = true; // Set colorLost to true noColorCounter = 0; // Reset counter } @@ -72,6 +73,7 @@ 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; @@ -80,6 +82,11 @@ }*/ } //pc.puts(serial_rx_buffer); + + lcd.cls(); + lcd.locate(0,0); + lcd.printf("%d;%d;%f", xpos, ypos, servoHor.read()); + return temp; // return the servo position } @@ -98,9 +105,7 @@ 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; @@ -110,26 +115,39 @@ void moveBot() { float irVal = irSensorFront; + + // + // colorLost = false; + 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(); + // if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) { + //followMeBot.stop(); + // lcd.printf("stop"); + //} else if(servoIsInHome() > 0) { + if(servoIsInHome() > 0) { + //if(servoHor.read() > .7) { + //servoHor = servoHor - .1; lcd.printf("right"); - } else if(servoIsInHome() < 0) { - //followMeBot.left(); + followMeBot.right(); + } else if(servoIsInHome() < 0) { + //} else if(servoHor.read() < .3) { + + //servoHor = servoHor + .1; lcd.printf("left"); - } else { + followMeBot.left(); + + } else if(servoIsInHome() == 0) { //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100)); //followMeBot.forward(); + followMeBot.stop(); lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100))); } + } else { + lcd.printf("Color Lost"); + followMeBot.right(); } - else{ - lcd.printf("Color Lost"); - } + } int servoIsInHome()