Colony

#include "odometry.h"

#include <encoders.h>

#include <math.h>

#include <avr/interrupt.h>

#include <dragonfly_lib.h>

int encoder_ltm1, encoder_rtm1;

long diff_x,diff_y;

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

        char buf[40];

        diff_x = 0;

        diff_y = 0;

        encoder_ltm1 = encoder_read(LEFT);

        encoder_rtm1 = encoder_read(RIGHT);

        //sprintf(buf,"(en_right,en_left) in um: (%d,%d)\n\r",encoder_ltm1,encoder_rtm1);

        //usb_puts(buf);

        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;

}*/

/*Generalized version of odometry.*/

void odometry_run(void){

        //Angle swept through in a time step CCW-

        double theta, rl, dc;

        double dr,dl;

        char buf[40];        

        //Relative to the inner wheel, turn radius.

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

        //if(dl >=-1 && dl <= 1  && dr >= -1 && dr <= 1) 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){

                //usb_putc('#');

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

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

                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

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

        diff_x += ((dc * cos(angle))/1000); //mm

        diff_y += ((dc * sin(angle))/1000); //mm

        //Change state variables.        

        //angle is necessarily CCW+, so subtract.

        angle -= theta; 

}

ISR(TIMER2_COMP_vect){

        odometry_run(); 

}