Position Estimation of a Wheeled Bot With Arduino.

This tutorial speaks about how to estimate the position of your indoor robot with encoders and an arduino (can be done on any microcontroller).

To do that you will need :
- 2 motors with wheels
- 2 optical encoders (attached to the motor or independent of the motor to avoid drift)
- 1 arduino mega (you need to have at least 4 external interrupt)

Step 1: Mechanics

As said previously, you need motors and encoder to estimate the position.

For example, I use the Lynxmotion GHM-04 motors with the attached encoder on 1 picture, and external encoders on another.

Step 2: Electronics

The electronics you will need is :
- a microcontroller (an arduino in my example) : mandatory.
- a motor controller : optional, it's only if you don't use servomotor to drive your robot (in my example I use a RoboClaw board).
- pull-up resistance for the encoders channels A and B.
- Power source and 5V regulator.

Here are the connections you have to do :
- On the left encoder, connect the A channel to the pin D2 (digital) of the arduino, and the B channel to the pin D10 of the arduino
- On the right encoder, connect the A channel to the pin D3 of the arduino, and the B channel to the pin A15 (analog) of the arduino
(Don't forget the pull-up resistances !)

You can see on the picture one of my prototype for the Eurobot contest.

Step 3: The Source Code

Haaa finally the interesting part ;)

This is an example of the code for an arduino-mega :
(Don't forget to put your own values into TICK_PER_MM and DIAMETER
TICK_PER_MM is the number of ticks that your encoder does to do 1millimeter on the ground)

/*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* JBot wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return.
* ----------------------------------------------------------------------------
*/

// Other includes
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <math.h>

/***********/
/* Defines */
/***********/
#define TICK_PER_MM_LEFT 90.9456817668
#define TICK_PER_MM_RIGHT 90.9456817668
#define DIAMETER 166.0 // Distance between the 2 wheels (in millimeter)

#define TWOPI 6.2831853070
#define RAD2DEG 57.2958 /* radians to degrees conversion */

/********************/
/* Global variables */
/********************/
volatile long left_cnt = 0;
volatile long right_cnt = 0;

double pos_X = 0;
double pos_Y = 0;
double theta = 0;

int last_left = 0;
int last_right = 0;

int left_diff = 0;
int right_diff = 0;

double total_distance = 0.0;

/***********************/
/* INTERRUPT FUNCTIONS */
/***********************/

// External Interrupt 4 service routine => PIN2
ISR(INT4_vect)
{
//#asm("cli")
if ((PINB & 0x10) != 0) {
if ((PINE & 0x10) != 0)
left_cnt--;
else
left_cnt++;
} else {
if ((PINE & 0x10) == 0)
left_cnt--;
else
left_cnt++;
}

//#asm("sei")
}

// External Interrupt 5 service routine => PIN3
ISR(INT5_vect)
{
if ((PINK & 0x80) != 0) {
if ((PINE & 0x20) != 0)
right_cnt++;
else
right_cnt--;
} else {
if ((PINE & 0x20) == 0)
right_cnt++;
else
right_cnt--;
}

}

// Pin change 0-7 interrupt service routine => PIN10
ISR(PCINT0_vect)
{
if ((PINE & 0x10) != 0) {
if ((PINB & 0x10) != 0) {
left_cnt++;
} else
left_cnt--;
} else {
if ((PINB & 0x10) == 0) {
left_cnt++;
} else
left_cnt--;
}

}

// Pin change 16-23 interrupt service routine => PIN-ADC15
ISR(PCINT2_vect)
{
if ((PINE & 0x20) != 0) {
if ((PINK & 0x80) != 0)
right_cnt--;
else
right_cnt++;
} else {
if ((PINK & 0x80) == 0)
right_cnt--;
else
right_cnt++;
}

}

// Timer 1 overflow interrupt service routine
ISR(TIMER1_OVF_vect)
{
sei(); // enable interrupts
get_Odometers();

}

