Colony

#include <dragonfly_lib.h>

#include "odometry.h"

int main(void)

{

	dragonfly_init(ALL_ON);

	odometry_init();

	int angle;

	while(1){

		usb_puts("X: ");

		usb_puti(odometry_dx());	

		usb_puts(" Y: ");

		usb_puti(odometry_dy());	

		usb_puts(" Theta: ");

		angle = (int)(odometry_angle()*150.0);	

		usb_puti(angle);	

		usb_puts("\n\r");

		delay_ms(500);

	}

	return 0;

}

#ifndef __ODOMETRY_C__

#define __ODOMETRY_C__

//Odometry resolution, *64 microseconds.

#define ODOMETRY_CLK 160  //= approx. 10 ms

//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_MM 137    //mm

#define CLICK_DISTANCE_UM 204 //um

//Standard measures will be mm and us

int odometry_dx(void);

int odometry_dy(void);

float odometry_angle(void);

void odometry_init(void);

void odometry_reset(void);

#endif