![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
test morning
Dependencies: ISR_Mini-explorer mbed
Fork of roboticLab_withclass_3_July by
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?
User | Revision | Line number | New 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 |