import com.ctre.CANTalon;
import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.I2C.Port;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
// ROBOT DRIVE
robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+ this.imu = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
+ BNO055.vector_type_t.VECTOR_EULER, Port.kOnboard, (byte) 0x28);
+ gyroZero = imu.getHeading();
}
public static DriveTrain getDriveTrain() {
return rightEncoder.getDistance();
}
+ public void printEncoderOutput() {
+ System.out.println("left: " + getLeftEncoderDistance());
+ System.out.println("right: " + getRightEncoderDistance());
+ }
+
public double getAvgEncoderDistance() {
return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
}