Rahul Shetty / Mbed 2 deprecated FollowMeBot3

The FollowMeBot follows the user based on the color of his or her shirt. The device hosts a webcam on a servo to find the object and orient the robot. The color is chosen through the user interface with push buttons. Currently, the MATLAB code supports red and green detection. The purpose of FollowMeBot is to be able to follow the user in order to be helpful for daily tasks.

Dependencies:   Rectangle Servo TextLCD mbed

Fork of FollowMeBot by richard choy

iRobot/iRobot.cpp@7:41b5307e4c03, 2013-12-06 (annotated)

Committer:
dhamilton31
Date:
Fri Dec 06 19:06:50 2013 +0000
Revision:
7:41b5307e4c03
Parent:
5:5fe2f8603be8
Child:
8:7758b1db4d5b
added red and green serial printing

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 1:6c399fc35deb 3
dhamilton31 0:3ed56271dd2d 4 // Definitions of iRobot Create OpenInterface Command Numbers
dhamilton31 0:3ed56271dd2d 5 // See the Create OpenInterface manual for a complete list
dhamilton31 1:6c399fc35deb 6
dhamilton31 1:6c399fc35deb 7 iRobot::iRobot(PinName tx, PinName rx) : device(tx, rx)
dhamilton31 1:6c399fc35deb 8 {
dhamilton31 1:6c399fc35deb 9 speed_left = 100;
dhamilton31 1:6c399fc35deb 10 speed_right = 100;
dhamilton31 1:6c399fc35deb 11 // set baud rate for Create factory default
dhamilton31 1:6c399fc35deb 12 device.baud(57600);
dhamilton31 1:6c399fc35deb 13 }
dhamilton31 1:6c399fc35deb 14
dhamilton31 1:6c399fc35deb 15 void iRobot::changeSpeed(int speed)
dhamilton31 1:6c399fc35deb 16 {
dhamilton31 1:6c399fc35deb 17 speed_left = speed;
dhamilton31 1:6c399fc35deb 18 speed_right = speed;
dhamilton31 1:6c399fc35deb 19 }
dhamilton31 1:6c399fc35deb 20
dhamilton31 1:6c399fc35deb 21 char iRobot::sensorCode()
dhamilton31 1:6c399fc35deb 22 {
dhamilton31 1:6c399fc35deb 23 char error = Sensor_Data_Byte_Error;
dhamilton31 1:6c399fc35deb 24 Sensor_Data_Byte_Error = 0;
dhamilton31 1:6c399fc35deb 25 return error;
dhamilton31 1:6c399fc35deb 26 }
dhamilton31 1:6c399fc35deb 27
dhamilton31 0:3ed56271dd2d 28 // Start - send start and safe mode, start streaming sensor data
dhamilton31 1:6c399fc35deb 29 void iRobot::start()
dhamilton31 1:6c399fc35deb 30 {
dhamilton31 3:de12b39ad805 31 device.printf("%c%c", Start, SafeMode);
dhamilton31 3:de12b39ad805 32 //device.putc(Start);
dhamilton31 3:de12b39ad805 33 //device.putc(SafeMode);
dhamilton31 0:3ed56271dd2d 34 wait(.5);
dhamilton31 3:de12b39ad805 35 /* device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
dhamilton31 0:3ed56271dd2d 36 device.putc(SensorStream);
dhamilton31 0:3ed56271dd2d 37 device.putc(1);
dhamilton31 0:3ed56271dd2d 38 device.putc(BumpsandDrops);
dhamilton31 3:de12b39ad805 39 wait(.2);*/
dhamilton31 1:6c399fc35deb 40 // Setup a serial interrupt function to receive data
dhamilton31 1:6c399fc35deb 41 device.attach(this, &iRobot::receive_sensor);
dhamilton31 0:3ed56271dd2d 42 }
dhamilton31 0:3ed56271dd2d 43 // Stop - turn off drive motors
dhamilton31 1:6c399fc35deb 44 void iRobot::stop()
dhamilton31 1:6c399fc35deb 45 {
dhamilton31 0:3ed56271dd2d 46 device.printf("%c%c%c%c%c", DriveDirect, char(0), char(0), char(0), char(0));
dhamilton31 0:3ed56271dd2d 47 }
dhamilton31 0:3ed56271dd2d 48 // Forward - turn on drive motors
dhamilton31 1:6c399fc35deb 49 void iRobot::forward()
dhamilton31 1:6c399fc35deb 50 {
rshetty6 5:5fe2f8603be8 51 device.printf("%c%c%c%c%c", DriveDirect, char((35>>8)&0xFF), char(35&0xFF),
rshetty6 5:5fe2f8603be8 52 char((35>>8)&0xFF), char(35&0xFF));
rshetty6 5:5fe2f8603be8 53 //10-speed
dhamilton31 0:3ed56271dd2d 54 }
dhamilton31 0:3ed56271dd2d 55 // Reverse - reverse drive motors
dhamilton31 1:6c399fc35deb 56 void iRobot::reverse()
dhamilton31 1:6c399fc35deb 57 {
dhamilton31 1:6c399fc35deb 58 device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF), char((-speed_right)&0xFF),
dhamilton31 1:6c399fc35deb 59 char(((-speed_left)>>8)&0xFF), char((-speed_left)&0xFF));
dhamilton31 1:6c399fc35deb 60
dhamilton31 0:3ed56271dd2d 61 }
dhamilton31 0:3ed56271dd2d 62 // Left - drive motors set to rotate to left
dhamilton31 1:6c399fc35deb 63 void iRobot::left()
dhamilton31 1:6c399fc35deb 64 {
rshetty6 5:5fe2f8603be8 65 device.printf("%c%c%c%c%c", DriveDirect, char((50>>8)&0xFF), char(50&0xFF),
rshetty6 5:5fe2f8603be8 66 char(((-50)>>8)&0xFF), char((-50)&0xFF));
dhamilton31 0:3ed56271dd2d 67 }
dhamilton31 0:3ed56271dd2d 68 // Right - drive motors set to rotate to right
dhamilton31 1:6c399fc35deb 69 void iRobot::right()
dhamilton31 1:6c399fc35deb 70 {
rshetty6 5:5fe2f8603be8 71 device.printf("%c%c%c%c%c", DriveDirect, char(((-50)>>8)&0xFF), char((-50)&0xFF),
rshetty6 5:5fe2f8603be8 72 char((50>>8)&0xFF), char(50&0xFF));
dhamilton31 1:6c399fc35deb 73
dhamilton31 0:3ed56271dd2d 74 }
dhamilton31 3:de12b39ad805 75 // Angle - The angle in degrees that iRobot Create has turned since the
dhamilton31 3:de12b39ad805 76 //angle was last requested
dhamilton31 3:de12b39ad805 77 int iRobot::getAngle()
dhamilton31 3:de12b39ad805 78 {
dhamilton31 3:de12b39ad805 79 char angleBuff[2];
dhamilton31 3:de12b39ad805 80 device.printf("%c%c", Sensors, Angle);
dhamilton31 3:de12b39ad805 81 device.gets(angleBuff, 2);
dhamilton31 3:de12b39ad805 82 return atoi(angleBuff);
dhamilton31 3:de12b39ad805 83 }
dhamilton31 3:de12b39ad805 84
dhamilton31 3:de12b39ad805 85 // Turn Angle - The angle in degrees that iRobot Create has turned since the
dhamilton31 3:de12b39ad805 86 //angle was last requested ccw is pos angle, cw is neg angle
dhamilton31 3:de12b39ad805 87 void iRobot::turnAngle(int angle)
dhamilton31 3:de12b39ad805 88 {
dhamilton31 3:de12b39ad805 89 char angleTurn[3];
dhamilton31 3:de12b39ad805 90 angleTurn[0] = waitAngle;
dhamilton31 3:de12b39ad805 91 angleTurn[1] = angle >> 8;
dhamilton31 3:de12b39ad805 92 angleTurn[2] = angle & 0xFF;
dhamilton31 3:de12b39ad805 93 if(angle > 0){
dhamilton31 3:de12b39ad805 94 device.printf("%c%c%c%c",char(137), char(0), char(200), char(0), char(1));
dhamilton31 3:de12b39ad805 95 }
dhamilton31 3:de12b39ad805 96 else if(angle < 0){
dhamilton31 3:de12b39ad805 97 device.printf("%c%c%c%c",char(137), char(0), char(200), char(255), char(255));
dhamilton31 3:de12b39ad805 98 }
dhamilton31 3:de12b39ad805 99 device.printf("%c%c%c", angleTurn[0], angleTurn[1], angleTurn[2]);
dhamilton31 3:de12b39ad805 100 wait(.05);
dhamilton31 3:de12b39ad805 101 }
dhamilton31 0:3ed56271dd2d 102 // Charger - search and return to charger using IR beacons (if found)
dhamilton31 1:6c399fc35deb 103 void iRobot::charger()
dhamilton31 1:6c399fc35deb 104 {
dhamilton31 0:3ed56271dd2d 105 device.printf("%c%c", Demo, char(1));
dhamilton31 0:3ed56271dd2d 106 }
dhamilton31 1:6c399fc35deb 107
dhamilton31 0:3ed56271dd2d 108 // Play Song - define and play a song
dhamilton31 1:6c399fc35deb 109 void iRobot::playsong() // Send out notes & duration to define song and then play song
dhamilton31 1:6c399fc35deb 110 {
dhamilton31 1:6c399fc35deb 111
dhamilton31 1:6c399fc35deb 112 device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
dhamilton31 0:3ed56271dd2d 113 Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
dhamilton31 0:3ed56271dd2d 114 char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
dhamilton31 0:3ed56271dd2d 115 char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
dhamilton31 0:3ed56271dd2d 116 char(24), char(86), char(12), char(87), char(48));
dhamilton31 1:6c399fc35deb 117
dhamilton31 0:3ed56271dd2d 118 wait(.2);
dhamilton31 0:3ed56271dd2d 119 device.printf("%c%c", PlaySong, char(0));
dhamilton31 1:6c399fc35deb 120 }
dhamilton31 1:6c399fc35deb 121
dhamilton31 1:6c399fc35deb 122 // Interrupt Routine to read in serial sensor data packets - BumpandDrop sensor only
dhamilton31 1:6c399fc35deb 123 void iRobot::receive_sensor()
dhamilton31 1:6c399fc35deb 124 {
dhamilton31 1:6c399fc35deb 125 char start_character;
dhamilton31 7:41b5307e4c03 126 //printf("reading\n");
dhamilton31 1:6c399fc35deb 127 // Loop just in case more than one character is in UART's receive FIFO buffer
dhamilton31 1:6c399fc35deb 128 while (device.readable()) {
dhamilton31 1:6c399fc35deb 129 switch (Sensor_byte_count) {
dhamilton31 1:6c399fc35deb 130 // Wait for Sensor Data Packet Header of 19
dhamilton31 1:6c399fc35deb 131 case 0: {
dhamilton31 1:6c399fc35deb 132 start_character = device.getc();
dhamilton31 1:6c399fc35deb 133 if (start_character == 19) Sensor_byte_count++;
dhamilton31 1:6c399fc35deb 134 break;
dhamilton31 1:6c399fc35deb 135 }
dhamilton31 1:6c399fc35deb 136 // Number of Packet Bytes
dhamilton31 1:6c399fc35deb 137 case 1: {
dhamilton31 1:6c399fc35deb 138 Sensor_Num_Bytes = device.getc();
dhamilton31 1:6c399fc35deb 139 Sensor_byte_count++;
dhamilton31 1:6c399fc35deb 140 break;
dhamilton31 1:6c399fc35deb 141 }
dhamilton31 1:6c399fc35deb 142 // Sensor ID of next data value
dhamilton31 1:6c399fc35deb 143 case 2: {
dhamilton31 1:6c399fc35deb 144 Sensor_ID = device.getc();
dhamilton31 1:6c399fc35deb 145 Sensor_byte_count++;
dhamilton31 1:6c399fc35deb 146 break;
dhamilton31 1:6c399fc35deb 147 }
dhamilton31 1:6c399fc35deb 148 // Sensor data value
dhamilton31 1:6c399fc35deb 149 case 3: {
dhamilton31 1:6c399fc35deb 150 Sensor_Data_Byte = device.getc();
dhamilton31 1:6c399fc35deb 151 if(Sensor_Data_Byte != 0){
dhamilton31 1:6c399fc35deb 152 Sensor_Data_Byte_Error = Sensor_Data_Byte;
dhamilton31 1:6c399fc35deb 153 }
dhamilton31 1:6c399fc35deb 154 Sensor_byte_count++;
dhamilton31 1:6c399fc35deb 155 break;
dhamilton31 1:6c399fc35deb 156 }
dhamilton31 1:6c399fc35deb 157 // Read Checksum and update LEDs with sensor data
dhamilton31 1:6c399fc35deb 158 case 4: {
dhamilton31 1:6c399fc35deb 159 Sensor_Checksum = device.getc();
dhamilton31 1:6c399fc35deb 160 // Could add code here to check the checksum and ignore a bad data packet
dhamilton31 1:6c399fc35deb 161 /*led1 = Sensor_Data_Byte &0x01;
dhamilton31 1:6c399fc35deb 162 led2 = Sensor_Data_Byte &0x02;
dhamilton31 1:6c399fc35deb 163 led3 = Sensor_Data_Byte &0x04;
dhamilton31 1:6c399fc35deb 164 led4 = Sensor_Data_Byte &0x08;*/
dhamilton31 1:6c399fc35deb 165 Sensor_byte_count = 0;
dhamilton31 1:6c399fc35deb 166 break;
dhamilton31 1:6c399fc35deb 167 }
dhamilton31 1:6c399fc35deb 168 }
dhamilton31 1:6c399fc35deb 169 }
dhamilton31 1:6c399fc35deb 170 return;
dhamilton31 0:3ed56271dd2d 171 }