Project

General

Profile

Revision 872

Small modifications to odometry. I'm all but committed to using floating point.

View differences:

trunk/code/projects/mapping/odometry/odometry.c
1 1
#include "encoders.h"
2
#include <math.h>
2 3

  
3 4
//Odometry resolution, *64 microseconds.
4 5
#define ODOMETRY_CLK=80 
......
8 9
//Distance per encoder click (circumference / 1024)  = 203.621224 um.
9 10
//Robot width = 5.3745 in. = 136.5123 mm
10 11

  
11
#define ROBOT_WIDTH=137    //mm
12
#define CLICK_DISTANCE=204 //um
12
#define ROBOT_WIDTH_MM=137    //mm
13
#define CLICK_DISTANCE_UM=204 //um
13 14

  
14 15
//Standard measures will be mm and us
15 16

  
16 17
int encoder_ltm1, encoder_rtm1;
17 18

  
18
int diff_x,diff_y,angle;
19
int diff_x,diff_y;
20
double angle;
19 21

  
20 22
void odometry_init(){
21 23
	encoder_init();
......
26 28
void odometry_reset(){
27 29
	diff_x = 0;
28 30
	diff_y = 0;
29
	angle = 0;
31
	angle = 0.0;
30 32
	
31 33
	//CTC Mode. CLK / 1024
32 34
	TCCR3A = 0; // (Fully disconnected)
......
36 38
}
37 39

  
38 40
void odometry_run(){
39
	int v_l, v_r, omega;
41
	int v_l, v_r;
42
	double omega;
40 43
	int encoder_left, encoder_right;	
41 44
	
42
	//TODO Handle the case where v_l = v_r
43 45
	encoder_left = encoder_read(LEFT);	
44 46
	encoder_right = encoder_read(RIGHT);	
45 47
	
46
	v_l = (CLICK_DISTANCE*(encoder_left - encoder_ltm1)) / (ODOMETRY_CLK * 64);  // um / us = m/s
47
	v_r = (CLICK_DISTANCE*(encoder_right - encoder_rtm1)) / (ODOMETRY_CLK * 64); // same
48
	v_l = (CLICK_DISTANCE_UM*(encoder_left - encoder_ltm1)) / (ODOMETRY_CLK * 64);  // um / us = m/s
49
	v_r = (CLICK_DISTANCE_UM*(encoder_right - encoder_rtm1)) / (ODOMETRY_CLK * 64); // same
48 50

  
49 51
	if(v_l == v_r){
50 52
		diff_x += v_l * cos(angle);
51 53
		diff_y += v_r * sin(angle);
54
		return;
52 55
	}
53 56
	
54 57
	//Relative to the center of the robot, the center of it's turn.
55
	turning_radius = ROBOT_WIDTH * ((v_l + v_r) / (v_l - v_r)); //  = mm
58
	turning_radius = ROBOT_WIDTH_MM * ((v_l + v_r) / (v_l - v_r)); //  = mm
56 59

  
57
	//Here non floating point operations get trickier.	
60
	//TODO I think I may have gotten lazy on the angular momentum units before, hard to tell now.	
58 61
	if(v_r > v_l) //CCW positive.
59
		omega = -v_r / (turning_radius + ROBOT_WIDTH/2);
62
		omega = ((double)(-v_r)) / (turning_radius + ROBOT_WIDTH_MM>>1);
60 63
	
61 64
	if(v_r > v_l) //CW negative.
62
		omega = -v_l / (turning_radius - ROBOT_WIDTH/2);
65
		omega = ((double)(-v_l)) / (turning_radius - ROBOT_WIDTH_MM>>1);
63 66
	
64 67
	//Omega positive whenever t_r is negative, vica versa.
65 68
	vmag = -omega * turning_radius;
......
67 70
	angle += omega; // * the timer interval.
68 71
	diff_x += vmag * cos(angle);
69 72
	diff_y += vmag * sin(angle);
70

  
71

  
72 73
}
73 74

  
74 75
ISR(TIMER3_COMPA_vect){

Also available in: Unified diff