/*************************/
/* SYSTEM INITIALIZATION */
/*************************/
void setup()
{
// Crystal Oscillator division factor: 1
#pragma optsize-
CLKPR = 0x80;
CLKPR = 0x00;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize+
#endif

// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA = 0x00;
DDRA = 0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=Out
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB = 0x00;
DDRB = 0x00;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC = 0x00;
DDRC = 0x00;

// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD = 0x00;
DDRD = 0x00;

// Port E initialization
// Func2=In Func1=In Func0=In
// State2=T State1=T State0=T
PORTE = 0x00;
DDRE = 0x00;

PORTK = 0x00;
DDRK = 0x00;

/**********************/
/* I/O INITIALIZATION */
/**********************/
// OUTPUTS
// pinMode(13, OUTPUT);

// Timer/Counter 1 initialization
// Clock source: System Clock
// Mode: Ph. correct PWM top=00FFh
// OC1A output: Discon.
// OC1B output: Discon.
// OC1C output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer 1 Overflow Interrupt: On
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
// Compare C Match Interrupt: Off
TCCR1A = 0x01;
TCCR1B = 0x04;
TCNT1H = 0x00;
TCNT1L = 0x00;
ICR1H = 0x00;
ICR1L = 0x00;
OCR1AH = 0x00;
OCR1AL = 0x00;
OCR1BH = 0x00;
OCR1BL = 0x00;
OCR1CH = 0x00;
OCR1CL = 0x00;

// External Interrupt(s) initialization
EICRA = 0x00;
EICRB = 0x05;
EIMSK = 0x30;
EIFR = 0x30;
// Interrupt on PCINT
PCICR = 0x05;
PCIFR = 0x05;
PCMSK0 = 0x10;
PCMSK1 = 0x00;
PCMSK2 = 0x80;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK1 |= 0x01;
TIFR1 |= 0x01;

/******************************/
/* Initialization of the code */
/******************************/

// Global enable interrupts
sei();

}

/******************/
/* MAIN CODE LOOP */
/******************/
void loop()
{

}

/********************************/
/* POSITION ESTIMATION FUNCTION */
/********************************/

/* Compute the position of the robot */
void get_Odometers(void)
{
long left_wheel = 0;
long right_wheel = 0;

double left_mm = 0.0;
double right_mm = 0.0;

double distance = 0.0;

left_wheel = left_cnt;
right_wheel = right_cnt;

left_diff = last_left - left_wheel;
right_diff = last_right - right_wheel;

last_left = left_wheel;
last_right = right_wheel;

left_mm = ((double) left_diff) / TICK_PER_MM_LEFT;
right_mm = ((double) right_diff) / TICK_PER_MM_RIGHT;

distance = (left_mm + right_mm) / 2;
total_distance += distance;

theta += (right_mm - left_mm) / DIAMETER;

if (theta > PI)
theta -= TWOPI;
if (theta < (-PI))
theta += TWOPI;

pos_Y += distance * sin(theta);
pos_X += distance * cos(theta);

}

Some explanations :
The ISR( __VECT__ ) function are the external interruptions. They are used to count the number of ticks of each encoder.
The ISR(TIMER1_OVF_vect) is a timer interruption. It is used to make a perfectly timed computing of the position.
The get_Odometers() function is the function which compute the position depending to the ticks counted before.

All is working for you ?
Congratulation ! Now your robot can compute its own position !
You can now do something like that :
Remote control your robot and let him compute his own position and send it to your computer to print the robot's path.

You find that cool but you want more for your robot ? Go to the next step to make it go autonomously to waypoints !

Step 4: Let's Go to This Position You Little Bot !

Welcome to the 4th step. To do this step, you must have the previous step working well.

So, you have a robot which know where he is but you need to control all his movements. Not very good !
Let's fix it by controlling the robot only with waypoints, which is much more fun.

To do that, you don't need any other mechanics nor electronics ! It's pure software ;)

Let's see the code (it's a little long and not comprehensive at the first sight :

/*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* JBot wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return.
* ----------------------------------------------------------------------------
*/

// Other includes
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <math.h>

/***********/
/* Defines */
/***********/
#define TICK_PER_MM_LEFT 9.2628378129 // 90.9456817668
#define TICK_PER_MM_RIGHT 9.2628378129 // 90.9456817668
#define DIAMETER 270.4 //275.0 // 166.0 // Distance between the 2 wheels

#define TWOPI 6.2831853070
#define RAD2DEG 57.2958 /* radians to degrees conversion */

// Types of motor
#define ALPHA_MOTOR 0
#define DELTA_MOTOR 1
#define LEFT_MOTOR 2
#define RIGHT_MOTOR 3

#define ALPHADELTA 0
#define LEFTRIGHT 1

#define COMMAND_DONE 0
#define PROCESSING_COMMAND 1
#define WAITING_BEGIN 2
#define ERROR 3

