Colony

/**

 * @file odometry.h

 * @brief Code for estimating the robots pose.

 *

 * Offers simple position and orientation information.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#ifndef __ODOMETRY_C__

#define __ODOMETRY_C__

/**

 * @addtogroup odometry

 * @{

 **/

//Odometry resolution, *64 microseconds.

#define ODOMETRY_CLK 255u

#define TIME_SCALE 64

//Wheel = 2.613 in.

//Circumference = 208.508133 mm

//Distance per encoder click (circumference / 1024)  = 203.621224 um.

//Robot width = 5.3745 in. = 136.5123 mm

#define ROBOT_WIDTH_UM 137000  //um

#define CLICK_DISTANCE_UM 204 //um

#define DISTANCE_SCALE 2.10526316 //Magic constant.

#define ANGLE_SCALE 1.12823207 //Magic constant.

/** @brief Retrieve the robots estimated x position*/

long odometry_dx(void);

/** @brief Retrieve the robots estimated y position*/

long odometry_dy(void);

/** @brief Retrieve the robots estimated orientation*/

double odometry_angle(void);

/** @brief Initialize odometry. MUST be called before

 * the other functions work.**/

void odometry_init(void);

/** @brief Reset position and orientation to the origin facing

 * the x axis.*/

void odometry_reset(void);

/** @brief Report estimated velocity [mm/s].*/

long odometry_velocity(void);

void odometry_set_velocity(long velocity, double angular_velocity,

        int start_speed, int start_angle);

/**@}**/ //end group

#endif