with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
MiniExplorerCoimbra.hpp@37:b4c45e43ad29, 2017-06-11 (annotated)
- Committer:
- Ludwigfr
- Date:
- Sun Jun 11 23:22:28 2017 +0000
- Revision:
- 37:b4c45e43ad29
- Parent:
- 36:b59d56d0b3b4
- Child:
- 39:890439b495e3
changed a few thing, notably tried to simplify the mapping funciton
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 | 34:c208497dd079 | 6 | #include<math.h> |
Ludwigfr | 34:c208497dd079 | 7 | |
Ludwigfr | 33:814bcd7d3cfe | 8 | |
Ludwigfr | 35:68f9edbb3cff | 9 | |
Ludwigfr | 35:68f9edbb3cff | 10 | |
Ludwigfr | 33:814bcd7d3cfe | 11 | /* |
Ludwigfr | 35:68f9edbb3cff | 12 | Robot coordinate system: World coordinate system: |
Ludwigfr | 35:68f9edbb3cff | 13 | ^ ^ |
Ludwigfr | 35:68f9edbb3cff | 14 | |x |y |
Ludwigfr | 35:68f9edbb3cff | 15 | <--R O--> |
Ludwigfr | 35:68f9edbb3cff | 16 | y x |
Ludwigfr | 35:68f9edbb3cff | 17 | |
Ludwigfr | 35:68f9edbb3cff | 18 | angles from pi/2 to 3pi/2->0 to pi angles from 0 to pi->0 to pi |
Ludwigfr | 35:68f9edbb3cff | 19 | angles from pi/2 to -pi/2->0 to -pi angles from pi to 2pi-> -pi to 0 |
Ludwigfr | 35:68f9edbb3cff | 20 | |
Ludwigfr | 35:68f9edbb3cff | 21 | The idea is that for every command to the robot we use to world coodinate system |
Ludwigfr | 33:814bcd7d3cfe | 22 | */ |
Ludwigfr | 33:814bcd7d3cfe | 23 | |
Ludwigfr | 33:814bcd7d3cfe | 24 | class MiniExplorerCoimbra { |
Ludwigfr | 34:c208497dd079 | 25 | |
Ludwigfr | 33:814bcd7d3cfe | 26 | public: |
Ludwigfr | 35:68f9edbb3cff | 27 | float xWorld; |
Ludwigfr | 35:68f9edbb3cff | 28 | float yWorld; |
Ludwigfr | 35:68f9edbb3cff | 29 | float thetaWorld; |
Ludwigfr | 33:814bcd7d3cfe | 30 | Map map; |
Ludwigfr | 33:814bcd7d3cfe | 31 | Sonar sonarLeft; |
Ludwigfr | 33:814bcd7d3cfe | 32 | Sonar sonarFront; |
Ludwigfr | 33:814bcd7d3cfe | 33 | Sonar sonarRight; |
Ludwigfr | 33:814bcd7d3cfe | 34 | float speed; |
Ludwigfr | 33:814bcd7d3cfe | 35 | float radiusWheels; |
Ludwigfr | 33:814bcd7d3cfe | 36 | float distanceWheels; |
Ludwigfr | 33:814bcd7d3cfe | 37 | float khro; |
Ludwigfr | 33:814bcd7d3cfe | 38 | float ka; |
Ludwigfr | 33:814bcd7d3cfe | 39 | float kb; |
Ludwigfr | 33:814bcd7d3cfe | 40 | float kv; |
Ludwigfr | 33:814bcd7d3cfe | 41 | float kh; |
Ludwigfr | 33:814bcd7d3cfe | 42 | |
Ludwigfr | 33:814bcd7d3cfe | 43 | float rangeForce; |
Ludwigfr | 33:814bcd7d3cfe | 44 | float attractionConstantForce; |
Ludwigfr | 33:814bcd7d3cfe | 45 | float repulsionConstantForce; |
Ludwigfr | 33:814bcd7d3cfe | 46 | |
Ludwigfr | 35:68f9edbb3cff | 47 | MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld); |
Ludwigfr | 33:814bcd7d3cfe | 48 | |
Ludwigfr | 33:814bcd7d3cfe | 49 | //generate a position randomly and makes the robot go there while updating the map |
Ludwigfr | 33:814bcd7d3cfe | 50 | void randomize_and_map(); |
Ludwigfr | 33:814bcd7d3cfe | 51 | |
Ludwigfr | 35:68f9edbb3cff | 52 | //move of targetXWorld and targetYWorld ending in a targetAngleWorld |
Ludwigfr | 35:68f9edbb3cff | 53 | void go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld); |
Ludwigfr | 33:814bcd7d3cfe | 54 | |
Ludwigfr | 35:68f9edbb3cff | 55 | //use virtual force field |
Ludwigfr | 33:814bcd7d3cfe | 56 | void try_to_reach_target(float targetXWorld,float targetYWorld); |
Ludwigfr | 36:b59d56d0b3b4 | 57 | |
Ludwigfr | 37:b4c45e43ad29 | 58 | void print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld); |
Ludwigfr | 36:b59d56d0b3b4 | 59 | |
Ludwigfr | 37:b4c45e43ad29 | 60 | void print_map_with_robot_position(); |
Ludwigfr | 36:b59d56d0b3b4 | 61 | |
Ludwigfr | 37:b4c45e43ad29 | 62 | void print_map(); |
Ludwigfr | 36:b59d56d0b3b4 | 63 | |
Ludwigfr | 36:b59d56d0b3b4 | 64 | private: |
Ludwigfr | 36:b59d56d0b3b4 | 65 | |
Ludwigfr | 36:b59d56d0b3b4 | 66 | void setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld); |
Ludwigfr | 36:b59d56d0b3b4 | 67 | |
Ludwigfr | 36:b59d56d0b3b4 | 68 | void myOdometria(); |
Ludwigfr | 36:b59d56d0b3b4 | 69 | |
Ludwigfr | 36:b59d56d0b3b4 | 70 | void update_sonar_values(float leftMm,float frontMm,float rightMm); |
Ludwigfr | 33:814bcd7d3cfe | 71 | |
Ludwigfr | 33:814bcd7d3cfe | 72 | void vff(bool* reached, float targetXWorld, float targetYWorld); |
Ludwigfr | 33:814bcd7d3cfe | 73 | |
Ludwigfr | 33:814bcd7d3cfe | 74 | //compute the force on X and Y |
Ludwigfr | 35:68f9edbb3cff | 75 | void compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld); |
Ludwigfr | 33:814bcd7d3cfe | 76 | |
Ludwigfr | 35:68f9edbb3cff | 77 | void update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ); |
Ludwigfr | 33:814bcd7d3cfe | 78 | |
Ludwigfr | 33:814bcd7d3cfe | 79 | void calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c); |
Ludwigfr | 33:814bcd7d3cfe | 80 | |
Ludwigfr | 33:814bcd7d3cfe | 81 | //currently line_c is not used |
Ludwigfr | 33:814bcd7d3cfe | 82 | void go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld); |
Ludwigfr | 33:814bcd7d3cfe | 83 | |
Ludwigfr | 35:68f9edbb3cff | 84 | float update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt); |
Ludwigfr | 33:814bcd7d3cfe | 85 | |
Ludwigfr | 33:814bcd7d3cfe | 86 | /*angleToTarget is obtained through atan2 so it s: |
Ludwigfr | 33:814bcd7d3cfe | 87 | < 0 if the angle is bettween PI and 2pi on a trigo circle |
Ludwigfr | 33:814bcd7d3cfe | 88 | > 0 if it is between 0 and PI |
Ludwigfr | 33:814bcd7d3cfe | 89 | */ |
Ludwigfr | 33:814bcd7d3cfe | 90 | void turn_to_target(float angleToTarget); |
Ludwigfr | 34:c208497dd079 | 91 | |
Ludwigfr | 35:68f9edbb3cff | 92 | void do_half_flip(); |
Ludwigfr | 35:68f9edbb3cff | 93 | |
Ludwigfr | 36:b59d56d0b3b4 | 94 | //Distance computation function |
Ludwigfr | 36:b59d56d0b3b4 | 95 | float dist(float x1, float y1, float x2, float y2); |
Ludwigfr | 34:c208497dd079 | 96 | |
Ludwigfr | 35:68f9edbb3cff | 97 | //return 1 if positiv, -1 if negativ |
Ludwigfr | 35:68f9edbb3cff | 98 | float sign1(float value); |
Ludwigfr | 34:c208497dd079 | 99 | |
Ludwigfr | 35:68f9edbb3cff | 100 | //return 1 if positiv, 0 if negativ |
Ludwigfr | 35:68f9edbb3cff | 101 | int sign2(float value); |
Ludwigfr | 33:814bcd7d3cfe | 102 | }; |
Ludwigfr | 33:814bcd7d3cfe | 103 | |
Ludwigfr | 34:c208497dd079 | 104 | |
Ludwigfr | 33:814bcd7d3cfe | 105 | #endif |
Ludwigfr | 33:814bcd7d3cfe | 106 |