Java 类edu.wpi.first.wpilibj.Gyro 实例源码

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit() {
    // instantiate the command used for the autonomous period
    autonomousCommand = new ExampleCommand();

    frontLeft = new Victor(1); // Creating Victor motors
    frontRight = new Victor(2);
    rearLeft = new Victor(3);
    rearRight = new Victor(4);

    myDrive = new RobotDrive(frontLeft, frontRight);
    // myDrive = new RobotDrive(frontLeft,frontRight,rearLeft,rearRight);

    driveStick = new Joystick(1);

    gyro = new Gyro(1);

    // Initialize all subsystems
    CommandBase.init();
}

/**
 * Creates a new {@link MecanumDriveAlgorithm} that controls the specified
 * {@link FourWheelDriveController}.
 *
 * @param controller the {@link FourWheelDriveController} to control
 * @param gyro the {@link Gyro} to use for orientation correction and
 * field-oriented driving
 */
public MecanumDriveAlgorithm(FourWheelDriveController controller, Gyro gyro) {
    super(controller);
    // Necessary because we hide the controller field inherited from
    // DriveAlgorithm (if this was >=Java 5 I would use generics).
    this.controller = controller;
    this.gyro = gyro;
    rotationPIDController = new PIDController(
            ROTATION_P,
            ROTATION_I,
            ROTATION_D,
            ROTATION_F,
            gyro,
            new PIDOutput() {
                public void pidWrite(double output) {
                    rotationSpeedPID = output;
                }
            }
    );
    SmartDashboard.putData("Rotation PID Controller", rotationPIDController);
}

public Drivetrain () {
    leftBackMotor   = new TalonWrapper (RobotMap.Drivetrain.LEFT_BACK_MOTOR_CHANNEL, Constants.Drivetrain.LEFT_BACK_MOTOR_FLIPPED);
    leftMiddleMotor = new TalonWrapper (RobotMap.Drivetrain.LEFT_MIDDLE_MOTOR_CHANNEL, Constants.Drivetrain.LEFT_MIDDLE_MOTOR_FLIPPED);
    leftFrontMotor  = new TalonWrapper (RobotMap.Drivetrain.LEFT_FRONT_MOTOR_CHANNEL, Constants.Drivetrain.LEFT_FRONT_MOTOR_FLIPPED);
    rightBackMotor  = new TalonWrapper (RobotMap.Drivetrain.RIGHT_BACK_MOTOR_CHANNEL, Constants.Drivetrain.RIGHT_BACK_MOTOR_FLIPPED);
    rightMiddleMotor = new TalonWrapper (RobotMap.Drivetrain.RIGHT_MIDDLE_MOTOR_CHANNEL, Constants.Drivetrain.RIGHT_MIDDLE_MOTOR_FLIPPED);
    rightFrontMotor = new TalonWrapper (RobotMap.Drivetrain.RIGHT_FRONT_MOTOR_CHANNEL, Constants.Drivetrain.RIGHT_FRONT_MOTOR_FLIPPED);

    leftEnc  = new EncoderWrapper (RobotMap.Drivetrain.LEFT_ENC_CHANNEL_A, RobotMap.Drivetrain.LEFT_ENC_CHANNEL_B);
    rightEnc = new EncoderWrapper (RobotMap.Drivetrain.RIGHT_ENC_CHANNEL_A, RobotMap.Drivetrain.RIGHT_ENC_CHANNEL_B);

    gyro = new Gyro(RobotMap.Drivetrain.GYRO_CHANNEL);

    shifter = new Solenoid(RobotMap.Drivetrain.SHIFTER_CHANNEL);

    leftTargetSpeed = rightTargetSpeed = leftCurrSpeed = rightCurrSpeed = 0;

    isBusy = false;
    isShifterLocked = false;
}

/**
 * Instantiates the Sensor Input module to read all sensors connected to the roboRIO.
 */
private SensorInput() { 
    limit_left = new DigitalInput(ChiliConstants.left_limit);
    limit_right = new DigitalInput(ChiliConstants.right_limit);
    accel = new BuiltInAccelerometer(Accelerometer.Range.k2G);
    gyro = new Gyro(ChiliConstants.gyro_channel);
    pdp = new PowerDistributionPanel();
    left_encoder = new Encoder(ChiliConstants.left_encoder_channelA, ChiliConstants.left_encoder_channelB, false);
    right_encoder = new Encoder(ChiliConstants.right_encoder_channelA, ChiliConstants.right_encoder_channelB, true);
    gyro_i2c = new GyroITG3200(I2C.Port.kOnboard);

    gyro_i2c.initialize();
    gyro_i2c.reset();
    gyro.initGyro();

    left_encoder.setDistancePerPulse(ChiliConstants.kDistancePerPulse);
    right_encoder.setDistancePerPulse(ChiliConstants.kDistancePerPulse);
}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
public void robotInit() {
    // instantiate the command used for the autonomous period
    autonomousCommand = new ExampleCommand();

    frontLeft = new Victor(1); // Creating Victor motors
    frontRight = new Victor(2);
    rearLeft = new Victor(3);
    rearRight = new Victor(4);

    myDrive = new RobotDrive(frontLeft, frontRight);
    // myDrive = new RobotDrive(frontLeft,frontRight,rearLeft,rearRight);

    driveStick = new Joystick(1);

    gyro = new Gyro(1);

    // Initialize all subsystems
    CommandBase.init();
}

