/*
 * odometrie.cpp
 *
 *  Created on: 14 janv. 2009
 *      Author: Gregory SANCHEZ
 *
 *      Description: cette m閠hode a pour but de d閒inir avec pr閏ision les coordonn閑s du robot apr鑣 un d閜lacement, ainsi que l'orientation de son rep鑢e, ces coordonn閑s seront donn閑s en polaire
 *      			de sorte de conna顃re la distance parcourue et sa direction. Ainsi on connait l'orientation et la position du robot.
 */

#include "Odometrie.h"

/**
 * On d閒init ici les param鑤res de construction du robot (i.e position des ODI, orientation des rep鑢es, ...)
 */
coord_cart odo_origine_ODI1_robot = {-3.5, 1.8};			//coordonn閑s de l'origine de l'ODI1 par rapport au rep鑢e du robot
coord_cart odo_origine_ODI2_robot = {3, -1.35};				//coordonn閑s de l'origine de l'ODI2 par rapport au rep鑢e du robot
coord_cart odo_origine_robot_monde_cart = {0, 0};		//coordonn閑s cart閟iennes de l'origine du robot par rapport au rep鑢e monde
coord_pol odo_origine_robot_monde_pol = {0, 0};			//coordonn閑s polaires de l'origine du robot par rapport au rep鑢e monde
coord_cart odo_vect_ODI1 = {12.95426445, 0.09267952};						//coordonn閑s renvoy閑s par l'ODI 1
coord_cart odo_vect_ODI2 = {0, 0};						//coordonn閑s renvoy閑s par l'ODI 2
coord_cart odo_origine_ODI1_monde;						//coordonn閑s de l'origine de l'ODI1 par rapport au rep鑢e monde
coord_cart odo_origine_ODI2_monde;						//coordonn閑s de l'origine de l'ODI2 par rapport au rep鑢e monde
double odo_angle_robot_ODI1 = 0; 						//angle entre le rep鑢e du robot et le rep鑢e de l'ODI1
double odo_angle_robot_ODI2 = 0; 						//angle entre le rep鑢e du robot et le rep鑢e de l'ODI2
double odo_angle_robot_monde = 0;						//angle entre le rep鑢e du robot et le rep鑢e Monde
double odo_angle_ODI2_ODI1_centre_robot = 0;			//angle entre la droite inter-ODIs et la droite (ODI1 centre du robot)
double odo_angle_ODI1_ODI2_centre_robot = 0;			//angle entre la droite inter-ODIs et la droite (ODI2 centre du robot)
double odo_distance_ODI1_centre_robot = 0;				//distance entre l'ODI1 et le centre du robot
double odo_distance_ODI2_centre_robot = 0;				//distance entre l'ODI2 et le centre du robot
int odo_precision = 1000000;							//pr閏ision de la racine carr閑
double odo_vitesse_max_robot = 1;						//vitesse (m/s) maximale que peut atteindre le robot
double odo_rapport_unite_souris_metre = 1000;			//rapport (Unit镾ouris/m鑤re) nous permettant de calculer la distance r閑lle parcourue par la souris