Commit ba1da656 authored by Alexander Ryndin's avatar Alexander Ryndin
Browse files

autopilot modeling

parent cdb141be
......@@ -20,7 +20,7 @@ component FollowTrajectory {
// change to params later
static Q NUMBER_OF_INTERMEDIATE_INTERPOLATION_POINTS = 10.0;
static Q TURN_ANGLE_THRESHOLD = 10.0 * M_PI / 180.0; // 10 degrees
static Q EPSILON = 0.000001;
static Q EPSILON = 0.00001;
desiredDirectionX = 0.0;
desiredDirectionY = 0.0;
signedDistanceToTrajectory = 0.0;
......@@ -39,9 +39,9 @@ component FollowTrajectory {
Q v12x = p2x - p1x;
Q v12y = p2y - p1y;
Q v12_sqr_norm = v12x*v12x + v12y*v12y;
if (v12_sqr_norm > EPSILON)
Q v12_norm = sqrt(v12_sqr_norm);
if (v12_norm > EPSILON)
// https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
Q v12_norm = sqrt(v12_sqr_norm);
signedDistanceToTrajectory = -(v12y*currentPositionX - v12x*currentPositionY + p2x*p1y - p1x*p2y) / v12_norm;
end
// DESIRED DIRECTION
......@@ -86,10 +86,13 @@ component FollowTrajectory {
Q vect1y = pt2y - pt1y;
Q vect2x = pt3x - pt2x;
Q vect2y = pt3y - pt2y;
Q alpha = 0.0;
Q vect1_norm = sqrt( vect1x*vect1x + vect1y*vect1y );
Q vect2_norm = sqrt( vect2x*vect2x + vect2y*vect2y );
Q cos_alpha = (vect1x*vect2x + vect1y*vect2y) / vect1_norm / vect2_norm;
Q alpha = acos(cos_alpha);
if ((vect1_norm > EPSILON) && (vect2_norm > EPSILON))
Q cos_alpha = (vect1x*vect2x + vect1y*vect2y) / vect1_norm / vect2_norm;
alpha = acos(cos_alpha);
end
if (alpha > TURN_ANGLE_THRESHOLD)
is_go_on = 0;
else
......
......@@ -2,33 +2,32 @@ package de.rwth.armin.modeling.autopilot.common;
component SignedAngleBetween {
port
in Q v1x,
in Q v1y,
in Q v2x,
in Q v2y,
in Q v1x,
in Q v1y,
in Q v2x,
in Q v2y,
out Q angle;
implementation Math {
static Q EPSILON = 0.001;
angle = 0.0;
Q norm1 = sqrt( v1x*v1x + v1y*v1y );
if norm1 <= EPSILON
angle = 0.0;
else
Q norm2 = sqrt( v2x*v2x + v2y*v2y );
if (norm2 <= EPSILON)
angle = 0.0;
Q norm2 = sqrt( v2x*v2x + v2y*v2y );
if ( (norm1 > EPSILON) && (norm2 > EPSILON) )
Q angle1 = -atan2(v1y, v1x);
Q c = cos(angle1);
Q s = sin(angle1);
// v2 rotated
Q v2xr = v2x * c - v2y * s;
Q v2yr = v2x * s + v2y * c;
Q angle2 = -atan2(v2yr, v2xr);
Q abs1 = abs(angle2);
Q abs2 = abs(abs1 - M_PI);
if (abs2 <= EPSILON)
angle = -M_PI;
else
Q angle1 = -atan2(v1y, v1x);
Q c = cos(angle1);
Q s = sin(angle1);
Q v2xr = v2x * c - v2y * s;
Q v2yr = v2x * s + v2y * c;
Q angle2 = -atan2(v2yr, v2xr);
if (abs( abs(angle2) - M_PI ) <= EPSILON)
angle = -M_PI;
else
angle = angle2;
end
angle = angle2;
end
end
}
......
......@@ -2,34 +2,34 @@ package de.rwth.armin.modeling.autopilot.motion;
component CalculatePidParameters {
port
in Q currentVelocity,
in Q desiredVelocity,
out Q paramP,
out Q paramI,
out Q paramD,
out Q paramDecayCoefficient;
in Q currentVelocity,
in Q desiredVelocity,
out Q paramP,
out Q paramI,
out Q paramD,
out Q paramDecayCoefficient;
implementation Math {
paramP = 0.0;
paramI = 0.0;
paramD = 0.0;
paramDecayCoefficient = 0.0;
static Q MIN_VELOCITY = 0.5;
static Q MAX_VELOCITY = 13.0 - MIN_VELOCITY / 10.0;
static Q P_FOR_MIN_VELOCITY = 1.76703;
static Q P_FOR_MAX_VELOCITY = 3.29578;
paramP = 0.0;
paramI = 0.0;
paramD = 0.0;
paramDecayCoefficient = 0.0;
Q v = currentVelocity;
if desiredVelocity < currentVelocity
if (desiredVelocity < currentVelocity)
v = desiredVelocity;
end
if v < MIN_VELOCITY
if (v < MIN_VELOCITY)
paramP = P_FOR_MIN_VELOCITY;
elseif v > MAX_VELOCITY
elseif (v > MAX_VELOCITY)
paramP = P_FOR_MAX_VELOCITY;
else
Q diff = v - MIN_VELOCITY;
paramP = P_FOR_MIN_VELOCITY + (P_FOR_MAX_VELOCITY - P_FOR_MIN_VELOCITY) * diff / (MAX_VELOCITY - MIN_VELOCITY);
......
......@@ -10,7 +10,7 @@ component DecideEngineOrBrakes {
implementation Math {
engine = 0.0;
brakes = 0.0;
if error > 0.0
if (error > 0)
engine = controlSignal;
else
brakes = controlSignal;
......
......@@ -4,17 +4,18 @@ import de.rwth.armin.modeling.autopilot.common.SignedAngleBetween;
component KeepDirection {
port
in Q currentDirectionX,
in Q currentDirectionY,
in Q desiredDirectionX,
in Q desiredDirectionY,
out Q steeringAngle;
in Q currentDirectionX,
in Q currentDirectionY,
in Q desiredDirectionX,
in Q desiredDirectionY,
instance SignedAngleBetween sab;
out Q steeringAngle;
connect currentDirectionX -> sab.v1x;
connect currentDirectionY -> sab.v1y;
connect desiredDirectionX -> sab.v2x;
connect desiredDirectionY -> sab.v2y;
connect sab.angle -> steeringAngle;
instance SignedAngleBetween sab;
connect currentDirectionX -> sab.v1x;
connect currentDirectionY -> sab.v1y;
connect desiredDirectionX -> sab.v2x;
connect desiredDirectionY -> sab.v2y;
connect sab.angle -> steeringAngle;
}
......@@ -4,46 +4,46 @@ import de.rwth.armin.modeling.autopilot.common.*;
component MotionPlanning {
port
in Q currentDirectionX,
in Q currentDirectionY,
in Q desiredDirectionX,
in Q desiredDirectionY,
in Q signedDistanceToTrajectory,
in Q (0.0 m/s : 0.01 m/s : oo m/s) currentVelocity,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity,
out Q engine,
out Q steering,
out Q brakes;
instance KeepDirection keepDirection;
instance SteeringAngleCorrection sac;
instance Sum sum1;
instance CalculateEngineAndBrakes engineAndBrakes;
instance EnsureBoundsForSteeringAngle boundsSteering;
instance EnsureBoundsForBrakes boundsBrakes;
instance EnsureBoundsForEngine boundsEngine;
connect currentDirectionX -> keepDirection.currentDirectionX;
connect currentDirectionY -> keepDirection.currentDirectionY;
connect desiredDirectionX -> keepDirection.desiredDirectionX;
connect desiredDirectionY -> keepDirection.desiredDirectionY;
connect keepDirection.steeringAngle -> sum1.t1;
connect signedDistanceToTrajectory -> sac.signedDistanceToTrajectory;
connect sac.steeringAngleCorrection -> sum1.t2;
in Q currentDirectionX,
in Q currentDirectionY,
in Q desiredDirectionX,
in Q desiredDirectionY,
in Q signedDistanceToTrajectory,
in Q (0.0 m/s : 0.01 m/s : oo m/s) currentVelocity,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity,
out Q engine,
out Q steering,
out Q brakes;
instance KeepDirection keepDirection;
instance SteeringAngleCorrection sac;
instance Sum sum1;
instance CalculateEngineAndBrakes engineAndBrakes;
instance EnsureBoundsForSteeringAngle boundsSteering;
instance EnsureBoundsForBrakes boundsBrakes;
instance EnsureBoundsForEngine boundsEngine;
connect currentDirectionX -> keepDirection.currentDirectionX;
connect currentDirectionY -> keepDirection.currentDirectionY;
connect desiredDirectionX -> keepDirection.desiredDirectionX;
connect desiredDirectionY -> keepDirection.desiredDirectionY;
connect keepDirection.steeringAngle -> sum1.t1;
connect signedDistanceToTrajectory -> sac.signedDistanceToTrajectory;
connect sac.steeringAngleCorrection -> sum1.t2;
// brakes and engine
connect currentVelocity -> engineAndBrakes.currentVelocity;
connect desiredVelocity -> engineAndBrakes.desiredVelocity;
connect currentVelocity -> engineAndBrakes.currentVelocity;
connect desiredVelocity -> engineAndBrakes.desiredVelocity;
// compose output
connect engineAndBrakes.engine -> boundsEngine.input;
connect boundsEngine.output -> engine;
connect engineAndBrakes.engine -> boundsEngine.input;
connect boundsEngine.output -> engine;
connect sum1.result -> boundsSteering.input;
connect boundsSteering.output -> steering;
connect sum1.result -> boundsSteering.input;
connect boundsSteering.output -> steering;
connect engineAndBrakes.brakes -> boundsBrakes.input;
connect boundsBrakes.output -> brakes;
connect engineAndBrakes.brakes -> boundsBrakes.input;
connect boundsBrakes.output -> brakes;
}
......@@ -8,25 +8,27 @@ component SteeringAngleCorrection {
implementation Math {
// TODO convert into parameter when parameters start working (bug #28)
static Q MAX_STEERING_ANGLE = 0.785;
static Q EPSILON = 0.01;
static Q EPSILON = 0.05;
// exponent params
static Q X1 = 0.1;
static Q Y1 = 0.01 * MAX_STEERING_ANGLE;
static Q X2 = 5.0;
static Q Y2 = 0.05 * MAX_STEERING_ANGLE;
static Q COEF_K = log( (Y1 / Y2) ) / (X2 - X1);
Q dist = abs(signedDistanceToTrajectory);
steeringAngleCorrection = 0.0;
if dist > EPSILON
if dist < X1
Q dist = abs(signedDistanceToTrajectory);
if (dist > EPSILON)
if (dist < X1)
steeringAngleCorrection = Y1;
elseif dist > X2
elseif (dist > X2)
steeringAngleCorrection = Y2;
else
steeringAngleCorrection = Y1 * exp(-COEF_K * (dist - X1));
end
// take sign into account
if signedDistanceToTrajectory < 0.0
if (signedDistanceToTrajectory < 0)
steeringAngleCorrection *= -1.0;
end
end
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment