Wiselib
|
00001 // vim: set noexpandtab ts=4 sw=4: 00002 00003 #ifndef ROOMBA_IR_DISTANCE_SENSORS_H 00004 #define ROOMBA_IR_DISTANCE_SENSORS_H 00005 00006 template<typename OsModel_P, typename Roomba_P> 00007 class RoombaIRDistanceSensors { 00008 public: 00009 RoombaIRDistanceSensor_not_implemented_yet 00010 00011 typedef OsModel_P OsModel; 00012 typedef Roomba_P Roomba; 00013 typedef typename Roomba::Math_P Math; 00014 typedef typename OsModel::size_t size_t; 00015 typedef uint16_t value_t; 00016 00017 typedef double angle_t; 00018 typedef double length_t; 00019 00020 init(Roomba& roomba) { 00021 roomba_ = &roomba; 00022 } 00023 00024 size_t number() { return 6; } 00025 00026 inline angle_t angle(size_t n) { 00027 // TODO: Measure actual values 00028 static const double a[6] = { 00029 Math::PI_2, Math::PI_3, Math::PI_6, 00030 -Math::PI_6, -Math::PI_3, -Math::PI_2 00031 }; 00032 return a[n]; 00033 } // angle() 00034 00035 size_t nearest(angle_t radians) { 00036 // TODO: Measure actual values 00037 double angle = Math::radians_to_degrees(radians); 00038 if(angle <= -75.0) { return 5; } 00039 else if(angle <= -45.0) { return 4; } 00040 else if(angle <= 0.0) { return 3; } 00041 else if(angle <= 45.0) { return 2; } 00042 else if(angle <= 75.0) { return 1; } 00043 else { return 0; } 00044 } 00045 00046 inline length_t distance(size_t n) { 00047 return 50.0; // TODO: Replace with actual value 00048 } 00049 00050 value_t operator()(size_t n) { 00051 typename Roomba::value_t data = (*roomba_)(); 00052 switch(n) { 00053 case 0: return data.light_bump_left_signal; 00054 case 1: return data.light_bump_front_left_signal; 00055 case 2: return data.light_bump_center_left_signal; 00056 case 3: return data.light_bump_center_right_signal; 00057 case 4: return data.light_bump_front_right_signal; 00058 case 5: return data.light_bump_right_signal; 00059 default: 00060 assert(false); 00061 } 00062 } // operator() 00063 00064 value_t max_value(size_t n) { return 4095; } // according to ROI spec 00065 00066 private: 00067 Roomba* roomba_; 00068 }; 00069 00070 #endif // ROOMBA_IR_DISTANCE_SENSORS_H 00071