class Robot(TimedRobot):
def robotInit(self):
# Right Motors
self.RightMotor1 = WPI_TalonFX(1)
self.RightSensor1 = SensorCollection(self.RightMotor1)
self.RightMotor2 = WPI_TalonFX(2)
self.RightMotor3 = WPI_TalonFX(3)
self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1,
self.RightMotor2,
self.RightMotor3)
# Left Motors
self.LeftMotor1 = WPI_TalonFX(4)
self.LeftMotor2 = WPI_TalonFX(5)
self.LeftMotor3 = WPI_TalonFX(6)
self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1,
self.LeftMotor2,
self.LeftMotor3)
# self.drive = DifferentialDrive(self.leftDriveMotors,
# self.rightDriveMotors)
self.testEncoder = Encoder(1, 2, 3, Encoder.EncodingType.k4X)
self.dashboard = NetworkTables.getTable("SmartDashboard")
def disabledInit(self):
pass
def autonomousInit(self):
pass
def teleopInit(self):
pass
def testInit(self):
pass
def robotPeriodic(self):
self.dashboard.putNumber("Encoder", self.RightSensor1.getQuadratureVelocity()/2048*60)
def disabledPeriodic(self):
pass
def autonomousPeriodic(self):
speed = self.testEncoder.get() / 1028
self.leftDriveMotors.set(speed)
self.rightDriveMotors.set(speed)
def teleopPeriodic(self):
pass
def testPeriodic(self):
pass
class Intake(Subsystem):
def __init__(self):
super().__init__("Intake")
self.talon_1 = WPI_TalonSRX(robotmap.talon_intake_1)
self.talon_2 = WPI_TalonSRX(robotmap.talon_intake_2)
self.talon_group = SpeedControllerGroup(self.talon_1, self.talon_2)
@classmethod
def setSpd(cls, spd_new):
robotmap.spd_intake = spd_new
def intake(self, spd_temp=None, is_fixed=None):
self.talon_1.setInverted(True)
self.talon_2.setInverted(False)
if (spd_temp is None) and (is_fixed is None):
if oi.joystick.getAxis(2) - oi.joystick.getAxis(3) >= 0.8:
self.talon_group.set(0.8)
elif oi.joystick.getAxis(2) - oi.joystick.getAxis(3) < 0.8:
self.talon_group.set(
oi.joystick.getAxis(2) - oi.joystick.getAxis(3))
elif (spd_temp is not None) and (is_fixed is None):
self.talon_group.set(spd_temp)
elif (spd_temp is not None) and (is_fixed is not None):
self.talon_group.set(robotmap.spd_intake)
else:
raise ("intake() fail!")
def eject(self, spd_temp=None, is_fixed=None):
self.talon_1.setInverted(False)
self.talon_2.setInverted(True)
if (spd_temp is None) and (is_fixed is None):
self.talon_group.set(oi.joystick.getAxis(3))
elif (spd_temp is not None) and (is_fixed is None):
self.talon_group.set(spd_temp)
elif (spd_temp is not None) and (is_fixed is not None):
self.talon_group.set(robotmap.spd_intake)
else:
raise ("eject() fail!")
def dislodge(self):
self.eject(True)
time.sleep(0.1)
self.intake(True)
time.sleep(0.2)
def stop(self):
self.talon_group.stopMotor()
def initDefaultCommand(self):
pass