richard choy / Mbed 2 deprecated FollowMeBot

turning

Dependencies:   Rectangle Servo TextLCD mbed

Fork of FollowMeBot by Daniel Hamilton

iRobot/iRobot.cpp@1:6c399fc35deb, 2013-11-18 (annotated)

Committer:
dhamilton31
Date:
Mon Nov 18 21:09:13 2013 +0000
Revision:
1:6c399fc35deb
Parent:
iRobot.cpp@0:3ed56271dd2d
Child:
3:de12b39ad805
how many things must I commit?!?

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 1:6c399fc35deb 31 // device.printf("%c%c", Start, SafeMode);
dhamilton31 0:3ed56271dd2d 32 device.putc(Start);
dhamilton31 0:3ed56271dd2d 33 device.putc(SafeMode);
dhamilton31 0:3ed56271dd2d 34 wait(.5);
dhamilton31 1:6c399fc35deb 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 1:6c399fc35deb 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 {
dhamilton31 1:6c399fc35deb 51 device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF), char(speed_right&0xFF),
dhamilton31 1:6c399fc35deb 52 char((speed_left>>8)&0xFF), char(speed_left&0xFF));
dhamilton31 1:6c399fc35deb 53
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 {
dhamilton31 1:6c399fc35deb 65 device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF), char(speed_right&0xFF),
dhamilton31 1:6c399fc35deb 66 char(((-speed_left)>>8)&0xFF), char((-speed_left)&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 {
dhamilton31 1:6c399fc35deb 71 device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF), char((-speed_right)&0xFF),
dhamilton31 1:6c399fc35deb 72 char((speed_left>>8)&0xFF), char(speed_left&0xFF));
dhamilton31 1:6c399fc35deb 73
dhamilton31 0:3ed56271dd2d 74 }
dhamilton31 0:3ed56271dd2d 75 // Charger - search and return to charger using IR beacons (if found)
dhamilton31 1:6c399fc35deb 76 void iRobot::charger()
dhamilton31 1:6c399fc35deb 77 {
dhamilton31 0:3ed56271dd2d 78 device.printf("%c%c", Demo, char(1));
dhamilton31 0:3ed56271dd2d 79 }
dhamilton31 1:6c399fc35deb 80
dhamilton31 0:3ed56271dd2d 81 // Play Song - define and play a song
dhamilton31 1:6c399fc35deb 82 void iRobot::playsong() // Send out notes & duration to define song and then play song
dhamilton31 1:6c399fc35deb 83 {
dhamilton31 1:6c399fc35deb 84
dhamilton31 1:6c399fc35deb 85 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 86 Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
dhamilton31 0:3ed56271dd2d 87 char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
dhamilton31 0:3ed56271dd2d 88 char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
dhamilton31 0:3ed56271dd2d 89 char(24), char(86), char(12), char(87), char(48));
dhamilton31 1:6c399fc35deb 90
dhamilton31 0:3ed56271dd2d 91 wait(.2);
dhamilton31 0:3ed56271dd2d 92 device.printf("%c%c", PlaySong, char(0));
dhamilton31 1:6c399fc35deb 93 }
dhamilton31 1:6c399fc35deb 94
dhamilton31 1:6c399fc35deb 95 // Interrupt Routine to read in serial sensor data packets - BumpandDrop sensor only
dhamilton31 1:6c399fc35deb 96 void iRobot::receive_sensor()
dhamilton31 1:6c399fc35deb 97 {
dhamilton31 1:6c399fc35deb 98 char start_character;
dhamilton31 1:6c399fc35deb 99 // Loop just in case more than one character is in UART's receive FIFO buffer
dhamilton31 1:6c399fc35deb 100 while (device.readable()) {
dhamilton31 1:6c399fc35deb 101 switch (Sensor_byte_count) {
dhamilton31 1:6c399fc35deb 102 // Wait for Sensor Data Packet Header of 19
dhamilton31 1:6c399fc35deb 103 case 0: {
dhamilton31 1:6c399fc35deb 104 start_character = device.getc();
dhamilton31 1:6c399fc35deb 105 if (start_character == 19) Sensor_byte_count++;
dhamilton31 1:6c399fc35deb 106 break;
dhamilton31 1:6c399fc35deb 107 }
dhamilton31 1:6c399fc35deb 108 // Number of Packet Bytes
dhamilton31 1:6c399fc35deb 109 case 1: {
dhamilton31 1:6c399fc35deb 110 Sensor_Num_Bytes = device.getc();
dhamilton31 1:6c399fc35deb 111 Sensor_byte_count++;
dhamilton31 1:6c399fc35deb 112 break;
dhamilton31 1:6c399fc35deb 113 }
dhamilton31 1:6c399fc35deb 114 // Sensor ID of next data value
dhamilton31 1:6c399fc35deb 115 case 2: {
dhamilton31 1:6c399fc35deb 116 Sensor_ID = device.getc();
dhamilton31 1:6c399fc35deb 117 Sensor_byte_count++;
dhamilton31 1:6c399fc35deb 118 break;
dhamilton31 1:6c399fc35deb 119 }
dhamilton31 1:6c399fc35deb 120 // Sensor data value
dhamilton31 1:6c399fc35deb 121 case 3: {
dhamilton31 1:6c399fc35deb 122 Sensor_Data_Byte = device.getc();
dhamilton31 1:6c399fc35deb 123 if(Sensor_Data_Byte != 0){
dhamilton31 1:6c399fc35deb 124 Sensor_Data_Byte_Error = Sensor_Data_Byte;
dhamilton31 1:6c399fc35deb 125 }
dhamilton31 1:6c399fc35deb 126 Sensor_byte_count++;
dhamilton31 1:6c399fc35deb 127 break;
dhamilton31 1:6c399fc35deb 128 }
dhamilton31 1:6c399fc35deb 129 // Read Checksum and update LEDs with sensor data
dhamilton31 1:6c399fc35deb 130 case 4: {
dhamilton31 1:6c399fc35deb 131 Sensor_Checksum = device.getc();
dhamilton31 1:6c399fc35deb 132 // Could add code here to check the checksum and ignore a bad data packet
dhamilton31 1:6c399fc35deb 133 /*led1 = Sensor_Data_Byte &0x01;
dhamilton31 1:6c399fc35deb 134 led2 = Sensor_Data_Byte &0x02;
dhamilton31 1:6c399fc35deb 135 led3 = Sensor_Data_Byte &0x04;
dhamilton31 1:6c399fc35deb 136 led4 = Sensor_Data_Byte &0x08;*/
dhamilton31 1:6c399fc35deb 137 Sensor_byte_count = 0;
dhamilton31 1:6c399fc35deb 138 break;
dhamilton31 1:6c399fc35deb 139 }
dhamilton31 1:6c399fc35deb 140 }
dhamilton31 1:6c399fc35deb 141 }
dhamilton31 1:6c399fc35deb 142 return;
dhamilton31 0:3ed56271dd2d 143 }