|
Wiselib
|
wiselib.testing/intermediate/robot/roomba/roomba.h
Go to the documentation of this file.
00001 // vim: set noexpandtab ts=3 sw=3: 00002 00003 #ifndef ROOMBA_H 00004 #define ROOMBA_H 00005 00006 #warning ">>> You are using the new ROOMBA API which is currently work in progress. You have been warned." 00007 00008 #include "util/delegates/delegate.hpp" 00009 #include "util/pstl/vector_static.h" 00010 #include "util/standalone_math.h" 00011 00012 namespace wiselib { 00013 00014 namespace { 00015 struct RoombaData { 00016 uint8_t bumps, wall, cliff_left, cliff_front_left, 00017 cliff_front_right, cliff_right, virtual_wall, 00018 wheel_overcurrents, dirt, unused_1, 00019 infrared_omni, buttons; 00020 int16_t distance; 00021 int16_t angle; 00022 uint8_t charging; 00023 uint16_t voltage; 00024 int16_t current; 00025 int8_t temperature; 00026 uint16_t charge, capacity, wall_signal, cliff_left_signal, 00027 cliff_front_left_signal, cliff_front_right_signal, 00028 cliff_right_signal; 00029 uint8_t unused_2, unused_3, unused_4; 00030 uint8_t charging_sources, oi_mode, song_nr, song_playing, 00031 stream_packet_count; 00032 int16_t requested_velocity, requested_radius, requested_right_velocity, 00033 requested_left_velocity; 00034 int16_t left_encoder_counts, right_encoder_counts; 00035 uint8_t light_bumper; 00036 uint16_t light_bump_left_signal, light_bump_front_left_signal, 00037 light_bump_center_left_signal, 00038 light_bump_center_right_signal, 00039 light_bump_front_right_signal, 00040 light_bump_right_signal; 00041 uint8_t ir_opcode_left, ir_opcode_right; 00042 int16_t left_motor_current, right_motor_current, 00043 main_brush_motor_current, 00044 side_brush_motor_current; 00045 uint8_t stasis; 00046 } __attribute__ ((packed)); // struct RoombaData 00047 } // anonymous namespace 00048 00061 template<typename OsModel_P, 00062 typename ComUartModel_P, 00063 typename Timer_P = typename OsModel_P::Timer, 00064 typename Debug_P = typename OsModel_P::Debug, 00065 typename Math_P = StandaloneMath<OsModel_P>, 00066 typename OsModel_P::size_t MAX_RECEIVERS = 16> 00067 class RoombaModel { 00068 public: 00069 typedef RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS> self_type; 00070 typedef self_type* self_pointer_t; 00071 00072 typedef OsModel_P OsModel; 00073 typedef Math_P Math; 00074 typedef ComUartModel_P ComUartModel; 00075 typedef Timer_P Timer; 00076 typedef Debug_P Debug; 00077 00078 typedef delegate1<void, int> state_delegate_t; 00079 typedef vector_static<OsModel, state_delegate_t, MAX_RECEIVERS> state_delegates_t; 00080 00081 typedef double angle_t; 00082 typedef RoombaData value_t; 00083 typedef int16_t angular_velocity_t; 00084 typedef double distance_t; 00085 typedef int16_t velocity_t; 00086 typedef typename OsModel::size_t size_t; 00087 typedef uint8_t block_data_t; 00088 00089 // The slower, the more precive the motions will be 00090 enum MotionParameters { 00091 PRECISE_ANGULAR_VELOCITY = 20, 00092 DEFAULT_ANGULAR_VELOCITY = 60, 00093 FAST_ANGULAR_VELOCITY = 200, 00094 PRECISE_VELOCITY = 50, 00095 DEFAULT_VELOCITY = 200, 00096 FAST_VELOCITY = 300 00097 }; 00098 00099 enum DataTypes { 00100 WALL = 0x01, POSITION = 0x02, 00101 BATTERY_AND_TEMPERATURE = 0x04, LIGHT_BUMPS = 0x08, 00102 ALL = 0x0f, 00103 }; 00104 00105 enum State { 00106 READY = 0, NO_VALUE = 1, INACTIVE = 2, DATA_AVAILABLE = 3 00107 }; 00108 00109 enum ErrorCodes { 00110 SUCCESS = OsModel::SUCCESS, ERR_UNSPEC = OsModel::ERR_UNSPEC 00111 }; 00112 00113 RoombaModel() { } 00114 RoombaModel(typename OsModel::Os& os) { } 00115 00116 inline int init(ComUartModel_P& uart, Timer_P& timer, int data_types); 00117 inline int init(ComUartModel_P& uart, Timer_P& timer, Debug_P& debug, int data_types); 00118 inline int init(); 00119 inline int destruct(); 00120 00121 inline int data_types(); 00122 00123 // --- TurnWalkMotion 00124 00125 inline int turn(angular_velocity_t v = DEFAULT_ANGULAR_VELOCITY); 00126 inline int move(velocity_t v = DEFAULT_VELOCITY); 00127 inline int stop(); 00128 int wait_for_stop(); 00129 00130 int enable_stable_motion(bool enable) { stable_motion_ = enable; return SUCCESS; } 00131 00132 // --- Odometer 00133 00134 inline int reset_angle(); 00135 inline angle_t angle(); 00136 inline int reset_distance(); 00137 inline distance_t distance(); 00138 00139 inline int state(); 00140 template<typename T, void (T::*TMethod)(int)> 00141 int register_state_callback(T* obj); 00142 int unregister_state_callback(typename OsModel::size_t); 00143 00144 inline value_t& operator()(); 00145 00146 inline int bump(); 00147 00148 // More or less "internal" methods 00149 00150 int notify_state_receivers(int); 00151 00152 typename OsModel::size_t packets(); 00153 typename OsModel::size_t errors(); 00154 00155 int set_ticks_per_mm(double t) { ticks_per_mm_ = t; return SUCCESS; } 00156 int set_ticks_per_radian(double t) { ticks_per_radian_ = t; return SUCCESS; } 00157 00158 private: 00159 typedef uint16_t rotation_t; 00160 00161 enum Command { 00162 CMD_START = 0x80, CMD_BAUD, CMD_CONTROL, CMD_SAFE, CMD_FULL, CMD_POWER, 00163 CMD_SPOT, CMD_CLEAN, CMD_MAX, 00164 CMD_DRIVE, CMD_MOTORS, CMD_LEDS, CMD_SONG, CMD_PLAY, CMD_SENSORS, 00165 CMD_DOCK, 00166 CMD_PWM_MOTORS = 0x90, CMD_DRIVE_DIRECT, CMD_DRIVE_PWM, 00167 CMD_STREAM = 0x94, CMD_QUERY_LIST, CMD_ENABLE_STREAM, 00168 CMD_SCHEDULE_LEDS = 0xa2, CMD_BUTTONS = 0xa5, 00169 CMD_SCHEDULE = 0xa7, CMD_SET_TIME 00170 }; 00171 00172 enum Rotation { 00173 ROTATE_NONE = 0x8000, 00174 ROTATE_CW = 0xffff, ROTATE_CCW = 0x0001 00175 }; 00176 00177 inline int execute_motion(velocity_t speed, rotation_t rotation); 00178 00179 int16_t get_packet_offset(uint8_t); 00180 int16_t get_packet_length(uint8_t); 00181 00182 void send(uint8_t); 00183 void read(typename ComUartModel::size_t, typename ComUartModel::block_data_t*); 00184 00185 void update_odometry(); 00186 00187 ComUartModel_P* uart_; 00188 int data_types_; 00189 RoombaData roomba_data_; 00190 state_delegates_t callbacks_; 00191 typename Timer::self_pointer_t timer_; 00192 typename Debug::self_pointer_t debug_; 00193 00194 typename OsModel::size_t packets_; 00195 typename OsModel::size_t errors_; 00196 00197 bool stop_, stable_motion_; 00198 double ticks_per_mm_, ticks_per_radian_; 00199 double distance_, angle_; 00200 int16_t latest_encoder_left_, 00201 latest_encoder_right_; 00202 00203 velocity_t stable_motion_speed_; 00204 angle_t stable_motion_angle_, 00205 stable_motion_max_err_angle_; 00206 }; 00207 00208 template< 00209 typename OsModel_P, 00210 typename ComUartModel_P, 00211 typename Timer_P, 00212 typename Debug_P, 00213 typename Math_P, 00214 typename OsModel_P::size_t MAX_RECEIVERS 00215 > 00216 int 00217 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00218 init(ComUartModel_P& uart, Timer_P& timer, int data_types) { 00219 ticks_per_mm_ = 2.27; 00220 ticks_per_radian_ = ticks_per_mm_ * 115.0; // half wheel dist. in mm 00221 00222 packets_ = 0; 00223 errors_ = 0; 00224 assert(data_types != 0); 00225 00226 uart_ = &uart; 00227 timer_ = &timer; 00228 debug_ = 0; 00229 data_types_ = data_types; 00230 stable_motion_ = false; 00231 stable_motion_speed_ = 0; 00232 stable_motion_max_err_angle_ = Math::degrees_to_radians(1.0); 00233 stable_motion_speed_ = 0; 00234 00235 uart_->template reg_read_callback<self_type, &self_type::read>(this); 00236 00237 send(CMD_START); 00238 send(CMD_SAFE); 00239 stop(); 00240 00241 uint8_t data_groups[10]; 00242 uint8_t n = 0; 00243 00244 // Enable data stream 00245 if(data_types_ & WALL) { 00246 data_groups[n++] = 7; 00247 } 00248 if(data_types_ & POSITION) { 00249 data_groups[n++] = 43; 00250 data_groups[n++] = 44; 00251 } 00252 if(data_types_ & BATTERY_AND_TEMPERATURE) { 00253 //data_groups[n++] = 3; // byte-swapping breaks for groups! 00254 data_groups[n++] = 22; 00255 data_groups[n++] = 23; 00256 data_groups[n++] = 25; 00257 data_groups[n++] = 26; 00258 } 00259 if(data_types_ & LIGHT_BUMPS) { 00260 data_groups[n++] = 106; 00261 } 00262 00263 send(CMD_STREAM); 00264 send(n); 00265 00266 for(uint8_t i = 0; i < n; i++) { 00267 send(data_groups[i]); 00268 } 00269 00270 memset(&roomba_data_, 0, sizeof(roomba_data_)); 00271 00272 send(CMD_ENABLE_STREAM); 00273 send(1); 00274 00275 timer_->sleep(200); 00276 00277 return SUCCESS; 00278 } 00279 00280 template< 00281 typename OsModel_P, 00282 typename ComUartModel_P, 00283 typename Timer_P, 00284 typename Debug_P, 00285 typename Math_P, 00286 typename OsModel_P::size_t MAX_RECEIVERS 00287 > 00288 int 00289 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00290 init(ComUartModel_P& uart, Timer_P& timer, Debug_P& debug, int data_types) { 00291 int r = init(uart, timer, data_types); 00292 debug_ = &debug; 00293 return r; 00294 } 00295 00296 template< 00297 typename OsModel_P, 00298 typename ComUartModel_P, 00299 typename Timer_P, 00300 typename Debug_P, 00301 typename Math_P, 00302 typename OsModel_P::size_t MAX_RECEIVERS 00303 > 00304 int 00305 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00306 init() { 00307 destruct(); 00308 init(*uart_, *timer_, data_types_); 00309 return SUCCESS; 00310 } // init() 00311 00312 template< 00313 typename OsModel_P, 00314 typename ComUartModel_P, 00315 typename Timer_P, 00316 typename Debug_P, 00317 typename Math_P, 00318 typename OsModel_P::size_t MAX_RECEIVERS 00319 > 00320 int 00321 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00322 destruct() { 00323 send(CMD_ENABLE_STREAM); 00324 send(0); 00325 uart_->unreg_read_callback(); 00326 stop(); 00327 send(CMD_POWER); 00328 return SUCCESS; 00329 } 00330 00331 template< 00332 typename OsModel_P, 00333 typename ComUartModel_P, 00334 typename Timer_P, 00335 typename Debug_P, 00336 typename Math_P, 00337 typename OsModel_P::size_t MAX_RECEIVERS 00338 > 00339 int 00340 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00341 data_types() { 00342 return data_types_; 00343 } 00344 00345 template< 00346 typename OsModel_P, 00347 typename ComUartModel_P, 00348 typename Timer_P, 00349 typename Debug_P, 00350 typename Math_P, 00351 typename OsModel_P::size_t MAX_RECEIVERS 00352 > 00353 int 00354 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00355 move(RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::velocity_t speed) { 00356 if(stable_motion_) { 00357 stable_motion_speed_ = speed; 00358 stable_motion_angle_ = 0; 00359 00360 send(CMD_DRIVE_DIRECT); 00361 // speed right 00362 send((speed / 2) >> 8); // i know this is the same as >> 9, but its clearer this way 00363 send((speed / 2) & 0xff); 00364 // speed left 00365 send((speed / 2) >> 8); // i know this is the same as >> 9, but its clearer this way 00366 send((speed / 2) & 0xff); 00367 stop_ = false; 00368 } 00369 return execute_motion(speed, ROTATE_NONE); 00370 } 00371 00372 template< 00373 typename OsModel_P, 00374 typename ComUartModel_P, 00375 typename Timer_P, 00376 typename Debug_P, 00377 typename Math_P, 00378 typename OsModel_P::size_t MAX_RECEIVERS 00379 > 00380 int 00381 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00382 turn(RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::angular_velocity_t r_speed) { 00383 if(r_speed < 0) { 00384 return execute_motion(-r_speed, ROTATE_CW); 00385 } 00386 else { 00387 return execute_motion(r_speed, ROTATE_CCW); 00388 } 00389 } 00390 00391 template< 00392 typename OsModel_P, 00393 typename ComUartModel_P, 00394 typename Timer_P, 00395 typename Debug_P, 00396 typename Math_P, 00397 typename OsModel_P::size_t MAX_RECEIVERS 00398 > 00399 int 00400 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00401 stop() { 00402 return execute_motion(0, ROTATE_NONE); 00403 } 00404 00405 template< 00406 typename OsModel_P, 00407 typename ComUartModel_P, 00408 typename Timer_P, 00409 typename Debug_P, 00410 typename Math_P, 00411 typename OsModel_P::size_t MAX_RECEIVERS 00412 > 00413 int 00414 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00415 wait_for_stop() { 00416 typename OsModel::Timer t; 00417 while(!stop_) { 00418 timer_->sleep(100); 00419 } 00420 return SUCCESS; 00421 } 00422 00423 template< 00424 typename OsModel_P, 00425 typename ComUartModel_P, 00426 typename Timer_P, 00427 typename Debug_P, 00428 typename Math_P, 00429 typename OsModel_P::size_t MAX_RECEIVERS 00430 > 00431 typename RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::value_t& 00432 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00433 operator()() { 00434 return roomba_data_; 00435 } 00436 00437 template< 00438 typename OsModel_P, 00439 typename ComUartModel_P, 00440 typename Timer_P, 00441 typename Debug_P, 00442 typename Math_P, 00443 typename OsModel_P::size_t MAX_RECEIVERS 00444 > 00445 int 00446 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00447 state() { 00448 return READY; 00449 } 00450 00451 template< 00452 typename OsModel_P, 00453 typename ComUartModel_P, 00454 typename Timer_P, 00455 typename Debug_P, 00456 typename Math_P, 00457 typename OsModel_P::size_t MAX_RECEIVERS 00458 > 00459 int 00460 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00461 reset_angle() { 00462 angle_ = 0; 00463 return SUCCESS; 00464 } 00465 00466 template< 00467 typename OsModel_P, 00468 typename ComUartModel_P, 00469 typename Timer_P, 00470 typename Debug_P, 00471 typename Math_P, 00472 typename OsModel_P::size_t MAX_RECEIVERS 00473 > 00474 typename RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::angle_t 00475 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00476 angle() { 00477 return angle_; 00478 } 00479 00480 template< 00481 typename OsModel_P, 00482 typename ComUartModel_P, 00483 typename Timer_P, 00484 typename Debug_P, 00485 typename Math_P, 00486 typename OsModel_P::size_t MAX_RECEIVERS 00487 > 00488 int 00489 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00490 reset_distance() { 00491 distance_ = 0; 00492 return SUCCESS; 00493 } 00494 00495 template< 00496 typename OsModel_P, 00497 typename ComUartModel_P, 00498 typename Timer_P, 00499 typename Debug_P, 00500 typename Math_P, 00501 typename OsModel_P::size_t MAX_RECEIVERS 00502 > 00503 typename RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::distance_t 00504 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00505 distance() { 00506 return distance_; 00507 } 00508 00509 template< 00510 typename OsModel_P, 00511 typename ComUartModel_P, 00512 typename Timer_P, 00513 typename Debug_P, 00514 typename Math_P, 00515 typename OsModel_P::size_t MAX_RECEIVERS 00516 > 00517 int 00518 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00519 bump() { 00520 return roomba_data_.bumps & 0x11; 00521 } 00522 00523 template< 00524 typename OsModel_P, 00525 typename ComUartModel_P, 00526 typename Timer_P, 00527 typename Debug_P, 00528 typename Math_P, 00529 typename OsModel_P::size_t MAX_RECEIVERS 00530 > 00531 template<typename T, void (T::*TMethod)(int)> 00532 int 00533 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00534 register_state_callback(T* obj) { 00535 if(callbacks_.empty()) { 00536 callbacks_.assign(MAX_RECEIVERS, state_delegate_t()); 00537 } 00538 00539 for(typename OsModel::size_t i = 0; i < callbacks_.size(); ++i) { 00540 if(callbacks_[i] == state_delegate_t()) { 00541 callbacks_[i] = state_delegate_t::template from_method<T, TMethod>(obj); 00542 return i; 00543 } 00544 } 00545 return -1; 00546 } 00547 00548 template< 00549 typename OsModel_P, 00550 typename ComUartModel_P, 00551 typename Timer_P, 00552 typename Debug_P, 00553 typename Math_P, 00554 typename OsModel_P::size_t MAX_RECEIVERS 00555 > 00556 int 00557 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00558 unregister_state_callback(typename OsModel_P::size_t idx) { 00559 callbacks_[idx] = state_delegate_t(); 00560 return OsModel::SUCCESS; 00561 } 00562 00563 template< 00564 typename OsModel_P, 00565 typename ComUartModel_P, 00566 typename Timer_P, 00567 typename Debug_P, 00568 typename Math_P, 00569 typename OsModel_P::size_t MAX_RECEIVERS 00570 > 00571 int 00572 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00573 notify_state_receivers(int state) { 00574 typedef typename state_delegates_t::iterator iter_t; 00575 00576 for(iter_t iter = callbacks_.begin(); iter != callbacks_.end(); ++iter) { 00577 if(*iter != state_delegate_t()) { 00578 (*iter)(state); 00579 } 00580 } 00581 return SUCCESS; 00582 } 00583 00584 template< 00585 typename OsModel_P, 00586 typename ComUartModel_P, 00587 typename Timer_P, 00588 typename Debug_P, 00589 typename Math_P, 00590 typename OsModel_P::size_t MAX_RECEIVERS 00591 > 00592 int16_t 00593 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00594 get_packet_offset(uint8_t packet_type) { 00595 switch(packet_type) { 00596 case 0: 00597 case 1: 00598 case 6: 00599 case 7: 00600 case 100: return 0; 00601 case 2: return 10; 00602 case 3: return 16; 00603 case 4: return 26; 00604 case 5: return 40; 00605 case 8: return 1; 00606 case 19: return (uint8_t*)&roomba_data_.distance - (uint8_t*)&roomba_data_; // 12 00607 case 20: return (uint8_t*)&roomba_data_.angle - (uint8_t*)&roomba_data_; // 14; 00608 case 22: return (uint8_t*)&roomba_data_.voltage - (uint8_t*)&roomba_data_; 00609 case 23: return (uint8_t*)&roomba_data_.current - (uint8_t*)&roomba_data_; 00610 case 25: return (uint8_t*)&roomba_data_.charge - (uint8_t*)&roomba_data_; 00611 case 26: return (uint8_t*)&roomba_data_.capacity - (uint8_t*)&roomba_data_; 00612 case 43: return (uint8_t*)&roomba_data_.left_encoder_counts - (uint8_t*)&roomba_data_; 00613 case 44: return (uint8_t*)&roomba_data_.right_encoder_counts - (uint8_t*)&roomba_data_; 00614 case 101: return 52; 00615 case 106: return 57; 00616 default: 00617 return -1; 00618 //assert(false && "Unknown packet type"); 00619 } 00620 } // get_packet_offset 00621 00622 template< 00623 typename OsModel_P, 00624 typename ComUartModel_P, 00625 typename Timer_P, 00626 typename Debug_P, 00627 typename Math_P, 00628 typename OsModel_P::size_t MAX_RECEIVERS 00629 > 00630 int16_t 00631 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00632 get_packet_length(uint8_t packet_type) { 00633 switch(packet_type) { 00634 case 1: 00635 case 3: return 10; 00636 case 2: return 6; 00637 case 4: return 14; 00638 case 5: return 12; 00639 case 7: 00640 case 8: return 1; 00641 case 19: 00642 case 20: 00643 case 22: 00644 case 23: 00645 case 25: 00646 case 26: 00647 case 43: 00648 case 44: return 2; 00649 case 100: return 80; 00650 case 106: return 12; 00651 default: 00652 return -1; 00653 } 00654 } // get_packet_length 00655 00656 template< 00657 typename OsModel_P, 00658 typename ComUartModel_P, 00659 typename Timer_P, 00660 typename Debug_P, 00661 typename Math_P, 00662 typename OsModel_P::size_t MAX_RECEIVERS 00663 > 00664 void 00665 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00666 send(uint8_t byte) { 00667 uart_->write(1, reinterpret_cast<char*>(&byte)); 00668 } 00669 00670 template< 00671 typename OsModel_P, 00672 typename ComUartModel_P, 00673 typename Timer_P, 00674 typename Debug_P, 00675 typename Math_P, 00676 typename OsModel_P::size_t MAX_RECEIVERS 00677 > 00678 typename OsModel_P::size_t RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::packets() { return packets_; } 00679 00680 template< 00681 typename OsModel_P, 00682 typename ComUartModel_P, 00683 typename Timer_P, 00684 typename Debug_P, 00685 typename Math_P, 00686 typename OsModel_P::size_t MAX_RECEIVERS 00687 > 00688 typename OsModel_P::size_t RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::errors() { return errors_; } 00689 00690 00691 template< 00692 typename OsModel_P, 00693 typename ComUartModel_P, 00694 typename Timer_P, 00695 typename Debug_P, 00696 typename Math_P, 00697 typename OsModel_P::size_t MAX_RECEIVERS 00698 > 00699 void 00700 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00701 read( 00702 typename RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::ComUartModel::size_t size, 00703 typename RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::ComUartModel::block_data_t* data 00704 ) { 00705 enum { STATE_HEADER, STATE_LEN, STATE_TYPE, STATE_PAYLOAD, STATE_CHECKSUM }; 00706 typedef typename ComUartModel::size_t size_t; 00707 00708 00709 static uint16_t state = STATE_HEADER, 00710 pos = 0, bytes_read = 0, len = 0; 00711 static int16_t packet_offset = 0, packet_length = 0; 00712 static uint8_t check = 0, packet_type = 0; 00713 00714 for(size_t i=0; i<size; i++) { 00715 block_data_t d = data[i]; 00716 00717 switch(state) { 00718 case STATE_HEADER: 00719 if(d == 19) { 00720 state = STATE_LEN; 00721 check = d; 00722 } 00723 break; 00724 00725 case STATE_LEN: 00726 check += d; 00727 len = d; 00728 bytes_read = 0; 00729 state = STATE_TYPE; 00730 break; 00731 00732 case STATE_TYPE: 00733 // Do byte-swapping for previous packet if necessary 00734 if(bytes_read) { 00735 if(packet_type >= 7 && packet_type <= 58 && packet_length == 2) { 00736 uint8_t *buf = reinterpret_cast<uint8_t*>(&roomba_data_) + packet_offset; 00737 if(OsModel::endianness == wiselib::WISELIB_LITTLE_ENDIAN) { 00738 uint8_t tmp = buf[0]; 00739 buf[0] = buf[1]; 00740 buf[1] = tmp; 00741 } 00742 } 00743 } 00744 00745 check += d; 00746 ++bytes_read; 00747 packet_type = d; 00748 packet_offset = get_packet_offset(packet_type); 00749 packet_length = get_packet_length(packet_type); 00750 if(packet_offset < 0 || packet_length < 0 || 00751 static_cast<size_t>(packet_offset + packet_length) >= sizeof(roomba_data_)) { 00752 state = STATE_HEADER; 00753 } 00754 else { 00755 state = STATE_PAYLOAD; 00756 pos = packet_offset; 00757 } 00758 break; 00759 00760 case STATE_PAYLOAD: 00761 assert(pos < sizeof(roomba_data_)); 00762 (reinterpret_cast<uint8_t*>(&roomba_data_))[pos] = d; 00763 check += d; 00764 pos++; 00765 bytes_read++; 00766 00767 if(pos == (packet_offset + packet_length)) { 00768 if(bytes_read == len) { state = STATE_CHECKSUM; } 00769 else { state = STATE_TYPE; } 00770 } 00771 break; 00772 00773 case STATE_CHECKSUM: 00774 // Do byte-swapping for previous packet if necessary 00775 if(bytes_read) { 00776 if(packet_type >= 7 && packet_type <= 58 && packet_length == 2) { 00777 uint8_t *buf = reinterpret_cast<uint8_t*>(&roomba_data_) + packet_offset; 00778 if(OsModel::endianness == wiselib::WISELIB_LITTLE_ENDIAN) { 00779 uint8_t tmp = buf[0]; 00780 buf[0] = buf[1]; 00781 buf[1] = tmp; 00782 } 00783 } 00784 } 00785 00786 packets_++; 00787 check += d; 00788 if(check == 0) { 00789 update_odometry(); 00790 notify_state_receivers(DATA_AVAILABLE); 00791 } 00792 else { 00793 errors_++; 00794 // checksum failure for uart packet 00795 debug_->debug("roomba.h: Checksum error on UART receive.\n"); 00796 } 00797 state = STATE_HEADER; 00798 break; 00799 } 00800 } // for 00801 00802 } // read() 00803 00804 template< 00805 typename OsModel_P, 00806 typename ComUartModel_P, 00807 typename Timer_P, 00808 typename Debug_P, 00809 typename Math_P, 00810 typename OsModel_P::size_t MAX_RECEIVERS 00811 > 00812 int 00813 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00814 execute_motion( 00815 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::velocity_t speed, 00816 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>::rotation_t rotation 00817 ) { 00818 send(CMD_DRIVE); 00819 send(speed >> 8); send(speed & 0xff); 00820 send(rotation >> 8); send(rotation & 0xff); 00821 stop_ = (speed == 0); 00822 return SUCCESS; 00823 } 00824 00825 template< 00826 typename OsModel_P, 00827 typename ComUartModel_P, 00828 typename Timer_P, 00829 typename Debug_P, 00830 typename Math_P, 00831 typename OsModel_P::size_t MAX_RECEIVERS 00832 > 00833 void 00834 RoombaModel<OsModel_P, ComUartModel_P, Timer_P, Debug_P, Math_P, MAX_RECEIVERS>:: 00835 update_odometry() { 00836 static int stable_motion_pause = 0; 00837 00838 int32_t left = (roomba_data_.left_encoder_counts - latest_encoder_left_), 00839 right = (roomba_data_.right_encoder_counts - latest_encoder_right_); 00840 00841 // Care for wrapping of encoder counts 00842 if(right > 0x8000 || right <= -0x8000) { 00843 right = right - Math::sgn(right) * 0x10000; 00844 } 00845 if(left > 0x8000 || left <= -0x8000) { 00846 left = left - Math::sgn(left) * 0x10000; 00847 } 00848 00849 //debug_->debug("right: %7d left: %7d\n", right, left); 00850 00851 angle_ += static_cast<double>(right - left) / (2.0 * ticks_per_radian_); 00852 stable_motion_angle_ += static_cast<double>(right - left) / (2.0 * ticks_per_radian_); 00853 distance_ += static_cast<double>(right + left) / (2.0 * ticks_per_mm_); 00854 00855 latest_encoder_left_ = roomba_data_.left_encoder_counts; 00856 latest_encoder_right_ = roomba_data_.right_encoder_counts; 00857 00858 // Stable motion stuff 00859 00860 if(!stop_ && stable_motion_ && (stable_motion_speed_ != 0)) { 00861 debug_->debug("[stable motion] angle diff: %f max: %f\n", Math::fabs(stable_motion_angle_), stable_motion_max_err_angle_); 00862 00863 // Update wheel speed estimation, decrease wait time 00864 if(stable_motion_pause > 0) { 00865 stable_motion_pause--; 00866 } 00867 else { 00868 stable_motion_pause = 0; 00869 } 00870 if(Math::fabs(stable_motion_angle_) >= stable_motion_max_err_angle_ && (stable_motion_pause == 0)) { 00871 double T = 0.50, R = (T * stable_motion_speed_) / stable_motion_angle_; 00872 double dw = 220.0; 00873 double f = (R + (dw / 2.0)) / (R - (dw / 2.0)); 00874 int16_t r = (2.0*stable_motion_speed_ / (1.0+f)), l = 2.0*stable_motion_speed_ - r; 00875 00876 stop(); 00877 stop_ = false; 00878 send(CMD_DRIVE_DIRECT); 00879 send(r >> 8); send(r & 0xff); 00880 send(l >> 8); send(l & 0xff); 00881 stable_motion_pause = 34; //66.67 * stable_motion_angle_ / stable_motion_speed_; 00882 debug_->debug("[stable motion] R: %f r: %7d l: %7d a: %7f pause: %7d\n", R, r, l, Math::radians_to_degrees(stable_motion_angle_), stable_motion_pause); 00883 } 00884 } 00885 } 00886 } // ns wiselib 00887 00888 #endif // ROOMBA_H 00889
