with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

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