#define ALPHA_MAX_SPEED 25000 //24000 //25000//13000 // 9000
#define ALPHA_MAX_ACCEL 300
#define ALPHA_MAX_DECEL 3500 //2500
#define DELTA_MAX_SPEED 45000 //45000 //50000//37000
#define DELTA_MAX_SPEED_BACK 35000 //45000 //50000//37000
#define DELTA_MAX_SPEED_BACK_PAWN 45000
#define DELTA_MAX_ACCEL 1000 //900 //600
#define DELTA_MAX_DECEL 10000 //4000 //1800

/***********************/
/* Specific structures */
/***********************/
struct motor {
int type;
signed long des_speed;
signed long cur_speed;
long last_error;
long error_sum;
int kP;
int kI;
int kD;
signed long accel;
signed long decel;
signed long max_speed;
double distance;
};

struct robot {
double pos_X;
double pos_Y;
double theta;
float yaw;
float pitch;
float roll;
float yaw_offset;
};

struct RobotCommand {
char state;
double current_distance;
double desired_distance;
};

struct Point {
int x;
int y;
};

/********************/
/* Global variables */
/********************/
struct motor left_motor;
struct motor right_motor;
struct motor alpha_motor;
struct motor delta_motor;

struct robot maximus;

struct RobotCommand bot_command_delta;
struct RobotCommand prev_bot_command_delta;
struct RobotCommand bot_command_alpha;

volatile long left_cnt = 0;
volatile long right_cnt = 0;

int last_left = 0;
int last_right = 0;

int left_diff = 0;
int right_diff = 0;

double total_distance = 0.0;

/***********************/
/* INTERRUPT FUNCTIONS */
/***********************/

// External Interrupt 4 service routine => PIN2
ISR(INT4_vect)
{
if ((PINB & 0x10) != 0) {
if ((PINE & 0x10) != 0)
left_cnt--;
else
left_cnt++;
} else {
if ((PINE & 0x10) == 0)
left_cnt--;
else
left_cnt++;
}

}

// External Interrupt 5 service routine => PIN3
ISR(INT5_vect)
{
if ((PINK & 0x80) != 0) {
if ((PINE & 0x20) != 0)
right_cnt++;
else
right_cnt--;
} else {
if ((PINE & 0x20) == 0)
right_cnt++;
else
right_cnt--;
}

}

// Pin change 0-7 interrupt service routine => PIN10
ISR(PCINT0_vect)
{
if ((PINE & 0x10) != 0) {
if ((PINB & 0x10) != 0) {
left_cnt++;
} else
left_cnt--;
} else {
if ((PINB & 0x10) == 0) {
left_cnt++;
} else
left_cnt--;
}

}

// Pin change 16-23 interrupt service routine => PIN-ADC15
ISR(PCINT2_vect)
{
if ((PINE & 0x20) != 0) {
if ((PINK & 0x80) != 0)
right_cnt--;
else
right_cnt++;
} else {
if ((PINK & 0x80) == 0)
right_cnt--;
else
right_cnt++;
}

}

// Timer 1 overflow interrupt service routine
ISR(TIMER1_OVF_vect)
{
sei(); // enable interrupts
get_Odometers();

do_motion_control();
move_motors(ALPHADELTA); // Update the motor speed

}

/*************************/
/* SYSTEM INITIALIZATION */
/*************************/
void setup()
{

// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA = 0x00;
DDRA = 0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=Out
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB = 0x00;
DDRB = 0x00;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC = 0x00;
DDRC = 0x00;

// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD = 0x00;
DDRD = 0x00;

// Port E initialization
// Func2=In Func1=In Func0=In
// State2=T State1=T State0=T
PORTE = 0x00;
DDRE = 0x00;

PORTK = 0x00;
DDRK = 0x00;

pinMode(13, OUTPUT);

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 62,500 kHz
// Mode: Ph. correct PWM top=00FFh
// OC1A output: Discon.
// OC1B output: Discon.
// OC1C output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer 1 Overflow Interrupt: On
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
// Compare C Match Interrupt: Off
TCCR1A = 0x01;
TCCR1B = 0x04;
TCNT1H = 0x00;
TCNT1L = 0x00;
ICR1H = 0x00;
ICR1L = 0x00;
OCR1AH = 0x00;
OCR1AL = 0x00;
OCR1BH = 0x00;
OCR1BL = 0x00;
OCR1CH = 0x00;
OCR1CL = 0x00;

// External Interrupt(s) initialization
EICRA = 0x00;
EICRB = 0x05;
EIMSK = 0x30;
EIFR = 0x30;
// Interrupt on PCINT
PCICR = 0x05;
PCIFR = 0x05;
PCMSK0 = 0x10;
PCMSK1 = 0x00;
PCMSK2 = 0x80;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK1 |= 0x01;
TIFR1 |= 0x01;

/******************************/
/* Initialization of the code */
/******************************/
init_motors(); // Init motors
init_Robot(&maximus); // Init robot status

init_Command(&bot_command_delta); // Init robot command
init_Command(&bot_command_alpha); // Init robot command
init_Command(&prev_bot_command_delta);

// Global enable interrupts
sei();

delay(10);

}

