public void init() {
System.out.println("Initializing drivetrain...");
// Initialize serial port //
if (!SabertoothSpeedController.isSerialPortInitialized())
SabertoothSpeedController.initializeSerialPort(9600);
// Initialize swerve pods //
_swervePod1 =
new TestBotSwervePod(
RobotMap.turningTalon1,
RobotMap.drive1SabertoothAddress,
RobotMap.drive1MotorNumber,
RobotMap.encoder1Pin1,
RobotMap.encoder1Pin2,
RobotMap.digipot1,
RobotMap.digipot1Offset);
_swervePod2 =
new TestBotSwervePod(
RobotMap.turningTalon2,
RobotMap.drive2SabertoothAddress,
RobotMap.drive2MotorNumber,
RobotMap.encoder2Pin1,
RobotMap.encoder2Pin2,
RobotMap.digipot2,
RobotMap.digipot2Offset);
_swervePod3 =
new TestBotSwervePod(
RobotMap.turningTalon3,
RobotMap.drive3SabertoothAddress,
RobotMap.drive3MotorNumber,
RobotMap.encoder3Pin1,
RobotMap.encoder3Pin2,
RobotMap.digipot3,
RobotMap.digipot3Offset);
_swervePod4 =
new TestBotSwervePod(
RobotMap.turningTalon4,
RobotMap.drive4SabertoothAddress,
RobotMap.drive4MotorNumber,
RobotMap.encoder4Pin1,
RobotMap.encoder4Pin2,
RobotMap.digipot4,
RobotMap.digipot4Offset);
_swervePod1.initSmartDashboard();
// Initialize swerve pod setpoints //
setAllWheelAngles(180.0);
}
