@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 float getRoll() {
   return mxp.getRoll();
 }
 /** @return the angle of the drive train from {@literal 0} to {@literal 360}. */
 public double getAngle360() // returns 0 -360
     {
   if (mxp.getYaw() < 0) return mxp.getYaw() + 360;
   else return mxp.getYaw();
 }
 public void resetMXPAngle() {
   mxp.zeroYaw();
 }
 /** @return the angle of the drive train from {@literal -180} to {@literal 180}. */
 public double getAngle() // return -180 - 180
     {
   return (double) mxp.getYaw();
 }