Commit db0053cd authored by Alex Ryndin's avatar Alex Ryndin
Browse files

corrected if syntax in EMAM

parent 79d41326
......@@ -14,17 +14,15 @@ port
static Q totalTime = 0.0;
totalTime = totalTime + timeInc;
if totalTime > 100 {
if totalTime > 100
engineVal = 0.0;
breakesVal = 2.5;
} elseif diff > 0 {
elseif diff > 0
engineVal = 2.5;
breakesVal = 0.0;
}
else {
else
engineVal = 0.0;
breakesVal = 2.5;
}
end
}
......
......@@ -26,7 +26,7 @@ component CalculateMotionCommands {
desired_direction_y = 0.0;
signedDistanceToTrajectory = 0.0;
desiredVelocity = 0.0;
if trimmed_trajectory_length > 0 {
if trimmed_trajectory_length > 0
// assumption: 1st point of trajectory is the projection of current vehicle's position on trajectory
Q p1x = trimmed_trajectory_x(1,1);
Q p1y = trimmed_trajectory_y(1,1);
......@@ -34,12 +34,12 @@ component CalculateMotionCommands {
Q is_vehicle_went_off_trajectory = dist_to_trajectory > TOO_FAR_FROM_TRAJECTORY_THRESHOLD;
Q is_only_one_position_specified = trimmed_trajectory_length == 1;
Q is_drive_to_first_position = is_vehicle_went_off_trajectory || is_only_one_position_specified;
if is_drive_to_first_position {
if is_drive_to_first_position
desired_direction_x = p1x - current_position_x;
desired_direction_y = p1y - current_position_y;
signedDistanceToTrajectory = 0.0;
desiredVelocity = 0.0; // TODO calc. as function of dist_to_trajectory
} else {
else
// vehicle is on track AND there is at least two points in trajectory
// TODO calc. signedDistanceToTrajectory
// using current position and first trajectory segment
......@@ -52,7 +52,7 @@ component CalculateMotionCommands {
Q interp_p2y = trimmed_trajectory_y(1,1);
Q interp_p3x = trimmed_trajectory_x(1,2);
Q interp_p3y = trimmed_trajectory_y(1,2);
if trimmed_trajectory_length > 2 {
if trimmed_trajectory_length > 2
// if there is at least 3 points
// then do not use the projection point
// but use 2nd and 3rd trajectory points instead
......@@ -60,7 +60,7 @@ component CalculateMotionCommands {
Q interp_p2y = trimmed_trajectory_y(1,2);
Q interp_p3x = trimmed_trajectory_x(1,3);
Q interp_p3y = trimmed_trajectory_y(1,3);
} end
end
// Bezier interpolation (only the 1st intermediate point is taken)
// de.rwth.ryndin.modelling.autopilot.geometry.Interpolation
Q t = 1.0 / (NUMBER_OF_INTERMEDIATE_INTERPOLATED_POINTS+1.0);
......@@ -80,7 +80,7 @@ component CalculateMotionCommands {
// example:
// de.rwth.ryndin.modelling.autopilot.component.behavior.Distance2VelocityFunction
// or use something similar
} end
} end
end
end
}
}
......@@ -20,11 +20,11 @@ component TrimPath {
implementation Math {
trimmed_trajectory_length = 0;
if planned_trajectory_length == 1 {
if planned_trajectory_length == 1
trimmed_trajectory_length = 1;
trimmed_trajectory_x(1,1) = planned_trajectory_x(1,1);
trimmed_trajectory_y(1,1) = planned_trajectory_y(1,1);
} elseif planned_trajectory_length > 1 {
elseif planned_trajectory_length > 1
Q closestSegmentIndex = -1;
Q closestSegmentDistance = -1;
//Q last_segment_index = planned_trajectory_length-1;
......@@ -43,26 +43,26 @@ component TrimPath {
Q projection_y = p1y + k * v12y;
//B cond1 = (p1x-projection_x) * (p2x-projection_x) <= 0 && (p1y-projection_y) * (p2y-projection_y) <= 0;
Q cond1 = (p1x-projection_x) * (p2x-projection_x) <= 0 && (p1y-projection_y) * (p2y-projection_y) <= 0;
if cond1 {
if cond1
Q d_proj = sqrt( (current_position_x-projection_x)^2 + (current_position_y-projection_y)^2 );
if closestSegmentDistance < 0 || d_proj < closestSegmentDistance {
if closestSegmentDistance < 0 || d_proj < closestSegmentDistance
closestSegmentIndex = i;
closestSegmentDistance = d_proj;
trimmed_trajectory_x(1,1) = projection_x;
trimmed_trajectory_y(1,1) = projection_y;
} end
} else {
end
else
Q d1 = sqrt( (current_position_x-p1x)^2 + (current_position_y-p1y)^2 );
Q d2 = sqrt( (current_position_x-p2x)^2 + (current_position_y-p2y)^2 );
Q d_min = min(d1,d2);
if closestSegmentDistance < 0 || d_min < closestSegmentDistance {
if closestSegmentDistance < 0 || d_min < closestSegmentDistance
closestSegmentIndex = i;
closestSegmentDistance = d_min;
trimmed_trajectory_x(1,1) = projection_x;
trimmed_trajectory_y(1,1) = projection_y;
} end
} end
end
end
end
} end
end
}
}
......@@ -8,12 +8,12 @@ component EnsureBounds {
out Q output;
implementation Math {
if input < lowerBound {
if input < lowerBound
output = lowerBound;
} elseif input > upperBound {
elseif input > upperBound
output = upperBound;
} else {
else
output = input;
} end
end
}
}
......@@ -15,9 +15,9 @@ component PID {
static Q acc = 0.0;
acc = paramDecayCoefficient * acc + error;
Q drv = 0;
if isPrevErrorSpecified {
if isPrevErrorSpecified
drv = error - prevError;
} end
end
control = paramP * error + paramI * acc + paramD * drv;
prevError = error;
isPrevErrorSpecified = true;
......
......@@ -11,25 +11,25 @@ component SignedAngleBetween {
implementation Math {
static Q EPSILON = 0.001;
Q norm1 = sqrt( v1x*v1x + v1y*v1y );
if norm1 <= EPSILON {
if norm1 <= EPSILON
angle = 0.0;
} else {
else
Q norm2 = sqrt( v2x*v2x + v2y*v2y );
if norm2 <= EPSILON {
if norm2 <= EPSILON
angle = 0.0;
} else {
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 {
if abs(abs(angle2) - M_PI) <= EPSILON
angle = -M_PI;
} else {
else
angle = angle2;
} end
} end
} end
end
end
end
}
}
......@@ -11,22 +11,17 @@ component CalculatePidError {
// de.rwth.ryndin.modelling.autopilot.component.motion.CalculatePidErrorImplementation
// TODO: Is the NaN Check necessary ?
if desiredVelocity < 0.001 {
if desiredVelocity < 0.001
error = -100.0;
}
elseif (currentVelocity - desiredVelocity) > velocityDifferenceThresholdForErrorLeap {
elseif (currentVelocity - desiredVelocity) > velocityDifferenceThresholdForErrorLeap
// 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) > velocityDifferenceThresholdForErrorLeap
// increase error to force vehicle to accelerate more rapidly
error = desiredVelocity;
}
else {
else
error = desiredVelocity - currentVelocity ;
}
end
}
......
......@@ -20,27 +20,21 @@ component CalculatePidParameters {
static Q P_FOR_MIN_VELOCITY = 1.76703;
static Q P_FOR_MAX_VELOCITY = 3.29578;
if currentVelocity < desiredVelocity {
if currentVelocity < desiredVelocity
static Q v = currentVelocity;
}
elseif desiredVelocity < currentVelocity {
elseif desiredVelocity < currentVelocity
static Q 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{
else
static Q diff = v - MIN_VELOCITY;
paramP = P_FOR_MIN_VELOCITY + (P_FOR_MAX_VELOCITY - P_FOR_MIN_VELOCITY) * diff / (MAX_VELOCITY - MIN_VELOCITY);
}
end
}
}
......@@ -12,12 +12,10 @@ component DecideEngineOrBrakes {
engine = 0.0;
brakes = 0.0;
if error > 0.0{
if error > 0.0
engine = controlSignal;
}
else{
else
brakes = controlSignal;
}
end
}
}
......@@ -8,12 +8,11 @@ component SteeringAngleCorrection {
implementation Math {
static Q EPSILON = 0.01;
Q d = abs(signedDistanceToTrajectory);
if d <= EPSILON {
B cond1 = abs(signedDistanceToTrajectory) <= EPSILON;
if cond1
steeringAngleCorrection = 0.0;
} else {
// de.rwth.ryndin.modelling.autopilot.component.motion.SteeringAngleCorrectionImplementation
else
steeringAngleCorrection = 0.0;
} end
end
}
}
......@@ -6,10 +6,10 @@ component MyComponent3 {
out Q (0.0 : 0.1 : 100.0) out1;
implementation Math {
if (in1 >= 20.0) && (in1 <= 70.0) {
if (in1 >= 20.0) && (in1 <= 70.0)
out1 = 0.5 * in1;
} else {
else
out1 = 0.75 * in1;
} end
end
}
}
......@@ -8,14 +8,14 @@ component MyComponent4v1 {
implementation Math {
Q result = 0.0;
if length_of_my_secret_numbers > 1 {
if length_of_my_secret_numbers > 1
Q last_before_last = length_of_my_secret_numbers - 1;
for i = 1:last_before_last
Q num1 = my_secret_numbers(1,i);
Q num2 = my_secret_numbers(1,i+1);
result += (num1 - num2) ^ 2;
end
} end
end
out1 = result;
}
}
......@@ -9,14 +9,14 @@ component MyComponent4v2 {
implementation Math {
Q result = 0.0;
B is_enough_secret_numbers = length_of_my_secret_numbers >= 2;
if is_enough_secret_numbers {
if is_enough_secret_numbers
Z (1 : 99) last_before_last = length_of_my_secret_numbers - 1;
for i = 1:last_before_last
Q num1 = my_secret_numbers(1,i);
Q num2 = my_secret_numbers(1,i+1);
result += (num1 - num2) ^ 2;
end
} end
end
out1 = result;
}
}
......@@ -7,10 +7,10 @@ component MyComponent5 {
implementation Math {
Q THRESHOLD = 20.0;
if (in1 > THRESHOLD) {
if in1 > THRESHOLD
out1 = in1 - THRESHOLD;
} else {
else
out1 = 0.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