turning
Dependencies: Rectangle Servo TextLCD mbed
Fork of FollowMeBot by
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?
User | Revision | Line number | New 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 |