Revision 884
odometry compiles - timer issues, interrupt never called.
| trunk/code/projects/mapping/odometry/encoders.c | ||
|---|---|---|
| 1 |
#include "encoders.h" |
|
| 2 |
#include "spi.h" |
|
| 3 |
#include <dragonfly_lib.h> |
|
| 4 |
#include "ring_buffer.h" |
|
| 1 |
#include "encoders.h"
|
|
| 2 |
#include "spi.h"
|
|
| 3 |
#include <dragonfly_lib.h>
|
|
| 4 |
#include "ring_buffer.h"
|
|
| 5 | 5 |
|
| 6 | 6 |
unsigned short int left_data_buf; |
| 7 | 7 |
unsigned short int right_data_buf; |
| ... | ... | |
| 37 | 37 |
void encoder_init(void){
|
| 38 | 38 |
int i; |
| 39 | 39 |
|
| 40 |
spi_init(encoder_recv); |
|
| 40 |
spi_init(encoder_recv,NULL);
|
|
| 41 | 41 |
buf_index = 0; |
| 42 | 42 |
left_data_buf = 0; |
| 43 | 43 |
right_data_buf= 0; |
| trunk/code/projects/mapping/odometry/odometry.c | ||
|---|---|---|
| 1 |
#include "odometry.h" |
|
| 1 | 2 |
#include "encoders.h" |
| 2 | 3 |
#include <math.h> |
| 4 |
#include <avr/interrupt.h> |
|
| 5 |
#include <dragonfly_lib.h> |
|
| 3 | 6 |
|
| 4 |
//Odometry resolution, *64 microseconds. |
|
| 5 |
#define ODOMETRY_CLK=80 |
|
| 7 |
int encoder_ltm1, encoder_rtm1; |
|
| 6 | 8 |
|
| 7 |
//Wheel = 2.613 in. |
|
| 8 |
//Circumference = 208.508133 mm |
|
| 9 |
//Distance per encoder click (circumference / 1024) = 203.621224 um. |
|
| 10 |
//Robot width = 5.3745 in. = 136.5123 mm |
|
| 9 |
int diff_x,diff_y; |
|
| 10 |
float angle; |
|
| 11 | 11 |
|
| 12 |
#define ROBOT_WIDTH_MM=137 //mm |
|
| 13 |
#define CLICK_DISTANCE_UM=204 //um |
|
| 14 | 12 |
|
| 15 |
//Standard measures will be mm and us |
|
| 13 |
int odometry_dx(void){
|
|
| 14 |
return diff_x; |
|
| 15 |
} |
|
| 16 | 16 |
|
| 17 |
int encoder_ltm1, encoder_rtm1; |
|
| 17 |
int odometry_dy(void){
|
|
| 18 |
return diff_y; |
|
| 19 |
} |
|
| 18 | 20 |
|
| 19 |
int diff_x,diff_y; |
|
| 20 |
double angle; |
|
| 21 |
float odometry_angle(void){
|
|
| 22 |
return angle; |
|
| 23 |
} |
|
| 21 | 24 |
|
| 22 |
void odometry_init(){
|
|
| 25 |
void odometry_init(void){
|
|
| 23 | 26 |
encoder_init(); |
| 24 | 27 |
odometry_reset(); |
| 25 |
|
|
| 26 | 28 |
} |
| 27 | 29 |
|
| 28 |
void odometry_reset(){
|
|
| 30 |
void odometry_reset(void){
|
|
| 29 | 31 |
diff_x = 0; |
| 30 | 32 |
diff_y = 0; |
| 31 | 33 |
angle = 0.0; |
| 32 | 34 |
|
| 33 | 35 |
//CTC Mode. CLK / 1024 |
| 34 |
TCCR3A = 0; // (Fully disconnected)
|
|
| 35 |
TCCR3B = _BV(WGM32) | _BV(CS32) | _BV(CS30); //CLK/1024 , CTC Mode.
|
|
| 36 |
TCCR2 = 0; // (Fully disconnected)
|
|
| 37 |
TCCR2 = _BV(WGM32) | _BV(CS32) | _BV(CS30); //CLK/1024 , CTC Mode.
|
|
| 36 | 38 |
|
| 37 |
OCR3A = ODOMETRY_CLK;
|
|
| 39 |
OCR2 = ODOMETRY_CLK;
|
|
| 38 | 40 |
} |
| 39 | 41 |
|
| 40 |
void odometry_run(){
|
|
| 42 |
void odometry_run(void){
|
|
| 43 |
usb_puts("*");
|
|
| 41 | 44 |
int v_l, v_r; |
| 42 |
double omega;
|
|
| 45 |
float omega;
|
|
| 43 | 46 |
int encoder_left, encoder_right; |
| 44 | 47 |
|
| 45 | 48 |
encoder_left = encoder_read(LEFT); |
| ... | ... | |
| 55 | 58 |
} |
| 56 | 59 |
|
| 57 | 60 |
//Relative to the center of the robot, the center of it's turn. |
| 58 |
turning_radius = ROBOT_WIDTH_MM * ((v_l + v_r) / (v_l - v_r)); // = mm |
|
| 61 |
float turning_radius = ROBOT_WIDTH_MM * ((v_l + v_r) / (v_l - v_r)); // = mm
|
|
| 59 | 62 |
|
| 60 | 63 |
//TODO I think I may have gotten lazy on the angular momentum units before, hard to tell now. |
| 61 | 64 |
if(v_r > v_l) //CCW positive. |
| 62 |
omega = ((double)(-v_r)) / (turning_radius + ROBOT_WIDTH_MM>>1);
|
|
| 65 |
omega = ((float)(-v_r)) / (turning_radius + (ROBOT_WIDTH_MM>>1));
|
|
| 63 | 66 |
|
| 64 |
if(v_r > v_l) //CW negative.
|
|
| 65 |
omega = ((double)(-v_l)) / (turning_radius - ROBOT_WIDTH_MM>>1);
|
|
| 67 |
else //CW negative.
|
|
| 68 |
omega = ((float)(-v_l)) / (turning_radius - (ROBOT_WIDTH_MM>>1));
|
|
| 66 | 69 |
|
| 67 | 70 |
//Omega positive whenever t_r is negative, vica versa. |
| 68 |
vmag = -omega * turning_radius; |
|
| 71 |
float vmag = -omega * turning_radius;
|
|
| 69 | 72 |
|
| 70 | 73 |
angle += omega; // * the timer interval. |
| 71 | 74 |
diff_x += vmag * cos(angle); |
| 72 | 75 |
diff_y += vmag * sin(angle); |
| 73 | 76 |
} |
| 74 | 77 |
|
| 75 |
ISR(TIMER3_COMPA_vect){
|
|
| 78 |
ISR(TIMER2_COMP_vect){
|
|
| 76 | 79 |
odometry_run(); |
| 77 | 80 |
} |
| 81 |
|
|
| 82 |
|
|
| trunk/code/projects/mapping/odometry/Makefile | ||
|---|---|---|
| 8 | 8 |
endif |
| 9 | 9 |
|
| 10 | 10 |
# Target file name (without extension). |
| 11 |
TARGET = robot_main
|
|
| 11 |
TARGET = main |
|
| 12 | 12 |
|
| 13 | 13 |
# Uncomment this to use the wireless library |
| 14 | 14 |
USE_WIRELESS = 1 |
| 15 | 15 |
|
| 16 | 16 |
# com1 = serial port. Use lpt1 to connect to parallel port. |
| 17 |
AVRDUDE_PORT = /dev/cu.usbserial-A4001hAK
|
|
| 17 |
AVRDUDE_PORT = /dev/tty.usbserial-A4001hAG
|
|
| 18 | 18 |
# |
| 19 | 19 |
# |
| 20 | 20 |
################################### |
Also available in: Unified diff