Mapping for TP2
Dependencies: ISR_Mini-explorer mbed
Fork of GoToPointWithAngle by
main.cpp.orig
- Committer:
- AurelienBernier
- Date:
- 2017-04-03
- Revision:
- 19:6a9062d54eb0
File content as of revision 19:6a9062d54eb0:
#include "mbed.h" #include "robot.h" // Initializes the robot. This include should be used in all main.cpp! #include "math.h" Timer t; float dist(float robot_x, float robot_y, float target_x, float target_y); int goToPointWithAngle(float target_x, float target_y, float target_angle); int randomizeAndMap(); int updateSonarValues(); int computeObstacle(); bool map[25][25]; float alpha; //angle error float rho; //distance from target float beta; float kRho=12, ka=30, kb=-13; //Kappa values float linear, angular, angular_left, angular_right; float dt=0.5; float temp; float d2; bool tooClose = false; int leftMm; int frontMm; int rightMm; //Diameter of a wheel and distance between the 2 float r=3.25, b=7.2; int speed=999; // Max speed at beggining of movement //Target example x,y values float target_x=46.8, target_y=78.6, target_angle=1.57; //Timeout time; int main(){ i2c1.frequency(100000); initRobot(); //Initializing the robot pc.baud(9600); // baud for the pc communication measure_always_on(); wait_ms(50); //Fill map for (int i = 0; i<25; i++) { for (int j = 0; j<25; j++) { map[i][j] = false; } } //Resetting coordinates before moving theta=0; X=0; Y=0; /*alpha = atan2((target_y-Y),(target_x-X))-theta; alpha = atan(sin(alpha)/cos(alpha)); rho = dist(X, Y, target_x, target_y); beta = -alpha-theta+target_angle;*/ for (int i = 0; i<10; i++) { randomizeAndMap(); } //Stop at the end leftMotor(1,0); rightMotor(1,0); pc.printf("\n\r %f -- arrived!", rho); } //Distance computation function float dist(float robot_x, float robot_y, float target_x, float target_y){ return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); } //Updates sonar values int updateSonarValues() { leftMm = get_distance_left_sensor(); frontMm = get_distance_front_sensor(); rightMm = get_distance_right_sensor(); return 0; } int randomizeAndMap() { target_x = (rand()%2500)/10;//for decimal precision target_y = (rand()%2500)/10; target_angle = ((rand()%31416)-15708)/1000; pc.printf("\n\r targ_X=%f", target_x); pc.printf("\n\r targ_Y=%f", target_y); pc.printf("\n\r targ_Angle=%f", target_angle); goToPointWithAngle(target_x, target_y, target_angle); return 0; } int computeObstacle() { //get the sensor values //compute the probabilistic shit of empty/non-empty in the direction of the sensor below 10 cm //update the map object with true where it's likely to be obstacled int xObstacle, yObstacle; if (leftMm < 10) { } else if (frontMm < 10) { } else if (rightMm < 10) { } map[xObstacle][yObstacle] = true; return 0; } int goToPointWithAngle(float target_x, float target_y, float target_angle) { do { pc.printf("\n\n\r entered while"); //Timer stuff dt = t.read(); t.reset(); t.start(); updateSonarValues(); if (leftMm < 100 || frontMm < 100 || rightMm < 100) { tooClose = true; } //Updating X,Y and theta with the odometry values Odometria(); alpha = atan2((target_y-Y),(target_x-X))-theta; alpha = atan(sin(alpha)/cos(alpha)); rho = dist(X, Y, target_x, target_y); d2 = rho; beta = -alpha-theta+target_angle; //Computing angle error and distance towards the target value rho += dt*(-kRho*cos(alpha)*rho); temp = alpha; alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta); beta += dt*(-kRho*sin(temp)); pc.printf("\n\r d2=%f", d2); pc.printf("\n\r dt=%f", dt); //Computing linear and angular velocities if(alpha>=-1.5708 && alpha<=1.5708){ linear=kRho*rho; angular=ka*alpha+kb*beta; } else{ linear=-kRho*rho; angular=-ka*alpha-kb*beta; } angular_left=(linear-0.5*b*angular)/r; angular_right=(linear+0.5*b*angular)/r; //Slowing down at the end for more precision if (d2<25) { speed = d2*30; } //Normalize speed for motors if(angular_left>angular_right) { angular_right=speed*angular_right/angular_left; angular_left=speed; } else { angular_left=speed*angular_left/angular_right; angular_right=speed; } pc.printf("\n\r X=%f", X); pc.printf("\n\r Y=%f", Y); pc.printf("\n\r leftMm=%f", leftMm); pc.printf("\n\r frontMm=%f", frontMm); pc.printf("\n\r rightMm=%f", rightMm); //Updating motor velocities leftMotor(1,angular_left); rightMotor(1,angular_right); wait(0.2); //Timer stuff t.stop(); } while(d2>1 & !tooClose); if (tooClose) { computeObstacle(); } return 0; }