Colony

#include "odometry.h"

#include "encoders.h"

#include <math.h>

#include <avr/interrupt.h>

#include <dragonfly_lib.h>

int encoder_ltm1, encoder_rtm1;

int diff_x,diff_y;

float angle;

int odometry_dx(void){

        return diff_x;

}

int odometry_dy(void){

        return diff_y;

}

float odometry_angle(void){

        return angle;

}

void odometry_init(void){

        encoder_init();

        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;

}

void odometry_reset(void){

        diff_x = 0;

        diff_y = 0;

        angle = 0.0;

}

void odometry_run(void){

        int v_l, v_r;

        float 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.

        float 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 = ((float)(-v_r)) / (turning_radius + (ROBOT_WIDTH_MM>>1));

        else //CW negative.

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

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

        float vmag = -omega * turning_radius;

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

        diff_x += vmag * cos(angle);

        diff_y += vmag * sin(angle);

}

ISR(TIMER2_COMP_vect){

        odometry_run();

}