![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Mapping for TP2
Dependencies: ISR_Mini-explorer mbed
Fork of GoToPointWithAngle by
Diff: main.cpp.orig
- Revision:
- 19:6a9062d54eb0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp.orig Mon Apr 03 17:07:37 2017 +0000 @@ -0,0 +1,194 @@ +#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; +} \ No newline at end of file