def createMotor(self, MotorSpec):
motr = None
if MotorSpec['ContType'] == 'CAN':
if MotorSpec['Type'] == 'TalonSRX':
if MotorSpec['jobType'] == 'master':
motr = TalonSRX(MotorSpec['port'])
elif MotorSpec['jobType'] == 'slave':
motr = TalonSRX(MotorSpec['port'])
motr.setInverted(MotorSpec['inverted'])
motr.set(ctre.ControlMode.Follower, MotorSpec['masterPort'])
if MotorSpec['Type'] == 'TalonFX':
if MotorSpec['jobType'] == 'master':
motr = TalonFX(MotorSpec['port'])
elif MotorSpec['jobType'] == 'slave':
motr = TalonFX(MotorSpec['port'])
motr.setInverted(MotorSpec['inverted'])
motr.set(ctre.ControlMode.Follower, MotorSpec['masterPort'])
if MotorSpec['Type'] == 'VictorSPX':
if MotorSpec['jobType'] == 'master':
motr = VictorSPX(MotorSpec['port'])
elif MotorSpec['jobType'] == 'slave':
motr = VictorSPX(MotorSpec['port'])
motr.setInverted(MotorSpec['inverted'])
motr.set(ctre.ControlMode.Follower, MotorSpec['masterPort'])
if MotorSpec['Type'] == 'SparkMax':
motr = SparkMax(MotorSpec['port'])
else:
print("IDK your motor")
return motr
class MyRobot(TimedRobot):
leftMotor: TalonSRX
rightMotor: TalonSRX
mySolenoid = Solenoid(0)
navx = AHRS.create_i2c()
def robotInit(self):
self.leftMotor = VictorSPX(0)
self.rightMotor = TalonSRX("pizza")
def teleopInit(self):
self.leftMotor.setSpeed(0.3)
self.rightMotor.setSpeed(0.3)
self.mySolenoid.set(True)
angle = self.navx.getAngle()
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 __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 robotInit(self):
self.leftMotor = VictorSPX(0)
self.rightMotor = TalonSRX("pizza")
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))