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

autopilot modeling

parent 953925b2
......@@ -6,11 +6,10 @@ import de.monticore.lang.embeddedmontiarc.embeddedmontiarc._symboltable.PortSymb
import de.monticore.lang.math.math._ast.ASTAssignmentType;
import de.monticore.lang.monticar.generator.Variable;
import de.monticore.lang.monticar.generator.cpp.BluePrintCPP;
import de.monticore.lang.monticar.ts.MCTypeSymbol;
import de.monticore.lang.monticar.ts.references.MCASTTypeSymbolReference;
import de.monticore.lang.monticar.ts.references.MCTypeReference;
import de.monticore.lang.monticar.types2._ast.ASTType;
import de.se_rwth.commons.logging.Log;
import javax.sound.sampled.Port;
/**
* @author Sascha Schneiders
......@@ -91,7 +90,8 @@ public class PortConverter {
}
private static void addVariableProperties(PortSymbol portSymbol, Variable variable) {
if (portSymbol.getTypeReference().getReferencedSymbol() instanceof MCASTTypeSymbolReference) {
MCTypeReference<? extends MCTypeSymbol> typeRef = portSymbol.getTypeReference();
if (typeRef.existsReferencedSymbol() && typeRef.getReferencedSymbol() instanceof MCASTTypeSymbolReference) {
MCASTTypeSymbolReference typeSymbolReference = (MCASTTypeSymbolReference) portSymbol.getTypeReference().getReferencedSymbol();
ASTType astType = typeSymbolReference.getAstType();
if (astType instanceof ASTAssignmentType) {
......
......@@ -34,8 +34,8 @@ component BehaviorGeneration {
connect tp.trimmed_trajectory_x -> cmc.trimmed_trajectory_x;
connect tp.trimmed_trajectory_y -> cmc.trimmed_trajectory_y;
connect cdv.desired_direction_x -> desired_direction_x;
connect cdv.desired_direction_y -> desired_direction_y;
connect cdv.signedDistanceToTrajectory -> signedDistanceToTrajectory;
connect cdv.desiredVelocity -> desiredVelocity;
connect cmc.desired_direction_x -> desired_direction_x;
connect cmc.desired_direction_y -> desired_direction_y;
connect cmc.signedDistanceToTrajectory -> signedDistanceToTrajectory;
connect cmc.desiredVelocity -> desiredVelocity;
}
......@@ -15,7 +15,7 @@ component SignedAngleBetween {
angle = 0.0;
else
Q norm2 = sqrt( v2x*v2x + v2y*v2y );
if norm2 <= EPSILON
if (norm2 <= EPSILON)
angle = 0.0;
else
Q angle1 = -atan2(v1y, v1x);
......@@ -24,7 +24,7 @@ component SignedAngleBetween {
Q v2xr = v2x * c - v2y * s;
Q v2yr = v2x * s + v2y * c;
Q angle2 = -atan2(v2yr, v2xr);
if abs(abs(angle2) - M_PI) <= EPSILON
if (abs( abs(angle2) - M_PI ) <= EPSILON)
angle = -M_PI;
else
angle = angle2;
......
......@@ -18,7 +18,6 @@ component CalculateEngineAndBrakes {
connect currentVelocity -> pidParams.currentVelocity;
connect desiredVelocity -> pidParams.desiredVelocity;
connect 1.5 -> pidError.velocityDifferenceThresholdForErrorLeap;
connect currentVelocity -> pidError.currentVelocity;
connect desiredVelocity -> pidError.desiredVelocity;
......
......@@ -2,27 +2,23 @@ package de.rwth.armin.modeling.autopilot.motion;
component CalculatePidError {
port
in Q (0.5 m/s : 0.01 m/s : 5.0 m/s) velocityDifferenceThresholdForErrorLeap,
in Q (0.0 m/s : 0.01 m/s : oo) currentVelocity,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity,
out Q error;
implementation Math {
// de.rwth.ryndin.modelling.autopilot.component.motion.CalculatePidErrorImplementation
// TODO: Is the NaN Check necessary ?
if desiredVelocity < 0.001
// TODO convert into parameter when parameters start working (bug #28)
static Q V_THRESHOLD_FOR_ERROR_LEAP = 1.5;
if (desiredVelocity <= 0.01)
error = -100.0;
elseif (currentVelocity - desiredVelocity) > velocityDifferenceThresholdForErrorLeap
elseif ((currentVelocity - desiredVelocity) > V_THRESHOLD_FOR_ERROR_LEAP)
// dv is treated as zero in order to increase error and force vehicle to decelerate better
error = -currentVelocity;
elseif (desiredVelocity - currentVelocity) > velocityDifferenceThresholdForErrorLeap
elseif ((desiredVelocity - currentVelocity) > V_THRESHOLD_FOR_ERROR_LEAP)
// increase error to force vehicle to accelerate more rapidly
error = desiredVelocity;
else
error = desiredVelocity - currentVelocity ;
error = desiredVelocity - currentVelocity;
end
}
}
package de.rwth.armin.modeling.autopilot.motion;
stream CalculatePidError for CalculatePidError {
velocityDifferenceThresholdForErrorLeap: 1.5 tick 1.5 tick 1.5 tick 1.5 tick 1.5 tick 1.5 ;
currentVelocity: 1.0 tick 1.0 tick 1.0 tick 10.0 tick 8.0 tick 10.0 ;
desiredVelocity: 1.0 tick 0.0 tick 5.0 tick 3.0 tick 9.1 tick 9.5 ;
error: 0.0 +/- 0.01 tick -100.0 +/- 0.01 tick 5.0 +/- 0.01 tick -10.0 +/- 0.01 tick 1.1 +/- 0.01 tick -0.5 +/- 0.01 ;
......
......@@ -10,7 +10,6 @@ component CalculatePidParameters {
out Q paramDecayCoefficient;
implementation Math {
// de.rwth.ryndin.modelling.autopilot.component.motion.CalculatePidParametersImplementation
paramP = 0.0;
paramI = 0.0;
paramD = 0.0;
......@@ -19,13 +18,12 @@ component CalculatePidParameters {
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;
if currentVelocity < desiredVelocity
static Q v = currentVelocity;
elseif desiredVelocity < currentVelocity
static Q v = desiredVelocity;
Q v = currentVelocity;
if desiredVelocity < currentVelocity
v = desiredVelocity;
end
if v < MIN_VELOCITY
paramP = P_FOR_MIN_VELOCITY;
......@@ -33,7 +31,7 @@ component CalculatePidParameters {
paramP = P_FOR_MAX_VELOCITY;
else
static Q diff = v - MIN_VELOCITY;
Q diff = v - MIN_VELOCITY;
paramP = P_FOR_MIN_VELOCITY + (P_FOR_MAX_VELOCITY - P_FOR_MIN_VELOCITY) * diff / (MAX_VELOCITY - MIN_VELOCITY);
end
}
......
......@@ -8,10 +8,8 @@ component DecideEngineOrBrakes {
out Q brakes;
implementation Math {
// de.rwth.ryndin.modelling.autopilot.component.motion.DecideEngineOrBrakesImplementation
engine = 0.0;
brakes = 0.0;
if error > 0.0
engine = controlSignal;
else
......
......@@ -30,7 +30,6 @@ component MotionPlanning {
connect desiredDirectionX -> keepDirection.desiredDirectionX;
connect desiredDirectionY -> keepDirection.desiredDirectionY;
connect keepDirection.steeringAngle -> sum1.t1;
connect 0.785 -> sac.maxSteeringAngle;
connect signedDistanceToTrajectory -> sac.signedDistanceToTrajectory;
connect sac.steeringAngleCorrection -> sum1.t2;
......
......@@ -2,17 +2,33 @@ package de.rwth.armin.modeling.autopilot.motion;
component SteeringAngleCorrection {
port
in Q maxSteeringAngle,
in Q signedDistanceToTrajectory,
out Q 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;
B cond1 = abs(signedDistanceToTrajectory) <= EPSILON;
if cond1
steeringAngleCorrection = 0.0;
else
steeringAngleCorrection = 0.0;
// 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
steeringAngleCorrection = Y1;
elseif dist > X2
steeringAngleCorrection = Y2;
else
steeringAngleCorrection = Y1 * exp(-COEF_K * (dist - X1));
end
// take sign into account
if signedDistanceToTrajectory < 0.0
steeringAngleCorrection *= -1.0;
end
end
}
}
package de.rwth.armin.modeling.autopilot.motion;
stream SteeringAngleCorrection for SteeringAngleCorrection {
maxSteeringAngle: 0.785 tick 0.785 tick 0.785 tick 0.785 tick 0.785 ;
signedDistanceToTrajectory: 0.0 tick 0.05 tick 2.5 tick 5.0 tick 15.5 ;
steeringAngleCorrection: 0.0 +/- 0.001 tick 0.007 +/- 0.001 tick 0.017 +/- 0.001 tick 0.039 +/- 0.001 tick 0.039 +/- 0.001 ;
}
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