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