turning
Dependencies: Rectangle Servo TextLCD mbed
Fork of FollowMeBot by
iRobot/iRobot.cpp@3:de12b39ad805, 2013-11-25 (annotated)
- Committer:
- dhamilton31
- Date:
- Mon Nov 25 19:55:12 2013 +0000
- Revision:
- 3:de12b39ad805
- Parent:
- 1:6c399fc35deb
Added additional servo control and printing commands to lcd
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 | 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 | } |