Commit 9f39e9ec authored by Alexander Ryndin's avatar Alexander Ryndin
Browse files

adjust model

parent a2fce27d
......@@ -14,8 +14,8 @@ component Distance2Velocity {
// exponent
static Q D1 = 1.0 m;
static Q V1 = 1.0 m/s;
static Q D2 = 10.0 m;
static Q V2 = 5.0 m/s;
static Q D2 = 15.0 m;
static Q V2 = 7.0 m/s;
static Q COEF_K = log( (V1 / V2) ) / (D2 - D1);
Q v = 0.0;
if (distance < D1)
......
......@@ -25,13 +25,13 @@ component DriveToFirstPosition {
maxVelocity = 0.0;
distance = 0.0;
if (isDriveToFirstPosition) && (trimmedTrajectoryLength >= 1)
maxVelocity = 5.0;
maxVelocity = 7.0;
desiredDirectionX = trimmedTrajectoryX(1,1) - currentPositionX;
desiredDirectionY = trimmedTrajectoryY(1,1) - currentPositionY;
Q sqr_dist = desiredDirectionX * desiredDirectionX + desiredDirectionY * desiredDirectionY;
distance = sqrt( sqr_dist );
if (trimmedTrajectoryLength >= 2)
minVelocity = 2.0;
minVelocity = 3.0;
end
end
}
......
......@@ -18,7 +18,7 @@ component FollowTrajectory {
implementation Math {
// change to params later
static Q BEZIER_COEF = 0.5;
static Q BEZIER_COEF = 0.1;
static Q TURN_ANGLE_THRESHOLD = 10.0 * M_PI / 180.0; // 10 degrees
static Q EPSILON = 0.00001;
static Q REACHED_POSITION_THRESHOLD = 5.0;
......@@ -31,7 +31,7 @@ component FollowTrajectory {
if (isDriveToFirstPosition)
Q doNothing;
elseif (trimmedTrajectoryLength >= 2)
maxVelocity = 5.0;
maxVelocity = 7.0;
// SIGNED DISTANCE
Q p1x = trimmedTrajectoryX(1,1);
Q p1y = trimmedTrajectoryY(1,1);
......@@ -98,7 +98,7 @@ component FollowTrajectory {
// DISTANCE
Q dist = sqrt( (p2x-p1x)*(p2x-p1x) + (p2y-p1y)*(p2y-p1y) );
if (trimmedTrajectoryLength >= 3)
minVelocity = 2.0;
minVelocity = 3.0;
Q is_go_on_2 = 1;
Q lastIndex = trimmedTrajectoryLength - 2;
for i = 1:lastIndex
......
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