public ControllerDrive(int limit)
{
    super(limit);
    //TODO
    leftEncoder = new Encoder(3,4);  //competition: 3,4 / practice: 8,7
    rightEncoder = new Encoder(2,1); //competition: 2,1 / practice: 6,5
    gyro = new Gyro(2);

    leftEncoderDistance = 0;
    rightEncoderDistance = 0;
    lastLeftEncoderDistance = 0;
    lastRightEncoderDistance = 0;

    timer = new Timer();
    timer.start();

    lastGyroValue = 0;
    System.out.println("[WiredCats] Drive Controller Initialized.");
}

public void update() {
    left_drive_speed = ((Double) WsOutputManager.getInstance().getOutput(WsOutputManager.LEFT_DRIVE_SPEED).get((IOutputEnum) null));
    right_drive_speed = ((Double) WsOutputManager.getInstance().getOutput(WsOutputManager.RIGHT_DRIVE_SPEED).get((IOutputEnum) null));

    Gyro gyro = ((WsDriveBase) (WsSubsystemContainer.getInstance().getSubsystem(WsSubsystemContainer.WS_DRIVE_BASE))).getGyro();
    double angle = gyro.getAngle();
    //Handle brakes
    if ((left_drive_speed > 0.1) && (right_drive_speed < 0.1)) {
        angle--;
    } else if ((left_drive_speed < 0.1) && (right_drive_speed > 0.1)) {
        angle++;
    }
    gyro.setAngle(angle);
    SmartDashboard.putNumber("Gyro Angle", angle);
}

@Override
public void initialize()
{
    rightFront = new Talon(0);
    rightBack = new Talon(1);
    leftFront = new Talon(3);
    leftBack = new Talon(2);
    drivetrain = new RobotDrive(leftBack, leftFront, rightBack, rightFront);        

    gyro = new Gyro(0);

    drivetrain.setInvertedMotor(MotorType.kFrontRight, true);
    drivetrain.setInvertedMotor(MotorType.kRearRight, true);
}

public void initialize() 
{
    rearLeftMotor = new Jaguar(0);
    frontLeftMotor = new Jaguar(1);
    liftMotor = new Jaguar(2);
    liftMotor2 = new Jaguar(3);
    liftEncoder = new Encoder(6, 7, false, EncodingType.k4X);

    drivetrain = new RobotDrive(rearLeftMotor, frontLeftMotor); 

    drivetrain.setInvertedMotor(MotorType.kFrontLeft, true);
    drivetrain.setInvertedMotor(MotorType.kFrontRight, true);

    halsensor = new DigitalInput(0);

    horizontalCamera = new Servo(8);
    verticalCamera = new Servo(9);

    solenoid = new DoubleSolenoid(0,1);

    gyro = new Gyro(1);
    accelerometer = new BuiltInAccelerometer();

    liftEncoder.reset();

    RobotHardwareWoodbot hardware = (RobotHardwareWoodbot)Robot.bot;

    LiveWindow.addActuator("Drive Train", "Front Left Motor", (Jaguar)hardware.frontLeftMotor);
    LiveWindow.addActuator("Drive Train", "Back Left Motor", (Jaguar)hardware.rearLeftMotor);
    //LiveWindow.addActuator("Drive Train", "Front Right Motor", (Jaguar)hardware.frontRightMotor);
    //LiveWindow.addActuator("Drive Train", "Back Right Motor", (Jaguar)hardware.rearRightMotor);
}

@Override
public void initialize()

{
    //PWM
    liftMotor = new Victor(0); //2);
    leftMotors = new Victor(1);
    rightMotors = new Victor(2); //0);
    armMotors = new Victor(3);
    transmission = new Servo(7);

    //CAN
    armSolenoid = new DoubleSolenoid(4,5);

    //DIO
    liftEncoder = new Encoder(0, 1, false, EncodingType.k4X);
    liftBottomLimit = new DigitalInput(2);
    liftTopLimit = new DigitalInput(3);
    backupLiftBottomLimit = new DigitalInput(5);

    switch1 = new DigitalInput(9);
    switch2 = new DigitalInput(8);

    //ANALOG
    gyro = new Gyro(0);

    //roboRio
    accelerometer = new BuiltInAccelerometer();

    //Stuff
    drivetrain = new RobotDrive(leftMotors, rightMotors);

    liftSpeedRatio = 1; //Half power default
    liftGear = 1;
    driverSpeedRatio = 1;
    debounceComp = 0;

    drivetrain.setInvertedMotor(MotorType.kRearLeft, true);
    drivetrain.setInvertedMotor(MotorType.kRearRight, true);
}

