Diff: MiniExplorerCoimbra.hpp

Revision:

33:814bcd7d3cfe

Child:

34:c208497dd079

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MiniExplorerCoimbra.hpp	Fri Jun 09 00:28:32 2017 +0000
@@ -0,0 +1,91 @@
+#ifndef MINIEXPLORERCOIMBRA_HPP
+#define MINIEXPLORERCOIMBRA_HPP
+
+#include "Map.hpp"
+#include "Sonar.hpp"
+#include "robot.h"
+#include "mbed.h"
+#include "math.h"
+
+	/*
+Robot coordinate system:      World coordinate system:
+      ^                 				^
+      |x                				|y
+   <--R                 				O-->
+    y                     				 x
+*/
+
+class MiniExplorerCoimbra {
+
+	public:
+
+	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 defaultX, float defaultY, float defaultTheta);
+
+	void setXYTheta(float x, float y, float theta);
+	
+	//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);
+	
+	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 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);
+	
+	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 update_force(int widthIndice, int heightIndice, float* forceX, float* forceY, float xRobotWorld, float yRobotWorld );
+	
+	//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);
+	
+	/*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);
+};
+
+#endif
+