Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

MiniExplorerCoimbra.hpp@39:890439b495e3, 2017-06-16 (annotated)

Committer:
Ludwigfr
Date:
Fri Jun 16 10:40:53 2017 +0000
Revision:
39:890439b495e3
Parent:
37:b4c45e43ad29 
last version with the 4th lab;

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