/******************/
/* MAIN CODE LOOP */
/******************/
void loop()
{
// Place your code here

delay(20);

}

/****************************/
/* INITIALIZATION FUNCTIONS */
/****************************/
void init_Robot(struct robot *my_robot)
{
my_robot->pos_X = 0;
my_robot->pos_Y = 0;
my_robot->theta = 0;
my_robot->yaw = 0.0;
my_robot->pitch = 0.0;
my_robot->roll = 0.0;
my_robot->yaw_offset = 0.0;
}

void init_Command(struct RobotCommand *cmd)
{
cmd->state = COMMAND_DONE;
cmd->current_distance = 0;
cmd->desired_distance = 0;
}

void init_motors(void)
{
/* Left motor initialization */
left_motor.type = LEFT_MOTOR;
left_motor.des_speed = 0;
left_motor.cur_speed = 0;
left_motor.last_error = 0;
left_motor.error_sum = 0;
left_motor.kP = 12;
left_motor.kI = 6;
left_motor.kD = 1;
left_motor.accel = 5;
left_motor.decel = 5;
left_motor.max_speed = 30;
left_motor.distance = 0.0;

/* Right motor initialization */
right_motor.type = RIGHT_MOTOR;
right_motor.des_speed = 0;
right_motor.cur_speed = 0;
right_motor.last_error = 0;
right_motor.error_sum = 0;
right_motor.kP = 12;
right_motor.kI = 6;
right_motor.kD = 1;
right_motor.accel = 5;
right_motor.decel = 5;
right_motor.max_speed = 30;
right_motor.distance = 0.0;

/* Alpha motor initialization */
alpha_motor.type = ALPHA_MOTOR;
alpha_motor.des_speed = 0;
alpha_motor.cur_speed = 0;
alpha_motor.last_error = 0;
alpha_motor.error_sum = 0;
alpha_motor.kP = 230; //250//350 // 600
alpha_motor.kI = 0;
alpha_motor.kD = 340; //300 //180 // 200
alpha_motor.accel = ALPHA_MAX_ACCEL; //300; //350//200; // 300
alpha_motor.decel = ALPHA_MAX_DECEL; //1300; //1200;//1100;//1200; // 500
alpha_motor.max_speed = ALPHA_MAX_SPEED; //7000; //8000
alpha_motor.distance = 0.0;

/* Delta motor initialization */
delta_motor.type = DELTA_MOTOR;
delta_motor.des_speed = 0;
delta_motor.cur_speed = 0;
delta_motor.last_error = 0;
delta_motor.error_sum = 0;
delta_motor.kP = 600; // 600
delta_motor.kI = 0;
delta_motor.kD = 200; // 100 * 1.09
delta_motor.accel = DELTA_MAX_ACCEL; //600; //400;//500;
delta_motor.decel = DELTA_MAX_DECEL; //1800; //1350;//1100;//1200;
delta_motor.max_speed = DELTA_MAX_SPEED; //25000;//35000;
delta_motor.distance = 0.0;
}

/*******************************/
/* MOTION CONTROL FUNCTIONS */
/*******************************/
void do_motion_control(void)
{

// PID angle
alpha_motor.des_speed = compute_position_PID(&bot_command_alpha, &alpha_motor);

// PID distance
if ((bot_command_alpha.state == WAITING_BEGIN) || (bot_command_alpha.state == PROCESSING_COMMAND)) { // If alpha motor have not finished its movement

} else {
if ((bot_command_delta.state != PROCESSING_COMMAND) && (prev_bot_command_delta.state == WAITING_BEGIN)) {
prev_bot_command_delta.state = PROCESSING_COMMAND;
set_new_command(&bot_command_delta, prev_bot_command_delta.desired_distance);
}

}
delta_motor.des_speed = compute_position_PID(&bot_command_delta, &delta_motor);

}

