Statistics
| Revision:

root / demos / hunter_prey / lib / src / libdragonfly / odometry.c @ 1828

History | View | Annotate | Download (3.24 KB)

1
#include "odometry.h"
2
#include "encoders.h"
3
#include <math.h>
4
#include <avr/interrupt.h>
5
#include "time.h"
6
#include "serial.h"
7

    
8
long lround(double d);
9

    
10

    
11
int encoder_ltm1, encoder_rtm1;
12

    
13
//Measure mm displacement in x and y from starting position. (+x:theta=0 +y:theta=M_PI/2)
14
long diff_x,diff_y, velocity;
15

    
16
//Measures radian angle displacement from starting position. (initially 0)
17
double angle;
18

    
19
/**
20
 * Retrieve the estimated x position. [millimeters]
21
 */
22
long odometry_dx(void){
23
        return diff_x;
24
}
25

    
26
/**
27
 * Retrieve the estimated y position. [millimeters]
28
 */
29
long odometry_dy(void){
30
        return diff_y;
31
}
32

    
33
/**
34
 * Retrieve the estimated angle [radians]
35
 */
36
double odometry_angle(void){
37
        return angle;
38
}
39

    
40
long odometry_velocity(void){
41
        return velocity;
42
}
43

    
44
/**
45
 * Initializes odometry to run on timer 2.
46
 * Also resets all values so that the center of the robot is 
47
 * considered to be at the origin facing the x direction.
48
 */
49
void odometry_init(void){
50
        
51
        encoders_init();
52
        
53
        delay_ms(100);
54

    
55
        odometry_reset();
56

    
57
        
58
        //CTC Mode. CLK / 1024
59
        TCCR2 = 0; // (Fully disconnected)
60
        TCCR2 = _BV(WGM21) | _BV(CS22) | _BV(CS20); //CLK/1024 , CTC Mode.
61

    
62
        TIMSK |= _BV(OCIE2);
63

    
64
        OCR2 = ODOMETRY_CLK;
65
}
66
/**
67
 * Resets all values so that the center of the robot is 
68
 * considered to be at the origin facing the x direction.
69
 */
70
void odometry_reset(void){
71
        diff_x = 0;
72
        diff_y = 0;
73
        encoder_ltm1 = encoder_read(LEFT);
74
        encoder_rtm1 = encoder_read(RIGHT);
75
        angle = 0.0;
76
}
77

    
78

    
79
void odometry_run(void){
80
        //Angle swept through in a time step CCW-
81
        double theta, rl, dc;
82
        long dr,dl;
83
        
84
        //Get the change in wheel positions
85
        {        
86
                int encoder_right = encoder_read(RIGHT);        
87
                int encoder_left = encoder_read(LEFT);        
88
        
89
                dl = encoder_left - encoder_ltm1;
90
                dr = encoder_right - encoder_rtm1;
91
                
92
                //No motion.
93
                if(dl == 0 && dr == 0) return;
94

    
95
                encoder_ltm1 = encoder_left;
96
                encoder_rtm1 = encoder_right;
97
        
98
                // Try to avoid over/underflow.
99
                dl = dl > 512 ? dl - 1024 :dl;
100
                dl = dl < -512 ? dl + 1024 :dl;
101
                dr = dr > 512 ? dr - 1024 :dr;
102
                dr = dr < -512 ? dr + 1024 :dr;
103
        
104
                //Convert "clicks" to um
105
                dl *= CLICK_DISTANCE_UM; //um
106
                dr *= CLICK_DISTANCE_UM;
107
        }
108
        
109

    
110
        if(dl == dr){
111
                diff_x += lround(dl*cos(angle)/1000.0); //mm
112
                diff_y += lround(dl*sin(angle)/1000.0); //mm
113
                velocity = lround((dl * 1000.0)/(ODOMETRY_CLK*TIME_SCALE)); //um / ms = mm/s
114
    usb_puts("dc: ");
115
    usb_puti(dl);
116
    usb_puts("um, velocity: ");
117
    usb_puti(velocity);
118
                usb_puts("\r\n mm/s");
119
                return;
120
        }
121
        
122
        //Call the left wheel 0, clockwise positive.
123
        rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl - dr)); //um
124
        theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM);   //rad
125
        
126
        //Distance the center has traveled.
127
        dc = (theta * (rl - ROBOT_WIDTH_UM)) / 2.0; //um
128
        velocity = lround( dc * 1000 /(ODOMETRY_CLK*TIME_SCALE));
129
        usb_puts("dc: ");
130
  usb_puti(dl);
131
  usb_puts("um, velocity: ");
132
  usb_puti(velocity);
133
  usb_puts("\r\n mm/s");
134
        
135
        //angle is necessarily CCW+, so subtract.
136
        angle -= ANGLE_SCALE * theta; 
137
        
138
        //Change state variables. Probably lose all measurements less then a mm.
139
        diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm
140
        diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm
141
}
142

    
143
ISR(TIMER2_COMP_vect){
144
        odometry_run(); 
145
}
146

    
147
long lround(double d){
148
        double f = floor(d);
149
        return (long)(d - f > 0.5 ? f + 1 : f);
150
}
151

    
152