Statistics
| Revision:

root / branches / encoders / code / projects / libdragonfly / odometry.c @ 1345

 1 ```#include "odometry.h" ``` ```#include ``` ```#include ``` ```#include ``` ```#include ``` ```long lround(double d); ``` ```int encoder_ltm1, encoder_rtm1; ``` ```//Measure mm displacement in x and y from starting position. (+x:theta=0 +y:theta=M_PI/2) ``` ```long diff_x,diff_y, velocity; ``` ```//Measures radian angle displacement from starting position. (initially 0) ``` ```double angle; ``` ```/** ``` ``` * Retrieve the estimated x position. [millimeters] ``` ``` */ ``` ```long odometry_dx(void){ ``` ``` return diff_x; ``` ```} ``` ```/** ``` ``` * Retrieve the estimated y position. [millimeters] ``` ``` */ ``` ```long odometry_dy(void){ ``` ``` return diff_y; ``` ```} ``` ```/** ``` ``` * Retrieve the estimated angle [radians] ``` ``` */ ``` ```double odometry_angle(void){ ``` ``` return angle; ``` ```} ``` ```long odometry_velocity(void){ ``` ``` return velocity; ``` ```} ``` ```/** ``` ``` * Initializes odometry to run on timer 2. ``` ``` * Also resets all values so that the center of the robot is ``` ``` * considered to be at the origin facing the x direction. ``` ``` */ ``` ```void odometry_init(void){ ``` ``` ``` ``` encoders_init(); ``` ``` ``` ``` delay_ms(100); ``` ``` odometry_reset(); ``` ``` ``` ``` //CTC Mode. CLK / 1024 ``` ``` TCCR2 = 0; // (Fully disconnected) ``` ``` TCCR2 = _BV(WGM21) | _BV(CS22) | _BV(CS20); //CLK/1024 , CTC Mode. ``` ``` TIMSK |= _BV(OCIE2); ``` ``` OCR2 = ODOMETRY_CLK; ``` ```} ``` ```/** ``` ``` * Resets all values so that the center of the robot is ``` ``` * considered to be at the origin facing the x direction. ``` ``` */ ``` ```void odometry_reset(void){ ``` ``` diff_x = 0; ``` ``` diff_y = 0; ``` ``` encoder_ltm1 = encoder_read(LEFT); ``` ``` encoder_rtm1 = encoder_read(RIGHT); ``` ``` angle = 0.0; ``` ```} ``` ```void odometry_run(void){ ``` ``` //Angle swept through in a time step CCW- ``` ``` double theta, rl, dc; ``` ``` long dr,dl; ``` ``` char buf[100]; ``` ``` ``` ``` //Get the change in wheel positions ``` ``` { ``` ``` int encoder_right = encoder_read(RIGHT); ``` ``` int encoder_left = encoder_read(LEFT); ``` ``` ``` ``` dl = encoder_left - encoder_ltm1; ``` ``` dr = encoder_right - encoder_rtm1; ``` ``` ``` ``` //No motion. ``` ``` if(dl == 0 && dr == 0) return; ``` ``` encoder_ltm1 = encoder_left; ``` ``` encoder_rtm1 = encoder_right; ``` ``` ``` ``` // Try to avoid over/underflow. ``` ``` dl = dl > 512 ? dl - 1024 :dl; ``` ``` dl = dl < -512 ? dl + 1024 :dl; ``` ``` dr = dr > 512 ? dr - 1024 :dr; ``` ``` dr = dr < -512 ? dr + 1024 :dr; ``` ``` ``` ``` //Convert "clicks" to um ``` ``` dl *= CLICK_DISTANCE_UM; //um ``` ``` dr *= CLICK_DISTANCE_UM; ``` ``` } ``` ``` ``` ``` if(dl == dr){ ``` ``` diff_x += lround(dl*cos(angle)/1000.0); //mm ``` ``` diff_y += lround(dl*sin(angle)/1000.0); //mm ``` ``` velocity = lround((dl * 1000.0)/(ODOMETRY_CLK*TIME_SCALE)); //um / ms = mm/s ``` ``` sprintf(buf,"dc: %ld, velocity: %ld\r\n mm/s",dl,velocity); ``` ``` usb_puts(buf); ``` ``` return; ``` ``` } ``` ``` ``` ``` //Call the left wheel 0, clockwise positive. ``` ``` rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl - dr)); //um ``` ``` theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM); //rad ``` ``` ``` ``` //Distance the center has traveled. ``` ``` dc = (theta * (rl - ROBOT_WIDTH_UM)) / 2.0; //um ``` ``` velocity = lround( dc * 1000 /(ODOMETRY_CLK*TIME_SCALE)); ``` ``` sprintf(buf,"dc: %lfum, velocity: %ld\r\n mm/s",dc,velocity); ``` ``` usb_puts(buf); ``` ``` ``` ``` //angle is necessarily CCW+, so subtract. ``` ``` angle -= ANGLE_SCALE * theta; ``` ``` ``` ``` //Change state variables. Probably lose all measurements less then a mm. ``` ``` diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm ``` ``` diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm ``` ```} ``` ```ISR(TIMER2_COMP_vect){ ``` ``` odometry_run(); ``` ```} ``` ```long lround(double d){ ``` ``` double f = floor(d); ``` ``` return (long)(d - f > 0.5 ? f + 1 : f); ``` ```} ```