Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

MiniExplorerCoimbra.hpp@36:b59d56d0b3b4, 2017-06-11 (annotated)

Committer:
Ludwigfr
Date:
Sun Jun 11 22:53:59 2017 +0000
Revision:
36:b59d56d0b3b4
Parent:
35:68f9edbb3cff
Child:
37:b4c45e43ad29
moved some fct to private

Who changed what in which revision?

UserRevisionLine numberNew 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 36:b59d56d0b3b4 58 void print_final_map_with_robot_position();
Ludwigfr 36:b59d56d0b3b4 59
Ludwigfr 36:b59d56d0b3b4 60 void print_final_map_with_robot_position_and_target(float targetXWorld, float targetYWorld);
Ludwigfr 36:b59d56d0b3b4 61
Ludwigfr 36:b59d56d0b3b4 62 void print_final_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