Project

General

Profile

Revision 3db79f25

ID3db79f259e8820845822e6176be5c3b81a405021
Parent dfb92d66
Child 479d25d8

Added by Priya over 11 years ago

Fixed Odometry behavior so that it now works and then added a get readings functionality to sonarcontrol.

View differences:

scout/libscout/src/SonarControl.cpp
189 189
    num_attempts = MAX_ATTEMPTS;
190 190
}
191 191

  
192
int* SonarControl::get_sonar_readings()
193
{
194
  return readings;
195
}
196

  
192 197
/**
193 198
 * @brief Converts value returne by sonar to physical distances.
194 199
 *
scout/libscout/src/SonarControl.h
63 63
        /** Stops sonar from panning and taking readings */
64 64
        void set_off();	
65 65

  
66
        /** Get sonar readings */
67
        int* get_sonar_readings();
68

  
66 69
    private:
67 70
        /** Record the new sonar distance measurement */
68 71
        void distance_callback(const sonar::sonar_distance::ConstPtr& msg);
scout/libscout/src/behaviors/Odometry.cpp
7 7
Odometry::Odometry(string scoutname):Behavior(scoutname, "odometry")
8 8
{
9 9
  scout_pos = new pos;
10
  motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
10 11
}
11 12

  
12 13
/** Query encoders and estimate position based on encoder reading */
13 14
void Odometry::get_position()
14 15
{
15 16
  float left_dist, right_dist, total_dist, theta;
16
//  float left_vel, right_vel;
17 17

  
18
  /* TODO: Get current encoder ticks */
19 18
  encoder_readings scout_enc = encoders->query();
20
  motor_fl_dist = scout_enc.fl_ticks;
21
  motor_fr_dist = scout_enc.fr_ticks;
22
  motor_bl_dist = scout_enc.bl_ticks;
23
  motor_br_dist = scout_enc.br_ticks;
24
/**
25 19
  motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks;
26 20
  motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks;
27 21
  motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
28 22
  motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
29
**/
30 23

  
31
  left_dist = DIST_PER_TICK*(motor_fl_dist + motor_bl_dist)/2;
32
  right_dist = DIST_PER_TICK*(motor_fr_dist + motor_br_dist)/2;
24
  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);
25
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks);
26
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist);
33 27

  
34
  total_dist = (left_dist+right_dist)/2;
35
  theta = (right_dist - left_dist)/WHEEL_BASE;
28
  // Get Left and Right distance
29
  left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
30
                             ((float)(motor_bl_dist))/2.0);
31
  right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + 
32
                              ((float)(motor_br_dist))/2.0);
36 33

  
37
  scout_pos->x = total_dist*sin(theta);
38
  scout_pos->y = total_dist*cos(theta);
39
  scout_pos->theta = theta;
34
  total_dist = (left_dist)/2.0 + (right_dist)/2.0;
35
  theta = scout_pos->theta + atan2(left_dist - right_dist, WHEEL_BASE);
36

  
37
  ROS_INFO("l: %f   r: %f   theta: %f", left_dist, right_dist, theta);
38

  
39
  scout_pos->x += total_dist*sin(theta);
40
  scout_pos->y += total_dist*cos(theta);
41
  scout_pos->theta = fmod(theta, 2*M_PI);
40 42

  
41
/**
42 43
  //Save state for next time in.
43 44
  motor_fl_ticks = scout_enc.fl_ticks;
44 45
  motor_fr_ticks = scout_enc.fr_ticks;
45 46
  motor_bl_ticks = scout_enc.bl_ticks;
46 47
  motor_br_ticks = scout_enc.br_ticks;
47
**/
48

  
49

  
48 50
 return;
49 51
}
50 52

  
scout/libscout/src/behaviors/Odometry.h
4 4
#include "../Behavior.h"
5 5

  
6 6

  
7
#define WHEEL_RADIUS  5
7
#define WHEEL_RADIUS  2
8 8
#define WHEEL_CIRCUM  (2*M_PI*WHEEL_RADIUS)
9
#define WHEEL_BASE    2
10
#define ENCODER_COUNT 8800000000
9
#define WHEEL_BASE    5
10
#define ENCODER_COUNT (4*1200*M_PI/5)
11 11
#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT)
12 12

  
13 13
typedef struct{
......
38 38

  
39 39
    float msg_time_in;
40 40

  
41
    float motor_fl_dist;
42
    float motor_fr_dist;
43
    float motor_bl_dist;
44
    float motor_br_dist;
41
    int motor_fl_dist;
42
    int motor_fr_dist;
43
    int motor_bl_dist;
44
    int motor_br_dist;
45 45

  
46
    float motor_fl_ticks;
47
    float motor_fr_ticks;
48
    float motor_bl_ticks;
49
    float motor_br_ticks;
46
    unsigned int motor_fl_ticks;
47
    unsigned int motor_fr_ticks;
48
    unsigned int motor_bl_ticks;
49
    unsigned int motor_br_ticks;
50 50
    float scout_theta;
51 51

  
52 52
    pos* scout_pos;

Also available in: Unified diff