richard choy / Mbed 2 deprecated FollowMeBot

turning

Dependencies:   Rectangle Servo TextLCD mbed

Fork of FollowMeBot by Daniel Hamilton

main.cpp@2:0b362c662997, 2013-11-21 (annotated)

Committer:
dhamilton31
Date:
Thu Nov 21 00:02:05 2013 +0000
Revision:
2:0b362c662997
Parent:
1:6c399fc35deb
Child:
3:de12b39ad805
It controls the servo!

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 0:3ed56271dd2d 5
dhamilton31 1:6c399fc35deb 6 // Macros/Constants
dhamilton31 2:0b362c662997 7 #define MAX_VIEW_X 1176 // maximum X value input from camera
dhamilton31 2:0b362c662997 8 #define MAX_VIEW_Y 711 // maximum Y value input from camera
dhamilton31 2:0b362c662997 9 #define CENTER_BOX_TOLLERANCE 200 // Size of our box
dhamilton31 2:0b362c662997 10 #define TO_SERVO_DIVISOR 1176.0 // Value to divide by to get the amount to move the servo by
dhamilton31 0:3ed56271dd2d 11
dhamilton31 1:6c399fc35deb 12 // Hardware sensors and devices
dhamilton31 1:6c399fc35deb 13 DigitalOut myled(LED1);
dhamilton31 2:0b362c662997 14 DigitalOut myled2(LED2);
dhamilton31 1:6c399fc35deb 15 iRobot followMeBot(p9, p10);
dhamilton31 1:6c399fc35deb 16 Servo servoHor(p22);
dhamilton31 1:6c399fc35deb 17 Servo servoVer(p21);
dhamilton31 1:6c399fc35deb 18 AnalogIn irSensorFront(p20);
dhamilton31 1:6c399fc35deb 19 AnalogIn irSensorLeft(p19);
dhamilton31 1:6c399fc35deb 20 AnalogIn irSensorRight(p18);
dhamilton31 1:6c399fc35deb 21 Serial pc(USBTX, USBRX); // tx, rx
dhamilton31 1:6c399fc35deb 22
dhamilton31 1:6c399fc35deb 23 // Software variables
dhamilton31 1:6c399fc35deb 24 char serial_rx_buffer[128]; // Input buffer for data from the PC
dhamilton31 1:6c399fc35deb 25 int xpos, ypos; // x and y positions read from matlab
dhamilton31 1:6c399fc35deb 26 Rectangle centerBox((MAX_VIEW_X/2)+CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)+CENTER_BOX_TOLLERANCE,
dhamilton31 1:6c399fc35deb 27 (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 1:6c399fc35deb 28
dhamilton31 2:0b362c662997 29 // Function Prototypes
dhamilton31 2:0b362c662997 30 void getXYpos();
dhamilton31 2:0b362c662997 31 void moveCamera();
dhamilton31 2:0b362c662997 32
dhamilton31 1:6c399fc35deb 33 int main()
dhamilton31 1:6c399fc35deb 34 {
dhamilton31 2:0b362c662997 35 //followMeBot.start();
dhamilton31 1:6c399fc35deb 36
dhamilton31 0:3ed56271dd2d 37 while(1) {
dhamilton31 2:0b362c662997 38 getXYpos();
dhamilton31 2:0b362c662997 39 wait(.5);
dhamilton31 0:3ed56271dd2d 40 }
dhamilton31 0:3ed56271dd2d 41 }
dhamilton31 1:6c399fc35deb 42
dhamilton31 1:6c399fc35deb 43 void moveCamera()
dhamilton31 1:6c399fc35deb 44 {
dhamilton31 2:0b362c662997 45 if(xpos == 0) {
dhamilton31 2:0b362c662997 46 servoHor = .5;
dhamilton31 2:0b362c662997 47 } else if(!centerBox.is_touch(xpos, ypos)) {
dhamilton31 1:6c399fc35deb 48 float temp;
dhamilton31 2:0b362c662997 49 //printf("Servo at: %f\n", servoHor.read());
dhamilton31 2:0b362c662997 50 temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR;
dhamilton31 2:0b362c662997 51 //printf("move servo by: %f\n", (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR);
dhamilton31 1:6c399fc35deb 52 if(temp > 0 && temp <= 1) {
dhamilton31 1:6c399fc35deb 53 servoHor = temp;
dhamilton31 2:0b362c662997 54 sprintf(serial_rx_buffer, "%f\n", temp);
dhamilton31 1:6c399fc35deb 55 }
dhamilton31 2:0b362c662997 56 /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
dhamilton31 1:6c399fc35deb 57 if(temp > 0 && temp <= 1) {
dhamilton31 1:6c399fc35deb 58 servoVer = temp;
dhamilton31 2:0b362c662997 59 }*/
dhamilton31 2:0b362c662997 60 }
dhamilton31 2:0b362c662997 61 pc.puts(serial_rx_buffer);
dhamilton31 2:0b362c662997 62 }
dhamilton31 2:0b362c662997 63
dhamilton31 2:0b362c662997 64 void getXYpos()
dhamilton31 2:0b362c662997 65 {
dhamilton31 2:0b362c662997 66 //char * temp;
dhamilton31 2:0b362c662997 67 //const char del = ';';
dhamilton31 2:0b362c662997 68
dhamilton31 2:0b362c662997 69 if(pc.readable()) {
dhamilton31 2:0b362c662997 70 myled = 1;
dhamilton31 2:0b362c662997 71 pc.gets(serial_rx_buffer, 128);
dhamilton31 2:0b362c662997 72 xpos = atoi(serial_rx_buffer);
dhamilton31 2:0b362c662997 73 //sprintf(serial_rx_buffer, "%d\n", xpos);
dhamilton31 2:0b362c662997 74 //pc.puts(serial_rx_buffer);
dhamilton31 2:0b362c662997 75 //temp = strtok(serial_rx_buffer, &del);
dhamilton31 2:0b362c662997 76 moveCamera();
dhamilton31 2:0b362c662997 77 } else {
dhamilton31 2:0b362c662997 78 myled = 0;
dhamilton31 2:0b362c662997 79 }
dhamilton31 2:0b362c662997 80 if(xpos > 500) {
dhamilton31 2:0b362c662997 81 myled2 = 1;
dhamilton31 2:0b362c662997 82 } else {
dhamilton31 2:0b362c662997 83 myled2 = 0;
dhamilton31 1:6c399fc35deb 84 }
dhamilton31 1:6c399fc35deb 85 }
dhamilton31 1:6c399fc35deb 86
dhamilton31 1:6c399fc35deb 87