Colony

#include "odometry.h"

#include <encoders.h>

#include <math.h>

#include <avr/interrupt.h>

#include <dragonfly_lib.h>

long lround(double d);

int encoder_ltm1, encoder_rtm1;

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

/**

 * Retrieve the estimated x position. [millimeters]

 */

long odometry_dx(void){

        return diff_x;

}

/**

 * Retrieve the estimated y position. [millimeters]

 */

long odometry_dy(void){

        return diff_y;

}

/**

 * Retrieve the estimated angle [radians]

 */

double odometry_angle(void){

        return angle;

}

/**

 * Initializes odometry to run on timer 2.

 * Also resets all values so that the center of the robot is 

 * considered to be at the origin facing the x direction.

 */

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;

}

/**

 * Resets all values so that the center of the robot is 

 * considered to be at the origin facing the x direction.

 */

void odometry_reset(void){

        diff_x = 0;

        diff_y = 0;

        encoder_ltm1 = encoder_read(LEFT);

        encoder_rtm1 = encoder_read(RIGHT);

        angle = 0.0;

}

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

}