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