with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
MiniExplorerCoimbra.hpp@33:814bcd7d3cfe, 2017-06-09 (annotated)
- Committer:
- Ludwigfr
- Date:
- Fri Jun 09 00:28:32 2017 +0000
- Revision:
- 33:814bcd7d3cfe
- Child:
- 34:c208497dd079
version with class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Ludwigfr | 33:814bcd7d3cfe | 1 | #ifndef MINIEXPLORERCOIMBRA_HPP |
Ludwigfr | 33:814bcd7d3cfe | 2 | #define MINIEXPLORERCOIMBRA_HPP |
Ludwigfr | 33:814bcd7d3cfe | 3 | |
Ludwigfr | 33:814bcd7d3cfe | 4 | #include "Map.hpp" |
Ludwigfr | 33:814bcd7d3cfe | 5 | #include "Sonar.hpp" |
Ludwigfr | 33:814bcd7d3cfe | 6 | #include "robot.h" |
Ludwigfr | 33:814bcd7d3cfe | 7 | #include "mbed.h" |
Ludwigfr | 33:814bcd7d3cfe | 8 | #include "math.h" |
Ludwigfr | 33:814bcd7d3cfe | 9 | |
Ludwigfr | 33:814bcd7d3cfe | 10 | /* |
Ludwigfr | 33:814bcd7d3cfe | 11 | Robot coordinate system: World coordinate system: |
Ludwigfr | 33:814bcd7d3cfe | 12 | ^ ^ |
Ludwigfr | 33:814bcd7d3cfe | 13 | |x |y |
Ludwigfr | 33:814bcd7d3cfe | 14 | <--R O--> |
Ludwigfr | 33:814bcd7d3cfe | 15 | y x |
Ludwigfr | 33:814bcd7d3cfe | 16 | */ |
Ludwigfr | 33:814bcd7d3cfe | 17 | |
Ludwigfr | 33:814bcd7d3cfe | 18 | class MiniExplorerCoimbra { |
Ludwigfr | 33:814bcd7d3cfe | 19 | |
Ludwigfr | 33:814bcd7d3cfe | 20 | public: |
Ludwigfr | 33:814bcd7d3cfe | 21 | |
Ludwigfr | 33:814bcd7d3cfe | 22 | Map map; |
Ludwigfr | 33:814bcd7d3cfe | 23 | Sonar sonarLeft; |
Ludwigfr | 33:814bcd7d3cfe | 24 | Sonar sonarFront; |
Ludwigfr | 33:814bcd7d3cfe | 25 | Sonar sonarRight; |
Ludwigfr | 33:814bcd7d3cfe | 26 | float speed; |
Ludwigfr | 33:814bcd7d3cfe | 27 | float radiusWheels; |
Ludwigfr | 33:814bcd7d3cfe | 28 | float distanceWheels; |
Ludwigfr | 33:814bcd7d3cfe | 29 | float khro; |
Ludwigfr | 33:814bcd7d3cfe | 30 | float ka; |
Ludwigfr | 33:814bcd7d3cfe | 31 | float kb; |
Ludwigfr | 33:814bcd7d3cfe | 32 | float kv; |
Ludwigfr | 33:814bcd7d3cfe | 33 | float kh; |
Ludwigfr | 33:814bcd7d3cfe | 34 | |
Ludwigfr | 33:814bcd7d3cfe | 35 | float rangeForce; |
Ludwigfr | 33:814bcd7d3cfe | 36 | float attractionConstantForce; |
Ludwigfr | 33:814bcd7d3cfe | 37 | float repulsionConstantForce; |
Ludwigfr | 33:814bcd7d3cfe | 38 | |
Ludwigfr | 33:814bcd7d3cfe | 39 | MiniExplorerCoimbra(float defaultX, float defaultY, float defaultTheta); |
Ludwigfr | 33:814bcd7d3cfe | 40 | |
Ludwigfr | 33:814bcd7d3cfe | 41 | void setXYTheta(float x, float y, float theta); |
Ludwigfr | 33:814bcd7d3cfe | 42 | |
Ludwigfr | 33:814bcd7d3cfe | 43 | //generate a position randomly and makes the robot go there while updating the map |
Ludwigfr | 33:814bcd7d3cfe | 44 | void randomize_and_map(); |
Ludwigfr | 33:814bcd7d3cfe | 45 | |
Ludwigfr | 33:814bcd7d3cfe | 46 | //go the the given position while updating the map |
Ludwigfr | 33:814bcd7d3cfe | 47 | void go_to_point_with_angle(float targetX, float targetY, float targetAngle); |
Ludwigfr | 33:814bcd7d3cfe | 48 | |
Ludwigfr | 33:814bcd7d3cfe | 49 | void update_sonar_values(float leftMm,float frontMm,float rightMm); |
Ludwigfr | 33:814bcd7d3cfe | 50 | |
Ludwigfr | 33:814bcd7d3cfe | 51 | float get_converted_robot_X_into_world(); |
Ludwigfr | 33:814bcd7d3cfe | 52 | |
Ludwigfr | 33:814bcd7d3cfe | 53 | float get_converted_robot_Y_into_world(); |
Ludwigfr | 33:814bcd7d3cfe | 54 | |
Ludwigfr | 33:814bcd7d3cfe | 55 | void do_half_flip(); |
Ludwigfr | 33:814bcd7d3cfe | 56 | |
Ludwigfr | 33:814bcd7d3cfe | 57 | //Distance computation function |
Ludwigfr | 33:814bcd7d3cfe | 58 | float dist(float robotX, float robotY, float targetX, float targetY); |
Ludwigfr | 33:814bcd7d3cfe | 59 | |
Ludwigfr | 33:814bcd7d3cfe | 60 | float update_angular_speed_wheels_go_to_point_with_angle(float targetX, float targetY, float targetAngle, float dt); |
Ludwigfr | 33:814bcd7d3cfe | 61 | |
Ludwigfr | 33:814bcd7d3cfe | 62 | void try_to_reach_target(float targetXWorld,float targetYWorld); |
Ludwigfr | 33:814bcd7d3cfe | 63 | |
Ludwigfr | 33:814bcd7d3cfe | 64 | void vff(bool* reached, float targetXWorld, float targetYWorld); |
Ludwigfr | 33:814bcd7d3cfe | 65 | |
Ludwigfr | 33:814bcd7d3cfe | 66 | //compute the force on X and Y |
Ludwigfr | 33:814bcd7d3cfe | 67 | void compute_forceX_and_forceY(float* forceX, float* forceY, float targetXWorld, float targetYWorld); |
Ludwigfr | 33:814bcd7d3cfe | 68 | |
Ludwigfr | 33:814bcd7d3cfe | 69 | void update_force(int widthIndice, int heightIndice, float* forceX, float* forceY, float xRobotWorld, float yRobotWorld ); |
Ludwigfr | 33:814bcd7d3cfe | 70 | |
Ludwigfr | 33:814bcd7d3cfe | 71 | //robotX and robotY are from Odometria, calculate line_a, line_b and line_c |
Ludwigfr | 33:814bcd7d3cfe | 72 | void calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c); |
Ludwigfr | 33:814bcd7d3cfe | 73 | |
Ludwigfr | 33:814bcd7d3cfe | 74 | //currently line_c is not used |
Ludwigfr | 33:814bcd7d3cfe | 75 | void go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld); |
Ludwigfr | 33:814bcd7d3cfe | 76 | |
Ludwigfr | 33:814bcd7d3cfe | 77 | //return 1 if positiv, -1 if negativ |
Ludwigfr | 33:814bcd7d3cfe | 78 | float sign1(float value); |
Ludwigfr | 33:814bcd7d3cfe | 79 | |
Ludwigfr | 33:814bcd7d3cfe | 80 | //return 1 if positiv, 0 if negativ |
Ludwigfr | 33:814bcd7d3cfe | 81 | int sign2(float value); |
Ludwigfr | 33:814bcd7d3cfe | 82 | |
Ludwigfr | 33:814bcd7d3cfe | 83 | /*angleToTarget is obtained through atan2 so it s: |
Ludwigfr | 33:814bcd7d3cfe | 84 | < 0 if the angle is bettween PI and 2pi on a trigo circle |
Ludwigfr | 33:814bcd7d3cfe | 85 | > 0 if it is between 0 and PI |
Ludwigfr | 33:814bcd7d3cfe | 86 | */ |
Ludwigfr | 33:814bcd7d3cfe | 87 | void turn_to_target(float angleToTarget); |
Ludwigfr | 33:814bcd7d3cfe | 88 | }; |
Ludwigfr | 33:814bcd7d3cfe | 89 | |
Ludwigfr | 33:814bcd7d3cfe | 90 | #endif |
Ludwigfr | 33:814bcd7d3cfe | 91 |