def __init__(self):
super().__init__()
self.ahrs = AHRS.create_spi()
self.pitch = None
self.yaw = None
self.roll = None
self.compass_heading = None
self.fused_heading = None
def __init__(self):
super().__init__()
self.ahrs = AHRS.create_spi()
self.pitch = None
self.yaw = None
self.roll = None
self.compass_heading = None
self.fused_heading = None
def __init__(self):
self.ahrs = AHRS.create_spi()
super().__init__(self.ahrs.getYaw, 'angle_ctrl')
if hasattr(self, 'pid'):
self.pid.setInputRange(-180.0, 180.0)
self.pid.setOutputRange(-1.0, 1.0)
self.pid.setContinuous(True)
self.report = 0
def __init__(self, port, imu_type='navx', interface='spi'):
self.type = imu_type
self.iface = interface
self.prefs = wpilib.Preferences.getInstance()
self.angle_offset = 0
if imu_type == 'navx':
try:
if interface == 'spi':
self.__imu = AHRS.create_spi(port)
elif interface == 'i2c':
self.__imu = AHRS.create_i2c(port)
self.__imu.reset()
except Exception as e:
print("Caught exception while trying to initialize AHRS: " +
e.args) # noqa: E501
self.__imu = None
self.type = 'none'
else:
raise NotImplementedError(
'IMU types other than NavX are not supported.') # noqa: E501
def __init__(self, left_motor: ctre.CANTalon, right_motor: ctre.CANTalon, **kwargs):
'''
Represents a drivetrain that uses CANTalons and so manages those advanced features
:param left_motor:
:param right_motor:
:param kwargs:
'''
Subsystem.__init__(self)
wpilib.MotorSafety.__init__(self)
self.robot_width = kwargs.pop("robot_width", 29.25 / 12)
self.max_turn_radius = kwargs.pop("max_radius", 10)
self.wheel_diameter = kwargs.pop("wheel_diameter", 4)
self.max_speed = kwargs.pop("max_speed", 13)
self.ahrs = AHRS.create_i2c()
self._left_motor = left_motor
self._right_motor = right_motor
self._model_left_dist = 0
self._model_right_dist = 0
self._model_last_time = robot_time.millis()
pose.init(left_encoder_callback=self.get_left_distance,
right_encoder_callback=self.get_right_distance,
gyro_callback=(None if wpilib.hal.isSimulation() else self.get_heading_rads),
wheelbase=self.robot_width)
dashboard2.graph("Pose X", lambda: pose.get_current_pose().x)
dashboard2.graph("Pose Y", lambda: pose.get_current_pose().y)
dashboard2.graph("Distance to target",
lambda: pose.get_current_pose().distance(mathutils.Vector2(6, -4)))
self._max_output = 1
self._mode = SmartDrivetrain.Mode.PercentVbus
self.set_mode(self._mode)
if wpilib.hal.isSimulation():
model_thread = threading.Thread(target=self._update_model)
model_thread.start()
# Motor safety
self.setSafetyEnabled(True)
pose.init(left_encoder_callback=self.get_left_distance, right_encoder_callback=self.get_right_distance,
gyro_callback=(self.get_heading if not wpilib.hal.isSimulation() else None),
wheelbase=self.robot_width,
encoder_factor=self.get_fps_rpm_ratio())
dashboard2.graph("Heading", lambda: pose.get_current_pose().heading * 180 / math.pi)
def createObjects(self):
wpilib.CameraServer.launch()
wpilib.LiveWindow.disableAllTelemetry()
self.left_drive_motor = WPI_TalonSRX(0)
WPI_TalonSRX(1).set(WPI_TalonSRX.ControlMode.Follower,
self.left_drive_motor.getDeviceID())
self.right_drive_motor = WPI_TalonSRX(2)
WPI_TalonSRX(3).set(WPI_TalonSRX.ControlMode.Follower,
self.right_drive_motor.getDeviceID())
self.robot_drive = wpilib.drive.DifferentialDrive(
self.left_drive_motor, self.right_drive_motor)
self.r_intake_motor = WPI_VictorSPX(4)
self.l_intake_motor = WPI_VictorSPX(5)
self.elevator_winch = WPI_TalonSRX(6)
self.climber_motor = WPI_TalonSRX(7)
self.wrist_motor = WPI_TalonSRX(8)
self.intake_ir = wpilib.AnalogInput(0)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 3)
self.right_drive_joystick = wpilib.Joystick(0)
self.left_drive_joystick = wpilib.Joystick(1)
self.operator_joystick = wpilib.Joystick(2)
self.compressor = wpilib.Compressor()
self.elevator_limit_switch = wpilib.DigitalInput(0)
self.climber_motor = WPI_TalonSRX(7)
self.navx = AHRS.create_spi()
self.path_tracking_table = NetworkTables.getTable("path_tracking")
self.down_button = ButtonDebouncer(self.operator_joystick, 1)
self.right_button = ButtonDebouncer(self.operator_joystick, 2)
self.left_button = ButtonDebouncer(self.operator_joystick, 3)
self.has_cube_button = ButtonDebouncer(self.operator_joystick, 5)
self.up_button = ButtonDebouncer(self.operator_joystick, 4)
self.left_bumper_button = JoystickButton(self.operator_joystick, 5)
self.right_bumper_button = JoystickButton(self.operator_joystick, 6)
def __init__(self, left_motor: ctre.talonsrx.TalonSRX,
right_motor: ctre.talonsrx.TalonSRX, **kwargs):
'''
Represents a drivetrain that uses CANTalons and so manages those advanced features
:param left_motor:
:param right_motor:
:param kwargs:
'''
wpilib.MotorSafety.__init__(self)
self.robot_width = kwargs.pop("robot_width", 24 / 12)
self.max_turn_radius = kwargs.pop("max_radius", 10)
self.wheel_diameter = kwargs.pop("wheel_diameter", 6)
self.max_speed = kwargs.pop("max_speed", 16)
self.ahrs = AHRS.create_spi()
self.ahrs.reset()
self._left_motor = left_motor
self._right_motor = right_motor
pose.init(left_encoder_callback=self.get_left_distance,
right_encoder_callback=self.get_right_distance,
gyro_callback=self.get_heading_rads,
wheelbase=self.robot_width)
dashboard2.add_graph("Pose X", lambda: pose.get_current_pose().x)
dashboard2.add_graph("Pose Y", lambda: pose.get_current_pose().y)
self._max_output = 1
self._mode = SmartRobotDrive.Mode.PercentVbus
self.set_mode(self._mode)
# Motor safety
self.setSafetyEnabled(True)
pose.init(left_encoder_callback=self.get_left_distance,
right_encoder_callback=self.get_right_distance,
gyro_callback=self.get_heading_rads,
wheelbase=self.robot_width,
encoder_factor=1)
dashboard2.add_graph(
"Heading", lambda: pose.get_current_pose().heading * 180 / math.pi)
def __init__(self, robot):
self.operate = operatorFunctions(drive=self,robot=robot)
self.gyro = AHRS.create_spi()
#self.gyro = wpilib.interfaces.Gyro()
"""Sets drive motors to a cantalon or victor"""
self.instantiateMotors()
self.instantiateEncoders()
#self.encoderReset()
#self.driveBase = arcadeDrive()
self.shifter = wpilib.DoubleSolenoid(51, 0, 1)#Initilizes the shifter's solenoid and sets it to read fron digital output 0
self.lfMotor = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.lbMotor = ctre.wpi_victorspx.WPI_VictorSPX(11)
self.rfMotor = ctre.wpi_victorspx.WPI_VictorSPX(9)
self.rbMotor = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.lfMotor.setNeutralMode(2)
self.lbMotor.setNeutralMode(2)
self.rfMotor.setNeutralMode(2)
self.rbMotor.setNeutralMode(2)
self.left = wpilib.SpeedControllerGroup(self.lfMotor, self.lbMotor)
self.right = wpilib.SpeedControllerGroup(self.rfMotor, self.rbMotor)
self.drive = DifferentialDrive(self.left, self.right)
self.firstTime = True#Check for autonDriveStraight
self.firstRun = True#Check for autonPivot
self.resetFinish = False#Check for encoder reset
self.setWheelCircumference()
self.oldGyro = 0
self.moveNumber = 1
self.shiftCounter = 0
def __init__(self):
self.ahrs = AHRS.create_spi(update_rate_hz=100)
self.pidsource = self.PIDSourceType.kDisplacement
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__()
# Initialize and calibrate the NavX-MXP.
self.gyro = AHRS.create_spi(wpilib.SPI.Port.kMXP)
self.gyro.reset()
# Initialize motors.
self.l1 = ctre.wpi_talonsrx.WPI_TalonSRX(robotMap.left1)
self.l2 = ctre.wpi_talonsrx.WPI_TalonSRX(robotMap.left2)
self.r1 = ctre.wpi_talonsrx.WPI_TalonSRX(robotMap.right1)
self.r2 = ctre.wpi_talonsrx.WPI_TalonSRX(robotMap.right2)
# Select a sensor for PID.
self.l1.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotMap.ctreTimeout)
self.r1.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotMap.ctreTimeout)
# Left sensor runs in reverse so the phase must be set for PID.
self.l1.setSensorPhase(True)
self.r1.setSensorPhase(True)
# Invert motor output as necessary.
self.r1.setInverted(True)
self.r2.setInverted(True)
# Set secondary motors to follow primary motor speed.
self.l2.follow(self.l1)
self.r2.follow(self.r1)
# Set talons to brake automatically.
self.l1.setNeutralMode(NeutralMode.Brake)
self.l2.setNeutralMode(NeutralMode.Brake)
self.r1.setNeutralMode(NeutralMode.Brake)
self.r2.setNeutralMode(NeutralMode.Brake)
# If code is running on a RoboRio, configure current limiting.
if RobotBase.isReal():
self.l1.configPeakCurrentLimit(robotMap.peakCurrent,
robotMap.ctreTimeout)
self.l1.configPeakCurrentDuration(robotMap.peakTime,
robotMap.ctreTimeout)
self.l1.configContinuousCurrentLimit(robotMap.continuousCurrent,
robotMap.ctreTimeout)
self.l1.enableCurrentLimit(True)
self.l2.configPeakCurrentLimit(robotMap.peakCurrent,
robotMap.ctreTimeout)
self.l2.configPeakCurrentDuration(robotMap.peakTime,
robotMap.ctreTimeout)
self.l2.configContinuousCurrentLimit(robotMap.continuousCurrent,
robotMap.ctreTimeout)
self.l2.enableCurrentLimit(True)
self.r1.configPeakCurrentLimit(robotMap.peakCurrent,
robotMap.ctreTimeout)
self.r1.configPeakCurrentDuration(robotMap.peakTime,
robotMap.ctreTimeout)
self.r1.configContinuousCurrentLimit(robotMap.continuousCurrent,
robotMap.ctreTimeout)
self.r1.enableCurrentLimit(True)
self.r2.configPeakCurrentLimit(robotMap.peakCurrent,
robotMap.ctreTimeout)
self.r2.configPeakCurrentDuration(robotMap.peakTime,
robotMap.ctreTimeout)
self.r2.configContinuousCurrentLimit(robotMap.continuousCurrent,
robotMap.ctreTimeout)
self.r2.enableCurrentLimit(True)
# Reset max recorded velocities
self.maxRecordedLeftVelocity = 0
self.maxRecordedRightVelocity = 0
self.r1.configNeutralDeadband(0.1, 10)
self.r2.configNeutralDeadband(0.1, 10)
self.l1.configNeutralDeadband(0.1, 10)
self.l2.configNeutralDeadband(0.1, 10)
def __init__(self):
self.ahrs = AHRS.create_spi(update_rate_hz=50)