roboticLab_withclass_3_July
Dependencies: ISR_Mini-explorer mbed
Fork of roboticLab_withclass by
MiniExplorerCoimbra.hpp
- Committer:
- geotsam
- Date:
- 2017-07-03
- Revision:
- 1:20f48907c726
- Parent:
- 0:9f7ee7ed13e4
File content as of revision 1:20f48907c726:
#ifndef MINIEXPLORERCOIMBRA_HPP #define MINIEXPLORERCOIMBRA_HPP #include "Map.hpp" #include "Sonar.hpp" #include<math.h> /* Robot coordinate system: World coordinate system: ^ ^ |x |y <--R O--> y x angles from pi/2 to 3pi/2->0 to pi angles from 0 to pi->0 to pi angles from pi/2 to -pi/2->0 to -pi angles from pi to 2pi-> -pi to 0 The idea is that for every command to the robot we use to world coodinate system */ class MiniExplorerCoimbra { public: float xWorld; float yWorld; float thetaWorld; Map map; Sonar sonarLeft; Sonar sonarFront; Sonar sonarRight; float speed; float radiusWheels; float distanceWheels; float khro; float ka; float kb; float kv; float kh; float kd; float k_linear; float k_angular; float rangeForce; float attractionConstantForce; float repulsionConstantForce; MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld); //generate a position randomly and makes the robot go there while updating the map void randomize_and_map(); //move of targetXWorld and targetYWorld ending in a targetAngleWorld void go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld); //move of targetXWorld and targetYWorld ending in a targetAngleWorld without checking the sonars void go_to_point_with_angle_first_lab(float targetXWorld, float targetYWorld, float targetAngleWorld); //use virtual force field void try_to_reach_target(float targetXWorld,float targetYWorld); void test_procedure_lab_4(); void go_to_point(float targetXWorld, float targetYWorld); void go_to_line_first_lab(float line_a, float line_b, float line_c); private: void myOdometria(); void setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld); float update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt); void update_sonar_values(float leftMm,float frontMm,float rightMm); void do_half_flip(); //Distance computation function float dist(float x1, float y1, float x2, float y2); float distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c); /*angleToTarget is obtained through atan2 so it s: < 0 if the angle is bettween PI and 2pi on a trigo circle > 0 if it is between 0 and PI */ void turn_to_target(float angleToTarget); void print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld); void vff(bool* reached, float targetXWorld, float targetYWorld); //compute the force on X and Y void compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld); void calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c); //currently line_c is not used void go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld); void update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ); //return 1 if positiv, -1 if negativ float sign1(float value); //return 1 if positiv, 0 if negativ int sign2(float value); void go_straight_line(float distanceCm); void procedure_lab_4(float xEstimatedK,float yEstimatedK, float thetaWorldEstimatedK, float distanceMoved, float angleMoved, float PreviousCovarianceOdometricPositionEstimate[3][3]); }; #endif