Revision c63c9752 scout/scoutsim/src/scout.cpp

View differences:

scout/scoutsim/src/scout.cpp
105 105
            linesensor_readings.push_back(0);
106 106
        }
107 107

  
108
        meter = scout.GetHeight();
109

  
110 108
        // Initialize sonar
111 109
        sonar_position = 0;
112 110
        sonar_stop_l = 0;
......
140 138
        {
141 139
            motor_br_speed = msg->br_speed;
142 140
        }
143

  
144 141
    }
145 142

  
146 143
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
......
194 191
    }
195 192

  
196 193
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
197
                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on)
194
                                    double robot_theta, int sonar_pos,
195
                                    wxMemoryDC& sonar_dc, bool sonar_on)
198 196
    {
199 197
        double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
200 198
        unsigned int d = 0;
201 199

  
202 200
        unsigned int reading = 0;
203
	int d_x = 0;
204
	int d_y = 0;
201
        int d_x = 0;
202
        int d_y = 0;
203

  
205 204
        do
206 205
        {
207 206
            d_x = x + (int) floor(d * cos(angle));
......
233 232
        /// @todo Consider using different cutoffs for different features
234 233
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
235 234
        
236
	if(sonar_on)
237
	{       
238
		if(isFront)
239
		{
240
			// draw a circle at the wall_x, wall_y where reading > 128
241
			sonar_dc.SelectObject(*path_bitmap);
242
			sonar_dc.SetBrush(*wxRED_BRUSH); //old value
243
			sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
244
			old_front_dx = d_x;
245
			old_front_dy = d_y;
246
		}
247
		else
248
		{
249
			// draw a circle at the wall_x, wall_y where reading > 128
250
			sonar_dc.SelectObject(*path_bitmap);
251
			sonar_dc.SetBrush(*wxRED_BRUSH); //old value
252
			sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
253
			old_back_dx = d_x;
254
			old_back_dy = d_y;
255
		}
256

  
257
		sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
258
		sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
259
		if (isFront) //for some reason isFront = (!isFront) is not working
260
		{
261
			isFront = FALSE;
262
		}
263
		else
264
		{
265
			isFront = TRUE;	
266
		}
267
			
268
	}
235
        if(sonar_on)
236
        {       
237
            if(isFront)
238
            {
239
                // draw a circle at the wall_x, wall_y where reading > 128
240
                sonar_dc.SelectObject(*path_bitmap);
241
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
242
                sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
243
                old_front_dx = d_x;
244
                old_front_dy = d_y;
245
            }
246
            else
247
            {
248
                // draw a circle at the wall_x, wall_y where reading > 128
249
                sonar_dc.SelectObject(*path_bitmap);
250
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
251
                sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
252
                old_back_dx = d_x;
253
                old_back_dy = d_y;
254
            }
255

  
256
            sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
257
            sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
258
            if (isFront) //for some reason isFront = (!isFront) is not working
259
            {
260
                isFront = FALSE;
261
            }
262
            else
263
            {
264
                isFront = TRUE;	
265
            }
266
                
267
        }
269 268
        return d;
270 269
    }
271 270

  
......
333 332
    /// Sends back the position of this scout so scoutsim can save
334 333
    /// the world state
335 334
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
336
					wxMemoryDC& sonar_dc,
337
					bool sonar_on,
335
                                        wxMemoryDC& sonar_dc,
336
                                        bool sonar_on,
338 337
                                        const wxImage& path_image,
339 338
                                        const wxImage& lines_image,
340 339
                                        const wxImage& walls_image,
341 340
                                        wxColour background_color,
342
					wxColour sonar_color,
341
                                        wxColour sonar_color,
343 342
                                        world_state state)