@Override
public void initialize()
{
    //PWM
    liftMotor = new Talon(0); //2);
    leftMotors = new Talon(1);
    rightMotors = new Talon(2); //0);
    armMotors = new Talon(3);
    transmission = new Servo(7);

    //CAN
    armSolenoid = new DoubleSolenoid(4,5);

    //DIO
    /*liftEncoder = new Encoder(0, 1, false, EncodingType.k4X);
    liftBottomLimit = new DigitalInput(2);
    liftTopLimit = new DigitalInput(3);
    backupLiftBottomLimit = new DigitalInput(4);

    switch1 = new DigitalInput(9);
    switch2 = new DigitalInput(8);*/

    //ANALOG
    gyro = new Gyro(0);

    //roboRio
    accelerometer = new BuiltInAccelerometer();

    //Stuff
    drivetrain = new RobotDrive(leftMotors, rightMotors);

    liftSpeedRatio = 1; //Half power default
    liftGear = 1;
    driverSpeedRatio = 1;
    debounceComp = 0;

    drivetrain.setInvertedMotor(MotorType.kRearLeft, true);
    drivetrain.setInvertedMotor(MotorType.kRearRight, true);
}

public static void init()
{

driveTrainRightVictor = new Victor(1, 1);
driveTrainLeftVictor = new Victor(1, 3);
loaderVictor = new Victor(1, 2);
CatapultVictor = new Victor(1, 4);  

FireSolenoidSpike = new Relay(2);
PrimeSolenoidSpike = new Relay(3);
lightSpike = new Relay(7);
compressorSpike = new Relay(8);

gyro = new Gyro(1);
ultrasonic = new AnalogChannel(3);

limitSwitch = new DigitalInput(3);
pressureSwitch = new DigitalInput(2);

LiveWindow.addActuator("Drive Train", "Right Victor", (Victor) driveTrainRightVictor);
LiveWindow.addActuator("Drive Train", "Left Victor", (Victor) driveTrainLeftVictor);

robotDriveTrain = new RobotDrive(driveTrainLeftVictor, driveTrainRightVictor);

robotDriveTrain.setSafetyEnabled(true);
robotDriveTrain.setExpiration(0.1);
robotDriveTrain.setSensitivity(0.5);
robotDriveTrain.setMaxOutput(1.0);
}

@Override
public void update() {
    left_drive_speed = ((Double) OutputManager.getInstance().getOutput(OutputManager.LEFT_DRIVE_SPEED_INDEX).get((IOutputEnum) null));
    right_drive_speed = ((Double) OutputManager.getInstance().getOutput(OutputManager.RIGHT_DRIVE_SPEED_INDEX).get((IOutputEnum) null));
    Gyro gyro = ((DriveBase) (SubsystemContainer.getInstance().getSubsystem(SubsystemContainer.DRIVE_BASE_INDEX))).getGyro();
    double angle = gyro.getAngle();
    //Handle brakes
    if ((left_drive_speed > 0.1) && (right_drive_speed < 0.1)) {
        angle--;
    } else if ((left_drive_speed < 0.1) && (right_drive_speed > 0.1)) {
        angle++;
    }
    gyro.setAngle(angle);
    SmartDashboard.putNumber("Gyro Angle", angle);
}

public Climber(int leftM, int rightM, int leftSecondaryM, int rightSecondaryM, int leftS, int rightS, int gyro)
{
    this.leftM = new Jaguar(leftM);
    this.rightM = new Jaguar(rightM);
    this.leftSecondaryM = new Victor(leftSecondaryM);
    this.rightSecondaryM = new Victor(rightSecondaryM);
    this.leftS = new Servo(leftS);
    this.rightS = new Servo(rightS);
    this.gyro = new Gyro(gyro);
    this.gyro.setSensitivity(.007);
    System.out.println("Sensitivity set");
    this.gyro.reset();

    Log.v(TAG, "Climber subsystem instantiated.");
}

public DriveTrain(int left, int right, int gyro)
{
    this.left = new Jaguar(left);
    this.right = new Jaguar(right);
    this.gyro = new Gyro(gyro);

    Log.v(TAG, "Drive train subsystem instantiated.");
}

/** The VerticalTurretAxis constructor is called by the ScraperBike constructor.
 *
 */
public VerticalTurretAxis(){
    super("VerticalTurretAxis");
    verTurretTalon = new Talon(RobotMap.VerTurretMotor);

    gyro = new Gyro(RobotMap.gyroAnalogInput);
}

void init() {
    gy = new Gyro(RobotMap.gyroport);
    gy.reset();
    rotaryEncoder = new Encoder(1,2);
    rotaryEncoder.start();
    rotaryEncoder.reset();//Justin Case, attorney at law!
    feederLimit = new DigitalInput(14);
}

public Sensors(UltimateAscentRobot robot){
    super(robot);
    leftStick      = new Joystick(1);
    rightStick     = new Joystick(2);
    frontEncoder = new Encoder(4,3);   //digital sources 1 and 2
    gyro           = new Gyro(1);      //analog channel 1
    armLimit       = new DigitalInput(2);
    //gyro.setSensitivity(.007);
    gyro.reset();
}

