Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / mapping / odometry / odometry.c @ 890

History | View | Annotate | Download (1.71 KB)

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

    
7
int encoder_ltm1, encoder_rtm1;
8

    
9
int diff_x,diff_y;
10
float angle;
11

    
12

    
13
int odometry_dx(void){
14
        return diff_x;
15
}
16

    
17
int odometry_dy(void){
18
        return diff_y;
19
}
20

    
21
float odometry_angle(void){
22
        return angle;
23
}
24

    
25
void odometry_init(void){
26
        encoder_init();
27
        odometry_reset();
28
        
29
        //CTC Mode. CLK / 1024
30
        TCCR2 = 0; // (Fully disconnected)
31
        TCCR2 = _BV(WGM21) | _BV(CS22) | _BV(CS20); //CLK/1024 , CTC Mode.
32

    
33
        TIMSK |= _BV(OCIE2);
34

    
35
        OCR2 = ODOMETRY_CLK;
36
}
37

    
38
void odometry_reset(void){
39
        diff_x = 0;
40
        diff_y = 0;
41
        angle = 0.0;
42
}
43

    
44
void odometry_run(void){
45
        int v_l, v_r;
46
        float omega;
47
        int encoder_left, encoder_right;        
48
        
49
        encoder_left = encoder_read(LEFT);        
50
        encoder_right = encoder_read(RIGHT);        
51
        
52
        v_l = (CLICK_DISTANCE_UM*(encoder_left - encoder_ltm1)) / (ODOMETRY_CLK * 64);  // um / us = m/s
53
        v_r = (CLICK_DISTANCE_UM*(encoder_right - encoder_rtm1)) / (ODOMETRY_CLK * 64); // same
54

    
55
        if(v_l == v_r){
56
                diff_x += v_l * cos(angle);
57
                diff_y += v_r * sin(angle);
58
                return;
59
        }
60
        
61
        //Relative to the center of the robot, the center of it's turn.
62
        float turning_radius = ROBOT_WIDTH_MM * ((v_l + v_r) / (v_l - v_r)); //  = mm
63

    
64
        //TODO I think I may have gotten lazy on the angular momentum units before, hard to tell now.        
65
        if(v_r > v_l) //CCW positive.
66
                omega = ((float)(-v_r)) / (turning_radius + (ROBOT_WIDTH_MM>>1));
67
        
68
        else //CW negative.
69
                omega = ((float)(-v_l)) / (turning_radius - (ROBOT_WIDTH_MM>>1));
70
        
71
        //Omega positive whenever t_r is negative, vica versa.
72
        float vmag = -omega * turning_radius;
73

    
74
        angle += omega; // * the timer interval.
75
        diff_x += vmag * cos(angle);
76
        diff_y += vmag * sin(angle);
77
}
78

    
79
ISR(TIMER2_COMP_vect){
80
        odometry_run();
81
}
82

    
83