344 343
    {
345 344
        // Assume that the two motors on the same side will be set to
......
348 347
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
349 348
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
350 349

  
351
        // Set the linear and angular speeds
352
        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
353
        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
350
        // Find linear and angular movement in m
351
        float lin_dist = SCOUTSIM_REFRESH_RATE * (l_speed + r_speed) / 2;
352
        float ang_dist = SCOUTSIM_REFRESH_RATE * (r_speed - l_speed);
354 353

  
355 354
        Vector2 old_pos = pos;
356 355

  
357 356
        // Update encoders
358
        /// @todo replace
359
        fl_ticks += (unsigned int) motor_fl_speed;
360
        fr_ticks += (unsigned int) motor_fr_speed;
361
        bl_ticks += (unsigned int) motor_bl_speed;
362
        br_ticks += (unsigned int) motor_br_speed;
357
        fl_ticks += (unsigned int) (motor_fl_speed * SCOUTSIM_REFRESH_RATE *
358
                                    ENCODER_TICKS_PER_METER);
359
        fr_ticks += (unsigned int) (motor_fr_speed * SCOUTSIM_REFRESH_RATE *
360
                                    ENCODER_TICKS_PER_METER);
361
        bl_ticks += (unsigned int) (motor_bl_speed * SCOUTSIM_REFRESH_RATE *
362
                                    ENCODER_TICKS_PER_METER);
363
        br_ticks += (unsigned int) (motor_br_speed * SCOUTSIM_REFRESH_RATE *
364
                                    ENCODER_TICKS_PER_METER);
365

  
366
        orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI));
363 367

  
364
        orient = fmod(orient + ang_vel * dt, 2*PI);
365
        pos.x += sin(orient + PI/2.0) * lin_vel * dt;
366
        pos.y += cos(orient + PI/2.0) * lin_vel * dt;
368
        // Multiply by dt, the time passed (SCOUTSIM_REFRESH_RATE)
369
        pos.x += sin(orient + PI/2.0) * lin_dist;
370
        pos.y += cos(orient + PI/2.0) * lin_dist;
367 371

  
368 372
        // Clamp to screen size
369 373
        if (pos.x < 0 || pos.x >= state.canvas_width
370 374
                || pos.y < 0 || pos.y >= state.canvas_height)
371 375
        {
372
            ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
376
            ROS_WARN("I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
373 377
        }
374 378

  
375 379
        pos.x = min(max(pos.x, 0.0f), state.canvas_width);
376 380
        pos.y = min(max(pos.y, 0.0f), state.canvas_height);
377 381

  
378
        int canvas_x = pos.x * meter;
379
        int canvas_y = pos.y * meter;
382
        int canvas_x = pos.x * PIX_PER_METER;
383
        int canvas_y = pos.y * PIX_PER_METER;
380 384

  
381 385
        {
382 386
            wxImage rotated_image =
......
405 409
        p.x = pos.x;
406 410
        p.y = state.canvas_height - pos.y;
407 411
        p.theta = orient;
408
        p.linear_velocity = lin_vel;
409
        p.angular_velocity = ang_vel;
412
        p.linear_velocity = l_speed;
413
        p.angular_velocity = r_speed;
410 414
        pose_pub.publish(p);
411 415

  
412 416
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
......
434 438
            {
435 439
                path_dc.SelectObject(*path_bitmap);
436 440
                path_dc.SetPen(pen);
437
                path_dc.DrawLine(pos.x * meter, pos.y * meter,
438
                                 old_pos.x * meter, old_pos.y * meter);
441
                path_dc.DrawLine(pos.x * PIX_PER_METER, pos.y * PIX_PER_METER,
442
                                 old_pos.x * PIX_PER_METER, old_pos.y * PIX_PER_METER);
439 443
            }
440 444
        }
441 445

  
......
450 454
    void Scout::paint(wxDC& dc)
451 455
    {
452 456
        wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
453
        dc.DrawBitmap(scout, pos.x * meter - (scout_size.GetWidth() / 2),
454
                      pos.y * meter - (scout_size.GetHeight() / 2), true);
457
        dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
458
                      pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
455 459
    }
456

  
457 460
}

Also available in: Unified diff