public GyroPID(double p, double i, double d, int gyroChannel, 
        double inputRangeMinimum, double inputRangeMaximum, double gyroSensitivity) {
    super("Gyro", p, i, d);
    gyro = new Gyro(gyroChannel);       
    this.getPIDController().setContinuous();
    setInputRange(inputRangeMinimum, inputRangeMaximum);

    // Use these to get going:
    // setSetpoint() -  Sets where the PID controller should move the system
    //                  to
    // enable() - Enables the PID controller.
    // 0.007 volts per degree per second
    gyro.setSensitivity(gyroSensitivity);   

}

/**
 * @param gyroPort slot in the analog module where the gyro is plugged in
 */
public MotionDetector(int gyroPort)
{
    _gyro   = new Gyro(gyroPort);
    _accelerometer  = new Accelerometer2();
    reset();
}

public void update() {
    left_drive_speed = ((Double) WsOutputManager.getInstance().getOutput(WsOutputManager.LEFT_DRIVE_SPEED).get((IOutputEnum) null));
    right_drive_speed = ((Double) WsOutputManager.getInstance().getOutput(WsOutputManager.RIGHT_DRIVE_SPEED).get((IOutputEnum) null));

    Gyro gyro = ((WsDriveBase) (WsSubsystemContainer.getInstance().getSubsystem(WsSubsystemContainer.WS_DRIVE_BASE))).getGyro();
    double angle = gyro.getAngle();
    //Handle brakes
    if ((left_drive_speed > 0.1) && (right_drive_speed < 0.1)) {
        angle--;
    } else if ((left_drive_speed < 0.1) && (right_drive_speed > 0.1)) {
        angle++;
    }
    gyro.setAngle(angle);
    gyroAngle.updateWithValue(angle, 360);
}

public RobotDrivePID(int leftMotorChannel, int rightMotorChannel,Gyro gyro) {   
    super(leftMotorChannel, rightMotorChannel);
    InitGyro(gyro);
    m_callCounter = 0;
}

