with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 11:e641aa08c92e
- Parent:
- 8:109314be5b68
- Child:
- 12:3c0ca2350624
diff -r 109314be5b68 -r e641aa08c92e main.cpp --- a/main.cpp Mon Mar 27 16:20:29 2017 +0000 +++ b/main.cpp Mon Mar 27 16:34:43 2017 +0000 @@ -8,17 +8,23 @@ int goToPointWithAngle(float target_x, float target_y, int theta); +int updateSonarValues(); + float alpha; //angle error float rho; //distance from target float beta; -//float k_linear=10, k_angular=200; -//kb = -15 and ka = 30 tabom -float kRho=12, ka=30, kb=-13; +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; @@ -58,6 +64,14 @@ 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 goToPointWithAngle(float target_x, float target_y, int theta) { do { pc.printf("\n\n\r entered while"); @@ -67,6 +81,11 @@ t.reset(); t.start(); + updateSonarValues(); + if (leftMm < 100 || frontMm < 100 || rightMm < 100) { + tooClose = true; + } + //Updating X,Y and theta with the odometry values Odometria(); @@ -122,7 +141,7 @@ wait(0.2); //Timer stuff t.stop(); - } while(d2>1); + } while(d2>1 & !tooClose); return 0; } \ No newline at end of file