File content as of revision 39:890439b495e3:

#ifndef MINIEXPLORERCOIMBRA_HPP
#define MINIEXPLORERCOIMBRA_HPP

#include "Map.hpp"
#include "Sonar.hpp"
#include<math.h>

	/*
Robot coordinate system:      			World coordinate system:
      ^                 							^
      |x                							|y
   <--R                 							O-->
    y                     							x
    
angles from pi/2 to 3pi/2->0 to pi              angles from 0 to pi->0 to pi 
angles from pi/2 to -pi/2->0 to -pi				angles from pi to 2pi-> -pi to 0

	The idea is that for every command to the robot we use to world coodinate system 
*/

class MiniExplorerCoimbra {
	
	public:
	float xWorld;
	float yWorld;
	float thetaWorld;
	Map map;
	Sonar sonarLeft;
	Sonar sonarFront;
	Sonar sonarRight;
	float speed;
	float radiusWheels;
	float distanceWheels; 
	float khro;
	float ka;
	float kb;
	float kv;
	float kh;

	float rangeForce;
	float attractionConstantForce;
	float repulsionConstantForce;

	MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld);
	
	//generate a position randomly and makes the robot go there while updating the map
	void randomize_and_map();
	
	//move of targetXWorld and targetYWorld ending in a targetAngleWorld
	void go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld);
	
	//use virtual force field
	void try_to_reach_target(float targetXWorld,float targetYWorld);

	void print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld);
	
	void print_map_with_robot_position();
	
	void print_map();
	
	private:
	
	void setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld);
	
	void myOdometria();
	
	void update_sonar_values(float leftMm,float frontMm,float rightMm);
	
	void vff(bool* reached, float targetXWorld, float targetYWorld);
	
	void procedure_lab_4(float xEstimatedK,float yEstimatedK, float thetaWorldEstimatedK, float distanceMoved, float angleMoved, float PreviousCovarianceOdometricPositionEstimate[3][3]){

	void test_procedure_lab_4();
	
	//compute the force on X and Y
	void compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld);
	
	void update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY );
	
	void calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c);
	
	//currently line_c is not used
	void go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld);
	
	float update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt);
	
	/*angleToTarget is obtained through atan2 so it s:
	< 0 if the angle is bettween PI and 2pi on a trigo circle
	> 0 if it is between 0 and PI
	*/
	void turn_to_target(float angleToTarget);
	
	void do_half_flip();
	
	//Distance computation function
	float dist(float x1, float y1, float x2, float y2);
	
	//return 1 if positiv, -1 if negativ
	float sign1(float value);
	
	//return 1 if positiv, 0 if negativ
	int sign2(float value);
};


#endif