Autopilot.emam 3.94 KB
 Alexander David Hellwig committed Feb 02, 2019 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 ``````package de.rwth.armin.modeling.autopilot; import de.rwth.armin.modeling.autopilot.common.*; import de.rwth.armin.modeling.autopilot.behavior.*; import de.rwth.armin.modeling.autopilot.motion.*; component Autopilot { port // length of simulation time frame // e.g. when simulation frequency is 200 Hz // this value will be equal to 5ms in Q (0.0 s : 0.001 s : 1.0 s) timeIncrement, // current velocity in Q (0.0 m/s : 0.01 m/s : oo m/s) currentVelocity, // current vehicle's position in Q (-oo m : 0.01 m : oo m) x, in Q (-oo m : 0.01 m : oo m) y, // orientation angle (also sometimes called yaw angle) in radians // it is measured from from the positive Y axis direction // to the actual vehicle's direction counter-clockwise // Example 1: vehicle has direction (-1.0; -1.0) // compass = 0.25 * PI (positive) // Example 2: vehicle has direction (1.0; 0.0) i.e. it is directed along the X axis // compass = -0.5 * PI (negative) in Q (-oo : 0.001 : oo) compass, // current engine actuation in Q (0.0 : 0.001 : 2.5) currentEngine, // current steering actuation // negative value: left turn // positive value: right turn in Q (-0.785 : 0.001 : 0.785) currentSteering, // current brakes actuation in Q (0.0 : 0.001 : 3.0) currentBrakes, // planned trajectory (with look ahead 100-200m) // represented by two arrays trajectory_x[] and trajectory_y[] // which both have length trajectory_length in Z (0 : 100) trajectory_length, in Q (-oo m : 0.01 m : oo m) ^ {1,100} trajectory_x, in Q (-oo m : 0.01 m : oo m) ^ {1,100} trajectory_y, // output actuation commands out Q (0.0 : 0.001 : 2.5) engine, out Q (-0.785 : 0.001 : 0.785) steering, out Q (0.0 : 0.001 : 3.0) brakes; instance Compass2CurrentDirection c2cd; instance BehaviorGeneration behaviorGeneration; instance MotionPlanning motionPlanning; connect compass -> c2cd.compass; connect x -> behaviorGeneration.currentPositionX; connect y -> behaviorGeneration.currentPositionY; connect trajectory_length -> behaviorGeneration.plannedTrajectoryLength; connect trajectory_x -> behaviorGeneration.plannedTrajectoryX; connect trajectory_y -> behaviorGeneration.plannedTrajectoryY; connect c2cd.currentDirectionX -> behaviorGeneration.currentDirectionX; connect c2cd.currentDirectionY -> behaviorGeneration.currentDirectionY; connect currentVelocity -> motionPlanning.currentVelocity; connect c2cd.currentDirectionX -> motionPlanning.currentDirectionX; connect c2cd.currentDirectionY -> motionPlanning.currentDirectionY; connect behaviorGeneration.desiredDirectionX -> motionPlanning.desiredDirectionX; connect behaviorGeneration.desiredDirectionY -> motionPlanning.desiredDirectionY; connect behaviorGeneration.signedDistanceToTrajectory -> motionPlanning.signedDistanceToTrajectory; connect behaviorGeneration.desiredVelocity -> motionPlanning.desiredVelocity; connect motionPlanning.engine -> engine; connect motionPlanning.steering -> steering; connect motionPlanning.brakes -> brakes; }``````