Colony

#include "odometry.h"

#include <encoders.h>

#include <math.h>

#include <avr/interrupt.h>

#include <dragonfly_lib.h>

long encoder_ltm1, encoder_rtm1;

long diff_x,diff_y;

double angle;

int odometry_dx(void){

        return diff_x;

}

int odometry_dy(void){

        return diff_y;

}

double odometry_angle(void){

        return angle;

}

void odometry_init(void){

        encoders_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;

}

/*Currently assumes robot only goes forward.*/

void odometry_run(void){

        double theta;

        int encoder_left, encoder_right,dl,dr; 

        long distance_um;

        char buf[100];        

        //Relative to the inner wheel, the radius of its turn.

        double turning_radius;

        encoder_left = encoder_read(LEFT);        

        encoder_right = encoder_read(RIGHT);        

        dl = encoder_left - encoder_ltm1;

        dr = encoder_right - encoder_rtm1;

        dl = dl > 512 ? dl - 1024 :dl;

        dl = dl < -512 ? dl + 1024 :dl;

        dr = dr > 512 ? dr - 1024 :dr;

        dr = dr < -512 ? dr + 1024 :dr;

        //idle?

        if(dl < 2 && dr < 2) return;

        if(dl == dr){

                distance_um = dl*CLICK_DISTANCE_UM;

                //sprintf(buf,"Distance in um: %ld\n\r",distance_um);

                //usb_puts(buf);

                diff_x += distance_um*cos(angle)/1000; 

                diff_y += distance_um*sin(angle)/1000; 

                return;

        }

        if(dr > dl){ //CCW positive.

                turning_radius = ROBOT_WIDTH_UM * dl / (dr - dl); //um

                theta = ((double)(dr-dl)*CLICK_DISTANCE_UM) / ((double)ROBOT_WIDTH_UM);

                distance_um = theta*((double)(turning_radius + ROBOT_WIDTH_UM/2)); 

                //sprintf(buf,"Distance in um: %d\n\r",distance_um);

                //usb_puts(buf);

        }

        else{ //CW negative.

                turning_radius = ROBOT_WIDTH_UM * dr / (dl - dr); //um

                theta = -((double)(dl-dr)*CLICK_DISTANCE_UM) / ((double)ROBOT_WIDTH_UM);

                distance_um = -theta*((double)(turning_radius + ROBOT_WIDTH_UM/2)); 

                //sprintf(buf,"Distance in um: %d\n\r",distance_um);

                //usb_puts(buf);

        }

        diff_x += distance_um * cos(angle)/1000;

        diff_y += distance_um * sin(angle)/1000;

        angle += theta;

        encoder_ltm1 = encoder_left;

        encoder_rtm1 = encoder_right;

}

ISR(TIMER2_COMP_vect){

        odometry_run();

}