![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
All the lab works are here!
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 8:109314be5b68
- Parent:
- 7:c94070f9af78
- Child:
- 9:b7138acdf4ac
- Child:
- 11:e641aa08c92e
diff -r c94070f9af78 -r 109314be5b68 main.cpp --- a/main.cpp Fri Mar 24 18:02:57 2017 +0000 +++ b/main.cpp Mon Mar 27 16:20:29 2017 +0000 @@ -6,30 +6,32 @@ float dist(float robot_x, float robot_y, float target_x, float target_y); +int goToPointWithAngle(float target_x, float target_y, int theta); + +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 linear, angular, angular_left, angular_right; +float dt=0.5; +float temp; +float d2; + +//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.7; + //Timeout time; int main(){ initRobot(); //Initializing the robot pc.baud(9600); // baud for the pc communication - //Target example x,y values - float target_x=46.8, target_y=78.6, target_angle=1.7; - - 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 linear, angular, angular_left, angular_right; - float dt=0.5; - float temp; - float d2; - - //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 - //Resetting coordinates before moving theta=0; X=0; @@ -41,8 +43,23 @@ beta = -alpha-theta+target_angle; //beta = atan(sin(beta)/cos(beta)); - - do { + + goToPointWithAngle(target_x, target_y, target_angle); + + //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)); +} + +int goToPointWithAngle(float target_x, float target_y, int theta) { + do { pc.printf("\n\n\r entered while"); //Timer stuff @@ -57,7 +74,6 @@ alpha = atan(sin(alpha)/cos(alpha)); rho = dist(X, Y, target_x, target_y); d2 = rho; - //beta = -alpha-theta; beta = -alpha-theta+target_angle; //beta = atan(sin(beta)/cos(beta)); @@ -107,15 +123,6 @@ //Timer stuff t.stop(); } while(d2>1); - - //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)); + + return 0; } \ No newline at end of file