Autopilot.emam 3.94 KB
Newer Older
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;
}