Diff: MiniExplorerCoimbra.hpp

Revision:

35:68f9edbb3cff

Parent:

34:c208497dd079

Child:

36:b59d56d0b3b4

--- a/MiniExplorerCoimbra.hpp	Fri Jun 09 14:30:21 2017 +0000
+++ b/MiniExplorerCoimbra.hpp	Sun Jun 11 22:40:37 2017 +0000
@@ -6,18 +6,27 @@
 #include<math.h>
 
 
+
+
 	/*
-Robot coordinate system:      World coordinate system:
-      ^                 				^
-      |x                				|y
-   <--R                 				O-->
-    y                     				 x
+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;
@@ -35,49 +44,40 @@
 	float attractionConstantForce;
 	float repulsionConstantForce;
 
-	MiniExplorerCoimbra(float defaultX, float defaultY, float defaultTheta);
+	MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld);
 
-	void setXYTheta(float x, float y, float theta);
+	void setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld);
+	
+	void myOdometria();
 	
 	//generate a position randomly and makes the robot go there while updating the map
 	void randomize_and_map();
 	
-	//go the the given position while updating the map
-	void go_to_point_with_angle(float targetX, float targetY, float targetAngle);
+	//move of targetXWorld and targetYWorld ending in a targetAngleWorld
+	void go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld);
 	
 	void update_sonar_values(float leftMm,float frontMm,float rightMm);
 	
-	float get_converted_robot_X_into_world();
-	
-	float get_converted_robot_Y_into_world();
-	
-	void do_half_flip();
+	//Distance computation function
+	float dist(float x1, float y1, float x2, float y2);
 	
-	//Distance computation function
-	float dist(float robotX, float robotY, float targetX, float targetY);
-	
-	float update_angular_speed_wheels_go_to_point_with_angle(float targetX, float targetY, float targetAngle, float dt);
-	
+	//use virtual force field
 	void try_to_reach_target(float targetXWorld,float targetYWorld);
 	
 	void vff(bool* reached, float targetXWorld, float targetYWorld);
 	
 	//compute the force on X and Y
-	void compute_forceX_and_forceY(float* forceX, float* forceY, float targetXWorld, float targetYWorld);
+	void compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld);
 	
-	void update_force(int widthIndice, int heightIndice, float* forceX, float* forceY, float xRobotWorld, float yRobotWorld );
+	void update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY );
 	
-	//robotX and robotY are from Odometria, calculate line_a, line_b and line_c
 	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);
 	
-	//return 1 if positiv, -1 if negativ
-	float sign1(float value);
 	
-	//return 1 if positiv, 0 if negativ
-	int sign2(float value);
+	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
@@ -85,11 +85,19 @@
 	*/
 	void turn_to_target(float angleToTarget);
 	
+	void do_half_flip();
+	
+	void print_final_map_with_robot_position();
+	
+	void print_final_map_with_robot_position_and_target(float targetXWorld, float targetYWorld);
+	
 	void print_final_map();
 	
-	void print_final_map_with_robot_position(float robot_x,float robot_y);
+	//return 1 if positiv, -1 if negativ
+	float sign1(float value);
 	
-	void print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld);
+	//return 1 if positiv, 0 if negativ
+	int sign2(float value);
 };