@Override
  protected void initialize() {
    backLeft = new Talon(ControlsManager.BACK_LEFT_SPEED_CONTROLLER);
    frontLeft = new Talon(ControlsManager.FRONT_LEFT_SPEED_CONTROLLER);
    backRight = new Talon(ControlsManager.BACK_RIGHT_SPEED_CONTROLLER);
    frontRight = new Talon(ControlsManager.FRONT_RIGHT_SPEED_CONTROLLER);

    backLeft.setInverted(true);
    frontLeft.setInverted(true);
    backRight.setInverted(true);
    frontRight.setInverted(true);

    lidar = new LIDAR(Port.kMXP);

    // ultrasonic = new Ultrasonic(RobotMap.ultraPing, RobotMap.ultraEcho);
    // ultrasonic.setEnabled(true); ultrasonic.setAutomaticMode(true);

    maxbotix = new BadUltrasonic(ControlsManager.MAXBOTIX_ULTRASONIC);

    // mxp stuff
    serialPort = new SerialPort(57600, SerialPort.Port.kMXP);

    byte update_rate_hz = 127;
    mxp = new IMU(serialPort, update_rate_hz);
    Timer.delay(0.3);
    mxp.zeroYaw();

    train = new RobotDrive(backLeft, frontLeft, backRight, frontRight);
  }
 public void resetMXPAngle() {
   mxp.zeroYaw();
 }