class Elevator(Subsystem):
"""
For both Hatches and Balls
"""
def __init__(self):
super().__init__("Elevator")
# Initializes the elevator Talon, put it as one for now
self.elevatorleader = TalonSRX(CAN_ELEVATOR_LEADER)
set_motor(self.elevatorleader, TalonSRX.NeutralMode.Brake, False)
self.elevatorleader.configSelectedFeedbackSensor(
FeedbackDevice.QuadEncoder, 0, 0)
self.elevatorleader.setSelectedSensorPosition(0, 0, 0)
self.elevatorleader.setSensorPhase(False)
self.elevatorfollower = VictorSPX(CAN_ELEVATOR_FOLLOWER)
set_motor(self.elevatorfollower, TalonSRX.NeutralMode.Brake, False)
self.elevatorfollower.follow(self.elevatorleader)
# The preference table is used to get values to the code while the
# robot is running, useful
self.prefs = Preferences.getInstance()
# for tuning the PIDs and stuff
self.elevator_zero = False
self.set_values()
def set_position(self, position):
if self.elevator_zero:
self.elevatorleader.set(ControlMode.MotionMagic, position)
else:
print("Position Not Set")
def elevator_speed(self, speed):
self.elevatorleader.set(ControlMode.PercentOutput, speed)
def set_values(self):
# This will set the PIDs, velocity, and acceleration, values from the
# preference table so we can tune them easily
#self.elevatorleader.config_kP(0, self.prefs.getFloat("Elevator P", 0.1), 0)
#self.elevatorleader.config_kI(0, self.prefs.getFloat("Elevator I", 0.0), 0)
#self.elevatorleader.config_kD(0, self.prefs.getFloat("Elevator D", 0.0), 0)
#self.elevatorleader.configMotionCruiseVelocity(
# int(self.prefs.getInt("Elevator Velocity", 1024)), 0)
#self.elevatorleader.configMotionAcceleration(
# int(self.prefs.getInt("Elevator Acceleration", 1024)), 0)
self.elevatorleader.config_kP(0, 3.2, 0)
self.elevatorleader.config_kI(0, 0.0, 0)
self.elevatorleader.config_kD(0, 0.0, 0)
self.elevatorleader.configMotionCruiseVelocity(int(1000), 0)
self.elevatorleader.configMotionAcceleration(int(1000), 0)
def publish_data(self):
# This will print the position and velocity to the smartDashboard
SmartDashboard.putNumber(
"Elevator Position",
self.elevatorleader.getSelectedSensorPosition(0))
#SmartDashboard.putNumber("Elevator Velocity", self.elevatorleader.getSelectedSensorVelocity(0))
#SmartDashboard.putNumber("Elevator Current", self.elevatorleader.getOutputCurrent())
#SmartDashboard.putNumber("Elevator Output", self.elevatorleader.getMotorOutputPercent())
def check_for_zero(self):
if not self.elevator_zero:
if self.elevatorleader.isFwdLimitSwitchClosed():
self.elevatorleader.setSelectedSensorPosition(0, 0, 0)
self.elevator_zero = True
def initDefaultCommand(self):
self.setDefaultCommand(elv_zero())
def get_position(self):
return self.elevatorleader.getSelectedSensorPosition(0)
class Payload(Subsystem):
"""
For both Hatches and Balls
"""
def __init__(self):
super().__init__("Payload")
# TODO Two limit switches on the leader
self.prefs = Preferences.getInstance()
self.elbowleader = TalonSRX(CAN_ELBOW_LEADER)
set_motor(self.elbowleader, TalonSRX.NeutralMode.Brake, False)
self.elbowleader.configSelectedFeedbackSensor(
FeedbackDevice.QuadEncoder, 0, 0)
self.elbowleader.setSelectedSensorPosition(0, 0, 0)
self.elbowleader.selectProfileSlot(0, 0)
self.elbowleader.setSensorPhase(True)
self.elbowfollower = VictorSPX(CAN_ELBOW_FOLLOWER)
set_motor(self.elbowfollower, VictorSPX.NeutralMode.Brake, False)
self.elbowfollower.follow(self.elbowleader)
self.baller = TalonSRX(CAN_BALL_INTAKE)
set_motor(self.baller, TalonSRX.NeutralMode.Brake, False)
self.elbow_zero = False
self.DS = DoubleSolenoid(2, 3)
self.set_values()
def initDefaultCommand(self):
self.setDefaultCommand(BallZ())
def set_wheels_speed(self, speed):
self.baller.set(ControlMode.PercentOutput, speed)
#print(speed)
def hatch_punch_out(self):
subsystems.SERIAL.fire_event('Punch It')
self.DS.set(DoubleSolenoid.Value.kForward)
def hatch_punch_in(self):
self.DS.set(DoubleSolenoid.Value.kReverse)
def set_position(self, pos):
if self.elbow_zero:
self.elbowleader.set(mode=ControlMode.MotionMagic, demand0=pos)
else:
print("Elbow Not Set")
def set_speed(self, speed):
self.elbowleader.set(mode=ControlMode.PercentOutput, demand0=speed)
def set_values(self):
self.elbowleader.config_kP(0, 0.8, 0)
self.elbowleader.config_kI(0, 0.0, 0)
self.elbowleader.config_kD(0, 0.0, 0)
self.elbowleader.configMotionCruiseVelocity(int(200), 0)
self.elbowleader.configMotionAcceleration(int(200), 0)
def get_position(self):
return self.elbowleader.getSelectedSensorPosition(0)
def check_for_zero(self):
if not self.elbow_zero:
if self.elbowleader.isRevLimitSwitchClosed():
self.elbowleader.setSelectedSensorPosition(0, 0, 0)
self.elbow_zero = True
def publish_data(self):
#print(self.elbowleader.getSelectedSensorPosition(0))
SmartDashboard.putNumber("elbowPosition",
self.elbowleader.getSelectedSensorPosition(0))