SteeringAngleCorrection.emam 1002 Bytes
Newer Older
1
2
3
4
5
6
7
8
package de.rwth.armin.modeling.autopilot.motion;

component SteeringAngleCorrection {
  port
    in Q signedDistanceToTrajectory,
    out Q steeringAngleCorrection;

  implementation Math {
Alexander Ryndin's avatar
Alexander Ryndin committed
9
10
    // TODO convert into parameter when parameters start working (bug #28)
    static Q MAX_STEERING_ANGLE = 0.785;
Alexander Ryndin's avatar
Alexander Ryndin committed
11
    static Q EPSILON = 0.01;
Alexander Ryndin's avatar
Alexander Ryndin committed
12
13
14
15
16
17
    // 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);
Alexander Ryndin's avatar
Alexander Ryndin committed
18

Alexander Ryndin's avatar
Alexander Ryndin committed
19
    steeringAngleCorrection = 0.0;
Alexander Ryndin's avatar
Alexander Ryndin committed
20
21
22
23

    Q dist = abs(signedDistanceToTrajectory);
    if (dist > EPSILON)
      if (dist < X1)
Alexander Ryndin's avatar
Alexander Ryndin committed
24
        steeringAngleCorrection = Y1;
Alexander Ryndin's avatar
Alexander Ryndin committed
25
      elseif (dist > X2)
Alexander Ryndin's avatar
Alexander Ryndin committed
26
27
28
29
30
        steeringAngleCorrection = Y2;
      else
        steeringAngleCorrection = Y1 * exp(-COEF_K * (dist - X1));
      end
      // take sign into account
Alexander Ryndin's avatar
Alexander Ryndin committed
31
      if (signedDistanceToTrajectory < 0)
Alexander Ryndin's avatar
Alexander Ryndin committed
32
33
        steeringAngleCorrection *= -1.0;
      end
Alex Ryndin's avatar
Alex Ryndin committed
34
    end
35
36
  }
}