@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();
}