richard choy / Mbed 2 deprecated FollowMeBot

turning

Dependencies:   Rectangle Servo TextLCD mbed

Fork of FollowMeBot by Daniel Hamilton

iRobot/iRobot.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 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 {
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 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 3:de12b39ad805 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 }