Yo Fleyt / Mbed 2 deprecated roboticLab_withclass_3_July

test morning

Dependencies:   ISR_Mini-explorer mbed

Fork of roboticLab_withclass_3_July by Georgios Tsamis

MiniExplorerCoimbra.hpp@14:696187e74411, 2017-07-12 (annotated)

Committer:
Ludwigfr
Date:
Wed Jul 12 18:08:07 2017 +0000
Revision:
14:696187e74411
Parent:
10:d0109d7cbe7c 
lol

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 8:072a76960e27 6 #include "myMatrix.hpp"
Ludwigfr 0:9f7ee7ed13e4 7 #include<math.h>
Ludwigfr 0:9f7ee7ed13e4 8
Ludwigfr 0:9f7ee7ed13e4 9 /*
Ludwigfr 0:9f7ee7ed13e4 10 Robot coordinate system: World coordinate system:
Ludwigfr 0:9f7ee7ed13e4 11 ^ ^
Ludwigfr 0:9f7ee7ed13e4 12 |x |y
Ludwigfr 0:9f7ee7ed13e4 13 <--R O-->
Ludwigfr 0:9f7ee7ed13e4 14 y x
Ludwigfr 0:9f7ee7ed13e4 15
Ludwigfr 0:9f7ee7ed13e4 16 angles from pi/2 to 3pi/2->0 to pi angles from 0 to pi->0 to pi
Ludwigfr 0:9f7ee7ed13e4 17 angles from pi/2 to -pi/2->0 to -pi angles from pi to 2pi-> -pi to 0
Ludwigfr 0:9f7ee7ed13e4 18
Ludwigfr 0:9f7ee7ed13e4 19 The idea is that for every command to the robot we use to world coodinate system
Ludwigfr 2:11cd5173aa36 20
Ludwigfr 2:11cd5173aa36 21 The target are relative to the robot position, so if the robot is in 100,80
Ludwigfr 2:11cd5173aa36 22 and the target is -20,30
Ludwigfr 2:11cd5173aa36 23 the robot will go to 80,110
Ludwigfr 2:11cd5173aa36 24 TODO I m pretty sure that a target angle is not relative to the current robot angle
Ludwigfr 0:9f7ee7ed13e4 25 */
Ludwigfr 0:9f7ee7ed13e4 26
Ludwigfr 0:9f7ee7ed13e4 27 class MiniExplorerCoimbra {
Ludwigfr 0:9f7ee7ed13e4 28
Ludwigfr 0:9f7ee7ed13e4 29 public:
Ludwigfr 0:9f7ee7ed13e4 30 float xWorld;
Ludwigfr 0:9f7ee7ed13e4 31 float yWorld;
Ludwigfr 0:9f7ee7ed13e4 32 float thetaWorld;
Ludwigfr 0:9f7ee7ed13e4 33 Map map;
Ludwigfr 0:9f7ee7ed13e4 34 Sonar sonarLeft;
Ludwigfr 0:9f7ee7ed13e4 35 Sonar sonarFront;
Ludwigfr 0:9f7ee7ed13e4 36 Sonar sonarRight;
Ludwigfr 0:9f7ee7ed13e4 37 float speed;
Ludwigfr 0:9f7ee7ed13e4 38 float radiusWheels;
Ludwigfr 0:9f7ee7ed13e4 39 float distanceWheels;
Ludwigfr 0:9f7ee7ed13e4 40 float khro;
Ludwigfr 0:9f7ee7ed13e4 41 float ka;
Ludwigfr 0:9f7ee7ed13e4 42 float kb;
Ludwigfr 0:9f7ee7ed13e4 43 float kv;
Ludwigfr 0:9f7ee7ed13e4 44 float kh;
geotsam 1:20f48907c726 45 float kd;
geotsam 1:20f48907c726 46 float k_linear;
geotsam 1:20f48907c726 47 float k_angular;
Ludwigfr 0:9f7ee7ed13e4 48
Ludwigfr 0:9f7ee7ed13e4 49 float rangeForce;
Ludwigfr 0:9f7ee7ed13e4 50 float attractionConstantForce;
Ludwigfr 0:9f7ee7ed13e4 51 float repulsionConstantForce;
Ludwigfr 5:19f24c363418 52
Ludwigfr 9:1cc27f33d3e1 53 myMatrix covariancePositionEstimationK;
Ludwigfr 14:696187e74411 54 float D_cm;
Ludwigfr 14:696187e74411 55 float L_cm;
Ludwigfr 14:696187e74411 56
Ludwigfr 14:696187e74411 57 float myX_r;
Ludwigfr 14:696187e74411 58 float myY_r;
Ludwigfr 14:696187e74411 59 float mytheta_r;
Ludwigfr 14:696187e74411 60
Ludwigfr 14:696187e74411 61 float myX_r_correct;
Ludwigfr 14:696187e74411 62 float myY_r_correct;
Ludwigfr 14:696187e74411 63 float mytheta_r_correct;
Ludwigfr 14:696187e74411 64
Ludwigfr 14:696187e74411 65 long int ticks2d_Special;
Ludwigfr 14:696187e74411 66 long int ticks2e_Special;
Ludwigfr 0:9f7ee7ed13e4 67
Ludwigfr 2:11cd5173aa36 68 MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap);
Ludwigfr 2:11cd5173aa36 69
Ludwigfr 2:11cd5173aa36 70 void test_procedure_lab2(int nbIteration);
Ludwigfr 0:9f7ee7ed13e4 71
Ludwigfr 0:9f7ee7ed13e4 72 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 0:9f7ee7ed13e4 73 void randomize_and_map();
Ludwigfr 0:9f7ee7ed13e4 74
Ludwigfr 0:9f7ee7ed13e4 75 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
Ludwigfr 0:9f7ee7ed13e4 76 void go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld);
Ludwigfr 0:9f7ee7ed13e4 77
geotsam 1:20f48907c726 78 //move of targetXWorld and targetYWorld ending in a targetAngleWorld without checking the sonars
geotsam 1:20f48907c726 79 void go_to_point_with_angle_first_lab(float targetXWorld, float targetYWorld, float targetAngleWorld);
geotsam 1:20f48907c726 80
Ludwigfr 0:9f7ee7ed13e4 81 //use virtual force field
Ludwigfr 0:9f7ee7ed13e4 82 void try_to_reach_target(float targetXWorld,float targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 83
geotsam 1:20f48907c726 84 void go_to_point(float targetXWorld, float targetYWorld);
geotsam 1:20f48907c726 85
geotsam 1:20f48907c726 86 void go_to_line_first_lab(float line_a, float line_b, float line_c);
geotsam 1:20f48907c726 87
Ludwigfr 2:11cd5173aa36 88 void print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld);
Ludwigfr 2:11cd5173aa36 89
Ludwigfr 2:11cd5173aa36 90 void print_map_with_robot_position();
Ludwigfr 2:11cd5173aa36 91
Ludwigfr 2:11cd5173aa36 92 void test_sonars_and_map(int nbIteration);
Ludwigfr 2:11cd5173aa36 93
Ludwigfr 10:d0109d7cbe7c 94 void test_procedure_lab_4(float sizeX, float sizeY);
Ludwigfr 5:19f24c363418 95
Ludwigfr 5:19f24c363418 96 void go_straight_kalman(float targetXWorld, float targetYWorld, float targetAngleWorld);
Ludwigfr 5:19f24c363418 97
Ludwigfr 5:19f24c363418 98 void go_turn_kalman(float targetXWorld, float targetYWorld, float targetAngleWorld);
Ludwigfr 5:19f24c363418 99
Ludwigfr 10:d0109d7cbe7c 100 void go_to_point_kalman(float targetXWorld, float targetYWorld);
Ludwigfr 5:19f24c363418 101
Ludwigfr 14:696187e74411 102 void go_to_point_kalman2(float targetXWorld, float targetYWorld);
Ludwigfr 6:0e8db3a23486 103
Ludwigfr 6:0e8db3a23486 104 void test_prediction_sonar();
Ludwigfr 5:19f24c363418 105
Ludwigfr 14:696187e74411 106 void OdometriaKalmanFilter();
Ludwigfr 14:696187e74411 107
Ludwigfr 10:d0109d7cbe7c 108 void printMatrix(myMatrix mat1);
Ludwigfr 10:d0109d7cbe7c 109
Ludwigfr 14:696187e74411 110 void myOdometriaBis();
Ludwigfr 14:696187e74411 111
Ludwigfr 14:696187e74411 112 void testOdometria(float *D_Special,float *L_Special);
Ludwigfr 0:9f7ee7ed13e4 113
Ludwigfr 0:9f7ee7ed13e4 114 void myOdometria();
Ludwigfr 0:9f7ee7ed13e4 115
Ludwigfr 0:9f7ee7ed13e4 116 void setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld);
Ludwigfr 0:9f7ee7ed13e4 117
Ludwigfr 14:696187e74411 118 private:
Ludwigfr 14:696187e74411 119
Ludwigfr 0:9f7ee7ed13e4 120 float update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt);
Ludwigfr 0:9f7ee7ed13e4 121
Ludwigfr 0:9f7ee7ed13e4 122 void update_sonar_values(float leftMm,float frontMm,float rightMm);
Ludwigfr 0:9f7ee7ed13e4 123
Ludwigfr 0:9f7ee7ed13e4 124 void do_half_flip();
Ludwigfr 0:9f7ee7ed13e4 125
Ludwigfr 0:9f7ee7ed13e4 126 //Distance computation function
Ludwigfr 0:9f7ee7ed13e4 127 float dist(float x1, float y1, float x2, float y2);
Ludwigfr 0:9f7ee7ed13e4 128
geotsam 1:20f48907c726 129 float distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c);
geotsam 1:20f48907c726 130
Ludwigfr 0:9f7ee7ed13e4 131 /*angleToTarget is obtained through atan2 so it s:
Ludwigfr 0:9f7ee7ed13e4 132 < 0 if the angle is bettween PI and 2pi on a trigo circle
Ludwigfr 0:9f7ee7ed13e4 133 > 0 if it is between 0 and PI
Ludwigfr 0:9f7ee7ed13e4 134 */
Ludwigfr 0:9f7ee7ed13e4 135 void turn_to_target(float angleToTarget);
Ludwigfr 0:9f7ee7ed13e4 136
Ludwigfr 0:9f7ee7ed13e4 137 void vff(bool* reached, float targetXWorld, float targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 138
Ludwigfr 0:9f7ee7ed13e4 139 //compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 140 void compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 141
Ludwigfr 0:9f7ee7ed13e4 142 void calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c);
Ludwigfr 0:9f7ee7ed13e4 143
Ludwigfr 0:9f7ee7ed13e4 144 //currently line_c is not used
Ludwigfr 0:9f7ee7ed13e4 145 void go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 146
Ludwigfr 0:9f7ee7ed13e4 147 void update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY );
Ludwigfr 0:9f7ee7ed13e4 148
Ludwigfr 0:9f7ee7ed13e4 149 //return 1 if positiv, -1 if negativ
Ludwigfr 0:9f7ee7ed13e4 150 float sign1(float value);
Ludwigfr 0:9f7ee7ed13e4 151
Ludwigfr 0:9f7ee7ed13e4 152 //return 1 if positiv, 0 if negativ
Ludwigfr 0:9f7ee7ed13e4 153 int sign2(float value);
Ludwigfr 0:9f7ee7ed13e4 154
Ludwigfr 0:9f7ee7ed13e4 155 void go_straight_line(float distanceCm);
Ludwigfr 0:9f7ee7ed13e4 156
Ludwigfr 0:9f7ee7ed13e4 157 void procedure_lab_4(float xEstimatedK,float yEstimatedK, float thetaWorldEstimatedK, float distanceMoved, float angleMoved, float PreviousCovarianceOdometricPositionEstimate[3][3]);
Ludwigfr 0:9f7ee7ed13e4 158
Ludwigfr 0:9f7ee7ed13e4 159 };
Ludwigfr 0:9f7ee7ed13e4 160
Ludwigfr 0:9f7ee7ed13e4 161
Ludwigfr 0:9f7ee7ed13e4 162 #endif
Ludwigfr 0:9f7ee7ed13e4 163