Colony

#include "odometry.h"

#include <math.h>

#include "encoders.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;

int odometry_getx(void){

        return diff_x;

}

int odometry_gety(void){

        return diff_y;

}

int odometry_get_angle(void){

        return angle;

}

void odometry_init(void){

        encoders_init();

        odometry_reset();

}

void odometry_reset(void){

        diff_x = 0;

        diff_y = 0;

        angle = 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(void){

        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.

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

        //Here non floating point operations get trickier.

        if(v_r > v_l) //CCW positive.

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

        if(v_r > v_l) //CW negative.

                omega = ((double)-v_l) / ((double)(turning_radius - ROBOT_WIDTH_MM/2));

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

        int vmag = -omega * turning_radius;

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

        diff_x += ((double)vmag) * cos(angle);

        diff_y += ((double)vmag) * sin(angle);

}

ISR(TIMER3_COMPA_vect){

        odometry_run();

}