public RobotDrivePID(int frontLeftMotor, int rearLeftMotor, int frontRightMotor, int rearRightMotor,Gyro gyro) {
    super(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
    InitGyro(gyro);
    m_callCounter = 0;
}

public RobotDrivePID(SpeedController leftMotor, SpeedController rightMotor,Gyro gyro) {
    super(leftMotor, rightMotor);
    InitGyro(gyro);
    m_callCounter = 0;
}

public RobotDrivePID(SpeedController frontLeftMotor, SpeedController rearLeftMotor, SpeedController frontRightMotor, SpeedController rearRightMotor,Gyro gyro) {
    super(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
    InitGyro(gyro);
    m_callCounter = 0;
}

private void InitGyro(Gyro gyro)
{
    m_gyro = gyro;
    m_gyro.setSensitivity(0.007);
}

public static void init() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    driveTrainleftFrontTalon0 = new TalonSRX(0);
    LiveWindow.addActuator("DriveTrain", "leftFrontTalon0", (TalonSRX) driveTrainleftFrontTalon0);

    driveTrainleftBackTalon1 = new TalonSRX(1);
    LiveWindow.addActuator("DriveTrain", "leftBackTalon1", (TalonSRX) driveTrainleftBackTalon1);

    driveTrainrightFrontTalon2 = new TalonSRX(2);
    LiveWindow.addActuator("DriveTrain", "rightFrontTalon2", (TalonSRX) driveTrainrightFrontTalon2);

    driveTrainrightBackTalon3 = new TalonSRX(3);
    LiveWindow.addActuator("DriveTrain", "rightBackTalon3", (TalonSRX) driveTrainrightBackTalon3);

    driveTrainRobotDrive = new RobotDrive(driveTrainleftFrontTalon0, driveTrainleftBackTalon1,
          driveTrainrightFrontTalon2, driveTrainrightBackTalon3);

    driveTrainRobotDrive.setSafetyEnabled(true);
    driveTrainRobotDrive.setExpiration(0.1);
    driveTrainRobotDrive.setSensitivity(0.5);
    driveTrainRobotDrive.setMaxOutput(1.0);

    driveTraingyro = new Gyro(0);
    LiveWindow.addSensor("DriveTrain", "gyro", driveTraingyro);
    driveTraingyro.setSensitivity(0.007);
    driveTrainleftFrontEncoder = new Encoder(17, 18, false, EncodingType.k4X);
    LiveWindow.addSensor("DriveTrain", "leftFrontEncoder", driveTrainleftFrontEncoder);
    driveTrainleftFrontEncoder.setDistancePerPulse(1.0);
    driveTrainleftFrontEncoder.setPIDSourceParameter(PIDSourceParameter.kRate);
    driveTrainleftBackEncoder = new Encoder(19, 20, false, EncodingType.k4X);
    LiveWindow.addSensor("DriveTrain", "leftBackEncoder", driveTrainleftBackEncoder);
    driveTrainleftBackEncoder.setDistancePerPulse(1.0);
    driveTrainleftBackEncoder.setPIDSourceParameter(PIDSourceParameter.kRate);
    driveTrainrightFrontEncoder = new Encoder(21, 22, false, EncodingType.k4X);
    LiveWindow.addSensor("DriveTrain", "rightFrontEncoder", driveTrainrightFrontEncoder);
    driveTrainrightFrontEncoder.setDistancePerPulse(1.0);
    driveTrainrightFrontEncoder.setPIDSourceParameter(PIDSourceParameter.kRate);
    driveTrainrightBackEncoder = new Encoder(23, 24, false, EncodingType.k4X);
    LiveWindow.addSensor("DriveTrain", "rightBackEncoder", driveTrainrightBackEncoder);
    driveTrainrightBackEncoder.setDistancePerPulse(1.0);
    driveTrainrightBackEncoder.setPIDSourceParameter(PIDSourceParameter.kRate);

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
}

public Chassis() {
        super(KP, KI, KD);

        try {
            m_drive = new RobotDrive(
                    new Victor(RobotMap.DRIVE_LEFT_TOP_FRONT_DSC_PWM_ID),
                    new Victor(RobotMap.DRIVE_LEFT_REAR_DSC_PWM_ID),
                    new Victor(RobotMap.DRIVE_RIGHT_TOP_FRONT_DSC_PWM_ID),
                    new Victor(RobotMap.DRIVE_RIGHT_REAR_DSC_PWM_ID));

            m_drive.setSafetyEnabled(false);
            m_drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
            m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
            m_drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
            m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);

            m_leftEncoder = new Encoder(
                    1, RobotMap.LEFT_DRIVE_ENCODER_A_DSC_DIO_ID, 
                    1, RobotMap.LEFT_DRIVE_ENCODER_B_DSC_DIO_ID, true, CounterBase.EncodingType.k4X);
            m_rightEncoder = new Encoder(
                    1, RobotMap.RIGHT_DRIVE_ENCODER_A_DSC_DIO_ID, 
                    1, RobotMap.RIGHT_DRIVE_ENCODER_B_DSC_DIO_ID, false, CounterBase.EncodingType.k4X);    

            m_leftEncoder.setDistancePerPulse(WHEEL_CIRCUM_IN / 360.0 / ENCODER_TO_WHEEL_GEAR_RATIO);
            m_rightEncoder.setDistancePerPulse(WHEEL_CIRCUM_IN / 360.0 / ENCODER_TO_WHEEL_GEAR_RATIO);

            resetEncoders();

            m_yawGyro = new Gyro(RobotMap.CHASSIS_YAW_RATE_ANALOG_BREAKOUT_PORT);
            m_yawGyro.setSensitivity(0.007);  // 7 mV/deg/sec

//            SmartDashboard.putNumber("Move NonLinear ", m_moveNonLinear);
//            SmartDashboard.putNumber("Steer NonLinear ", m_steerNonLinear);
//            SmartDashboard.putNumber("Move Scale ", m_moveScale);
//            SmartDashboard.putNumber("Steer Scale ", m_steerScale);
//            SmartDashboard.putNumber("Move Trim ", -m_moveTrim);
//            SmartDashboard.putNumber("Steer Trim ", m_steerTrim); 
        } catch (Exception e) {
            System.out.println("Unknown error initializing chassis.  Message = " + e.getMessage());
        }
    }

public void init(Environment e) {
  gyro = new Gyro(RobotMap.GYRO_PORT);
  timer = new Timer();
  timer.start();
}

