richard choy / Mbed 2 deprecated FollowMeBot

turning

Dependencies:   Rectangle Servo TextLCD mbed

Fork of FollowMeBot by Daniel Hamilton

main.cpp@4:aa21c6609858, 2013-12-02 (annotated)

Committer:
rchoy3
Date:
Mon Dec 02 17:20:53 2013 +0000
Revision:
4:aa21c6609858
Parent:
3:de12b39ad805 
turning

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dhamilton31 0:3ed56271dd2d 1 #include "mbed.h"
dhamilton31 0:3ed56271dd2d 2 #include "iRobot.h"
dhamilton31 0:3ed56271dd2d 3 #include "Servo.h"
dhamilton31 1:6c399fc35deb 4 #include "Rectangle.h"
dhamilton31 3:de12b39ad805 5 #include "TextLCD.h"
dhamilton31 0:3ed56271dd2d 6
dhamilton31 1:6c399fc35deb 7 // Macros/Constants
dhamilton31 3:de12b39ad805 8 #define MAX_VIEW_X 1600 // maximum X value input from camera
dhamilton31 3:de12b39ad805 9 #define MAX_VIEW_Y 1200 // maximum Y value input from camera
dhamilton31 2:0b362c662997 10 #define CENTER_BOX_TOLLERANCE 200 // Size of our box
rchoy3 4:aa21c6609858 11 #define TO_SERVO_DIVISOR 5000.0 // Value to divide by to get the amount to move the servo by
dhamilton31 3:de12b39ad805 12 #define NO_COLOR_MAX_COUNT 10
dhamilton31 3:de12b39ad805 13 #define COLLISION_DIST .4
dhamilton31 3:de12b39ad805 14 #define BLOB_CLOSE_DIST 200
dhamilton31 3:de12b39ad805 15 #define SERVO_HOME .5
dhamilton31 3:de12b39ad805 16 #define SERVO_HOME_TOL .2
dhamilton31 3:de12b39ad805 17 #define SPEED_CONST 65535
dhamilton31 0:3ed56271dd2d 18
dhamilton31 1:6c399fc35deb 19 // Hardware sensors and devices
dhamilton31 1:6c399fc35deb 20 DigitalOut myled(LED1);
dhamilton31 2:0b362c662997 21 DigitalOut myled2(LED2);
dhamilton31 1:6c399fc35deb 22 iRobot followMeBot(p9, p10);
dhamilton31 1:6c399fc35deb 23 Servo servoHor(p22);
dhamilton31 1:6c399fc35deb 24 Servo servoVer(p21);
dhamilton31 3:de12b39ad805 25 AnalogIn irSensorFront(p15);
dhamilton31 3:de12b39ad805 26 //AnalogIn irSensorLeft(p19);
dhamilton31 3:de12b39ad805 27 //AnalogIn irSensorRight(p18);
dhamilton31 1:6c399fc35deb 28 Serial pc(USBTX, USBRX); // tx, rx
dhamilton31 3:de12b39ad805 29 TextLCD lcd(p14, p16, p17, p18, p19, p20); // rs, e, d4-d7
dhamilton31 1:6c399fc35deb 30
dhamilton31 1:6c399fc35deb 31 // Software variables
dhamilton31 3:de12b39ad805 32 char serial_rx_buffer[256]; // Input buffer for data from the PC
dhamilton31 3:de12b39ad805 33 int xpos, ypos, blobArea; // x and y positions read from matlab
dhamilton31 3:de12b39ad805 34 Rectangle centerBox((MAX_VIEW_X/2)-CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)-CENTER_BOX_TOLLERANCE,
dhamilton31 3:de12b39ad805 35 (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
dhamilton31 3:de12b39ad805 36 int noColorCounter; // Counts how long it has been since we have seen a color to follow
rchoy3 4:aa21c6609858 37 bool colorLost = true; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT)
dhamilton31 1:6c399fc35deb 38
dhamilton31 2:0b362c662997 39 // Function Prototypes
dhamilton31 2:0b362c662997 40 void getXYpos();
dhamilton31 3:de12b39ad805 41 float moveCamera();
dhamilton31 3:de12b39ad805 42 void moveBot();
dhamilton31 3:de12b39ad805 43 int servoIsInHome();
dhamilton31 2:0b362c662997 44
dhamilton31 1:6c399fc35deb 45 int main()
dhamilton31 1:6c399fc35deb 46 {
rchoy3 4:aa21c6609858 47 followMeBot.start();
rchoy3 4:aa21c6609858 48 servoHor = .5;
rchoy3 4:aa21c6609858 49
dhamilton31 0:3ed56271dd2d 50 while(1) {
dhamilton31 2:0b362c662997 51 getXYpos();
dhamilton31 3:de12b39ad805 52 moveBot();
dhamilton31 0:3ed56271dd2d 53 }
dhamilton31 0:3ed56271dd2d 54 }
dhamilton31 1:6c399fc35deb 55
dhamilton31 3:de12b39ad805 56 /**
dhamilton31 3:de12b39ad805 57 * Moves the servo to move the camera based upon where the
dhamilton31 3:de12b39ad805 58 * color is located on the reported x and y
dhamilton31 3:de12b39ad805 59 *
dhamilton31 3:de12b39ad805 60 */
dhamilton31 3:de12b39ad805 61 float moveCamera()
dhamilton31 1:6c399fc35deb 62 {
dhamilton31 3:de12b39ad805 63 float temp = 0;
dhamilton31 3:de12b39ad805 64 if(xpos == 0) { // If we recieve a 0 for the location
dhamilton31 3:de12b39ad805 65 if(!colorLost && (++noColorCounter > NO_COLOR_MAX_COUNT)) { // Check to see if we have seen enough to consider the color lost
rchoy3 4:aa21c6609858 66 // servoHor = .5; // If the color is lost, return servo to home
dhamilton31 3:de12b39ad805 67 colorLost = true; // Set colorLost to true
dhamilton31 3:de12b39ad805 68 noColorCounter = 0; // Reset counter
dhamilton31 3:de12b39ad805 69 }
dhamilton31 3:de12b39ad805 70 } else if(!centerBox.is_touch(xpos, (MAX_VIEW_Y/2))) { // If we have a location
dhamilton31 3:de12b39ad805 71 noColorCounter = 0; // Reset counter
dhamilton31 3:de12b39ad805 72 colorLost = false; // We have found the color!
dhamilton31 3:de12b39ad805 73 temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR; // Calculate location to move servo to
dhamilton31 3:de12b39ad805 74 if(temp > 0 && temp <= 1) { // If the value is within the servo range
dhamilton31 3:de12b39ad805 75 servoHor = temp; // Set the servo equal to the position
rchoy3 4:aa21c6609858 76
dhamilton31 3:de12b39ad805 77 //sprintf(serial_rx_buffer, "%f\n", temp);
dhamilton31 1:6c399fc35deb 78 }
dhamilton31 2:0b362c662997 79 /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
dhamilton31 1:6c399fc35deb 80 if(temp > 0 && temp <= 1) {
dhamilton31 1:6c399fc35deb 81 servoVer = temp;
dhamilton31 2:0b362c662997 82 }*/
dhamilton31 2:0b362c662997 83 }
dhamilton31 3:de12b39ad805 84 //pc.puts(serial_rx_buffer);
rchoy3 4:aa21c6609858 85
rchoy3 4:aa21c6609858 86 lcd.cls();
rchoy3 4:aa21c6609858 87 lcd.locate(0,0);
rchoy3 4:aa21c6609858 88 lcd.printf("%d;%d;%f", xpos, ypos, servoHor.read());
rchoy3 4:aa21c6609858 89
dhamilton31 3:de12b39ad805 90 return temp; // return the servo position
dhamilton31 2:0b362c662997 91 }
dhamilton31 2:0b362c662997 92
dhamilton31 3:de12b39ad805 93 // Will return the number of degrees to turn the irobot by
dhamilton31 2:0b362c662997 94 void getXYpos()
dhamilton31 2:0b362c662997 95 {
dhamilton31 3:de12b39ad805 96 char * temp;
dhamilton31 3:de12b39ad805 97 const char del = ';';
dhamilton31 3:de12b39ad805 98 if(pc.readable()) { // See if matlab has data for us
dhamilton31 2:0b362c662997 99 myled = 1;
dhamilton31 3:de12b39ad805 100 pc.gets(serial_rx_buffer, 256); // Get position data
dhamilton31 3:de12b39ad805 101 pc.puts(serial_rx_buffer);
dhamilton31 3:de12b39ad805 102 temp = strtok(serial_rx_buffer, &del);
dhamilton31 3:de12b39ad805 103 xpos = atoi(temp); // Convert data to xposition int
dhamilton31 3:de12b39ad805 104 temp = strtok(NULL, &del);
dhamilton31 3:de12b39ad805 105 ypos = atoi(temp);
dhamilton31 3:de12b39ad805 106 temp = strtok(NULL, &del);
dhamilton31 3:de12b39ad805 107 blobArea = atoi(temp);
rchoy3 4:aa21c6609858 108
dhamilton31 3:de12b39ad805 109 moveCamera(); // Move the camera
dhamilton31 2:0b362c662997 110 } else {
dhamilton31 2:0b362c662997 111 myled = 0;
dhamilton31 2:0b362c662997 112 }
dhamilton31 3:de12b39ad805 113 }
dhamilton31 3:de12b39ad805 114
dhamilton31 3:de12b39ad805 115 void moveBot()
dhamilton31 3:de12b39ad805 116 {
dhamilton31 3:de12b39ad805 117 float irVal = irSensorFront;
rchoy3 4:aa21c6609858 118
rchoy3 4:aa21c6609858 119 //
rchoy3 4:aa21c6609858 120 // colorLost = false;
rchoy3 4:aa21c6609858 121
dhamilton31 3:de12b39ad805 122 lcd.locate(0,1);
dhamilton31 3:de12b39ad805 123 if(!colorLost) {
rchoy3 4:aa21c6609858 124 // if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) {
rchoy3 4:aa21c6609858 125 //followMeBot.stop();
rchoy3 4:aa21c6609858 126 // lcd.printf("stop");
rchoy3 4:aa21c6609858 127 //} else if(servoIsInHome() > 0) {
rchoy3 4:aa21c6609858 128 if(servoIsInHome() > 0) {
rchoy3 4:aa21c6609858 129 //if(servoHor.read() > .7) {
rchoy3 4:aa21c6609858 130 //servoHor = servoHor - .1;
dhamilton31 3:de12b39ad805 131 lcd.printf("right");
rchoy3 4:aa21c6609858 132 followMeBot.right();
rchoy3 4:aa21c6609858 133 } else if(servoIsInHome() < 0) {
rchoy3 4:aa21c6609858 134 //} else if(servoHor.read() < .3) {
rchoy3 4:aa21c6609858 135
rchoy3 4:aa21c6609858 136 //servoHor = servoHor + .1;
dhamilton31 3:de12b39ad805 137 lcd.printf("left");
rchoy3 4:aa21c6609858 138 followMeBot.left();
rchoy3 4:aa21c6609858 139
rchoy3 4:aa21c6609858 140 } else if(servoIsInHome() == 0) {
dhamilton31 3:de12b39ad805 141 //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100));
dhamilton31 3:de12b39ad805 142 //followMeBot.forward();
rchoy3 4:aa21c6609858 143 followMeBot.stop();
dhamilton31 3:de12b39ad805 144 lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));
dhamilton31 3:de12b39ad805 145 }
rchoy3 4:aa21c6609858 146 } else {
rchoy3 4:aa21c6609858 147 lcd.printf("Color Lost");
rchoy3 4:aa21c6609858 148 followMeBot.right();
dhamilton31 3:de12b39ad805 149 }
rchoy3 4:aa21c6609858 150
dhamilton31 1:6c399fc35deb 151 }
dhamilton31 1:6c399fc35deb 152
dhamilton31 3:de12b39ad805 153 int servoIsInHome()
dhamilton31 3:de12b39ad805 154 {
dhamilton31 3:de12b39ad805 155 if(servoHor.read() > SERVO_HOME + SERVO_HOME_TOL) {
dhamilton31 3:de12b39ad805 156 return 1;
dhamilton31 3:de12b39ad805 157 } else if(servoHor.read() < SERVO_HOME - SERVO_HOME_TOL) {
dhamilton31 3:de12b39ad805 158 return -1;
dhamilton31 3:de12b39ad805 159 } else {
dhamilton31 3:de12b39ad805 160 return 0;
dhamilton31 3:de12b39ad805 161 }
dhamilton31 3:de12b39ad805 162 }