void set_new_command(struct RobotCommand *cmd, long distance)
{
cmd->state = WAITING_BEGIN;
cmd->current_distance = 0;
cmd->desired_distance = distance;
}

long compute_position_PID(struct RobotCommand *cmd, struct motor *used_motor)
{
long P, I, D;
long errDif, err;
long tmp = 0;

if (cmd->state == WAITING_BEGIN) {
cmd->state = PROCESSING_COMMAND;
}

if (used_motor->type == ALPHA_MOTOR)
err = cmd->desired_distance * 10 - cmd->current_distance * 10 * RAD2DEG;
else
err = cmd->desired_distance - cmd->current_distance;

used_motor->error_sum += err; // Error sum
if (used_motor->error_sum > 10)
used_motor->error_sum = 10;
if (used_motor->error_sum < -10)
used_motor->error_sum = -10;

errDif = err - used_motor->last_error; // Compute the error variation

used_motor->last_error = err;

P = err * used_motor->kP; // Proportionnal
I = used_motor->error_sum * used_motor->kI; // Integral
D = errDif * used_motor->kD; // Derivative

tmp = (P + I + D);

if (abs(tmp) < abs(used_motor->des_speed)) { // Deceleration
if (tmp > (used_motor->des_speed + used_motor->decel))
tmp = (used_motor->des_speed + used_motor->decel);
else if (tmp < (used_motor->des_speed - used_motor->decel))
tmp = (used_motor->des_speed - used_motor->decel);
} else { // Acceleration
if (tmp > (used_motor->des_speed + used_motor->accel))
tmp = (used_motor->des_speed + used_motor->accel);
else if (tmp < (used_motor->des_speed - used_motor->accel))
tmp = (used_motor->des_speed - used_motor->accel);
}

if (tmp > (used_motor->max_speed))
tmp = (used_motor->max_speed);
if (tmp < -(used_motor->max_speed))
tmp = -(used_motor->max_speed);

if (used_motor->type == ALPHA_MOTOR) {
if ((cmd->state == PROCESSING_COMMAND) && (abs(err) < 3)
&& (abs(errDif) < 3)) { // 2 before
cmd->state = COMMAND_DONE;
}
} else {
if ((cmd->state == PROCESSING_COMMAND) && (abs(err) < 6)
&& (abs(errDif) < 5)) { // 2 before
cmd->state = COMMAND_DONE;
}
}

return tmp;
}

// Compute the distance to do to go to (x, y)
double distance_coord(struct robot *my_robot, double x1, double y1)
{
double x = 0;
x = sqrt(pow(fabs(x1 - my_robot->pos_X), 2) + pow(fabs(y1 - my_robot->pos_Y), 2));
return x;
}

// Compute the angle to do to go to (x, y)
double angle_coord(struct robot *my_robot, double x1, double y1)
{
double angletodo = 0;
if ((x1 < my_robot->pos_X) && (y1 < my_robot->pos_Y)) {
angletodo = -PI / 2 - atan(fabs((x1 - my_robot->pos_X) / (y1 - my_robot->pos_Y)));
} else if ((x1 > my_robot->pos_X) && (y1 < my_robot->pos_Y)) {
angletodo = -atan(fabs((y1 - my_robot->pos_Y) / (x1 - my_robot->pos_X)));
} else if ((x1 > my_robot->pos_X) && (y1 > my_robot->pos_Y)) {
angletodo = atan(fabs((y1 - my_robot->pos_Y) / (x1 - my_robot->pos_X)));
} else if ((x1 < my_robot->pos_X) && (y1 > my_robot->pos_Y)) {
angletodo = PI / 2 + atan(fabs((x1 - my_robot->pos_X) / (y1 - my_robot->pos_Y)));
} else if ((x1 < my_robot->pos_X) && (y1 == my_robot->pos_Y)) { //
angletodo = -PI;
} else if ((x1 > my_robot->pos_X) && (y1 == my_robot->pos_Y)) { //
angletodo = 0;
} else if ((x1 == my_robot->pos_X) && (y1 < my_robot->pos_Y)) { //
angletodo = -PI / 2;
} else if ((x1 == my_robot->pos_X) && (y1 > my_robot->pos_Y)) { //
angletodo = PI / 2;
} else
angletodo = 0;

angletodo = angletodo - my_robot->theta;

if (angletodo > PI)
angletodo = angletodo - 2 * PI;
if (angletodo < -PI)
angletodo = 2 * PI + angletodo;

return angletodo;
}