public WsDriveBase(String name) {
    super(name);

    WHEEL_DIAMETER_config = new DoubleConfigFileParameter(this.getClass().getName(), "wheel_diameter", 6);
    TICKS_PER_ROTATION_config = new DoubleConfigFileParameter(this.getClass().getName(), "ticks_per_rotation", 360.0);
    THROTTLE_LOW_GEAR_ACCEL_FACTOR_config = new DoubleConfigFileParameter(this.getClass().getName(), "throttle_low_gear_accel_factor", 0.250);
    HEADING_LOW_GEAR_ACCEL_FACTOR_config = new DoubleConfigFileParameter(this.getClass().getName(), "heading_low_gear_accel_factor", 0.500);
    THROTTLE_HIGH_GEAR_ACCEL_FACTOR_config = new DoubleConfigFileParameter(this.getClass().getName(), "throttle_high_gear_accel_factor", 0.125);
    HEADING_HIGH_GEAR_ACCEL_FACTOR_config = new DoubleConfigFileParameter(this.getClass().getName(), "heading_high_gear_accel_factor", 0.250);
    MAX_HIGH_GEAR_PERCENT_config = new DoubleConfigFileParameter(this.getClass().getName(), "max_high_gear_percent", 0.80);
    ENCODER_GEAR_RATIO_config = new DoubleConfigFileParameter(this.getClass().getName(), "encoder_gear_ratio", 7.5);
    DEADBAND_config = new DoubleConfigFileParameter(this.getClass().getName(), "deadband", 0.05);
    SLOW_TURN_FORWARD_SPEED_config = new DoubleConfigFileParameter(this.getClass().getName(), "slow_turn_forward_speed", 0.16);
    SLOW_TURN_BACKWARD_SPEED_config = new DoubleConfigFileParameter(this.getClass().getName(), "slow_turn_backward_speed", -0.19);
    FEED_FORWARD_VELOCITY_CONSTANT_config = new DoubleConfigFileParameter(this.getClass().getName(), "feed_forward_velocity_constant", 1.00);
    FEED_FORWARD_ACCELERATION_CONSTANT_config = new DoubleConfigFileParameter(this.getClass().getName(), "feed_forward_acceleration_constant", 0.00018);
    MAX_ACCELERATION_DRIVE_PROFILE_config = new DoubleConfigFileParameter(this.getClass().getName(), "max_acceleration_drive_profile", 600.0);
    MAX_SPEED_INCHES_LOWGEAR_config = new DoubleConfigFileParameter(this.getClass().getName(), "max_speed_inches_lowgear", 90.0);
    DECELERATION_VELOCITY_THRESHOLD_config = new DoubleConfigFileParameter(this.getClass().getName(), "deceleration_velocity_threshold", 48.0);
    DECELERATION_MOTOR_SPEED_config = new DoubleConfigFileParameter(this.getClass().getName(), "deceleration_motor_speed", 0.3);
    STOPPING_DISTANCE_AT_MAX_SPEED_LOWGEAR_config = new DoubleConfigFileParameter(this.getClass().getName(), "stopping_distance_at_max_speed_lowgear", 10.0);
    DRIVE_OFFSET_config = new AutonomousDoubleConfigFileParameter("DriveOffset", 1.00);
    USE_LEFT_SIDE_FOR_OFFSET_config = new AutonomousBooleanConfigFileParameter("UseLeftDriveForOffset", true);
    QUICK_TURN_CAP_config = new DoubleConfigFileParameter(this.getClass().getName(), "quick_turn_cap", 10.0);
    QUICK_TURN_ANTITURBO_config = new DoubleConfigFileParameter(this.getClass().getName(), "quick_turn_antiturbo", 10.0);

    //Anti-Turbo button
    registerForJoystickButtonNotification(WsJoystickButtonEnum.DRIVER_BUTTON_8);
    //Turbo button
    registerForJoystickButtonNotification(WsJoystickButtonEnum.DRIVER_BUTTON_7);
    //Shifter Button
    registerForJoystickButtonNotification(WsJoystickButtonEnum.DRIVER_BUTTON_6);
    //Left/right slow turn buttons
    registerForJoystickButtonNotification(WsJoystickButtonEnum.DRIVER_BUTTON_1);
    registerForJoystickButtonNotification(WsJoystickButtonEnum.DRIVER_BUTTON_3);

    //Initialize the drive base encoders
    leftDriveEncoder = new Encoder(2, 3, true, EncodingType.k4X);
    leftDriveEncoder.reset();
    leftDriveEncoder.start();
    rightDriveEncoder = new Encoder(4, 5, false, EncodingType.k4X);
    rightDriveEncoder.reset();
    rightDriveEncoder.start();

    //Initialize the gyro
    //@TODO: Get the correct port
    driveHeadingGyro = new Gyro(1);

    //Initialize the PIDs
    driveDistancePidInput = new WsDriveBaseDistancePidInput();
    driveDistancePidOutput = new WsDriveBaseDistancePidOutput();
    driveDistancePid = new WsPidController(driveDistancePidInput, driveDistancePidOutput, "WsDriveBaseDistancePid");

    driveHeadingPidInput = new WsDriveBaseHeadingPidInput();
    driveHeadingPidOutput = new WsDriveBaseHeadingPidOutput();
    driveHeadingPid = new WsPidController(driveHeadingPidInput, driveHeadingPidOutput, "WsDriveBaseHeadingPid");

    driveSpeedPidInput = new WsDriveBaseSpeedPidInput();
    driveSpeedPidOutput = new WsDriveBaseSpeedPidOutput();
    driveSpeedPid = new WsSpeedPidController(driveSpeedPidInput, driveSpeedPidOutput, "WsDriveBaseSpeedPid");
    continuousAccelerationFilter = new ContinuousAccelFilter(0, 0, 0);
    init();
}

public Gyro getGyro() {
    return driveHeadingGyro;
}

