with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 4:8c56c3ba6e54
- Parent:
- 3:1e0f4cb93eda
- Child:
- 5:dea05b8f30d0
--- a/main.cpp Fri Mar 24 15:46:46 2017 +0000 +++ b/main.cpp Fri Mar 24 16:30:30 2017 +0000 @@ -2,8 +2,10 @@ #include "robot.h" // Initializes the robot. This include should be used in all main.cpp! #include "math.h" + float dist(float robot_x, float robot_y, float target_x, float target_y); +//Timeout time; int main(){ initRobot(); //Initializing the robot pc.baud(9600); // baud for the pc communication @@ -11,11 +13,11 @@ //Target example x,y values float target_x=46.8, target_y=78.6, target_angle=0; - float a; //angle error - float d; //distance from target + float alpha; //angle error + float rho; //distance from target float beta; //float k_linear=10, k_angular=200; - float kr=3, ka=8, kb=-2; + float kRho=3, ka=8, kb=-2; float linear, angular, angular_left, angular_right; float dt=0.5; float temp; @@ -30,10 +32,10 @@ X=0; Y=0; - a = atan2((target_y-Y),(target_x-X))-theta; - a = atan(sin(a)/cos(a)); - d = dist(X, Y, target_x, target_y); - beta = -a-theta+target_angle; + 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; do { pc.printf("\n\n\r entered while"); @@ -41,33 +43,34 @@ //Updating X,Y and theta with the odometry values Odometria(); - a = atan2((target_y-Y),(target_x-X))-theta; - a = atan(sin(a)/cos(a)); - d = dist(X, Y, target_x, target_y); - beta = -a-theta+target_angle; + 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; //Computing angle error and distance towards the target value - d += dt*(-kr*cos(a)*d); - a = temp; - a += dt*(kr*sin(a)-ka*a-kb*b); - b += dt*(-kr*sin(temp)); - pc.printf("\n\r d=%f", d); + rho += dt*(-kRho*cos(alpha)*rho); + temp = alpha; + alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*b); + b += dt*(-kRho*sin(temp)); + pc.printf("\n\r rho=%f", rho); //Computing linear and angular velocities - if(a>=-1.5708 && a<=1.5708){ - linear=kr*d; - angular=ka*a+kb*beta; + if(alpha>=-1.5708 && alpha<=1.5708){ + linear=kRho*rho; + angular=ka*alpha+kb*beta; } else{ - linear=-kr*d; - angular=-ka*a-kb*beta; + 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 (d<25) { - speed = d*30; + if (rho<25) { + speed = rho*30; } //Normalize speed for motors @@ -87,16 +90,23 @@ rightMotor(1,angular_right); wait(dt); - } while(d>1); + } while(rho>1); //Stop at the end leftMotor(1,0); rightMotor(1,0); - pc.printf("\n\r %f -- arrived!", d); + 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)); -} \ No newline at end of file +} + +/*static double diffclock(clock_t clock1,clock_t clock2) +{ + double diffticks=clock1-clock2; + double diffms=(diffticks)/(CLOCKS_PER_SEC/1000); + return diffms; +}*/ \ No newline at end of file