void goto_xy(double x, double y)
{
double ang, dist;

ang = angle_coord(&maximus, x, y) * RAD2DEG;
set_new_command(&bot_command_alpha, ang);

dist = distance_coord(&maximus, x, y);
set_new_command(&prev_bot_command_delta, dist);
bot_command_delta.state = WAITING_BEGIN;

}

void goto_xy_back(double x, double y)
{
double ang, dist;

ang = angle_coord(&maximus, x, y);
if (ang < 0)
ang = (ang + PI) * RAD2DEG;
else
ang = (ang - PI) * RAD2DEG;
set_new_command(&bot_command_alpha, ang);

dist = -distance_coord(&maximus, x, y);
set_new_command(&prev_bot_command_delta, dist);
bot_command_delta.state = WAITING_BEGIN;
}

/********************/
/* MOTORS FUNCTIONS */
/********************/
void move_motors(char type)
{
if (type == ALPHADELTA) {
//write_RoboClaw_speed_M1M2(128, delta_motor.des_speed - alpha_motor.des_speed, delta_motor.des_speed + alpha_motor.des_speed);
// PUT YOUR DRIVE MOTOR CODE HERE
}
else {
//write_RoboClaw_speed_M1M2(128, left_motor.des_speed, right_motor.des_speed);
// PUT YOUR DRIVE MOTOR CODE HERE
}
}

void update_motor(struct motor *used_motor)
{
switch (used_motor->type) {
case LEFT_MOTOR:
used_motor->cur_speed = left_diff;
break;
case RIGHT_MOTOR:
used_motor->cur_speed = right_diff;
break;
case ALPHA_MOTOR:
used_motor->cur_speed = left_diff - right_diff;
break;
case DELTA_MOTOR:
used_motor->cur_speed = (left_diff + right_diff) / 2;
break;
default:
break;
}
}

/********************************/
/* POSITION ESTIMATION FUNCTION */
/********************************/

/* Compute the position of the robot */
void get_Odometers(void)
{
long left_wheel = 0;
long right_wheel = 0;

double left_mm = 0.0;
double right_mm = 0.0;

double distance = 0.0;

left_wheel = left_cnt;
right_wheel = right_cnt;

left_diff = last_left - left_wheel;
right_diff = last_right - right_wheel;

last_left = left_wheel;
last_right = right_wheel;

left_mm = ((double) left_diff) / TICK_PER_MM_LEFT;
right_mm = ((double) right_diff) / TICK_PER_MM_RIGHT;

distance = (left_mm + right_mm) / 2;
total_distance += distance;
bot_command_delta.current_distance += distance;

maximus.theta += (right_mm - left_mm) / DIAMETER;
bot_command_alpha.current_distance += (right_mm - left_mm) / DIAMETER;

if (maximus.theta > PI)
maximus.theta -= TWOPI;
if (maximus.theta < (-PI))
maximus.theta += TWOPI;

maximus.pos_Y += distance * sin(maximus.theta);
maximus.pos_X += distance * cos(maximus.theta);

update_motor(&left_motor);
update_motor(&right_motor);
update_motor(&alpha_motor);
update_motor(&delta_motor);

}

So, let's see what it does (it's not really understandable at the first sight) :
- init_motors(void)
it inits all the variables of the motors such as the PID constants, the acceleration and the max speed we want

- do_motion_control(void)
This function is very important. It compute the angle and the distance value using the PID function and put it in the corresponding motor

- set_new_command(struct RobotCommand *cmd, long distance)
Give a new command to a motor, for example for the distance motor (delta) we can say him to go a 100millimeters

- long compute_position_PID(struct RobotCommand *cmd, struct motor *used_motor)
This function compute the PID for a motor using his constant, acceleration and max speed

- goto_xy(double x, double y) and goto_xy_back(double x, double y)
These functions are used to give a waypoint to the robot

You can now do something like that (giving waypoint to your robot and seeing him going there alone) :

Step 5: An Example of Use in a Contest

If everything works well, you can do a cool robot like that !
It's the robot that I presented to the Eurobot contest 2011.
It is totally autonomous and the goal was to build tower and put the pawns on the right color (In the video : RED)