def robotInit(self):
if not wpilib.RobotBase.isSimulation():
import ctre
self.motor1 = ctre.CANTalon(1)
self.motor2 = ctre.CANTalon(2)
self.motor3 = ctre.CANTalon(3)
self.motor4 = ctre.CANTalon(4)
else:
self.motor1 = wpilib.Talon(1)
self.motor2 = wpilib.Talon(2)
self.motor3 = wpilib.Talon(3)
self.motor4 = wpilib.Talon(4)
#Defining Constants
# self.LeftTread = wpilib.Talon(0)
# self.RightTread = wpilib.Talon(1)
self.robotDrive = wpilib.RobotDrive(self.motor1, self.motor2,
self.motor3, self.motor4)
self.xboxController = wpilib.Joystick(0)
self.xboxAbutton = wpilib.buttons.JoystickButton(
self.xboxController, 1)
self.xboxBbutton = wpilib.buttons.JoystickButton(
self.xboxController, 2)
self.xboxYbutton = wpilib.buttons.JoystickButton(
self.xboxController, 4)
#self.navx = navx.AHRS.create_spi()
# self.drive = drive.Drive(self.robotDrive, self.xboxController, self.navx)
#Defining Variables
self.dm = True
def __init__(self):
super().__init__()
self.winchL = ctre.CANTalon(robotMap.WINCHL)
print("winch l created")
self.winchR = ctre.CANTalon(robotMap.WINCHR)
print("winch r created")
self.led = wpilib.Relay(robotMap.LED)
def __init__(self, robot, talon_front_id, talon_back_id,
solenoid_forward_id, solenoid_reverse_id):
self.robot = robot # Reference from the main robot instance
# Initialize Shooter Motor Controllers
self.talon_front = ctre.CANTalon(talon_front_id)
self.talon_back = ctre.CANTalon(talon_back_id)
# Invert Shooter Motors since natively wrong direction
self.talon_front.setInverted(True)
self.talon_back.setInverted(True)
# Initialize Shooter Solenoid
self.solenoid_shoot = wpilib.DoubleSolenoid(solenoid_forward_id,
solenoid_reverse_id)
# Initialize State Attributes
self.enabled = False
self.dpad_up_held = False
self.dpad_down_held = False
self.speed_state = Values.SHOOTER_DEFAULT_SPEED_STATE
self.piston_state = 0
# Calculate Speed State Values
max_speed = min(Values.SHOOTER_MAX_SPEED, 1)
min_speed = max(Values.SHOOTER_MIN_SPEED, 0)
val_step = (max_speed - min_speed) / float(4)
self.speed_range = [
round(min_speed + (val_num * val_step), 2)
for val_num in range(Values.SHOOTER_SPEED_INTERVALS - 1)
] + [max_speed]
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# Configure shooter motor controller.
self.shooter = ctre.CANTalon(1) # Create a CANTalon object.
self.shooter.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder
) # Choose an encoder as a feedback device. The default should be QuadEncoder already, but might as well make sure.
# I thought the encoder was 20 pulses per revolution per phase, which would require "80" as an argument below, but after trying it, it looks like there are 12.
# Setting this parameter changes things so getPosition() returns decimal revolutions, and getSpeed() returns RPM.
self.shooter.configEncoderCodesPerRev(48)
# resets shooter position on startup
self.shooter.setPosition(0)
self.shooter.enableBrakeMode(
False) # This should change between brake and coast modes.
self.l_motor = ctre.CANTalon(2)
self.l_motor.setInverted(True)
self.r_motor = ctre.CANTalon(3)
self.r_motor.setInverted(True)
self.stick = wpilib.Joystick(0)
self.drive = wpilib.RobotDrive(self.l_motor, self.r_motor)
self.counter = 0
def __init__(self):
"""
Instantiates the motor objects.
"""
super().__init__('Motors')
# main motors
self.left_motor = ctre.CANTalon(robotmap.motors.left_id)
self.left_motor.setControlMode(ctre.CANTalon.ControlMode.Speed)
self.left_motor.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.left_motor.setInverted(False)
self.right_motor = ctre.CANTalon(robotmap.motors.right_id)
self.right_motor.setControlMode(ctre.CANTalon.ControlMode.Speed)
self.right_motor.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.right_motor.setInverted(False)
self.sd = NetworkTables.getTable("SmartDashboard")
self.sd.putNumber("direction", 1)
# follower motors
left_motor_follower = ctre.CANTalon(robotmap.motors.left_follower_id)
left_motor_follower.setControlMode(ctre.CANTalon.ControlMode.Follower)
left_motor_follower.set(robotmap.motors.left_id)
right_motor_follower = ctre.CANTalon(robotmap.motors.right_follower_id)
right_motor_follower.setControlMode(ctre.CANTalon.ControlMode.Follower)
right_motor_follower.set(robotmap.motors.right_id)
self.robot_drive = wpilib.RobotDrive(self.left_motor, self.right_motor)
self.max_speed = 1760 # maximum edges per 100 ms, approx. 27 ft/s
self.robot_drive.setMaxOutput(
self.max_speed) # maximum edges per 10ms, approx. 27 ft/s
def __init__(self):
super().__init__('Climbing Mechanism')
self.motor = ctre.CANTalon(robotmap.climbing_mech.id)
follower = ctre.CANTalon(robotmap.climbing_mech.follower_id)
follower.setControlMode(ctre.CANTalon.ControlMode.Follower)
follower.set(robotmap.climbing_mech.id)
follower.setInverted(True)
def robotInit(self):
self.lJoy = wpilib.Joystick(0)
self.rJoy = wpilib.Joystick(1)
self.FL = ctre.CANTalon(2)
self.FR = ctre.CANTalon(1)
self.BL = ctre.CANTalon(0)
self.BR = ctre.CANTalon(3)
# Position gets automatically updated as robot moves
self.gyro = wpilib.AnalogGyro(1)
def setup(self):
self.left_master = ctre.CANTalon(0)
self.left_slave = ctre.CANTalon(1)
self.right_master = ctre.CANTalon(2)
self.right_slave = ctre.CANTalon(3)
self.gyro = navx.AHRS.create_spi()
self.current_state = self._DriveState.OPEN_LOOP
self.left_slave.changeControlMode(ctre.CANTalon.ControlMode.Follower)
self.left_slave.set(self.left_master.getDeviceID())
self.right_slave.changeControlMode(ctre.CANTalon.ControlMode.Follower)
self.right_slave.set(self.right_master.getDeviceID())
self.left_master.changeControlMode(
ctre.CANTalon.ControlMode.PercentVbus)
self.left_master.set(0)
self.right_master.changeControlMode(
ctre.CANTalon.ControlMode.PercentVbus)
self.right_master.set(0)
self.left_master.setPID(Config.Drive.DRIVE_VELOCITY_P,
Config.Drive.DRIVE_VELOCITY_I,
Config.Drive.DRIVE_VELOCITY_D,
Config.Drive.DRIVE_VELOCITY_F,
Config.Drive.DRIVE_VELOCITY_I_ZONE,
Config.Drive.DRIVE_VELOCITY_RAMP_RATE, 0)
self.right_master.setPID(Config.Drive.DRIVE_VELOCITY_P,
Config.Drive.DRIVE_VELOCITY_I,
Config.Drive.DRIVE_VELOCITY_D,
Config.Drive.DRIVE_VELOCITY_F,
Config.Drive.DRIVE_VELOCITY_I_ZONE,
Config.Drive.DRIVE_VELOCITY_RAMP_RATE, 0)
self.left_master.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.left_master.reverseSensor(True)
self.right_master.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.right_master.reverseSensor(False)
self.right_master.reverseOutput(True)
self.left_value = 0
self.right_value = 0
def robotInit(self):
wpilib.CameraServer.launch('misc/vision.py:main')
if not wpilib.RobotBase.isSimulation(
): #This makes simulator show motor outputs for debugging
import ctre
self.RLC = ctre.CANTalon(self.rLeftChannel)
self.FLC = ctre.CANTalon(self.fLeftChannel)
self.FRC = ctre.CANTalon(self.fRightChannel)
self.RRC = ctre.CANTalon(self.rRightChannel)
else:
self.RLC = wpilib.Talon(self.rLeftChannel)
self.FLC = wpilib.Talon(self.fLeftChannel)
self.FRC = wpilib.Talon(self.fRightChannel)
self.RRC = wpilib.Talon(self.rRightChannel)
self.robotDrive = wpilib.RobotDrive(
self.RLC, self.RRC, self.FRC,
self.FLC) #Sets motors for robotDrive commands
#Controller Input Variables
self.controller = wpilib.Joystick(self.joystickChannel)
self.winch_backward = wpilib.buttons.JoystickButton(self.controller, 5)
self.paddleGet = wpilib.buttons.JoystickButton(self.controller, 1)
self.gearDrop = wpilib.buttons.JoystickButton(self.controller,
6) # Right Bumper
self.xboxMec = wpilib.buttons.JoystickButton(self.controller, 8)
self.xboxMec2 = wpilib.buttons.JoystickButton(self.controller, 7)
#CRio Output Variables
self.winch_motor1 = wpilib.Talon(7)
self.winch_motor2 = wpilib.Talon(8)
self.paddle = wpilib.Solenoid(1)
self.gear = wpilib.DoubleSolenoid(2, 3)
self.ultrasonic = wpilib.Ultrasonic(5, 4) #trigger to echo
self.ultrasonic.setAutomaticMode(True)
self.navx = navx.AHRS.create_spi()
#Auto mode variables
self.components = {
'drive': self.robotDrive,
'gearDrop': self.gear,
'ultrasonic': self.ultrasonic,
'navx': self.navx
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def __init__( self, robot ): super( ).__init__( 'shooter' ) self.robot = robot # PIDF values for shooter # Can be changed on-the-fly via the smartdashboard triggers self.default_kP = 0.034 self.default_kI = 0 self.default_kD = 0.130 self.default_kF = 0.245 self.shooter_talon = ctre.CANTalon( 1 ) self.shooter_talon.setFeedbackDevice( TalonSRXConst.kFeedbackDev_CtreMagEncoder_Relative ) self.shooter_talon.setPIDSourceType( PIDSource.PIDSourceType.kRate ) self.shooter_talon.setInverted( True ) self.shooter_talon.changeControlMode( ctre.CANTalon.ControlMode.Speed ) #self.shooter_talon.changeControlMode( ctre.CANTalon.ControlMode.Voltage ) self.shooter_talon.configNominalOutputVoltage( 0.0, -0.0 ) self.shooter_talon.configPeakOutputVoltage( 12.0, -12.0 ) self.shooter_talon.setProfile( 0 ) self.shooter_talon.setP( self.default_kP ) self.shooter_talon.setI( self.default_kI ) self.shooter_talon.setD( self.default_kD ) self.shooter_talon.setF( self.default_kF )
def robotInit(self):
flRotate = ctre.CANTalon(10)
flRotate.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
flRotate.setPID(1.0, 0.0, 5.0, 0)
frRotate = ctre.CANTalon(2)
frRotate.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
frRotate.setPID(1.0, 0.0, 5.0, 0)
self.flDrive = ctre.CANTalon(1)
self.frDrive = ctre.CANTalon(15)
self.drive = seamonsters.SwerveDrive()
self.drive.addWheel(1.0, -1.0, self.flDrive, flRotate, 3106)
self.drive.addWheel(1.0, 1.0, self.frDrive, frRotate, 3106)
self.joy = wpilib.Joystick(0)
def robotInit(self):
self.gamepad = seamonsters.gamepad.globalGamepad(port=0)
self.climberMotor = ctre.CANTalon(4)
self.lockLog = LogState("Climber lock mode")
self.statusLog = LogState("Climber status")
self.currentLog = LogState("Climber current")
#self.encoderLog = LogState("Climber encoder")
self.encoderLog = None
def robotInit(self):
self.gamepad = Gamepad(1)
self.LeftFly = ctre.CANTalon(4)
self.RightFly = ctre.CANTalon(5)
self.LeftFly.setPID(1, 0.0009, 1, 0.0)
self.RightFly.setPID(1, 0.0009, 1, 0.0)
self.LeftFly.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.RightFly.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.Intake = ctre.CANTalon(8)
self.Intake.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.motorSpeed = 0
self.hold = False
self.flywheelsLog = LogState("Flywheels")
self.holdLog = LogState("Hold flywheel speed")
self.intakeLog = LogState("Intake")
def __init__(self):
self.flywheelMotors = [ctre.CANTalon(5), ctre.CANTalon(6)]
self.speedVoltage = .5
self.speedSpeed = 27632 * self.speedVoltage
# encoder resolution is 512 (* 4)
for motor in self.flywheelMotors:
motor.setPID(0.15, 0.0, 5.0, 0)
motor.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
for motor in self.flywheelMotors:
motor.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.talonSpeedModeEnabled = False
self.voltageModeStartupCount = 0
self.speedLog = LogState("Flywheel speed")
self.controlModeLog = LogState("Flywheel mode")
def __init__(self, robot, left_main_id, left_slave_id, right_main_id, right_slave_id):
self.robot = robot # Reference to the main robot instance
# Initialize Drive Train Motors
self.talon_left_main = ctre.CANTalon(left_main_id)
self.talon_left_slave = ctre.CANTalon(left_slave_id)
self.talon_right_main = ctre.CANTalon(right_main_id)
self.talon_right_slave = ctre.CANTalon(right_slave_id)
# Set Slave Motors to Follower Mode
self.talon_left_slave.changeControlMode(ctre.CANTalon.ControlMode.Follower)
self.talon_left_slave.set(self.talon_left_main.getDeviceID())
self.talon_right_slave.changeControlMode(ctre.CANTalon.ControlMode.Follower)
self.talon_right_slave.set(self.talon_right_main.getDeviceID())
# Invert Right Talon Direction
self.talon_right_main.setInverted(True)
# Driver Station Instance
self.driver_station = wpilib.DriverStation.getInstance()
def robotInit(self):
super().robotInit()
# Front Left wheel
flDrive = ctre.CANTalon(0)
flDrive.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
flDrive.setPID(1.0, 0.0, 3.0, 0.0)
flRotate = ctre.CANTalon(1)
flRotate.reverseOutput(True)
flRotate.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
flRotate.setPID(1.0, 0.0, 3.0, 0.0)
# Back Right wheel
brDrive = ctre.CANTalon(2)
brDrive.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
brDrive.setPID(1.0, 0.0, 3.0, 0.0)
brRotate = ctre.CANTalon(3)
brRotate.reverseOutput(True)
brRotate.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
brRotate.setPID(1.0, 0.0, 3.0, 0.0)
# Drive controller
drive = SwerveDrive()
# 104 gear teeth / 18 gear teeth * 280 ticks per rotation * 4 (quad)
# then divide by 2 for some reason
drive.addWheel(-1.0, 1.0, flDrive, flRotate, -104 / 18 * 280 * 4 / 2)
drive.addWheel(1.0, -1.0, brDrive, brRotate, -104 / 18 * 280 * 4 / 2)
filterDrive = AccelerationFilterDrive(drive)
DriveTest.initDrive(self,
filterDrive,
driveMode=DriveInterface.DriveMode.VOLTAGE)
def robotInit(self):
self.motor0 = ctre.CANTalon(
1) # Initialize the CanTalonSRX on device 1.
self.motor1 = ctre.CANTalon(2)
self.motor2 = ctre.CANTalon(3)
self.motor3 = ctre.CANTalon(
4) # Initialize the CanTalonSRX on device 1.
self.motor4 = ctre.CANTalon(5)
self.motor5 = ctre.CANTalon(6)
self.joy = wpilib.Joystick(1)
def createMotors(self,motorMap):
driveMotors = {}
for motorName in motorMap:
motor = motorMap[motorName]
newMotor = None
if motor['type'] == 'CANTalon':
print("Found CANTalon for channel ", motor['channel'])
#create talon
newMotor = ctre.CANTalon(motor['channel'])
#set invereted based on property
newMotor.setInverted(motor['inverted'])
else:
print("Unknown motor %s of type %s"%(motorName,motor['type']))
#add motor to the drive motor dictonary
driveMotors[motorName] = newMotor
#save motors to the robot class. We will use this to initlize the robot
self.driveMotors = driveMotors
def robotInit(self):
self.talon = ctre.CANTalon(3)
self.joystick = wpilib.Joystick(0)
self.talon.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
# self.talon.configEncoderCodesPerRev(XXX), if using FeedbackDevice.QuadEncoder
# self.talon.configPotentiometerTurns(XXX), if using FeedbackDevice.AnalogEncoder or AnalogPot
# Set the peak nominal outputs. 12V means full
self.talon.configNominalOutputVoltage(+0.0, -0.0)
self.talon.configPeakOutputVoltage(12.0, -12.0)
# Set closed loop gains in slot0 - see documentation
self.talon.setProfile(0)
self.talon.setPID(0, 0, 0, 0) # P,I,D,F = 0
# Set acceleration and vcruise velocity - see documentation
self.talon.setMotionMagicCruiseVelocity(0)
self.talon.setMotionMagicAcceleration(0)
def createMotors(self, motorMap):
createMotors = {}
for motorName in motorMap:
motor = motorMap[motorName]
newMotor = None
if motor['type'] == 'CANTalon':
print("Found CANTalon for channel ", motor['channel'])
#create talon
newMotor = ctre.CANTalon(motor['channel'])
#set invereted based on property
newMotor.setInverted(motor['inverted'])
else:
print("Unknown motor %s of type %s" %
(motorName, motor['type']))
#add motor to the drive motor dictonary
createMotors[motorName] = newMotor
#return this motor list
return createMotors
def robotInit(self):
self.talon = ctre.CANTalon(7)
self.joy = wpilib.Joystick(0)
self.loops = 0
# first choose the sensor
self.talon.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.talon.reverseSensor(False)
# self.talon.configEncoderCodesPerRev(XXX), // if using FeedbackDevice.QuadEncoder
# self.talon.configPotentiometerTurns(XXX), // if using FeedbackDevice.AnalogEncoder or AnalogPot
# set the peak and nominal outputs, 12V means full
self.talon.configNominalOutputVoltage(+0.0, -0.0)
self.talon.configPeakOutputVoltage(+12.0, 0.0)
# set closed loop gains in slot0
self.talon.setProfile(0)
self.talon.setF(0.1097)
self.talon.setP(0.22)
self.talon.setI(0)
self.talon.setD(0)
def robotInit(self):
# Initialize the CanTalonSRX on device 1.
self.motor = ctre.CANTalon(1)
# This sets the mode of the m_motor. The options are:
# PercentVbus: basic throttle; no closed-loop.
# Current: Runs the motor with the specified current if possible.
# Speed: Runs a PID control loop to keep the motor going at a constant
# speed using the specified sensor.
# Position: Runs a PID control loop to move the motor to a specified move
# the motor to a specified sensor position.
# Voltage: Runs the m_motor at a constant voltage, if possible.
# Follower: The m_motor will run at the same throttle as the specified
# other talon.
self.motor.changeControlMode(ctre.CANTalon.ControlMode.Position)
# This command allows you to specify which feedback device to use when doing
# closed-loop control. The options are:
# AnalogPot: Basic analog potentiometer
# QuadEncoder: Quadrature Encoder
# AnalogEncoder: Analog Encoder
# EncRising: Counts the rising edges of the QuadA pin (allows use of a
# non-quadrature encoder)
# EncFalling: Same as EncRising, but counts on falling edges.
self.motor.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.AnalogPot)
# This sets the basic P, I , and D values (F, Izone, and rampRate can also
# be set, but are ignored here).
# These must all be positive floating point numbers (reverseSensor will
# multiply the sensor values by negative one in case your sensor is flipped
# relative to your motor).
# These values are in units of throttle / sensor_units where throttle ranges
# from -1023 to +1023 and sensor units are from 0 - 1023 for analog
# potentiometers, encoder ticks for encoders, and position / 10ms for
# speeds.
self.motor.setPID(1.0, 0.0, 0.0)
def robotInit(self):
self.leftFront = ctre.CANTalon(2)
self.leftBack = ctre.CANTalon(0)
self.rightFront = ctre.CANTalon(1)
self.rightBack = ctre.CANTalon(3)
self.ahrs = ahrs
def cantalon_and_data():
ct = ctre.CANTalon(2)
data = hal_data['CAN'][2]
return ct, data
def robotInit(self):
"""
Motors
"""
if not wpilib.RobotBase.isSimulation():
import ctre
self.motor1 = ctre.CANTalon(1) #Drive Motors
self.motor2 = ctre.CANTalon(2)
self.motor3 = ctre.CANTalon(3)
self.motor4 = ctre.CANTalon(4)
else:
self.motor1 = wpilib.Talon(1) #Drive Motors
self.motor2 = wpilib.Talon(2)
self.motor3 = wpilib.Talon(3)
self.motor4 = wpilib.Talon(4)
self.climb1 = wpilib.VictorSP(7)
self.climb2 = wpilib.VictorSP(8)
"""
Spike Relay for LED
"""
self.ledRing = wpilib.Relay(
0, wpilib.Relay.Direction.kForward) #Only goes forward voltage
"""
Sensors
"""
self.navx = navx.AHRS.create_spi() #the Gyro
self.psiSensor = wpilib.AnalogInput(2)
self.powerBoard = wpilib.PowerDistributionPanel(0) #Might need or not
self.ultrasonic = wpilib.Ultrasonic(5, 4) #trigger to echo
self.ultrasonic.setAutomaticMode(True)
self.encoder = wpilib.Encoder(2, 3)
self.switch = wpilib.DigitalInput(6)
self.servo = wpilib.Servo(0)
self.joystick = wpilib.Joystick(0) #xbox controller
wpilib.CameraServer.launch('misc/vision.py:main')
"""
Buttons
"""
self.visionEnable = wpilib.buttons.JoystickButton(self.joystick,
7) #X button
self.gearPistonButton = wpilib.buttons.JoystickButton(self.joystick, 5)
self.safetyPistonButton = wpilib.buttons.JoystickButton(
self.joystick, 3)
self.controlSwitch = button_debouncer.ButtonDebouncer(
self.joystick, 8,
period=0.5) #Controll switch init for auto lock direction
self.driveControlButton = button_debouncer.ButtonDebouncer(
self.joystick, 1, period=0.5) #Mecanum to tank and the other way
self.climbReverseButton = wpilib.buttons.JoystickButton(
self.joystick, 2) #Button for reverse out of climb
"""
Solenoids
"""
self.drivePiston = wpilib.DoubleSolenoid(
3, 4) #Changes us from mecanum to hi-grip
self.gearPiston = wpilib.Solenoid(2)
self.safetyPiston = wpilib.Solenoid(1)
self.robodrive = wpilib.RobotDrive(self.motor1, self.motor4,
self.motor3, self.motor2)
self.Drive = drive.Drive(self.robodrive, self.drivePiston, self.navx,
self.encoder, self.ledRing)
self.climber = climb.Climb(self.climb1, self.climb2)
"""
All the variables that need to be defined
"""
self.motorWhere = True #IF IT IS IN MECANUM BY DEFAULT
self.rotationXbox = 0
self.climbVoltage = 0
"""
Timers
"""
self.timer = wpilib.Timer()
self.timer.start()
self.vibrateTimer = wpilib.Timer()
self.vibrateTimer.start()
self.vibrator = vibrator.Vibrator(self.joystick, self.vibrateTimer,
.25, .15)
"""
The great NetworkTables part
"""
self.vision_table = NetworkTable.getTable('/GRIP/myContoursReport')
self.alignGear = alignGear.AlignGear(self.vision_table)
self.robotStats = NetworkTable.getTable('SmartDashboard')
self.matchTime = matchTime.MatchTime(self.timer, self.robotStats)
"""
Drive Straight
"""
self.DS = driveStraight.driveStraight(self.timer, self.vibrator,
self.Drive, self.robotStats)
self.components = {
'drive': self.Drive,
'alignGear': self.alignGear,
'gearPiston': self.gearPiston,
'ultrasonic': self.ultrasonic
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
self.updater()
def __init__(self):
super().__init__()
self.winchL = ctre.CANTalon(robotMap.WINCHL)
self.winchR = ctre.CANTalon(robotMap.WINCHR)
self.led = wpilib.Relay(robotMap.LED)
self.pdp = PowerDistributionPanel(0)
def createObjects(self):
"""
Create basic components (motor controllers, joysticks, etc.).
"""
# NavX
self.navx = navx.AHRS.create_spi()
# Initialize SmartDashboard
self.sd = NetworkTable.getTable('SmartDashboard')
# Joysticks
self.left_joystick = wpilib.Joystick(0)
self.right_joystick = wpilib.Joystick(1)
self.secondary_joystick = wpilib.Joystick(2)
# Triggers and buttons
self.secondary_trigger = ButtonDebouncer(self.secondary_joystick, 1)
# Drive motors
self.fr_module = swervemodule.SwerveModule(ctre.CANTalon(30), wpilib.VictorSP(3), wpilib.AnalogInput(0), sd_prefix='fr_module', zero=1.85, has_drive_encoder=True)
self.fl_module = swervemodule.SwerveModule(ctre.CANTalon(20), wpilib.VictorSP(1), wpilib.AnalogInput(2), sd_prefix='fl_module', zero=3.92, inverted=True)
self.rr_module = swervemodule.SwerveModule(ctre.CANTalon(10), wpilib.VictorSP(2), wpilib.AnalogInput(1), sd_prefix='rr_module', zero=4.59)
self.rl_module = swervemodule.SwerveModule(ctre.CANTalon(5), wpilib.VictorSP(0), wpilib.AnalogInput(3), sd_prefix='rl_module', zero=2.44, has_drive_encoder=True, inverted=True)
# Drive control
self.field_centric_button = ButtonDebouncer(self.left_joystick, 6)
self.predict_position = ButtonDebouncer(self.left_joystick, 7)
self.field_centric_drive = True
self.field_centric_hot_switch = toggle_button.TrueToggleButton(self.left_joystick, 1)
self.left_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 4)
self.right_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 5)
self.align_button = toggle_button.TrueToggleButton(self.right_joystick, 10)
# Shooting motors
self.shooter_motor = ctre.CANTalon(15)
self.belt_motor = wpilib.spark.Spark(9)
self.light_controller = wpilib.VictorSP(8)
# Pistons for gear picker
self.picker = wpilib.DoubleSolenoid(6, 7)
self.pivot = wpilib.DoubleSolenoid(4, 5)
self.pessure_sensor = pressure_sensor.REVAnalogPressureSensor(navx.pins.getNavxAnalogInChannel(0))
# Toggling button on secondary joystick
self.pivot_toggle_button = ButtonDebouncer(self.secondary_joystick, 2)
# Or, up and down buttons on right joystick
self.pivot_down_button = ButtonDebouncer(self.right_joystick, 2)
self.pivot_up_button = ButtonDebouncer(self.right_joystick, 3)
# Climb motors
self.climb_motor1 = wpilib.spark.Spark(4)
self.climb_motor2 = wpilib.spark.Spark(5)
# Camera gimble
self.gimbal_yaw = wpilib.Servo(6)
self.gimbal_pitch = wpilib.Servo(7)
# PDP
self.pdp = wpilib.PowerDistributionPanel(0)
def robotInit(self):
### CONSTANTS ###
# if the joystick direction is within this number of radians on either
# side of straight up, left, down, or right, it will be rounded
self.driveDirectionDeadZone = math.radians(10)
# rate of increase of velocity per 1/50th of a second:
accelerationRate = 1.0
# PIDF values for fast driving:
fastPID = (1.0, 0.0009, 3.0, 0.0)
# speed at which fast PID's should be used:
fastPIDScale = 0.09
# PIDF values for slow driving:
slowPID = (30.0, 0.0009, 3.0, 0.0)
# speed at which slow PID's should be used:
slowPIDScale = 0.01
pidLookBackRange = 10
self.maxVelocity = 650
### END OF CONSTANTS ###
self.driverGamepad = seamonsters.gamepad.globalGamepad(port=0)
fl = ctre.CANTalon(2)
fr = ctre.CANTalon(1)
bl = ctre.CANTalon(0)
br = ctre.CANTalon(3)
self.talons = [fl, fr, bl, br]
for talon in self.talons:
talon.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.driveModeLog = LogState("Drive mode")
self._setPID(fastPID)
# encoder has 100 raw ticks -- with a QuadEncoder that makes 400 ticks
# the motor gear has 12 teeth and the wheel has 85 teeth
# 85 / 12 * 400 = 2833.333 = ~2833
ticksPerWheelRotation = 2833
self.holoDrive = HolonomicDrive(fl, fr, bl, br, ticksPerWheelRotation)
self.holoDrive.invertDrive(True)
self.holoDrive.setWheelOffset(math.radians(45.0)) # angle of rollers
self.pidDrive = DynamicPIDDrive(self.holoDrive, self.talons, slowPID,
slowPIDScale, fastPID, fastPIDScale,
pidLookBackRange)
self.filterDrive = AccelerationFilterDrive(self.pidDrive,
accelerationRate)
self.ahrs = AHRS.create_spi() # the NavX
self.fieldDrive = FieldOrientedDrive(self.filterDrive,
self.ahrs,
offset=0)
self.fieldDrive.zero()
self.fieldDriveLog = LogState("Field oriented")
self.pdp = wpilib.PowerDistributionPanel()
self.currentLog = LogState("Drive current", logFrequency=2.0)
self.encoderLog = LogState("Wheel encoders")
self.speedLog = LogState("Wheel speeds")
if self.pdp.getVoltage() < 12:
print("Battery Level below 12 volts!!!")
def createObjects(self):
#navx
self.navx = AHRS.create_spi()
self.navx.setPIDSourceType(PIDSource.PIDSourceType.kDisplacement)
#Drivetrain
self.left_talon0 = ctre.CANTalon(0)
self.left_talon1 = ctre.CANTalon(1)
self.right_talon0 = ctre.CANTalon(2)
self.right_talon1 = ctre.CANTalon(3)
#Set up talon slaves
self.left_talon1.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.left_talon1.set(self.left_talon0.getDeviceID())
self.right_talon1.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.right_talon1.set(self.right_talon0.getDeviceID())
#Set talon feedback device
self.left_talon0.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.right_talon0.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
#Set the Ticks per revolution in the talons
self.left_talon0.configEncoderCodesPerRev(1440)
self.right_talon0.configEncoderCodesPerRev(1440)
#Reverse left talon
self.left_talon0.setInverted(True)
self.right_talon0.setInverted(False)
#Climber
self.climber_motor = wpilib.Spark(0)
self.climber_2 = wpilib.Spark(1)
#Sensors
self.left_enc = encoder.Encoder(self.left_talon0)
self.right_enc = encoder.Encoder(self.right_talon0, True)
#Controls
self.left_joystick = wpilib.Joystick(0)
self.right_joystick = wpilib.Joystick(1)
self.climber_joystick = wpilib.Joystick(2)
self.buttons = unifiedjoystick.UnifiedJoystick(
[self.left_joystick, self.right_joystick])
self.last_button_state = False
#Bling
self.leds = ledstrip.LEDStrip()
#Autonomous Placement
self.auto_positions = wpilib.SendableChooser()
self.auto_positions.addDefault("Position 1", 1)
self.auto_positions.addObject("Position 2", 2)
self.auto_positions.addObject("Position 3", 3)
SmartDashboard.putData("auto_position", self.auto_positions)
#SD variables
SmartDashboard.putNumber("Vision/Turn", 0)
SmartDashboard.putBoolean("Reversed", True)
#LiveWindow
LiveWindow.addActuator("Drive", "Left Master Talon", self.left_talon0)
LiveWindow.addActuator("Drive", "Right Master Talon",
self.right_talon0)
def __init__(self):
super().__init__()
self.diabloDrive = RobotDrive(ctre.CANTalon(1), ctre.CANTalon(3),
ctre.CANTalon(4), ctre.CANTalon(2))
