turning
Dependencies: Rectangle Servo TextLCD mbed
Fork of FollowMeBot by
main.cpp
00001 #include "mbed.h" 00002 #include "iRobot.h" 00003 #include "Servo.h" 00004 #include "Rectangle.h" 00005 #include "TextLCD.h" 00006 00007 // Macros/Constants 00008 #define MAX_VIEW_X 1600 // maximum X value input from camera 00009 #define MAX_VIEW_Y 1200 // maximum Y value input from camera 00010 #define CENTER_BOX_TOLLERANCE 200 // Size of our box 00011 #define TO_SERVO_DIVISOR 5000.0 // Value to divide by to get the amount to move the servo by 00012 #define NO_COLOR_MAX_COUNT 10 00013 #define COLLISION_DIST .4 00014 #define BLOB_CLOSE_DIST 200 00015 #define SERVO_HOME .5 00016 #define SERVO_HOME_TOL .2 00017 #define SPEED_CONST 65535 00018 00019 // Hardware sensors and devices 00020 DigitalOut myled(LED1); 00021 DigitalOut myled2(LED2); 00022 iRobot followMeBot(p9, p10); 00023 Servo servoHor(p22); 00024 Servo servoVer(p21); 00025 AnalogIn irSensorFront(p15); 00026 //AnalogIn irSensorLeft(p19); 00027 //AnalogIn irSensorRight(p18); 00028 Serial pc(USBTX, USBRX); // tx, rx 00029 TextLCD lcd(p14, p16, p17, p18, p19, p20); // rs, e, d4-d7 00030 00031 // Software variables 00032 char serial_rx_buffer[256]; // Input buffer for data from the PC 00033 int xpos, ypos, blobArea; // x and y positions read from matlab 00034 Rectangle centerBox((MAX_VIEW_X/2)-CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)-CENTER_BOX_TOLLERANCE, 00035 (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 00036 int noColorCounter; // Counts how long it has been since we have seen a color to follow 00037 bool colorLost = true; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT) 00038 00039 // Function Prototypes 00040 void getXYpos(); 00041 float moveCamera(); 00042 void moveBot(); 00043 int servoIsInHome(); 00044 00045 int main() 00046 { 00047 followMeBot.start(); 00048 servoHor = .5; 00049 00050 while(1) { 00051 getXYpos(); 00052 moveBot(); 00053 } 00054 } 00055 00056 /** 00057 * Moves the servo to move the camera based upon where the 00058 * color is located on the reported x and y 00059 * 00060 */ 00061 float moveCamera() 00062 { 00063 float temp = 0; 00064 if(xpos == 0) { // If we recieve a 0 for the location 00065 if(!colorLost && (++noColorCounter > NO_COLOR_MAX_COUNT)) { // Check to see if we have seen enough to consider the color lost 00066 // servoHor = .5; // If the color is lost, return servo to home 00067 colorLost = true; // Set colorLost to true 00068 noColorCounter = 0; // Reset counter 00069 } 00070 } else if(!centerBox.is_touch(xpos, (MAX_VIEW_Y/2))) { // If we have a location 00071 noColorCounter = 0; // Reset counter 00072 colorLost = false; // We have found the color! 00073 temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR; // Calculate location to move servo to 00074 if(temp > 0 && temp <= 1) { // If the value is within the servo range 00075 servoHor = temp; // Set the servo equal to the position 00076 00077 //sprintf(serial_rx_buffer, "%f\n", temp); 00078 } 00079 /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR; 00080 if(temp > 0 && temp <= 1) { 00081 servoVer = temp; 00082 }*/ 00083 } 00084 //pc.puts(serial_rx_buffer); 00085 00086 lcd.cls(); 00087 lcd.locate(0,0); 00088 lcd.printf("%d;%d;%f", xpos, ypos, servoHor.read()); 00089 00090 return temp; // return the servo position 00091 } 00092 00093 // Will return the number of degrees to turn the irobot by 00094 void getXYpos() 00095 { 00096 char * temp; 00097 const char del = ';'; 00098 if(pc.readable()) { // See if matlab has data for us 00099 myled = 1; 00100 pc.gets(serial_rx_buffer, 256); // Get position data 00101 pc.puts(serial_rx_buffer); 00102 temp = strtok(serial_rx_buffer, &del); 00103 xpos = atoi(temp); // Convert data to xposition int 00104 temp = strtok(NULL, &del); 00105 ypos = atoi(temp); 00106 temp = strtok(NULL, &del); 00107 blobArea = atoi(temp); 00108 00109 moveCamera(); // Move the camera 00110 } else { 00111 myled = 0; 00112 } 00113 } 00114 00115 void moveBot() 00116 { 00117 float irVal = irSensorFront; 00118 00119 // 00120 // colorLost = false; 00121 00122 lcd.locate(0,1); 00123 if(!colorLost) { 00124 // if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) { 00125 //followMeBot.stop(); 00126 // lcd.printf("stop"); 00127 //} else if(servoIsInHome() > 0) { 00128 if(servoIsInHome() > 0) { 00129 //if(servoHor.read() > .7) { 00130 //servoHor = servoHor - .1; 00131 lcd.printf("right"); 00132 followMeBot.right(); 00133 } else if(servoIsInHome() < 0) { 00134 //} else if(servoHor.read() < .3) { 00135 00136 //servoHor = servoHor + .1; 00137 lcd.printf("left"); 00138 followMeBot.left(); 00139 00140 } else if(servoIsInHome() == 0) { 00141 //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100)); 00142 //followMeBot.forward(); 00143 followMeBot.stop(); 00144 lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100))); 00145 } 00146 } else { 00147 lcd.printf("Color Lost"); 00148 followMeBot.right(); 00149 } 00150 00151 } 00152 00153 int servoIsInHome() 00154 { 00155 if(servoHor.read() > SERVO_HOME + SERVO_HOME_TOL) { 00156 return 1; 00157 } else if(servoHor.read() < SERVO_HOME - SERVO_HOME_TOL) { 00158 return -1; 00159 } else { 00160 return 0; 00161 } 00162 }
Generated on Thu Jul 14 2022 17:03:57 by 1.7.2