Revision 71e1154d
Sonar works!
Also added new testing files for sonar - maps/racetest*.bmp
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 140 | 140 |
|
| 141 | 141 |
} |
| 142 | 142 |
|
| 143 |
float Scout::getSonar(float angle) |
|
| 144 |
{
|
|
| 145 |
|
|
| 146 |
return 0.0; |
|
| 147 |
} |
|
| 148 |
|
|
| 149 | 143 |
bool Scout::setPenCallback(scoutsim::SetPen::Request& req, |
| 150 | 144 |
scoutsim::SetPen::Response&) |
| 151 | 145 |
{
|
| ... | ... | |
| 199 | 193 |
unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y, |
| 200 | 194 |
double robot_theta, int sonar_pos) |
| 201 | 195 |
{
|
| 202 |
double angle = robot_theta + (180.0 * ((float) sonar_pos) / 24.0);
|
|
| 196 |
double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
|
|
| 203 | 197 |
unsigned int d = 0; |
| 204 | 198 |
|
| 205 | 199 |
unsigned int reading = 0; |
| ... | ... | |
| 230 | 224 |
|
| 231 | 225 |
d++; |
| 232 | 226 |
} |
| 233 |
while (reading < 128); |
|
| 227 |
/// @todo Consider using different cutoffs for different features |
|
| 228 |
while (reading < 128); /// @todo Get rid of hardcoded stuff like this |
|
| 234 | 229 |
|
| 235 | 230 |
return d; |
| 236 | 231 |
} |
| 237 | 232 |
|
| 233 |
// x and y is current position of the sonar |
|
| 238 | 234 |
void Scout::update_sonar(const wxImage& walls_image, int x, int y, |
| 239 | 235 |
double robot_theta) |
| 240 | 236 |
{
|
| scout/scoutsim/src/scout.h | ||
|---|---|---|
| 122 | 122 |
|
| 123 | 123 |
private: |
| 124 | 124 |
void setMotors(const motors::set_motors::ConstPtr& msg); |
| 125 |
float getSonar(float angle); |
|
| 126 | 125 |
bool setPenCallback(scoutsim::SetPen::Request&, |
| 127 | 126 |
scoutsim::SetPen::Response&); |
| 128 | 127 |
bool query_encoders_callback(encoders::query_encoders::Request&, |
Also available in: Unified diff