27 #include <fvmodels/relative_position/omni_relative.h>
43 this->mirror_model = mirror_model;
45 avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
46 avg_x_num = avg_y_num = 0;
48 ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
53 DEFAULT_X_VARIANCE = 36.f;
54 DEFAULT_Y_VARIANCE = 25.f;
75 OmniRelative::get_distance()
const
77 return distance_ball_motor;
82 OmniRelative::get_bearing(
void)
const
89 OmniRelative::get_slope()
const
96 OmniRelative::get_y(
void)
const
103 OmniRelative::get_x(
void)
const
110 OmniRelative::set_center(
float x,
float y)
112 image_x = (
unsigned int)roundf(x);
113 image_y = (
unsigned int)roundf(y);
124 OmniRelative::set_radius(
float r)
133 OmniRelative::get_radius()
const
140 OmniRelative::set_pan_tilt(
float pan,
float tilt)
146 OmniRelative::get_pan_tilt(
float *pan,
float *tilt)
const
152 OmniRelative::get_name()
const
154 return "OmniRelative";
159 OmniRelative::reset()
161 last_available =
false;
170 if ( mirror_model->isValidPoint( image_x, image_y ) ) {
173 distance_ball_cam = rel_pos.
r;
174 bearing = rel_pos.
phi;
177 ball_x = cos( bearing ) * distance_ball_cam;
178 ball_y = sin( bearing ) * distance_ball_cam;
182 distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );
188 OmniRelative::is_pos_valid()
const
190 return mirror_model->isValidPoint( image_x, image_y );
195 OmniRelative::calc_unfiltered()
200 distance_ball_cam = rel_pos.
r;
201 bearing = rel_pos.
phi;
207 ball_x = cos( bearing ) * distance_ball_cam;
208 ball_y = sin( bearing ) * distance_ball_cam;
210 distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );