Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 5:dea05b8f30d0
- Parent:
- 4:8c56c3ba6e54
- Child:
- 6:afde4b08166b
diff -r 8c56c3ba6e54 -r dea05b8f30d0 main.cpp --- a/main.cpp Fri Mar 24 16:30:30 2017 +0000 +++ b/main.cpp Fri Mar 24 16:38:58 2017 +0000 @@ -17,7 +17,7 @@ float rho; //distance from target float beta; //float k_linear=10, k_angular=200; - float kRho=3, ka=8, kb=-2; + float kRho=1, ka=8, kb=-2; float linear, angular, angular_left, angular_right; float dt=0.5; float temp; @@ -46,8 +46,8 @@ 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; - //beta = -alpha-theta+target_angle; + //beta = -alpha-theta; + beta = -alpha-theta+target_angle; //Computing angle error and distance towards the target value rho += dt*(-kRho*cos(alpha)*rho);