def __init__(self, navx_type):
self.prev_x_accel = 0
self.prev_y_accel = 0
if navx_type is robotmap.NavXType.I2C:
self.navx = AHRS.create_i2c()
elif navx_type is robotmap.NavXType.SPI:
self.navx = AHRS.create_spi()
else:
print("warning navx instaniated with unknown navx_type")
def robotInit(self):
# Channels for the wheels
self.l_motor = wpilib.Spark(1)
self.r_motor = wpilib.Spark(2)
self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor)
self.stick = wpilib.Joystick(0)
#
# Communicate w/navX MXP via the MXP SPI Bus.
# - Alternatively, use the i2c bus.
# See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details
#
self.ahrs = AHRS.create_spi()
# self.ahrs = AHRS.create_i2c()
turnController = wpilib.controller.PIDController(
self.kP,
self.kI,
self.kD,
)
turnController.enableContinuousInput(-180.0, 180.0)
turnController.setTolerance(self.kToleranceDegrees)
self.turnController = turnController
def createObjects(self):
""" Create motors, sensors and all your components here. """
self.chassis_left_master = WPI_TalonSRX(1)
self.chassis_left_slave = WPI_TalonSRX(2)
self.chassis_right_master = WPI_TalonSRX(3)
self.chassis_right_slave = WPI_TalonSRX(4)
self.chassis_gyro = AHRS.create_spi()
self.joystick = Joystick(0)
def __init__(self):
self.leftEnc = wpilib.Encoder(
robot_map.LEFT_ENC_ONE, robot_map.LEFT_ENC_TWO, True, wpilib.Encoder.EncodingType.k4X)
self.rightEnc = wpilib.Encoder(
robot_map.RIGHT_ENC_ONE, robot_map.RIGHT_ENC_TWO, True, wpilib.Encoder.EncodingType.k4X)
self.elevatorEnc = wpilib.Encoder(
robot_map.ELEVATOR_ENC_ONE, robot_map.ELEVATOR_ENC_TWO, False, wpilib.Encoder.EncodingType.k4X)
self.ahrs = AHRS.create_i2c()
def createObjects(self):
self.chassis_left_motor_master = WPI_TalonSRX(1)
self.chassis_left_motor_slave = WPI_TalonSRX(2)
self.chassis_right_motor_master = WPI_TalonSRX(3)
self.chassis_right_motor_slave = WPI_TalonSRX(4)
self.chassis_navx = AHRS.create_spi()
self.joystick = Joystick(0)
def robotInit(self):
#Controllers
self.driver = wpilib.XboxController(0)
self.operator = wpilib.XboxController(1)
#Motors
self.left_motor_1 = rev.CANSparkMax(robotmap.LEFT_LEADER_ID, rev.MotorType.kBrushed)
self.left_motor_2 = rev.CANSparkMax(robotmap.LEFT_FOLLOWER_ID, rev.MotorType.kBrushed)
self.right_motor_1 = rev.CANSparkMax(robotmap.RIGHT_LEADER_ID, rev.MotorType.kBrushed)
self.right_motor_2 = rev.CANSparkMax(robotmap.RIGHT_FOLLOWER_ID, rev.MotorType.kBrushed)
self.left_motor_1.setClosedLoopRampRate(1.0)
self.left_motor_2.setClosedLoopRampRate(1.0)
self.right_motor_1.setClosedLoopRampRate(1.0)
self.right_motor_2.setClosedLoopRampRate(1.0)
self.left_side = wpilib.SpeedControllerGroup(self.left_motor_1, self.left_motor_2)
self.right_side = wpilib.SpeedControllerGroup(self.right_motor_1, self.right_motor_2)
#Drivetrain
self.drivetrain = wpilib.drive.DifferentialDrive(self.left_side, self.right_side)
#TODO: Add subsystems and sensors as the code is written
#TODO: SmartDashboard
#Pneumatics
self.colorPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.COLOR_SENSOR_EXTEND, robotmap.COLOR_SENSOR_RETRACT))
self.climberPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.CLIMBER_EXTEND, robotmap.CLIMBER_RETRACT))
self.gearshiftPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.DRIVE_SHIFT_EXTEND, robotmap.DRIVE_SHIFT_RETRACT))
self.climberWinchMotor1 = rev.CANSparkMax(robotmap.CLIMBER_WINCH_MOTOR1, rev.MotorType.kBrushed)
self.climberWinchMotor2 = rev.CANSparkMax(robotmap.CLIMBER_WINCH_MOTOR2, rev.MotorType.kBrushed)
# Color Sensor
self.colorSensor = color_sensor()
self.colorSensorMotor = rev.CANSparkMax(robotmap.COLOR_SENSOR_MOTOR, rev.MotorType.kBrushed)
self.stopColorMap = {"R":"B", "Y":"G", "B":"R", "G":"Y"}
self.gameData = ""
self.setupColorSensor()
#Shooter
self.shooter = shooter(robotmap.LOADER, robotmap.SHOOTER)
# Gyro
self.ahrs = AHRS.create_spi()
self.Aimer = Aimer(self.ahrs)
#network tables
self.sd = NetworkTables.getTable('VISION')
def robotInit(self):
self.navx = AHRS.create_spi()
self.navx.reset()
#joysticks
self.joystick = wpilib.Joystick(0)
self.buttonsJoystick1 = wpilib.Joystick(1)
self.buttonsJoystick2 = wpilib.Joystick(2)
self.joystickDeadband = .2
#buttons
'''self.climbExtendButton = wpilib.DriverStation.getStickButton(1, 1)
self.climbContractButton = wpilib.DriverStation.getStickButton(1, 2)'''
self.feederInButton = wpilib.DriverStation.getStickButton(
1, 1) #queueueue clear
#self.feederOutButton = wpilib.DriverStation.getStickButton(1, 4) #KILL THIS
self.intakeInButton = wpilib.DriverStation.getStickButton(0, 1)
self.intakeOutButton = wpilib.DriverStation.getStickButton(0, 2)
self.autoManualSwitch = wpilib.DriverStation.getStickButton(2, 1)
self.manualFlywheelSwitch = wpilib.DriverStation.getStickButton(2, 4)
'''self.manualTurretHoodDial = wpilib.DriverStation.getStickButton(1, 5)
self.manualTurretSpinDial = wpilib.DriverStation.getStickButton(1, 6)
#climbing object instantiation
self.climb = Climb(9,0.5) #motorID and speed
'''
#feeder object instantiation
self.feeder = Feed(10, 0.5) #motorID and speed
#intake object instantiation
self.intake = Intake(
11, 12, 0.5, 0.7) #intakeID, halfMoonID, and corresponding speeds
#manual turret object instantiation
self.manualTurret = ManualTurret(
13, 14, 1, 20
) #rotate motor, flywheel motor, servoID, and rotateThreshold from middle to max
#drivetrain object instantiation and init
self.drive = DriveTrain()
self.drive.zeroEncoders()
self.fieldOriented = False
self.perfectlyLegitFlywheelRPM = 0
#debugging messages
self.scaling = .5
wpilib.SmartDashboard.putNumber("Joystick scale factor", self.scaling)
wpilib.SmartDashboard.putNumber("Joystick deadband",
self.joystickDeadband)
wpilib.SmartDashboard.putNumber("Flywheel RPM",
self.perfectlyLegitFlywheelRPM)
wpilib.SmartDashboard.putBoolean("Field Oriented", self.fieldOriented)
def robotInit(self):
self.left_motor = wpilib.Spark(0)
self.right_motor = wpilib.Spark(1)
self.drive = wpilib.drive.DifferentialDrive(self.left_motor, self.right_motor)
self.stick = wpilib.Joystick(0)
self.ahrs = AHRS.create_spi()
# self.ahrs = AHRS.create_i2c()
turnController = PIDController(
self.kP, self.kI, self.kD, period = 1.0
)
self.turnController = turnController
self.rotateToAngleRate = 0
def robotInit(self):
self.navx = AHRS.create_spi()
self.navx.reset()
self.joystick = wpilib.Joystick(0)
self.joystickDeadband = .2
self.drive = DriveTrain() #keep in mind that these are all arbitrary names
self.drive.zeroEncoders() #feel free to change the weird ones
self.fieldOriented = False
self.scaling = .5
wpilib.SmartDashboard.putNumber("Joystick scale factor", self.scaling)
wpilib.SmartDashboard.putNumber("Joystick deadband", self.joystickDeadband)
wpilib.SmartDashboard.putBoolean("Field Oriented", self.fieldOriented)
def __init__(self):
super().__init__("FollowJoystick")
# Hardware
self.ahrs = AHRS.create_spi()
self.stick = oi.joystick
# Quantities
self.angle = 0
self.stime = None
self.xInv = -1
self.yInv = -1
self.zInv = 1
if config.centric:
self.angle = self.ahrs.getAngle()
def robotInit(self):
#assigns driver as controller 0 and operator as controller 1
self.driver = wpilib.XboxController(0)
self.operator = wpilib.XboxController(1)
#self.elevatorController = ElevatorController(self.operator, self.logger)
#GYRO
self.gyro = AHRS.create_spi()
#DRIVETRAIN
left = createTalonAndsub_units(LEFT_MASTER_ID, LEFT_sub_unit_1_ID,
LEFT_sub_unit_2_ID)
right = createTalonAndsub_units(RIGHT_MASTER_ID, RIGHT_sub_unit_1_ID,
RIGHT_sub_unit_2_ID)
self.drivetrain = Drivetrain(left, right, self.gyro)
self.autoBalancing = False
def robotInit(self):
# Channels for the wheels
frontLeftChannel = 2
rearLeftChannel = 3
frontRightChannel = 1
rearRightChannel = 0
self.myRobot = wpilib.RobotDrive(frontLeftChannel, rearLeftChannel,
frontRightChannel, rearRightChannel)
self.myRobot.setExpiration(0.1)
self.stick = wpilib.Joystick(0)
#
# Communicate w/navX MXP via the MXP SPI Bus.
# - Alternatively, use the i2c bus.
# See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details
#
self.ahrs = AHRS.create_spi()
# self.ahrs = AHRS.create_i2c()
turnController = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.ahrs,
output=self)
turnController.setInputRange(-180.0, 180.0)
turnController.setOutputRange(-1.0, 1.0)
turnController.setAbsoluteTolerance(self.kToleranceDegrees)
turnController.setContinuous(True)
self.turnController = turnController
self.rotateToAngleRate = 0
# Add the PID Controller to the Test-mode dashboard, allowing manual */
# tuning of the Turn Controller's P, I and D coefficients. */
# Typically, only the P value needs to be modified. */
wpilib.LiveWindow.addActuator("DriveSystem", "RotateController",
turnController)
def robotInit(self):
# Channels for the wheels
frontLeftChannel = 1
rearLeftChannel = 2
frontRightChannel = 3
rearRightChannel = 4
self.frontLeftMotor = wpilib.Talon(frontLeftChannel)
self.rearLeftMotor = wpilib.Talon(rearLeftChannel)
self.frontRightMotor = wpilib.Talon(frontRightChannel)
self.rearRightMotor = wpilib.Talon(rearRightChannel)
self.drive = wpilib.drive.MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.stick = wpilib.Joystick(0)
#
# Communicate w/navX MXP via the MXP SPI Bus.
# - Alternatively, use the i2c bus.
# See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details
#
self.ahrs = AHRS.create_spi()
# self.ahrs = AHRS.create_i2c()
turnController = wpimath.controller.PIDController(
self.kP,
self.kI,
self.kD,
)
turnController.enableContinuousInput(-180.0, 180.0)
turnController.setTolerance(self.kToleranceDegrees)
self.turnController = turnController
def robotInit(self):
try:
wpilib.CameraServer.launch()
except:
pass
self.stick = wpilib.Joystick(0)
self.base = Base(rev.CANSparkMax(4, rev.MotorType.kBrushless),
rev.CANSparkMax(2, rev.MotorType.kBrushless),
rev.CANSparkMax(3, rev.MotorType.kBrushless),
rev.CANSparkMax(1, rev.MotorType.kBrushless),
AHRS.create_spi(wpilib.SPI.Port.kMXP), self.stick)
self.lift = Lift(WPI_TalonSRX(3), wpilib.DigitalInput(0),
wpilib.DigitalInput(1), self.stick)
self.arm = Arm(wpilib.Solenoid(5, 2), wpilib.Solenoid(5, 4),
wpilib.Solenoid(5, 1), wpilib.Solenoid(5, 5),
self.stick)
self.testvar = 0
self.ballintake = BallIntake(WPI_TalonSRX(4))
try:
self.ledController = LEDController.getInstance()
except:
pass
# Various one-time initializations happen here.
self.lift.initSensor()
self.base.navx.reset()
try:
NetworkTables.initialize()
except:
pass
try:
self.visiontable = NetworkTables.getTable("visiontable")
except:
pass
self.lights = False
self.op = False
self.testButton = JoystickButton(self.stick, 16)
self.roller = Roller(WPI_TalonSRX(20), WPI_TalonSRX(10), self.stick,
"init")
def createObjects(self):
'''Create motors and stuff here'''
with self.consumeExceptions():
Map.load_json(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
"map.json"))
self.chassis_ports = Map.get_map(
)["subsystems"]["chassis"]["can_motors"]
self.chassis_left_master = WPI_TalonSRX(
self.chassis_ports["left_master"])
self.chassis_left_slave = WPI_TalonSRX(
self.chassis_ports["left_slave"])
self.chassis_right_master = WPI_TalonSRX(
self.chassis_ports["right_master"])
self.chassis_right_slave = WPI_TalonSRX(
self.chassis_ports["right_slave"])
self.chassis_navx = AHRS.create_spi()
self.left_joystick = Joystick(
Map.get_map()["operator_ports"]["left_stick"])
def robotInit(self):
self.navx = AHRS.create_spi()
wpilib.CameraServer.launch()
wpilib.DigitalOutput(0).set(0)
self.joystick = wpilib.Joystick(0)
self.auxiliary = wpilib.XboxController(1)
self.drive = DriveTrain()
self.climb = Climb(16) #climb ID
self.feeder = Feeder(9) #feeder ID
self.intake = Intake(11, 10) #intake ID, half moon ID
self.turret = Turret(15, 14) #rotate ID, flywheel ID
self.drive.zeroEncoders()
self.climb.coast()
self.intake.coast()
#constants
self.joystickDeadband = .2
self.scaling = .55
self.intakeSpeed = .35
self.halfMoonSpeed = .8
self.feederSpeed = .85
self.turretSpeed = .2
self.climbExtend = .35
self.climbContract = .75
self.manualFlywheelSpeed = 8.5 #volts
self.navx.reset()
self.kP = 0.0016
self.kI = 0.0003
self.kD = 0
self.driveAngleController = wpilib.controller.PIDController(
self.kP, self.kI, self.kD)
self.driveAngleController.enableContinuousInput(-180, 180)
wpilib.SmartDashboard.putNumber("Shooter RPM", 0)
def __init__(self):
super().__init__('Follow Joystick')
self.stime = None
# Communicate w/navX MXP via the MXP SPI Bus.
# - Alternatively, use the i2c bus.
# See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details
#
self.kP = 0.75
self.kI = 0.00
self.kD = 0.00
self.kF = 0.00
self.ahrs = AHRS.create_spi()
self.kToleranceDegrees = 2.0
turnController = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.ahrs,
output=self)
turnController.setInputRange(-180.0, 180.0)
turnController.setOutputRange(-1.0, 1.0)
turnController.setAbsoluteTolerance(self.kToleranceDegrees)
turnController.setContinuous(True)
self.turnController = turnController
self.rotateToAngleRate = 0
# Add the PID Controller to the Test-mode dashboard, allowing manual */
# tuning of the Turn Controller's P, I and D coefficients. */
# Typically, only the P value needs to be modified. */
wpilib.Sendable.setName(turnController, "RotateController")
self.tm = wpilib.Timer()
self.tm.start()
self.stick = oi.joystick
def robotInit(self):
self.timer = wpilib.Timer()
self.timer.start()
self.auto_timer = wpilib.Timer()
self.gyro = AHRS.create_spi()
self.gyro.reset()
self.compressor = wpilib.Compressor(self.PCM_CANID)
self.compressor.setClosedLoopControl(False)
#self.compressor.setClosedLoopControl(True)
#Solenoids galore
self.gearAdjustExtend = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_ADJUST_EXTEND_SOLENOID)
self.gearAdjustRetract = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_ADJUST_RETRACT_SOLENOID)
self.gearPusherExtend = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_PUSHER_EXTEND_SOLENOID)
self.gearPusherRetract = wpilib.Solenoid(
self.PCM_CANID, self.GEAR_PUSHER_RETRACT_SOLENOID)
self.gearDoorDrop = wpilib.Solenoid(self.PCM_CANID,
self.GEAR_DOOR_DROP_SOLENOID)
self.gearDoorRaise = wpilib.Solenoid(self.PCM_CANID,
self.GEAR_DOOR_RAISE_SOLENOID)
self.ballDoorOpen = wpilib.Solenoid(self.PCM_CANID,
self.BALL_DOOR_OPEN_SOLENOID)
self.ballDoorClose = wpilib.Solenoid(self.PCM_CANID,
self.BALL_DOOR_CLOSE_SOLENOID)
"""Robot initialization function"""
self.frontLeftMotor = ctre.WPI_TalonSRX(self.frontLeftChannel)
self.rearLeftMotor = ctre.WPI_TalonSRX(self.rearLeftChannel)
self.frontRightMotor = ctre.WPI_TalonSRX(self.frontRightChannel)
self.rearRightMotor = ctre.WPI_TalonSRX(self.rearRightChannel)
self.tickler = Spark(self.ballTickler)
self.grabber = Victor(self.ballGrabMotor)
# invert the left side motors
self.frontLeftMotor.setInverted(True)
self.frontRightMotor.setInverted(True)
self.rearLeftMotor.setInverted(True)
self.rearRightMotor.setInverted(True)
self.talons = [
self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor,
self.rearRightMotor
]
for talon in self.talons:
talon.configNominalOutputForward(0.0, 25)
talon.configNominalOutputReverse(0.0, 25)
talon.configPeakOutputForward(1.0, 25)
talon.configPeakOutputReverse(-1.0, 25)
talon.enableVoltageCompensation(True)
talon.configVoltageCompSaturation(11.5, 25)
talon.configOpenLoopRamp(0.125, 25)
talon.config_kP(0, 0.375, 25)
talon.config_kI(0, 0.0, 25)
talon.config_kD(0, 0.0, 25)
talon.config_kF(0, 0.35, 25)
talon.selectProfileSlot(0, 0)
talon.configClosedLoopPeakOutput(0, 1.0, 25)
talon.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,
0, 25)
talon.configSelectedFeedbackCoefficient(1.0, 0, 25)
talon.set(ctre.ControlMode.PercentOutput, 0)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(False)
self.lstick = wpilib.Joystick(self.lStickChannel)
self.rstick = wpilib.Joystick(self.rStickChannel)
def robotInit(self):
#I got fed up with how long 'frontLeft, frontRight, etc.' looked
#Drive motors
self.flDriveMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.frDriveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.rlDriveMotor = rev.CANSparkMax(7, rev.MotorType.kBrushless)
self.rrDriveMotor = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.driveMotors = (self.flDriveMotor, self.frDriveMotor,
self.rlDriveMotor, self.rrDriveMotor)
#Turn motors
self.flTurnMotor = rev.CANSparkMax(3, rev.MotorType.kBrushless)
self.frTurnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.rlTurnMotor = rev.CANSparkMax(8, rev.MotorType.kBrushless)
self.rrTurnMotor = rev.CANSparkMax(6, rev.MotorType.kBrushless)
self.turnMotors = (self.flTurnMotor, self.frTurnMotor,
self.rlTurnMotor, self.rrTurnMotor)
#Turn encoders
self.flTurnEncoder = self.flTurnMotor.getEncoder()
self.frTurnEncoder = self.frTurnMotor.getEncoder()
self.rlTurnEncoder = self.rlTurnMotor.getEncoder()
self.rrTurnEncoder = self.rrTurnMotor.getEncoder()
self.turnEncoders = (self.flTurnEncoder, self.frTurnEncoder,
self.rlTurnEncoder, self.rrTurnEncoder)
for encoder in self.turnEncoders:
encoder.setPositionConversionFactor(
20) #makes the encoder output in degrees
'''We should use whichever encoder (built-in or absolute) gives the greatest results. If we use the absolute, easy. If
we use the build-in, the following will be an issue and a potential solution.
Every time we turn off/on the robot, these encoders will read 0 and the absolute encoder will read the real value.
I didn't do it here, but we will need a function that moves all the turn motors so that their positions are set to what
the absolute encoder says it is. Something like
for index, encoder in enumerate(self.turnEncoders):
encoder.setPosition(self.absoluteEncoders[index]%360)
because I've been using an unhealthy amount of for loops this reason. This would allow the auto to go from there, although
the motors should have been zeroed beforehand. There would be a similar function that can be ran in teleop when a sufficiently
out-of-the-way button is pressed so that we can automatically reset the motors to 0 when in the pits, something like
for index, encoder in enumerate(self.turnEncoders):
encoder.setPosition(self.absoluteEncoders[index]%360)
self.turnControllers[index].setReference(0, rev.controlType.kPosition)
that of course then removes itself from the queue once all four motor encoders are at 0 and all the absolute encoders are at 0.
Despite a reset being available in the pits, we would still need to set the motor encoders to the absolute encoder positions
to account for possible bumping, absent-minded turning, or in case we didn't have time to return to the pits and so were only
able to reset the wheel positions by hand'''
# PID coefficients done in a very lazy way
carpet = False
if carpet:
self.kP = .006
self.kI = .001
self.kD = 2.0e-6
self.PIDTolerance = 1
else:
self.kP = .0039
self.kI = 0
self.kD = 2.0e-6
self.PIDTolerance = 1.0
#PID controllers for the turn motors
self.flTurnController = PIDController(self.kP, self.kI, self.kD)
self.frTurnController = PIDController(self.kP, self.kI, self.kD)
self.rlTurnController = PIDController(self.kP, self.kI, self.kD)
self.rrTurnController = PIDController(self.kP, self.kI, self.kD)
self.turnControllers = (self.flTurnController, self.frTurnController,
self.rlTurnController, self.rrTurnController)
for controller in self.turnControllers:
controller.setTolerance(self.PIDTolerance)
controller.enableContinuousInput(0, 360)
self.joystick = wpilib.Joystick(0)
self.joystickDeadband = .05
self.timer = wpilib.Timer(
) #used to use it while testing stuff, don't need it now, but oh well
self.robotLength = 34.0
self.robotWidth = 30.0
self.ahrs = AHRS.create_spi() #interface with the navx
def __init__(self, robot):
print("[DriveTrain] initializing")
super().__init__("DriveTrain")
self.robot = robot
# set PIDF values
if robot.isReal():
self.kP = 0.030
self.kI = 0.025
self.kD = 0.001
self.kF = 0.000
else:
self.kP = 0.04
self.kI = 0.0004
self.kD = 0.14
self.kF = 0.00
left_front_motor = ctre.WPI_TalonSRX(self.DRIVETRAIN_LEFT_FRONT_MOTOR)
left_front_motor.setInverted(False)
left_front_motor.setSensorPhase(False)
left_front_motor.setNeutralMode(BaseMotorController.NeutralMode.Brake)
self.left_front_motor = left_front_motor
left_rear_motor = ctre.WPI_TalonSRX(self.DRIVETRAIN_LEFT_REAR_MOTOR)
left_rear_motor.setInverted(False)
left_rear_motor.setSensorPhase(False)
left_rear_motor.setNeutralMode(BaseMotorController.NeutralMode.Brake)
left_rear_motor.follow(left_front_motor)
self.left_rear_motor = left_rear_motor
right_front_motor = ctre.WPI_TalonSRX(self.DRIVETRAIN_RIGHT_FRONT_MOTOR)
right_front_motor.setInverted(True)
right_front_motor.setSensorPhase(False)
right_front_motor.setNeutralMode(BaseMotorController.NeutralMode.Brake)
self.right_front_motor = right_front_motor
right_rear_motor = ctre.WPI_TalonSRX(self.DRIVETRAIN_RIGHT_REAR_MOTOR)
right_rear_motor.setInverted(True)
right_rear_motor.setSensorPhase(False)
right_rear_motor.setNeutralMode(BaseMotorController.NeutralMode.Brake)
right_rear_motor.follow(right_front_motor)
self.right_rear_motor = right_rear_motor
self.left = wpilib.SpeedControllerGroup(left_front_motor, left_rear_motor)
self.right = wpilib.SpeedControllerGroup(right_front_motor, right_rear_motor)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.drive.setDeadband(0)
self.drive.setSafetyEnabled(False)
# setup NavX and turn controller
self.ahrs = AHRS.create_spi()
turnController = wpilib.PIDController(
self.kP, self.kI, self.kD, source=self.ahrs, output=self
)
turnController.setInputRange(-180.0, 180.0)
turnController.setOutputRange(-1.0, 1.0)
turnController.setAbsoluteTolerance(self.kToleranceDegrees)
turnController.setContinuous(True)
self.turnController = turnController
self.rotateToAngleRate = 0
# setup LiveWindow
# self.left_front_motor.setName("DriveTrain", "Front Left Motor")
# self.left_rear_motor.setName("DriveTrain", "Rear Left Motor")
# self.right_front_motor.setName("DriveTrain", "Front Right Motor")
# self.right_rear_motor.setName("DriveTrain", "Rear Right Motor")
# self.ahrs.setName("DriveTrain", "Gyross")
# self.turnController.setName("DriveTrain", "RotateControllers")
wpilib.LiveWindow.addActuator("Drive Train", "Front Left Motor", self.left_front_motor)
wpilib.LiveWindow.addActuator("Drive Train", "Rear Left Motor", self.left_rear_motor)
wpilib.LiveWindow.addActuator("Drive Train", "Front Right Motor", self.right_front_motor)
wpilib.LiveWindow.addActuator("Drive Train", "Rear Right Motor", self.right_rear_motor)
wpilib.LiveWindow.addActuator("Drive Train", "RotateController", self.turnController)
wpilib.LiveWindow.addSensor("Drive Train", "Gyro", self.ahrs)
