Yo Fleyt / Mbed 2 deprecated roboticLab_withclass

lab robotic coimbra

Dependencies:   ISR_Mini-explorer mbed

MiniExplorerCoimbra.hpp@0:9f7ee7ed13e4, 2017-06-26 (annotated)

Committer:
Ludwigfr
Date:
Mon Jun 26 12:05:20 2017 +0000
Revision:
0:9f7ee7ed13e4
this version should work, though it would be nice to test all the lab demo; there's also some code on the 4th lab at the end of MiniExplorerCoimbra.cpp

Who changed what in which revision?

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