public static void init() {
       visionFrontSonar = new AnalogChannel(2);
       visionFrontSonarSolenoidRelay = new Solenoid(3);

       ColorLedsRelay = new Relay(3);

       FlashingLedsRelay = new Relay(2);

       m_compressor = new Compressor(7, 1);

       canonAngleAllWheelAngleMotor = new Talon(1, 4);
LiveWindow.addActuator("CanonAngle", "AllWheelAngleMotor", (Talon) canonAngleAllWheelAngleMotor);

       canonAngleAnglePot = new AnalogChannel(1, 4);
LiveWindow.addSensor("CanonAngle", "AnglePot", canonAngleAnglePot);

       canonAngleUpperAngleLimitSwitch = new DigitalInput(1, 1);
LiveWindow.addSensor("CanonAngle", "UpperAngleLimitSwitch", canonAngleUpperAngleLimitSwitch);

       canonAngleLowerAngleLimitSwitch = new DigitalInput(1, 2);
LiveWindow.addSensor("CanonAngle", "LowerAngleLimitSwitch", canonAngleLowerAngleLimitSwitch);

       canonSpinnerAllWheelSpinnerMotor = new Talon(1, 3);
LiveWindow.addActuator("CanonSpinner", "AllWheelSpinnerMotor", (Talon) canonSpinnerAllWheelSpinnerMotor);

       canonShooterShooterSolenoid = new DoubleSolenoid(1, 1, 2);
LiveWindow.addActuator("CanonShooter", "ShooterSolenoid", canonShooterShooterSolenoid);

       canonShooterShooterSolenoid.set(DoubleSolenoid.Value.kReverse);

       driveTrainAllWheelRightMotor = new Talon(1, 2);
LiveWindow.addActuator("DriveTrain", "AllWheelRightMotor", (Talon) driveTrainAllWheelRightMotor);

       driveTrainAllWheelLeftMotor = new Talon(1, 1);
LiveWindow.addActuator("DriveTrain", "AllWheelLeftMotor", (Talon) driveTrainAllWheelLeftMotor);

       driveTrainAllWheelRobotDrive = new RobotDrive(driveTrainAllWheelLeftMotor, driveTrainAllWheelRightMotor);

       driveTrainAllWheelRobotDrive.setSafetyEnabled(true);
       driveTrainAllWheelRobotDrive.setExpiration(0.1);
       driveTrainAllWheelRobotDrive.setSensitivity(0.5);
       driveTrainAllWheelRobotDrive.setMaxOutput(1.0);
       driveTrainAllWheelRobotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, false);
       driveTrainAllWheelRobotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, false);  

       driveTrainRobotFrameGyro = new Gyro(1, 1);
LiveWindow.addSensor("DriveTrain", "RobotFrameGyro", driveTrainRobotFrameGyro);
       driveTrainRobotFrameGyro.setSensitivity(0.007);
   }

private void init() {
        if (!initialized) {
            flTalon = new Talon(RobotMap.DIGITAL_MODULE, RobotMap.FL_WHEEL);
            frTalon = new Talon(RobotMap.DIGITAL_MODULE, RobotMap.FR_WHEEL);
            blTalon = new Talon(RobotMap.DIGITAL_MODULE, RobotMap.BL_WHEEL);
            brTalon = new Talon(RobotMap.DIGITAL_MODULE, RobotMap.BR_WHEEL);

            System.out.println("Module: "+ RobotMap.DIGITAL_MODULE +" used for drive.");
            System.out.println("FL wheel at: "+ RobotMap.FL_WHEEL +" FR wheel at: "+ RobotMap.FR_WHEEL);
            System.out.println("BL wheel at: "+ RobotMap.BL_WHEEL +" BR wheel at: "+ RobotMap.BR_WHEEL);

//            if (useRobotDrive)
                robotDrive = new RobotDrive(flTalon, blTalon, frTalon, brTalon);
//            else
//                robotDrive = null;

            aButton_Held = false;
            driveForward = true;
            SmartDashboard.putBoolean("Drive_Direction", driveForward);

            leftEncoder = new Encoder(RobotMap.DIGITAL_MODULE, RobotMap.LEFT_ENCODER_A,
                    RobotMap.DIGITAL_MODULE, RobotMap.LEFT_ENCODER_B, true, CounterBase.EncodingType.k4X);
            rightEncoder = new Encoder(RobotMap.DIGITAL_MODULE, RobotMap.RIGHT_ENCODER_A,
                    RobotMap.DIGITAL_MODULE, RobotMap.RIGHT_ENCODER_B, false, CounterBase.EncodingType.k4X);

    //        leftEncoder.setReverseDirection(false);
            leftEncoder.setReverseDirection(true); // Flipped on comp bot
            leftEncoder.setMinRate(10);
            leftEncoder.setDistancePerPulse(6 * Math.PI / 250);
            leftEncoder.start();
    //        rightEncoder.setReverseDirection(true);
            rightEncoder.setReverseDirection(false); // Flipped on comp bot
            rightEncoder.setMinRate(10);
            rightEncoder.setDistancePerPulse(6 * Math.PI / 250);
            rightEncoder.start();

            gyro = new Gyro(RobotMap.ANALOG_MODULE, RobotMap.GYRO);

            SmartDashboard.putNumber("Max turn speed", 1);
        }
        initialized = true;
    }

public IMU(int gyroChannel) {
    gyro = new Gyro(gyroChannel);
    accel = new Accel(Accel.FOURG);
}

public Gyro getGyro(){
    return gyro;
}

public ADW22307Gyro(int port) {
    g = new Gyro(port);
    lastUpdateTime = System.currentTimeMillis();
    g.setSensitivity(7000);
    initialize();
}

