Colony

#include "odometry.h"

#include <encoders.h>

#include <math.h>

#include <avr/interrupt.h>

#include <dragonfly_lib.h>

int encoder_ltm1, encoder_rtm1;

//Measures mm displacement in x and y from starting position. (+x:theta=0 +y:theta=M_PI/2)

long diff_x,diff_y;

//Measures radian angle displacement from starting position. (initially 0)

double angle;

long odometry_dx(void){

        return diff_x;

}

long odometry_dy(void){

        return diff_y;

}

double odometry_angle(void){

        return angle;

}

void odometry_init(void){

        encoders_init();

        delay_ms(100);

        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;

        encoder_ltm1 = encoder_read(LEFT);

        encoder_rtm1 = encoder_read(RIGHT);

        angle = 0.0;

}

/*Currently assumes robot only goes forward (neither wheel goes backward).

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;

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

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

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

                //usb_puts(buf);

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

        }

        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;

}*/

/*Generalized version of odometry.*/

void odometry_run(void){

        //Angle swept through in a time step CCW-

        double theta, rl, dc;

        long dr,dl;

        //Get the change in wheel positions

        {        

                int encoder_right = encoder_read(RIGHT);        

                int encoder_left = encoder_read(LEFT);        

                dl = encoder_left - encoder_ltm1;

                dr = encoder_right - encoder_rtm1;

                //No motion.

                if(dl == 0 && dr == 0) return;

                encoder_ltm1 = encoder_left;

                encoder_rtm1 = encoder_right;

                // Try to avoid over/underflow.

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

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

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

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

                //Convert "clicks" to um

                dl *= CLICK_DISTANCE_UM; //um

                dr *= CLICK_DISTANCE_UM;

        }

        if(dl == dr){

                diff_x += lround(dl*cos(angle)/1000.0); //mm

                diff_y += lround(dl*sin(angle)/1000.0); //mm

                return;

        }

        //Call the left wheel 0, clockwise positive.

        rl = ((double)(ROBOT_WIDTH_UM*dl))/((double)(dl - dr)); //um

        theta = ((double)(dl - dr))/((double)ROBOT_WIDTH_UM);   //rad

        //Distance the center has traveled.

        dc = lround( (theta * (rl - ROBOT_WIDTH_UM)) / 2.0 ); //um

        //angle is necessarily CCW+, so subtract.

        angle -= ANGLE_SCALE * theta; 

        //Change state variables.        

        diff_x += lround((DISTANCE_SCALE * dc * cos(angle))/1000.0); //mm

        diff_y += lround((DISTANCE_SCALE * dc * sin(angle))/1000.0); //mm

}

ISR(TIMER2_COMP_vect){

        odometry_run(); 

}

long lround(double d){

        double f = floor(d);

        return (long)(d - f > 0.5 ? f + 1 : f);

}