Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / mapping / matlab / testRobot / odometry.c @ 953

History | View | Annotate | Download (3.75 KB)

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

    
8
long lround(double d);
9

    
10
char control_velocity = 0;
11
int encoder_ltm1, encoder_rtm1, move_speed, move_angle;
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, target_velocity, target_angular_velocity;
15

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

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

    
26
/**
27
 * Retrieve the estimated y position. [mm]
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
 * Retrieve the estimated velocity [mm / s]
41
 */
42
long odometry_velocity(void){
43
        return velocity;
44
}
45

    
46
void odometry_set_velocity(long t_velocity, double t_angular_velocity, int start_speed, int start_angle){
47
        target_velocity = t_velocity;        
48
        target_angular_velocity = t_angular_velocity;
49
        move_angle = start_angle;
50
        move_speed = start_speed;
51
        control_velocity = 1;
52
}
53

    
54
/**
55
 * Initializes odometry to run on timer 2.
56
 * Also resets all values so that the center of the robot is 
57
 * considered to be at the origin facing the x direction.
58
 */
59
void odometry_init(void){
60
        
61
        encoders_init();
62
        
63
        delay_ms(100);
64

    
65
        odometry_reset();
66

    
67
        
68
        //CTC Mode. CLK / 1024
69
        TCCR2 = 0; // (Fully disconnected)
70
        TCCR2 = _BV(WGM21) | _BV(CS22) | _BV(CS20); //CLK/1024 , CTC Mode.
71

    
72
        TIMSK |= _BV(OCIE2);
73

    
74
        OCR2 = ODOMETRY_CLK;
75
}
76
/**
77
 * Resets all values so that the center of the robot is 
78
 * considered to be at the origin facing the x direction.
79
 */
80
void odometry_reset(void){
81
        diff_x = 0;
82
        diff_y = 0;
83
        encoder_ltm1 = encoder_read(LEFT);
84
        encoder_rtm1 = encoder_read(RIGHT);
85
        angle = 0.0;
86
}
87

    
88

    
89
void odometry_run(void){
90
        //Angle swept through in a time step CCW-
91
        double theta, rl, dc;
92
        long dr,dl;
93
        char buf[100];
94
        
95
        //Get the change in wheel positions
96
        {        
97
                int encoder_right = encoder_read(RIGHT);        
98
                int encoder_left = encoder_read(LEFT);        
99
        
100
                dl = encoder_left - encoder_ltm1;
101
                dr = encoder_right - encoder_rtm1;
102
                
103
                //No motion.
104
                if(dl == 0 && dr == 0){ 
105
                        if(control_velocity) modify_velocity();
106
                        return;
107
                }
108

    
109
                encoder_ltm1 = encoder_left;
110
                encoder_rtm1 = encoder_right;
111
        
112
                // Try to avoid over/underflow.
113
                dl = dl > 512 ? dl - 1024 :dl;
114
                dl = dl < -512 ? dl + 1024 :dl;
115
                dr = dr > 512 ? dr - 1024 :dr;
116
                dr = dr < -512 ? dr + 1024 :dr;
117
        
118
                //Convert "clicks" to um
119
                dl *= CLICK_DISTANCE_UM; //um
120
                dr *= CLICK_DISTANCE_UM;
121
        }
122
        
123

    
124
        if(dl == dr){
125
                diff_x += lround(dl*cos(angle)/1000.0); //mm
126
                diff_y += lround(dl*sin(angle)/1000.0); //mm
127
                velocity = lround((dl * 1000.0)/(ODOMETRY_CLK*TIME_SCALE)); //um / ms = mm/s
128
                if(control_velocity) modify_velocity();
129
                return;
130
        }
131
        
132
        //Call the left wheel 0, clockwise positive.
133
        rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl - dr)); //um
134
        theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM);   //rad
135
        
136
        //Distance the center has traveled.
137
        dc = (theta * (rl - ROBOT_WIDTH_UM)) / 2.0; //um
138
        velocity = lround( dc * 1000 /(ODOMETRY_CLK*TIME_SCALE));
139
        
140
        //angle is necessarily CCW+, so subtract.
141
        angle -= ANGLE_SCALE * theta; 
142
        
143
        //Change state variables. Probably lose all measurements less then a mm.
144
        diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm
145
        diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm
146

    
147
        if(control_velocity) modify_velocity();
148
}
149

    
150
void modify_velocity(){
151
        int diff = target_velocity - velocity;
152
        if(diff > 0){
153
                move_speed++;
154
                move(move_speed,move_angle);
155
        }
156
        else if(diff < 0){
157
                move_speed--;
158
                move(move_speed,move_angle);
159
        }
160
}
161

    
162
ISR(TIMER2_COMP_vect){
163
        odometry_run(); 
164
}
165

    
166
long lround(double d){
167
        double f = floor(d);
168
        return (long)(d - f > 0.5 ? f + 1 : f);
169
}
170

    
171