def robotInit(self):
"""Robot initialization function"""
self.gyro = wpilib.ADXRS450_Gyro()
self.gyro.reset()
self.gyro.calibrate()
self.modules = [
SwerveModule(
0.8 / 2 - 0.125,
0.75 / 2 - 0.1,
CANSparkMax(9, MotorType.kBrushless),
ctre.WPI_TalonFX(3),
steer_reversed=False,
drive_reversed=True,
),
SwerveModule(
-0.8 / 2 + 0.125,
-0.75 / 2 + 0.1,
CANSparkMax(7, MotorType.kBrushless),
ctre.WPI_TalonFX(5),
steer_reversed=False,
drive_reversed=True,
),
]
self.kinematics = SwerveDrive2Kinematics(self.modules[0].translation,
self.modules[1].translation)
self.joystick = wpilib.Joystick(0)
self.spin_rate = 1.5
def robotInit(self):
#Drive Motors
self.motor1 = ctre.WPI_TalonSRX(
1) # Initialize the TalonSRX on device 1.
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(3)
self.motor4 = ctre.WPI_TalonSRX(4)
self.motor5 = ctre.WPI_TalonFX(5) #intake Motor
self.motor6 = ctre.WPI_TalonFX(6) #Shooter Motor
self.motor7 = ctre.WPI_VictorSPX(7) #Intake Arm
self.motor8 = ctre.WPI_VictorSPX(8) #Belt Drive
self.joy = wpilib.Joystick(0)
self.stick = wpilib.Joystick(
1) #this is a controller, also acceptable to use Joystick
self.intake = wpilib.DoubleSolenoid(0, 1)
self.balls = wpilib.DoubleSolenoid(2, 3)
self.left = wpilib.SpeedControllerGroup(self.motor1, self.motor2)
self.right = wpilib.SpeedControllerGroup(self.motor3, self.motor4)
#This is combining the Speed controls from above to make a left and right
#for the drive chassis. Note that self.motor1 and self.motor2 are combined to make self.left
#then self.motor3 and self.motor4 are combined to make self.right. This is done to Make
#the differential drive easier to setup
self.myRobot = wpilib.drive.DifferentialDrive(self.left, self.right)
#Here is our DifferentialDrive, Ultimately stating, Left side and Right side of chassis
#An Alternative to DifferentialDrive is this:
#self.robodrive = wpilib.RobotDrive(self.motor1, self.motor4, self.motor3, self.motor2)
#where motor 1 & 4 are left, and 2 & 3 are right
self.myRobot.setExpiration(0.1)
#These components here are for Autonomous Modes, and allows you to call parts and
#components here to be used IN automodes. For example- Our chassis above is labeled
#'self.myRobot', below in the self.components, If we want to use our chassis to drive
#in Autonomous, we need to call it in the fashion below. It is also encouraged to
#reuse the naming of the components from above to avoid confusion below. i.e.
#'Chassis': self.myRobot, In autonomous creation, I would then be using the variable
#self.chassis.set() instead of self.myRobot.set() when doing commands.
self.components = {
'myRobot': self.myRobot, #Chassis Driving
'motor5': self.motor5,
'motor6': self.motor6, #A speed control that is used for intake
'intake': self.intake,
'balls': self.balls #pneumatics arm that is not setup on bot yet
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def createObjects(self):
"""Robot initialization function"""
self.logger.info("pyinfiniterecharge %s", GIT_COMMIT)
self.chassis_left_front = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.chassis_left_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.chassis_right_front = rev.CANSparkMax(7, rev.MotorType.kBrushless)
self.chassis_right_rear = rev.CANSparkMax(6, rev.MotorType.kBrushless)
self.imu = NavX()
self.hang_winch_motor = ctre.WPI_TalonFX(30)
self.hang_kracken_hook_latch = wpilib.DoubleSolenoid(4, 5)
self.indexer_motors = [
ctre.WPI_TalonSRX(18),
ctre.WPI_TalonSRX(11),
ctre.WPI_TalonSRX(12),
ctre.WPI_TalonSRX(13),
ctre.WPI_TalonSRX(14),
]
self.piston_switch = wpilib.DigitalInput(
4) # checks if injector retracted
self.intake_arm_piston = wpilib.Solenoid(1)
self.intake_left_motor = rev.CANSparkMax(8, rev.MotorType.kBrushless)
self.intake_right_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless)
self.led_screen_led = wpilib.AddressableLED(0)
self.loading_piston = wpilib.DoubleSolenoid(6, 7)
self.shooter_centre_motor = ctre.WPI_TalonFX(2)
self.shooter_outer_motor = ctre.WPI_TalonFX(3)
self.range_counter = wpilib.Counter(6)
self.colour_sensor = rev.color.ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.spinner_motor = wpilib.Spark(2)
self.spinner_solenoid = wpilib.DoubleSolenoid(2, 3)
self.turret_centre_index = wpilib.DigitalInput(0)
self.turret_right_index = wpilib.DigitalInput(1)
self.turret_left_index = wpilib.DigitalInput(2)
self.turret_motor = ctre.WPI_TalonSRX(10)
# operator interface
self.driver_joystick = wpilib.Joystick(0)
self.MEMORY_CONSTANT = int(0.1 / self.control_loop_wait_time)
def createObjects(self):
"""Robot initialization function"""
self.logger.info("pyinfiniterecharge %s", GIT_COMMIT)
self.chassis_left_front = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.chassis_left_rear = rev.CANSparkMax(6, rev.MotorType.kBrushless)
self.chassis_right_front = rev.CANSparkMax(7, rev.MotorType.kBrushless)
self.chassis_right_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.imu = navx.AHRS.create_spi(update_rate_hz=50)
self.hang_winch_motor = ctre.WPI_TalonFX(30)
self.hang_kracken_hook_latch = wpilib.Solenoid(0)
self.intake_main_motor = ctre.WPI_TalonSRX(18)
self.indexer_motors = [
ctre.WPI_TalonSRX(11),
ctre.WPI_TalonSRX(12),
ctre.WPI_TalonSRX(13),
]
self.injector_motor = ctre.WPI_TalonSRX(14)
self.piston_switch = wpilib.DigitalInput(
4) # checks if injector retracted
self.intake_arm_piston = wpilib.Solenoid(1)
self.intake_left_motor = rev.CANSparkMax(8, rev.MotorType.kBrushless)
self.intake_right_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless)
self.led_screen_led = wpilib.AddressableLED(0)
self.loading_piston = wpilib.DoubleSolenoid(2, 3)
self.shooter_centre_motor = ctre.WPI_TalonFX(2)
self.shooter_outer_motor = ctre.WPI_TalonFX(3)
self.range_counter = wpilib.Counter(6)
self.turret_centre_index = wpilib.DigitalInput(0)
self.turret_right_index = wpilib.DigitalInput(1)
self.turret_left_index = wpilib.DigitalInput(2)
self.turret_motor = ctre.WPI_TalonSRX(10)
# operator interface
self.driver_joystick = wpilib.Joystick(0)
self.codriver_gamepad = wpilib.XboxController(1)
self.MEMORY_CONSTANT = int(0.1 / self.control_loop_wait_time)
# how long before data times out
self.has_zeroed = False
def createObjects(self):
"""Robot initialization function"""
# Initialize motor controllers
self.frontLeftMotor = ctre.WPI_TalonFX(1)
self.frontRightMotor = ctre.WPI_TalonFX(2)
self.rearLeftMotor = ctre.WPI_TalonFX(3)
self.rearRightMotor = ctre.WPI_TalonFX(4)
self.FLSparkMax = rev.CANSparkMax(5,
rev.CANSparkMaxLowLevel.MotorType(1))
self.FRSparkMax = rev.CANSparkMax(6,
rev.CANSparkMaxLowLevel.MotorType(1))
self.RLSparkMax = rev.CANSparkMax(7,
rev.CANSparkMaxLowLevel.MotorType(1))
self.RRSparkMax = rev.CANSparkMax(8,
rev.CANSparkMaxLowLevel.MotorType(1))
self.FLTalon = ctre.WPI_TalonSRX(9)
self.FRTalon = ctre.WPI_TalonSRX(10)
self.RLTalon = ctre.WPI_TalonSRX(11)
self.RRTalon = ctre.WPI_TalonSRX(12)
self.LVictor = ctre.WPI_VictorSPX(13)
self.RVictor = ctre.WPI_VictorSPX(14)
# Initialize switches and hall-effect sensors
self.limit_switch = wpilib.DigitalInput(0)
# Configure drive object
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
# Initialize controller
self.controller = wpilib.XboxController(0)
wpilib.CameraServer.launch()
NetworkTables.initialize()
self.sd = NetworkTables.getTable("SmartDashboard")
# 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.navxSensor = navx.AHRS.create_spi()
def createObjects(self):
self.value = DoubleSolenoid.Value.kForward
self.outin = DoubleSolenoid.Value.kForward
self.drive_controller = wpilib.XboxController(0)
#drivetrain
self.drivetrain_left_motor_master = ctre.WPI_TalonFX(1)
self.drivetrain_left_motor_slave = ctre.WPI_TalonFX(2)
self.drivetrain_left_motor_slave2 = ctre.WPI_TalonFX(3)
self.drivetrain_right_motor_master = ctre.WPI_TalonFX(4)
self.drivetrain_right_motor_slave = ctre.WPI_TalonFX(5)
self.drivetrain_right_motor_slave2 = ctre.WPI_TalonFX(6)
self.left = wpilib.SpeedControllerGroup(
self.drivetrain_left_motor_master, self.drivetrain_left_motor_slave, self.drivetrain_left_motor_slave2
)
self.right = wpilib.SpeedControllerGroup(
self.drivetrain_right_motor_master, self.drivetrain_right_motor_slave, self.drivetrain_right_motor_slave2
)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
self.shifter_shiftsolenoid = wpilib.Solenoid(1)
#shooter
self.hood_solenoid = wpilib.DoubleSolenoid(4,5)
self.shooter_motor = ctre.WPI_TalonFX(21)
self.shooter_motor_slave = ctre.WPI_TalonFX(20)
#intake
self.intake_roller_motor = ctre.WPI_TalonSRX(10)
self.intake_arm_solenoid = wpilib.DoubleSolenoid(2,3)
#feed
self.tower_feed_motor_master = ctre.WPI_TalonSRX(9)
self.tower_feed_motor_slave = ctre.WPI_TalonSRX(11)
#tower
self.tower_motor = ctre.WPI_TalonSRX(12)
#climb
self.climb_master = ctre.WPI_TalonSRX(8)
self.climb_slave = ctre.WPI_TalonSRX(13)
def createObjects(self):
self.drive_controller = wpilib.XboxController(0)
#self.compressor = wpilib.Compressor()
#drivetrain
self.drivetrain_left_motor_master = ctre.WPI_TalonSRX(1)
self.drivetrain_left_motor_slave = ctre.WPI_TalonSRX(2)
self.drivetrain_left_motor_slave2 = ctre.WPI_TalonSRX(3)
self.drivetrain_right_motor_master = ctre.WPI_TalonSRX(4)
self.drivetrain_right_motor_slave = ctre.WPI_TalonSRX(5)
self.drivetrain_right_motor_slave2 = ctre.WPI_TalonSRX(6)
'''
self.drivetrain_left_motor_master = ctre.WPI_TalonFX(1)
self.drivetrain_left_motor_slave = ctre.WPI_TalonFX(2)
self.drivetrain_left_motor_slave2 = ctre.WPI_TalonFX(3)
self.drivetrain_right_motor_master = ctre.WPI_TalonFX(4)
self.drivetrain_right_motor_slave = ctre.WPI_TalonFX(5)
self.drivetrain_right_motor_slave2 = ctre.WPI_TalonFX(6)
self.left = wpilib.SpeedControllerGroup(
self.drivetrain_left_motor_master, self.drivetrain_left_motor_slave, self.drivetrain_left_motor_slave2
)
self.right = wpilib.SpeedControllerGroup(
self.drivetrain_right_motor_master, self.drivetrain_right_motor_slave, self.drivetrain_right_motor_slave2
)
self.spartaBot = DifferentialDrive(self.left, self.right)
self.spartaBot.setExpiration(0.1)
'''
self.drive_shifter_solenoid = wpilib.Solenoid(1)
#self.navx = navx.AHRS.create_spi()
#intake
self.intake_roller_motor = ctre.WPI_VictorSPX(12)
self.intake_arm_solenoid = wpilib.DoubleSolenoid(2, 3)
#tower
self.tower_motor = ctre.WPI_TalonSRX(8)
#feed
self.feed_motor_master = ctre.WPI_TalonSRX(7)
self.feed_motor_slave = ctre.WPI_TalonSRX(9)
#shooter
#self.shooter_motor_master = ctre.WPI_VictorSPX(11)
#self.shooter_motor_slave = ctre.WPI_VictorSPX(13)
self.shooter_hood_solenoid = wpilib.DoubleSolenoid(4, 5)
self.shooter_motor = ctre.WPI_TalonFX(11)
#climb
self.climb_motor_master = ctre.WPI_TalonSRX(14)
self.climb_motor_slave = ctre.WPI_TalonSRX(15)
''' limelight PID Turning
self.PIDF = [2.5, 0.002, 10.0, 0.0]
self.target_pos = self.tx
self.pending_move = None
self.target_angle = 0
self.lock_target = False'''
self.kP = 0
self.kI = 0
self.kD = 0
def createMotor(motorDescp, motors={}):
'''This is where all motors are set up.
Motors include CAN Talons, CAN Talon Followers, CAN Talon FX, CAN Talon FX Followers, and SparkMax and its follower.
Not all are functional, it's up to you to find out. Good luck!'''
if motorDescp['type'] == 'CANTalonSRX':
#if we want to use the built in encoder set it here
if ('pid' in motorDescp) and motorDescp['pid'] != None:
motor = WPI_TalonSRXFeedback(motorDescp)
motor.setupPid()
else:
motor = ctre.WPI_TalonSRX(motorDescp['channel'])
motors[str(motorDescp['channel'])] = motor
elif motorDescp['type'] == 'CANTalonSRXFollower':
motor = ctre.WPI_TalonSRX(motorDescp['channel'])
motor.set(mode=ctre.ControlMode.Follower,
value=motorDescp['masterChannel'])
motors[str(motorDescp['channel'])] = motor
elif motorDescp['type'] == 'CANTalonFX':
if ('pid' in motorDescp) and motorDescp['pid'] != None:
motor = WPI_TalonFXFeedback(motorDescp)
motor.setupPid()
else:
motor = ctre.WPI_TalonFX(motorDescp['channel'])
elif motorDescp['type'] == 'CANTalonFXFollower':
motor = ctre.WPI_TalonFX(motorDescp['channel'])
motor.set(mode=ctre.TalonFXControlMode.Follower,
value=motorDescp['masterChannel'])
motors[str(motorDescp['channel'])] = motor
elif motorDescp['type'] == 'SparkMax':
'''This is where SparkMax motor controllers are set up'''
motorDescp['motorType'] = getattr(rev.MotorType,
motorDescp['motorType'])
if 'pid' in motorDescp and motorDescp['pid'] != None:
motor = SparkMaxFeedback(motorDescp, motors)
motor.setupPid()
else:
motor = rev.CANSparkMax(motorDescp['channel'],
motorDescp['motorType'])
motors[str(motorDescp['channel'])] = motor
elif motorDescp['type'] == 'SparkMaxFollower':
'''This is where SparkMax followers are set up
For masterChannel, use a motor object. MASTER MUST BE A "CANSparkMax" '''
motorDescp['motorType'] = getattr(rev.MotorType,
motorDescp['motorType'])
motor = SparkMaxFeedback(motorDescp, motors)
motor.follow(motors.get(str(motorDescp['masterChannel'])),
motorDescp['inverted'])
else:
print("Unknown Motor")
if 'inverted' in motorDescp:
motor.setInverted(motorDescp['inverted'])
if 'currentLimits' in motorDescp:
currentLimits = motorDescp['currentLimits']
absMax = currentLimits['absMax']
absMaxTimeMs = currentLimits['absMaxTimeMs']
nominalMaxCurrent = currentLimits['maxNominal']
motor.configPeakCurrentLimit(absMax, 10)
motor.configPeakCurrentDuration(absMaxTimeMs, 10)
motor.configContinuousCurrentLimit(nominalMaxCurrent, 10)
motor.enableCurrentLimit(True)
#if 'rampRate' in motorDescp:
# motor.configOpenLoopRamp(motorDescp['rampRate'],10)
return motor
def createMotor(motorDescp, motors = {}):
'''This is where all motors are set up.
Motors include CAN Talons, CAN Talon Followers, CAN Talon FX, CAN Talon FX Followers, and SparkMax and its follower.
Not all are functional, it's up to you to find out. Good luck!'''
if motorDescp['type'] == 'CANTalonSRX':
#if we want to use the built in encoder set it here
if('pid' in motorDescp) and motorDescp['pid'] != None:
motor = WPI_TalonSRXFeedback(motorDescp)
motor.setupPid()
else:
motor = ctre.WPI_TalonSRX(motorDescp['channel'])
setTalonSRXCurrentLimits(motor, motorDescp)
motors[str(motorDescp['channel'])] = motor
elif motorDescp['type'] == 'CANTalonSRXFollower':
'''This is where we set up Talon SRXs over CAN'''
motor =ctre.WPI_TalonSRX(motorDescp['channel'])
motor.set(mode = ctre.ControlMode.Follower, value = motorDescp['masterChannel'])
setTalonSRXCurrentLimits(motor, motorDescp)
motors[str(motorDescp['channel'])] = motor
elif motorDescp['type'] == 'CANTalonFX':
'''This is where CANTalon FXs are set up'''
if('pid' in motorDescp) and motorDescp['pid'] != None:
motor = WPI_TalonFXFeedback(motorDescp)
motor.setupPid()
else:
motor = ctre.WPI_TalonFX(motorDescp['channel'])
setTalonFXCurrentLimits(motor, motorDescp)
elif motorDescp['type'] == 'CANTalonFXFollower':
'''This is where CANTalon FX Followers are set up'''
motor =ctre.WPI_TalonFX(motorDescp['channel'])
motor.set(mode = ctre.TalonFXControlMode.Follower, value = motorDescp['masterChannel'])
motors[str(motorDescp['channel'])] = motor
setTalonFXCurrentLimits(motor, motorDescp)
elif motorDescp['type'] == 'SparkMax':
'''This is where SparkMax motor controllers are set up'''
motorDescp['motorType'] = getattr(rev.MotorType, motorDescp['motorType'])
if 'pid' in motorDescp and motorDescp['pid'] != None:
motor = SparkMaxFeedback(motorDescp, motors)
motor.setupPid()
else:
motor = rev.CANSparkMax(motorDescp['channel'], motorDescp['motorType'])
motors[str(motorDescp['channel'])] = motor
setREVCurrentLimits(motor, motorDescp)
elif motorDescp['type'] == 'SparkMaxFollower':
'''This is where SparkMax followers are set up
For masterChannel, use a motor object. MASTER MUST BE A "CANSparkMax" because blame rev'''
motorDescp['motorType'] = getattr(rev.MotorType, motorDescp['motorType'])
motor = SparkMaxFeedback(motorDescp, motors)
motor.follow(motors.get(str(motorDescp['masterChannel'])), motorDescp['inverted'])
setREVCurrentLimits(motor, motorDescp)
else:
print("Unknown Motor")
if 'inverted' in motorDescp:
motor.setInverted(motorDescp['inverted'])
return motor
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# Talon CAN motor init
'''
Talon closed loop control guide
https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html
Talon (CTRE) Python API documentation
https://robotpy.readthedocs.io/projects/ctre/en/latest/api.html
Talon example code (Java and C++)
https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages
## One-time setup for each Talon
Use the diagnostics tool in Windows to:
1. Update the FW
2. Set the ID number
3. Write the ID down somewhere (ideally, label the motor)
4. Use the Self-Test Snapshot and Plot to make sure the motor works
NOTE: MOTOR POSITION CONTROL IS EXTREMELY DANGEROUS RIGHT NOW
NEVER USE POSITION CONTROL WITHOUT CURRENT LIMIT
NEVER USE POSITION CONTROL WITHOUT SUPERVISION + E-STOP READINESS
Look into limit switches for motor auto-shutoff
'''
self.shooterCAN = ctre.TalonFX(6)
self.initTalonFX(self.shooterCAN)
self.shooterCANfollow = ctre.TalonFX(1)
self.initTalonFX(self.shooterCANfollow, inverted=True)
self.shooterCANfollow.set(mode=ctre.ControlMode.Follower, value=6)
self.shooterHoodCAN = ctre.TalonFX(5)
self.initTalonFX(self.shooterHoodCAN)
#self.initTalonFX(self.shooterHoodCAN, kF=0, kP=0.02, kI=0, inverted=True, enCurrentLimit=True)
#self.shooterHoodCAN.setSelectedSensorPosition(0, 0, 10)
#Get information from network tables
NetworkTables.initialize()
self.sd = NetworkTables.getTable("SmartDashboard")
self.lime = NetworkTables.getTable("limelight")
#Set up all the motor controllers
self.leftDriveCAN = ctre.WPI_TalonFX(13)
# self.initTalonFX(self.leftDriveCAN)
self.leftDriveCANfollow = ctre.TalonFX(12)
# self.initTalonFX(self.leftDriveCANfollow)
self.leftDriveCANfollow.set(mode=ctre.ControlMode.Follower, value=13)
self.rightDriveCAN = ctre.WPI_TalonFX(11)
# self.initTalonFX(self.rightDriveCAN)
self.rightDriveCANfollow = ctre.TalonFX(10)
# self.initTalonFX(self.rightDriveCANfollow)
self.rightDriveCANfollow.set(mode=ctre.ControlMode.Follower, value=11)
self.indexerMotorCAN = ctre.VictorSPX(9)
self.intakeMotorCAN = ctre.VictorSPX(8)
self.leftClimbCAN = ctre.VictorSPX(15)
self.rightClimbCAN = ctre.VictorSPX(14)
#Set up the drivetrain motors.
self.drive = wpilib.drive.DifferentialDrive(self.leftDriveCAN,
self.rightDriveCAN)
self.intakeOn = False
self.indexerOn = False
self.shooterOn = False
self.hoodOn = False
self.cycleCount = 0
self.hoodDirection = 'stopped' #stopped, forward, backward
self.currentHoodPos = 0
self.hoodSlop = 0.005
self.zeroHood = False
self.hoodSpeed = .06
self.hoodCtsPerRot = 2048 * 70.0 # counts per rotation * gear reduction / quadrature?
self.joystick = wpilib.XboxController(0)
self.indexerTimer = wpilib.Timer()
self.indexerTimer.start()
self.shooterTimer = wpilib.Timer()
self.shooterTimer.start()
self.hoodTimer = wpilib.Timer()
self.hoodTimer.start()
def test_follow():
m1 = ctre.WPI_TalonFX(3)
m2 = ctre.WPI_TalonFX(4)
m2.follow(m1)
m1.set(0.5)
assert m1.get() == 0.5
def test_wpi_talonfx():
m = ctre.WPI_TalonFX(1)
m.setNeutralMode(ctre.NeutralMode.Brake)
m.set(0.5)
assert m.get() == 0.5
del m
