Revision 71e1154d

View differences:

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