Colony

#include "encoders.h"

#include <math.h>

//Odometry resolution, *64 microseconds.

#define ODOMETRY_CLK=80 

//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 encoder_ltm1, encoder_rtm1;

int diff_x,diff_y;

double angle;

void odometry_init(){

        encoder_init();

        odometry_reset();

}

void odometry_reset(){

        diff_x = 0;

        diff_y = 0;

        angle = 0.0;

        //CTC Mode. CLK / 1024

        TCCR3A = 0; // (Fully disconnected)

        TCCR3B = _BV(WGM32) | _BV(CS32) | _BV(CS30); //CLK/1024 , CTC Mode.

        OCR3A = ODOMETRY_CLK;

}

void odometry_run(){

        int v_l, v_r;

        double omega;

        int encoder_left, encoder_right;        

        encoder_left = encoder_read(LEFT);        

        encoder_right = encoder_read(RIGHT);        

        v_l = (CLICK_DISTANCE_UM*(encoder_left - encoder_ltm1)) / (ODOMETRY_CLK * 64);  // um / us = m/s

        v_r = (CLICK_DISTANCE_UM*(encoder_right - encoder_rtm1)) / (ODOMETRY_CLK * 64); // same

        if(v_l == v_r){

                diff_x += v_l * cos(angle);

                diff_y += v_r * sin(angle);

                return;

        }

        //Relative to the center of the robot, the center of it's turn.

        turning_radius = ROBOT_WIDTH_MM * ((v_l + v_r) / (v_l - v_r)); //  = mm

        //TODO I think I may have gotten lazy on the angular momentum units before, hard to tell now.        

        if(v_r > v_l) //CCW positive.

                omega = ((double)(-v_r)) / (turning_radius + ROBOT_WIDTH_MM>>1);

        if(v_r > v_l) //CW negative.

                omega = ((double)(-v_l)) / (turning_radius - ROBOT_WIDTH_MM>>1);

        //Omega positive whenever t_r is negative, vica versa.

        vmag = -omega * turning_radius;

        angle += omega; // * the timer interval.

        diff_x += vmag * cos(angle);

        diff_y += vmag * sin(angle);

}

ISR(TIMER3_COMPA_vect){

        odometry_run();

}