Revision ddfeb111

View differences:

scout/libscout/src/behaviors/Odometry.cpp
23 23
  motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
24 24
  motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
25 25

  
26
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", scout_enc.fl_ticks, scout_enc.fr_ticks, scout_enc.bl_ticks, scout_enc.br_ticks);
27
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks);
28
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist);
29

  
30 26
  // Get Left and Right distance
31 27
  left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
32 28
                             ((float)(motor_bl_dist))/2.0);
......
34 30
                              ((float)(motor_br_dist))/2.0);
35 31

  
36 32
  total_dist = (left_dist)/2.0 + (right_dist)/2.0;
37
  theta = scout_pos->theta + atan2(left_dist - right_dist, WHEEL_BASE);
38

  
39
  ROS_INFO("l: %f   r: %f   theta: %f", left_dist, right_dist, theta);
40

  
41
  scout_pos->x += total_dist*sin(theta);
42
  scout_pos->y += total_dist*cos(theta);
33
  theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE);
34

  
35
  //Use negative theta because we measure theta from 0 on the
36
  //right to 180 on the left counter-clock-wise, but this is
37
  //negative theta in the coordinate frame.
38
  //Also, subtract the delta from y because positive y is down.
39
  scout_pos->x += total_dist*cos(-theta);
40
  scout_pos->y -= total_dist*sin(-theta);
43 41
  scout_pos->theta = fmod(theta, 2*M_PI);
44 42

  
45 43
  //Save state for next time in.
......
65 63
    position.y = scout_pos->y;
66 64
    position.theta = scout_pos->theta;
67 65
    scout_position.publish(position);
68

  
69
    ROS_INFO("Scout is at x: %f  y: %f theta: %f\n", scout_pos->x, scout_pos->y, scout_pos->theta);
70 66
  }
71 67
}
scout/libscout/src/behaviors/Odometry.h
6 6
#include "messages/ScoutPosition.h"
7 7

  
8 8

  
9
#define WHEEL_RADIUS  2
9
#define WHEEL_RADIUS  .2
10 10
#define WHEEL_CIRCUM  (2*M_PI*WHEEL_RADIUS)
11
#define WHEEL_BASE    5
12
#define ENCODER_COUNT (4*1200*M_PI/5)
11
#define WHEEL_BASE    1.2
12
#define ENCODER_COUNT (2*WHEEL_RADIUS*350*M_PI/WHEEL_BASE)
13 13
#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT)
14 14

  
15 15
typedef struct{
scout/scoutsim/src/ghost_scout.cpp
76 76

  
77 77
  void GhostScout::setPosition(const ::messages::ScoutPosition& msg)
78 78
  {
79
    ROS_INFO("UPDATING GHOST SCOUT POSITION");
80

  
81 79
    pos.x = start_x + msg.x;
82 80
    pos.y = start_y - msg.y;
83 81
    orient = msg.theta;
......
104 102
      world_state state)
105 103
  {
106 104

  
105
    wxImage rotated_image = scout_image.Rotate(orient - M_PI/2.0,
106
        wxPoint(scout_image.GetWidth() / 2,
107
          scout_image.GetHeight() / 2),
108
        false);
109

  
110
    for (int y = 0; y < rotated_image.GetHeight(); ++y)
111
    {
112
      for (int x = 0; x < rotated_image.GetWidth(); ++x)
113
      {
114
        if (rotated_image.GetRed(x, y) == 255
115
            && rotated_image.GetBlue(x, y) == 255
116
            && rotated_image.GetGreen(x, y) == 255)
117
        {
118
          rotated_image.SetAlpha(x, y, 0);
119
        }
120
        else
121
        {
122
          rotated_image.SetAlpha(x, y, 125);
123
        }
124
      }
125
    }
126

  
127
    scout = wxBitmap(rotated_image);
128

  
107 129
    geometry_msgs::Pose2D my_pose;
108 130
    my_pose.x = pos.x;
109 131
    my_pose.y = pos.y;
110 132
    my_pose.theta = orient;
111 133

  
112
    //ROS_INFO("Ghost scout is at position %f  %f  %f", pos.x, pos.y, orient);
134
    ROS_INFO("Scout position %f  %f  %f", pos.x, pos.y, orient);
113 135

  
114 136
    return my_pose;
115 137
  }
scout/scoutsim/src/scout.cpp
465 465

  
466 466
        Pose p;
467 467
        p.x = pos.x;
468
        p.y = state.canvas_height - pos.y;
468
        p.y = pos.y;
469 469
        p.theta = orient;
470 470
        p.linear_velocity = l_speed;
471 471
        p.angular_velocity = r_speed;
scout/scoutsim/src/sim_frame.cpp
122 122
            &SimFrame::wirelessCallback, this);
123 123

  
124 124
        // Teleop
125
        teleop_type = TELEOP_OFF;
125
        teleop_type = TELEOP_PRECISE;
126 126
        teleop_l_speed = 0;
127 127
        teleop_r_speed = 0;
128 128
        teleop_pub = nh.advertise<motors::set_motors>("/scout1/set_motors", 1000);

Also available in: Unified diff