00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "cereal_port/CerealPort.h"
00038
00039
00040 #define OI_PACKET_GROUP_0_SIZE 26
00041 #define OI_PACKET_GROUP_1_SIZE 10
00042 #define OI_PACKET_GROUP_2_SIZE 6
00043 #define OI_PACKET_GROUP_3_SIZE 10
00044 #define OI_PACKET_GROUP_4_SIZE 14
00045 #define OI_PACKET_GROUP_5_SIZE 12
00046 #define OI_PACKET_GROUP_6_SIZE 52
00047 #define OI_PACKET_GROUP_100_SIZE 80
00048 #define OI_PACKET_GROUP_101_SIZE 28
00049 #define OI_PACKET_GROUP_106_SIZE 12
00050 #define OI_PACKET_GROUP_107_SIZE 9
00051 #define OI_PACKET_BUMPS_DROPS_SIZE 1
00052 #define OI_PACKET_WALL_SIZE 1
00053 #define OI_PACKET_CLIFF_LEFT_SIZE 1
00054 #define OI_PACKET_CLIFF_FRONT_LEFT_SIZE 1
00055 #define OI_PACKET_CLIFF_FRONT_RIGHT_SIZE 1
00056 #define OI_PACKET_CLIFF_RIGHT_SIZE 1
00057 #define OI_PACKET_VIRTUAL_WALL_SIZE 1
00058 #define OI_PACKET_WHEEL_OVERCURRENTS_SIZE 1
00059 #define OI_PACKET_DIRT_DETECT_SIZE 1
00060 #define OI_PACKET_IR_CHAR_OMNI_SIZE 1
00061 #define OI_PACKET_IR_CHAR_LEFT_SIZE 1
00062 #define OI_PACKET_IR_CHAR_RIGHT_SIZE 1
00063 #define OI_PACKET_BUTTONS_SIZE 1
00064 #define OI_PACKET_DISTANCE_SIZE 2
00065 #define OI_PACKET_ANGLE_SIZE 2
00066 #define OI_PACKET_CHARGING_STATE_SIZE 1
00067 #define OI_PACKET_VOLTAGE_SIZE 2
00068 #define OI_PACKET_CURRENT_SIZE 2
00069 #define OI_PACKET_TEMPERATURE_SIZE 1
00070 #define OI_PACKET_BATTERY_CHARGE_SIZE 2
00071 #define OI_PACKET_BATTERY_CAPACITY_SIZE 2
00072 #define OI_PACKET_WALL_SIGNAL_SIZE 2
00073 #define OI_PACKET_CLIFF_LEFT_SIGNAL_SIZE 2
00074 #define OI_PACKET_CLIFF_FRONT_LEFT_SIGNAL_SIZE 2
00075 #define OI_PACKET_CLIFF_FRONT_RIGHT_SIGNAL_SIZE 2
00076 #define OI_PACKET_CLIFF_RIGHT_SIGNAL_SIZE 2
00077 #define OI_PACKET_CHARGE_SOURCES_SIZE 1
00078 #define OI_PACKET_OI_MODE_SIZE 1
00079 #define OI_PACKET_SONG_NUMBER_SIZE 1
00080 #define OI_PACKET_SONG_PLAYING_SIZE 1
00081 #define OI_PACKET_STREAM_PACKETS_SIZE 1
00082 #define OI_PACKET_REQ_VELOCITY_SIZE 2
00083 #define OI_PACKET_REQ_RADIUS_SIZE 2
00084 #define OI_PACKET_REQ_RIGHT_VELOCITY_SIZE 2
00085 #define OI_PACKET_REQ_LEFT_VELOCITY_SIZE 2
00086 #define OI_PACKET_RIGHT_ENCODER_SIZE 2
00087 #define OI_PACKET_LEFT_ENCODER_SIZE 2
00088 #define OI_PACKET_LIGHT_BUMPER_SIZE 1
00089 #define OI_PACKET_LIGHT_BUMPER_LEFT_SIZE 2
00090 #define OI_PACKET_LIGHT_BUMPER_FRONT_LEFT_SIZE 2
00091 #define OI_PACKET_LIGHT_BUMPER_CENTER_LEFT_SIZE 2
00092 #define OI_PACKET_LIGHT_BUMPER_CENTER_RIGHT_SIZE 2
00093 #define OI_PACKET_LIGHT_BUMPER_FRONT_RIGHT_SIZE 2
00094 #define OI_PACKET_LIGHT_BUMPER_RIGHT_SIZE 2
00095 #define OI_PACKET_LEFT_MOTOR_CURRENT_SIZE 2
00096 #define OI_PACKET_RIGHT_MOTOR_CURRENT_SIZE 2
00097 #define OI_PACKET_BRUSH_MOTOR_CURRENT_SIZE 2
00098 #define OI_PACKET_SIDE_BRUSH_MOTOR_CURRENT_SIZE 2
00099 #define OI_PACKET_STASIS_SIZE 1
00100
00101
00102 #define OI_MODE_OFF 0
00103 #define OI_MODE_PASSIVE 1
00104 #define OI_MODE_SAFE 2
00105 #define OI_MODE_FULL 3
00106
00107
00108 #define OI_DELAY_MODECHANGE_MS 20
00109
00110
00111 #define OI_CHARGING_NO 0
00112 #define OI_CHARGING_RECOVERY 1
00113 #define OI_CHARGING_CHARGING 2
00114 #define OI_CHARGING_TRICKLE 3
00115 #define OI_CHARGING_WAITING 4
00116 #define OI_CHARGING_ERROR 5
00117
00118
00119 #define FORCE_FIELD 161
00120 #define GREEN_BUOY 164
00121 #define GREEN_BUOY_FORCE_FIELD 165
00122 #define RED_BUOY 168
00123 #define RED_BUOY_FORCE_FIELD 169
00124 #define RED_BUOY_GREEN_BUOY 172
00125 #define RED_BUOY_GREEN_BUOY_FORCE_FIELD 173
00126 #define VIRTUAL_WALL 162
00127
00128
00129 #define LEFT 0
00130 #define RIGHT 1
00131 #define FRONT_LEFT 2
00132 #define FRONT_RIGHT 3
00133 #define CENTER_LEFT 4
00134 #define CENTER_RIGHT 5
00135 #define OMNI 2
00136 #define MAIN_BRUSH 2
00137 #define SIDE_BRUSH 3
00138
00139
00140 #define BUTTON_CLOCK 7
00141 #define BUTTON_SCHEDULE 6
00142 #define BUTTON_DAY 5
00143 #define BUTTON_HOUR 4
00144 #define BUTTON_MINUTE 3
00145 #define BUTTON_DOCK 2
00146 #define BUTTON_SPOT 1
00147 #define BUTTON_CLEAN 0
00148
00149
00150 #define ROOMBA_BUMPER_X_OFFSET 0.050
00151 #define ROOMBA_DIAMETER 0.330
00152 #define ROOMBA_AXLE_LENGTH 0.235
00153
00154 #define ROOMBA_MAX_LIN_VEL_MM_S 500
00155 #define ROOMBA_MAX_ANG_VEL_RAD_S 2
00156 #define ROOMBA_MAX_RADIUS_MM 2000
00157
00159 #define ROOMBA_MAX_ENCODER_COUNTS 65535
00160
00161 #define ROOMBA_PULSES_TO_M 0.000445558279992234
00162
00163 #define MAX_PATH 32
00164
00165
00166 #ifndef MIN
00167 #define MIN(a,b) ((a < b) ? (a) : (b))
00168 #endif
00169 #ifndef MAX
00170 #define MAX(a,b) ((a > b) ? (a) : (b))
00171 #endif
00172 #ifndef NORMALIZE
00173 #define NORMALIZE(z) atan2(sin(z), cos(z))
00174 #endif
00175
00176 namespace irobot
00177 {
00179
00182 typedef enum _OI_Opcode {
00183
00184
00185 OI_OPCODE_START = 128,
00186 OI_OPCODE_BAUD = 129,
00187 OI_OPCODE_CONTROL = 130,
00188 OI_OPCODE_SAFE = 131,
00189 OI_OPCODE_FULL = 132,
00190 OI_OPCODE_POWER = 133,
00191 OI_OPCODE_SPOT = 134,
00192 OI_OPCODE_CLEAN = 135,
00193 OI_OPCODE_MAX = 136,
00194 OI_OPCODE_DRIVE = 137,
00195 OI_OPCODE_MOTORS = 138,
00196 OI_OPCODE_LEDS = 139,
00197 OI_OPCODE_SONG = 140,
00198 OI_OPCODE_PLAY = 141,
00199 OI_OPCODE_SENSORS = 142,
00200 OI_OPCODE_FORCE_DOCK = 143,
00201 OI_OPCODE_PWM_MOTORS = 144,
00202 OI_OPCODE_DRIVE_DIRECT = 145,
00203 OI_OPCODE_DRIVE_PWM = 146,
00204 OI_OPCODE_STREAM = 148,
00205 OI_OPCODE_QUERY = 149,
00206 OI_OPCODE_PAUSE_RESUME_STREAM = 150,
00207 OI_OPCODE_SCHEDULE_LEDS = 162,
00208 OI_OPCODE_DIGIT_LEDS_RAW = 163,
00209 OI_OPCODE_DIGIT_LEDS_ASCII = 164,
00210 OI_OPCODE_BUTTONS = 165,
00211 OI_OPCODE_SCHEDULE = 167,
00212 OI_OPCODE_SET_DAY_TIME = 168
00213
00214 } OI_Opcode;
00215
00216
00218
00221 typedef enum _OI_Packet_ID {
00222
00223
00224 OI_PACKET_GROUP_0 = 0,
00225 OI_PACKET_GROUP_1 = 1,
00226 OI_PACKET_GROUP_2 = 2,
00227 OI_PACKET_GROUP_3 = 3,
00228 OI_PACKET_GROUP_4 = 4,
00229 OI_PACKET_GROUP_5 = 5,
00230 OI_PACKET_GROUP_6 = 6,
00231 OI_PACKET_GROUP_100 = 100,
00232 OI_PACKET_GROUP_101 = 101,
00233 OI_PACKET_GROUP_106 = 106,
00234 OI_PACKET_GROUP_107 = 107,
00235 OI_PACKET_BUMPS_DROPS = 7,
00236 OI_PACKET_WALL = 8,
00237 OI_PACKET_CLIFF_LEFT = 9,
00238 OI_PACKET_CLIFF_FRONT_LEFT = 10,
00239 OI_PACKET_CLIFF_FRONT_RIGHT = 11,
00240 OI_PACKET_CLIFF_RIGHT = 12,
00241 OI_PACKET_VIRTUAL_WALL = 13,
00242 OI_PACKET_WHEEL_OVERCURRENTS = 14,
00243 OI_PACKET_DIRT_DETECT = 15,
00244 OI_PACKET_IR_CHAR_OMNI = 17,
00245 OI_PACKET_BUTTONS = 18,
00246 OI_PACKET_DISTANCE = 19,
00247 OI_PACKET_ANGLE = 20,
00248 OI_PACKET_CHARGING_STATE = 21,
00249 OI_PACKET_VOLTAGE = 22,
00250 OI_PACKET_CURRENT = 23,
00251 OI_PACKET_TEMPERATURE = 24,
00252 OI_PACKET_BATTERY_CHARGE = 25,
00253 OI_PACKET_BATTERY_CAPACITY = 26,
00254 OI_PACKET_WALL_SIGNAL = 27,
00255 OI_PACKET_CLIFF_LEFT_SIGNAL = 28,
00256 OI_PACKET_CLIFF_FRONT_LEFT_SIGNAL = 29,
00257 OI_PACKET_CLIFF_FRONT_RIGHT_SIGNAL = 30,
00258 OI_PACKET_CLIFF_RIGHT_SIGNAL = 31,
00259 OI_PACKET_CHARGE_SOURCES = 34,
00260 OI_PACKET_OI_MODE = 35,
00261 OI_PACKET_SONG_NUMBER = 36,
00262 OI_PACKET_SONG_PLAYING = 37,
00263 OI_PACKET_STREAM_PACKETS = 38,
00264 OI_PACKET_REQ_VELOCITY = 39,
00265 OI_PACKET_REQ_RADIUS = 40,
00266 OI_PACKET_REQ_RIGHT_VELOCITY = 41,
00267 OI_PACKET_REQ_LEFT_VELOCITY = 42,
00268 OI_PACKET_RIGHT_ENCODER = 43,
00269 OI_PACKET_LEFT_ENCODER = 44,
00270 OI_PACKET_LIGHT_BUMPER = 45,
00271 OI_PACKET_LIGHT_BUMPER_LEFT = 46,
00272 OI_PACKET_LIGHT_BUMPER_FRONT_LEFT = 47,
00273 OI_PACKET_LIGHT_BUMPER_CENTER_LEFT = 48,
00274 OI_PACKET_LIGHT_BUMPER_CENTER_RIGHT = 49,
00275 OI_PACKET_LIGHT_BUMPER_FRONT_RIGHT = 50,
00276 OI_PACKET_LIGHT_BUMPER_RIGHT = 51,
00277 OI_PACKET_IR_CHAR_LEFT = 52,
00278 OI_PACKET_IR_CHAR_RIGHT = 53,
00279 OI_PACKET_LEFT_MOTOR_CURRENT = 54,
00280 OI_PACKET_RIGHT_MOTOR_CURRENT = 55,
00281 OI_PACKET_BRUSH_MOTOR_CURRENT = 56,
00282 OI_PACKET_SIDE_BRUSH_MOTOR_CURRENT = 57,
00283 OI_PACKET_STASIS = 58
00284
00285 } OI_Packet_ID;
00286
00287
00293 class OpenInterface
00294 {
00295 public:
00296
00298
00305 OpenInterface(const char * new_serial_port);
00307 ~OpenInterface();
00308
00310
00313 int openSerialPort(bool full_control);
00315 int closeSerialPort();
00316
00318 int powerDown();
00319
00321
00330 int setSensorPackets(OI_Packet_ID * new_sensor_packets, int new_num_of_packets, size_t new_buffer_size);
00332
00341 int getSensorPackets(int timeout);
00342
00344 int streamSensorPackets();
00346 int startStream();
00348 int stopStream();
00349
00351 void calculateOdometry();
00352
00354
00362 int drive(double linear_speed, double angular_speed);
00364
00372 int driveDirect(int left_speed, int right_speed);
00374
00382 int drivePWM(int left_pwm, int right_pwm);
00383
00385
00396 int brushes(unsigned char side_brush, unsigned char vacuum, unsigned char main_brush, unsigned char side_brush_clockwise, unsigned char main_brush_dir);
00398
00407 int brushesPWM(char main_brush, char side_brush, char vacuum);
00408
00410 int clean();
00412 int max();
00414 int spot();
00416 int goDock();
00417
00418
00420
00432 int setSong(unsigned char song_number, unsigned char song_length, unsigned char *notes, unsigned char *note_lengths);
00434
00443 int playSong(unsigned char song_number);
00444
00446
00458 int setLeds(unsigned char check_robot, unsigned char dock, unsigned char spot, unsigned char debris, unsigned char power_color, unsigned char power_intensity);
00460
00478 int setSchedulingLeds(unsigned char sun, unsigned char mon, unsigned char tue, unsigned char wed, unsigned char thu, unsigned char fri, unsigned char sat, unsigned char colon, unsigned char pm, unsigned char am, unsigned char clock, unsigned char schedule);
00480
00490 int setDigitLeds(unsigned char digit3, unsigned char digit2, unsigned char digit1, unsigned char digit0);
00491
00493 unsigned char OImode_;
00494
00496 void resetOdometry();
00497 void setOdometry(double new_x, double new_y, double new_yaw);
00498
00500 double odometry_x_;
00502 double odometry_y_;
00504 double odometry_yaw_;
00505
00506 bool wall_;
00507 bool virtual_wall_;
00508 bool cliff_[4];
00509 bool bumper_[2];
00510 bool ir_bumper_[6];
00511 bool wheel_drop_[2];
00512 int wall_signal_;
00513 int cliff_signal_[4];
00514 int ir_bumper_signal_[6];
00515 unsigned char ir_char_[3];
00516
00517 bool buttons_[8];
00518
00519 unsigned char dirt_detect_;
00520
00521 int motor_current_[4];
00522 bool overcurrent_[4];
00523
00524 unsigned char charging_state_;
00525 bool power_cord_;
00526 bool dock_;
00527 float voltage_;
00528 float current_;
00529 char temperature_;
00530 float charge_;
00531 float capacity_;
00532
00533 int stasis_;
00534
00535 private:
00536
00538
00546 int parseSensorPackets(unsigned char * buffer, size_t buffer_length);
00547
00548 int parseBumpersAndWheeldrops(unsigned char * buffer, int index);
00549 int parseWall(unsigned char * buffer, int index);
00550 int parseLeftCliff(unsigned char * buffer, int index);
00551 int parseFrontLeftCliff(unsigned char * buffer, int index);
00552 int parseFrontRightCliff(unsigned char * buffer, int index);
00553 int parseRightCliff(unsigned char * buffer, int index);
00554 int parseVirtualWall(unsigned char * buffer, int index);
00555 int parseOvercurrents(unsigned char * buffer, int index);
00556 int parseDirtDetector(unsigned char * buffer, int index);
00557 int parseIrOmniChar(unsigned char * buffer, int index);
00558 int parseButtons(unsigned char * buffer, int index);
00559 int parseDistance(unsigned char * buffer, int index);
00560 int parseAngle(unsigned char * buffer, int index);
00561 int parseChargingState(unsigned char * buffer, int index);
00562 int parseVoltage(unsigned char * buffer, int index);
00563 int parseCurrent(unsigned char * buffer, int index);
00564 int parseTemperature(unsigned char * buffer, int index);
00565 int parseBatteryCharge(unsigned char * buffer, int index);
00566 int parseBatteryCapacity(unsigned char * buffer, int index);
00567 int parseWallSignal(unsigned char * buffer, int index);
00568 int parseLeftCliffSignal(unsigned char * buffer, int index);
00569 int parseFrontLeftCliffSignal(unsigned char * buffer, int index);
00570 int parseFontRightCliffSignal(unsigned char * buffer, int index);
00571 int parseRightCliffSignal(unsigned char * buffer, int index);
00572 int parseChargingSource(unsigned char * buffer, int index);
00573 int parseOiMode(unsigned char * buffer, int index);
00574 int parseSongNumber(unsigned char * buffer, int index);
00575 int parseSongPlaying(unsigned char * buffer, int index);
00576 int parseNumberOfStreamPackets(unsigned char * buffer, int index);
00577 int parseRequestedVelocity(unsigned char * buffer, int index);
00578 int parseRequestedRadius(unsigned char * buffer, int index);
00579 int parseRequestedRightVelocity(unsigned char * buffer, int index);
00580 int parseRequestedLeftVelocity(unsigned char * buffer, int index);
00581 int parseRightEncoderCounts(unsigned char * buffer, int index);
00582 int parseLeftEncoderCounts(unsigned char * buffer, int index);
00583 int parseLightBumper(unsigned char * buffer, int index);
00584 int parseLightBumperLeftSignal(unsigned char * buffer, int index);
00585 int parseLightBumperFrontLeftSignal(unsigned char * buffer, int index);
00586 int parseLightBumperCenterLeftSignal(unsigned char * buffer, int index);
00587 int parseLightBumperCenterRightSignal(unsigned char * buffer, int index);
00588 int parseLightBumperFrontRightSignal(unsigned char * buffer, int index);
00589 int parseLightBumperRightSignal(unsigned char * buffer, int index);
00590 int parseIrCharLeft(unsigned char * buffer, int index);
00591 int parseIrCharRight(unsigned char * buffer, int index);
00592 int parseLeftMotorCurrent(unsigned char * buffer, int index);
00593 int parseRightMotorCurrent(unsigned char * buffer, int index);
00594 int parseMainBrushMotorCurrent(unsigned char * buffer, int index);
00595 int parseSideBrushMotorCurrent(unsigned char * buffer, int index);
00596 int parseStasis(unsigned char * buffer, int index);
00597
00599
00609 int buffer2signed_int(unsigned char * buffer, int index);
00611
00621 int buffer2unsigned_int(unsigned char * buffer, int index);
00622
00624
00631 int startOI(bool full_control);
00633
00640 int sendOpcode(OI_Opcode code);
00641
00643 std::string port_name_;
00645 cereal::CerealPort * serial_port_;
00646
00648 bool stream_defined_;
00649
00651 int num_of_packets_;
00653 OI_Packet_ID * sensor_packets_;
00655 size_t packets_size_;
00656
00658 int distance_;
00660 int angle_;
00662 int encoder_counts_[2];
00664 uint16_t last_encoder_counts_[2];
00665 };
00666
00667 }
00668
00669