with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
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 +