public DriveBase(String name) {
    super(name);

    WHEEL_DIAMETER_config = new DoubleConfigFileParameter(this.getClass().getName(), "wheel_diameter", 6);
    TICKS_PER_ROTATION_config = new DoubleConfigFileParameter(this.getClass().getName(), "ticks_per_rotation", 360.0);
    THROTTLE_LOW_GEAR_ACCEL_FACTOR_config = new DoubleConfigFileParameter(this.getClass().getName(), "throttle_low_gear_accel_factor", 0.250);
    HEADING_LOW_GEAR_ACCEL_FACTOR_config = new DoubleConfigFileParameter(this.getClass().getName(), "heading_low_gear_accel_factor", 0.500);
    THROTTLE_HIGH_GEAR_ACCEL_FACTOR_config = new DoubleConfigFileParameter(this.getClass().getName(), "throttle_high_gear_accel_factor", 0.125);
    HEADING_HIGH_GEAR_ACCEL_FACTOR_config = new DoubleConfigFileParameter(this.getClass().getName(), "heading_high_gear_accel_factor", 0.250);
    MAX_HIGH_GEAR_PERCENT_config = new DoubleConfigFileParameter(this.getClass().getName(), "max_high_gear_percent", 0.80);
    ENCODER_GEAR_RATIO_config = new DoubleConfigFileParameter(this.getClass().getName(), "encoder_gear_ratio", 7.5);
    DEADBAND_config = new DoubleConfigFileParameter(this.getClass().getName(), "deadband", 0.05);
    SLOW_TURN_FORWARD_SPEED_config = new DoubleConfigFileParameter(this.getClass().getName(), "slow_turn_forward_speed", 0.16);
    SLOW_TURN_BACKWARD_SPEED_config = new DoubleConfigFileParameter(this.getClass().getName(), "slow_turn_backward_speed", -0.19);
    FEED_FORWARD_VELOCITY_CONSTANT_config = new DoubleConfigFileParameter(this.getClass().getName(), "feed_forward_velocity_constant", 1.00);
    FEED_FORWARD_ACCELERATION_CONSTANT_config = new DoubleConfigFileParameter(this.getClass().getName(), "feed_forward_acceleration_constant", 0.00018);
    MAX_ACCELERATION_DRIVE_PROFILE_config = new DoubleConfigFileParameter(this.getClass().getName(), "max_acceleration_drive_profile", 600.0);
    MAX_SPEED_INCHES_LOWGEAR_config = new DoubleConfigFileParameter(this.getClass().getName(), "max_speed_inches_lowgear", 90.0);
    DECELERATION_VELOCITY_THRESHOLD_config = new DoubleConfigFileParameter(this.getClass().getName(), "deceleration_velocity_threshold", 48.0);
    DECELERATION_MOTOR_SPEED_config = new DoubleConfigFileParameter(this.getClass().getName(), "deceleration_motor_speed", 0.3);
    STOPPING_DISTANCE_AT_MAX_SPEED_LOWGEAR_config = new DoubleConfigFileParameter(this.getClass().getName(), "stopping_distance_at_max_speed_lowgear", 10.0);
    DRIVE_OFFSET_config = new AutonomousDoubleConfigFileParameter("DriveOffset", 1.00);
    USE_LEFT_SIDE_FOR_OFFSET_config = new AutonomousBooleanConfigFileParameter("UseLeftDriveForOffset", true);
    QUICK_TURN_CAP_config = new DoubleConfigFileParameter(this.getClass().getName(), "quick_turn_cap", 10.0);
    QUICK_TURN_ANTITURBO_config = new DoubleConfigFileParameter(this.getClass().getName(), "quick_turn_antiturbo", 10.0);

    //Anti-Turbo button
    registerForJoystickButtonNotification(JoystickButtonEnum.DRIVER_BUTTON_8);
    //Turbo button
    registerForJoystickButtonNotification(JoystickButtonEnum.DRIVER_BUTTON_7);
    //Shifter Button
    registerForJoystickButtonNotification(JoystickButtonEnum.DRIVER_BUTTON_6);

    //Initialize the drive base encoders
    leftDriveEncoder = new Encoder(2, 3, true, EncodingType.k4X);
    leftDriveEncoder.reset();
    leftDriveEncoder.start();
    rightDriveEncoder = new Encoder(4, 5, false, EncodingType.k4X);
    rightDriveEncoder.reset();
    rightDriveEncoder.start();

    //Initialize the gyro
    //@TODO: Get the correct port
    driveHeadingGyro = new Gyro(1);

    continuousAccelerationFilter = new ContinuousAccelFilter(0, 0, 0);
    driveSpeedPidInput = new DriveBaseSpeedPidInput();
    driveSpeedPidOutput = new DriveBaseSpeedPidOutput();
    driveSpeedPid = new SpeedPidController(driveSpeedPidInput, driveSpeedPidOutput, "DriveBaseSpeedPid");
    init();
}

public Gyro getGyro() {
    return driveHeadingGyro;
}

@Override
public void init() {
   Gyro gyro = ((DriveBase) (SubsystemContainer.getInstance().getSubsystem(SubsystemContainer.DRIVE_BASE_INDEX))).getGyro();
   gyro.setAngle(0);
}

public void initGyroAccel()
{
    accel = new ADXL345_I2C(1, DataFormat_Range.k2G);

    gyro = new Gyro(RobotMap.GYRO);
}