def __init__(self):
'''Instantiates the motor object.'''
super().__init__('SingleMotor')
self.frontLeftMotor = WPI_TalonSRX(channels.frontLeftChannel)
self.rearLeftMotor = WPI_TalonSRX(channels.rearLeftChannel)
self.frontRightMotor = WPI_TalonSRX(channels.frontRightChannel)
self.rearRightMotor = WPI_TalonSRX(channels.rearRightChannel)
self.crossbow = wpilib.DoubleSolenoid(0, 1)
self.crossbow.set(wpilib.DoubleSolenoid.Value.kOff)
self.frontLeftMotor.setInverted(False)
# self.rearLeftMotor.configSelectedFeedbackSensor(WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 30)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(False)
self.drive = MecanumDrive(self.frontLeftMotor, self.rearLeftMotor,
self.frontRightMotor, self.rearRightMotor)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(True)
def createObjects(self):
self.front_left_motor = WPI_TalonSRX(robotmap.front_left_drive)
self.front_right_motor = WPI_TalonSRX(robotmap.front_right_drive)
self.back_left_motor = WPI_TalonSRX(robotmap.back_left_drive)
self.back_right_motor = WPI_TalonSRX(robotmap.back_right_drive)
motor_configuration.bulk_configure(self.front_left_motor, self.front_right_motor, self.back_left_motor, self.back_right_motor)
self.left_motors = wpilib.SpeedControllerGroup(self.front_left_motor, self.back_left_motor)
self.right_motors = wpilib.SpeedControllerGroup(self.front_right_motor, self.back_right_motor)
self.front_left_motor.setInverted(True)
self.front_right_motor.setInverted(True)
self.drivetrain_object = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors)
self.oi = OI()
# self.arduino_serial_connection = serial.Serial(
# port=robotmap.com_port,
# baudrate=9600,
# parity=serial.PARITY_ODD,
# stopbits=serial.STOPBITS_TWO,
# bytesize=serial.SEVENBITS
# )
wpilib.CameraServer.launch('vision.py:main')
def createObjects(self):
""" Create motors, sensors and all your components here. """
self.chassis_left_master = WPI_TalonSRX(1)
self.chassis_left_slave = WPI_TalonSRX(2)
self.chassis_right_master = WPI_TalonSRX(3)
self.chassis_right_slave = WPI_TalonSRX(4)
self.chassis_gyro = AHRS.create_spi()
self.joystick = Joystick(0)
def createObjects(self):
self.chassis_left_motor_master = WPI_TalonSRX(1)
self.chassis_left_motor_slave = WPI_TalonSRX(2)
self.chassis_right_motor_master = WPI_TalonSRX(3)
self.chassis_right_motor_slave = WPI_TalonSRX(4)
self.chassis_navx = AHRS.create_spi()
self.joystick = Joystick(0)
def __init__(self):
# Verify motor ports when placed on frame
self.intake = WPI_TalonSRX(6)
self.intake_solenoid = DoubleSolenoid(2, 3)
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kOff)
self.gear_shift = DoubleSolenoid(0, 1)
self.gear_shift.set(DoubleSolenoid.Value.kOff)
self.crossbow = WPI_TalonSRX(5)
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.motor1 = WPI_TalonSRX(1)
self.motor2 = WPI_TalonSRX(2)
self.drive = wpilib.drive.DifferentialDrive(self.motor1, self.motor2)
# joysticks 1 & 2 on the driver station
#self.motor1 = WPI_TalonSRX(2)
self.stick = wpilib.Joystick(0)
def createObjects(self):
# Motors
self.m_lfront = WPI_TalonSRX(1)
self.m_rfront = WPI_TalonSRX(2)
self.m_lback = WPI_TalonSRX(3)
self.m_rback = WPI_TalonSRX(4)
self.m_hatch = wpilib.Spark(0)
self.m_shooter = wpilib.Spark(1)
self.ls_shooter = wpilib.DigitalInput(0)
self.s_intake = wpilib.DoubleSolenoid(2, 3)
# Joysticks (PS4 Controller, in our case)
self.joystick = wpilib.Joystick(0)
def __init__(self):
# Hardware
self.motor_lf = WPI_TalonSRX(1)
self.motor_lr = WPI_TalonSRX(2)
self.motor_rf = WPI_TalonSRX(3)
self.motor_rr = WPI_TalonSRX(4)
self.drive = MecanumDrive(self.motor_lf, self.motor_lr, self.motor_rf,
self.motor_rr)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(True)
def __init__(self): self.lDriveF = WPI_TalonSRX(TALON_DRIVE_LF) self.lDriveR = WPI_TalonSRX(TALON_DRIVE_LR) self.rDriveF = WPI_TalonSRX(TALON_DRIVE_RF) self.rDriveR = WPI_TalonSRX(TALON_DRIVE_RR) #self.rDriveF.setInverted(True) #self.rDriveR.setInverted(True) self.lSpeedGroup = wpilib.SpeedControllerGroup(self.lDriveF, self.lDriveR) self.rSpeedGroup = wpilib.SpeedControllerGroup(self.rDriveF, self.rDriveR) self.drive = DifferentialDrive(self.lSpeedGroup, self.rSpeedGroup) self.drive.setSafetyEnabled(False) self.drivePID = ProtoPID(1/5.0, 0.0, 0.0, 0.0, 0.02)
def robotInit(self):
self.talon = WPI_TalonSRX(3)
self.joy = wpilib.Joystick(0)
self.loops = 0
self.timesInMotionMagic = 0
# first choose the sensor
self.talon.configSelectedFeedbackSensor(WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, self.kPIDLoopIdx, self.kTimeoutMs)
self.talon.setSensorPhase(True)
self.talon.setInverted(False)
# Set relevant frame periods to be at least as fast as periodic rate
self.talon.setStatusFramePeriod(WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, self.kTimeoutMs)
self.talon.setStatusFramePeriod(WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10, self.kTimeoutMs)
# set the peak and nominal outputs
self.talon.configNominalOutputForward(0, self.kTimeoutMs)
self.talon.configNominalOutputReverse(0, self.kTimeoutMs)
self.talon.configPeakOutputForward(1, self.kTimeoutMs)
self.talon.configPeakOutputReverse(-1, self.kTimeoutMs)
# set closed loop gains in slot0 - see documentation */
self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.talon.config_kF(0, 0.2, self.kTimeoutMs)
self.talon.config_kP(0, 0.2, self.kTimeoutMs)
self.talon.config_kI(0, 0, self.kTimeoutMs)
self.talon.config_kD(0, 0, self.kTimeoutMs)
# set acceleration and vcruise velocity - see documentation
self.talon.configMotionCruiseVelocity(15000, self.kTimeoutMs)
self.talon.configMotionAcceleration(6000, self.kTimeoutMs)
# zero the sensor
self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def robotInit(self):
# self.gyro = wpilib.I2C(wpilib.I2C.Port.kOnboard, 0x68)
self.leftFront = WPI_TalonSRX(5)
self.leftBack = WPI_TalonSRX(6)
self.rightFront = WPI_TalonSRX(7)
self.rightBack = WPI_TalonSRX(8)
self.leftDrive = wpilib.SpeedControllerGroup(self.leftBack,
self.leftFront)
self.rightDrive = wpilib.SpeedControllerGroup(self.rightBack,
self.rightFront)
self.drive = wpilib.drive.DifferentialDrive(self.leftDrive,
self.rightDrive)
self.joystick = wpilib.Joystick(0)
def __init__(self, robot):
super().__init__("Drive")
self.robot = robot
self.left_master = WPI_TalonSRX(robotmap.DRIVELEFTMASTER)
self.right_master = WPI_TalonSRX(robotmap.DRIVERIGHTMASTER)
self.left_slave = WPI_TalonSRX(robotmap.DRIVELEFTSLAVE)
self.right_slave = WPI_TalonSRX(robotmap.DRIVERIGHTSLAVE)
self.shifter_high = Solenoid(robotmap.PCM1_CANID,
robotmap.SHIFTER_HIGH)
self.shifter_low = Solenoid(robotmap.PCM1_CANID, robotmap.SHIFTER_LOW)
self.differential_drive = DifferentialDrive(self.left_master,
self.right_master)
self.TalonConfig()
self.shiftLow()
def robotInit(self):
self.leftFront = WPI_TalonSRX(1)
self.rightFront = WPI_TalonSRX(0)
self.leftBack = WPI_TalonSRX(2)
self.rightBack = WPI_TalonSRX(3)
NetworkTables.initialize(server="10.17.57.2")
self.sd = NetworkTables.getTable("SmartDashboard")
self.sd.putNumber(
("Talon " + str(self.leftFront.getDeviceID()) + " Speed"), 0)
self.sd.putNumber(
("Talon " + str(self.leftBack.getDeviceID()) + " Speed"), 0)
self.sd.putNumber(
("Talon " + str(self.rightFront.getDeviceID()) + " Speed"), 0)
self.sd.putNumber(
("Talon " + str(self.rightBack.getDeviceID()) + " Speed"), 0)
def __init__(self):
# Verify motor ports when placed on frame
self.motor_lf = WPI_TalonSRX(1)
self.motor_lr = WPI_TalonSRX(2)
self.motor_rf = WPI_TalonSRX(3)
self.motor_rr = WPI_TalonSRX(4)
self.motor_lf.setInverted(False)
self.motor_lr.setInverted(False)
self.drive = MecanumDrive(self.motor_lf, self.motor_lr, self.motor_rf,
self.motor_rr)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(True)
def __init__(self):
self.LF = WPI_TalonSRX(1)
self.LR = WPI_TalonSRX(2)
#self.LF.setInverted(True)
self.RF = WPI_TalonSRX(3)
self.RR = WPI_TalonSRX(4)
self.left = wpilib.SpeedControllerGroup(self.LF, self.LR)
self.right = wpilib.SpeedControllerGroup(self.RF, self.RR)
#self.RF.setInverted(True)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.drive.setSafetyEnabled(True)
self.LeftEncoderCount = 0
self.RightEncoderCount = 0
self.PrevLeftEncoderCount = self.LF.getSelectedSensorPosition()
self.PrevRightEncoderCount = self.RF.getSelectedSensorPosition()
def teleopInit(self):
"""Called when teleop starts; optional"""
self.intake.intake_clamp(False)
self.intake.intake_push(False)
self.lift_motor = WPI_TalonSRX(0)
self.motion.enabled = False
self.chassis.set_inputs(0, 0, 0)
self.intake.extension(True)
def createObjects(self):
if subsystems_enabled['limelight']:
self.limelight_table = NetworkTables.getTable("limelight")
if subsystems_enabled['navx']:
self.navx = navx.AHRS.create_spi()
if subsystems_enabled['shooter']:
self.shooter_srx_a = WPI_TalonSRX(1)
self.shooter_srx_b = WPI_TalonSRX(2)
if subsystems_enabled['neo']:
self.neo_motor = CANSparkMax(3, MotorType.kBrushless)
if subsystems_enabled['camera']:
wpilib.CameraServer.launch()
self.driver = DriverController(wpilib.XboxController(0))
self.sysop = SysopController(wpilib.XboxController(1))
def robotInit(self):
"""Create motors and stuff here"""
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
# Left side
self.drive_motor_a = WPI_TalonSRX(0)
self.drive_motor_b = WPI_TalonSRX(1)
self.left = wpilib.SpeedControllerGroup(self.drive_motor_a, self.drive_motor_b)
self.left.setInverted(True)
# Right side
self.drive_motor_c = WPI_TalonSRX(2)
self.drive_motor_d = WPI_TalonSRX(3)
self.right = wpilib.SpeedControllerGroup(self.drive_motor_c, self.drive_motor_d)
self.right.setInverted(True)
self.drive_base = wpilib.drive.DifferentialDrive(self.left, self.right)
def __init__(self):
Subsystem.__init__(self, "GateShot")
self.gate_shot = WPI_TalonSRX(0)
# Close
self.switchClose = wpilib.DigitalInput(0)
# Open
self.switchOpen = wpilib.DigitalInput(1)
# Switch beneath sushi wheel
self.switchSushi = wpilib.DigitalInput(2)
def createObjects(self):
motor_object_left = [
WPI_TalonSRX(port) for port in RobotMap.Drivetrain.motors_left
]
self.motors_left = wpilib.SpeedControllerGroup(motor_object_left[0], *motor_object_left[1:])
motor_object_right = [
WPI_TalonSRX(port) for port in RobotMap.Drivetrain.motors_right
]
self.motors_right = wpilib.SpeedControllerGroup(motor_object_right[0], *motor_object_right[1:])
for motor in motor_object_left + motor_object_right:
config_talon(motor, RobotMap.Drivetrain.motor_config)
self.drivetrain = wpilib.drive.DifferentialDrive(self.motors_left, self.motors_right)
self.drivetrain.setMaxOutput(RobotMap.Drivetrain.max_motor_power)
self.encoder_left = WPI_TalonSRX(RobotMap.Encoders.left_encoder)
self.encoder_right = WPI_TalonSRX(RobotMap.Encoders.right_encoder)
self.navx = navx.AHRS.create_spi()
self.driver = DriverController(wpilib.XboxController(0))
self.sysop = SysopController(wpilib.XboxController(1))
self.pid_mode = DriveMode.PID_TURN
self.robot_controller = wpilib.RobotController()
self.dash_target_angle = Tunable("Target Angle")
self.dash_target_position = Tunable("Target Position")
self.dash_v_max = Tunable("V_max")
self.dash_arc_distance = Tunable("Arc Distance")
self.dash_arc_radius = Tunable("Arc Radius")
self.dash_target_velocity_left = Tunable("Target Velocity Left")
self.dash_target_velocity_right = Tunable("Target Velocity Right")
self.tunables = [
self.dash_target_angle, self.dash_target_position, self.dash_v_max, self.dash_arc_distance, self.dash_arc_radius,
self.dash_target_velocity_left, self.dash_target_velocity_right
]
def createObjects(self):
'''Create motors and stuff here'''
with self.consumeExceptions():
Map.load_json(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
"map.json"))
self.chassis_ports = Map.get_map(
)["subsystems"]["chassis"]["can_motors"]
self.chassis_left_master = WPI_TalonSRX(
self.chassis_ports["left_master"])
self.chassis_left_slave = WPI_TalonSRX(
self.chassis_ports["left_slave"])
self.chassis_right_master = WPI_TalonSRX(
self.chassis_ports["right_master"])
self.chassis_right_slave = WPI_TalonSRX(
self.chassis_ports["right_slave"])
self.chassis_navx = AHRS.create_spi()
self.left_joystick = Joystick(
Map.get_map()["operator_ports"]["left_stick"])
def __init__(self):
left_front = WPI_TalonSRX(6)
left_rear = WPI_TalonSRX(1)
right_front = WPI_TalonSRX(4)
right_rear = WPI_TalonSRX(7)
left = SpeedControllerGroup(left_front, left_rear)
right = SpeedControllerGroup(right_front, right_rear)
self.left_encoder_motor = left_rear
self.right_encoder_motor = right_front
self.gear_solenoid = DoubleSolenoid(0, 1)
self.driver_gyro = ADXRS450_Gyro()
self.drive_train = DifferentialDrive(left, right)
# setup encoders
self.left_encoder_motor.setSensorPhase(True)
self.drive_train.setDeadband(0.1)
self.moving_linear = [0] * DriverComponent.LINEAR_SAMPLE_RATE
self.moving_angular = [0] * DriverComponent.ANGULAR_SAMPLE_RATE
def robotInit(self):
try:
wpilib.CameraServer.launch()
except:
pass
self.stick = wpilib.Joystick(0)
self.base = Base(rev.CANSparkMax(4, rev.MotorType.kBrushless),
rev.CANSparkMax(2, rev.MotorType.kBrushless),
rev.CANSparkMax(3, rev.MotorType.kBrushless),
rev.CANSparkMax(1, rev.MotorType.kBrushless),
AHRS.create_spi(wpilib.SPI.Port.kMXP), self.stick)
self.lift = Lift(WPI_TalonSRX(3), wpilib.DigitalInput(0),
wpilib.DigitalInput(1), self.stick)
self.arm = Arm(wpilib.Solenoid(5, 2), wpilib.Solenoid(5, 4),
wpilib.Solenoid(5, 1), wpilib.Solenoid(5, 5),
self.stick)
self.testvar = 0
self.ballintake = BallIntake(WPI_TalonSRX(4))
try:
self.ledController = LEDController.getInstance()
except:
pass
# Various one-time initializations happen here.
self.lift.initSensor()
self.base.navx.reset()
try:
NetworkTables.initialize()
except:
pass
try:
self.visiontable = NetworkTables.getTable("visiontable")
except:
pass
self.lights = False
self.op = False
self.testButton = JoystickButton(self.stick, 16)
self.roller = Roller(WPI_TalonSRX(20), WPI_TalonSRX(10), self.stick,
"init")
def __init__(self, intake_motor_channel, right_piston_module_number,
right_piston_forward, right_piston_reverse,
left_piston_module_number, left_piston_forward,
left_piston_reverse):
self.intake_motor = WPI_TalonSRX(intake_motor_channel)
self.right_piston = wpilib.DoubleSolenoid(right_piston_module_number,
right_piston_forward,
right_piston_reverse)
self.left_piston = wpilib.DoubleSolenoid(left_piston_module_number,
left_piston_forward,
left_piston_reverse)
self.right_piston_target = self.right_piston.Value.kReverse
self.left_piston_target = self.left_piston.Value.kReverse
self.collect_speed = 0
def robotInit(self):
self.talon = WPI_TalonSRX(3)
self.joy = wpilib.Joystick(0)
self.loops = 0
self.lastButton1 = False
self.targetPos = 0
# choose the sensor and sensor direction
self.talon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
# choose to ensure sensor is positive when output is positive
self.talon.setSensorPhase(True)
# choose based on what direction you want forward/positive to be.
# This does not affect sensor phase.
self.talon.setInverted(False)
# set the peak and nominal outputs, 12V means full
self.talon.configNominalOutputForward(0, self.kTimeoutMs)
self.talon.configNominalOutputReverse(0, self.kTimeoutMs)
self.talon.configPeakOutputForward(1, self.kTimeoutMs)
self.talon.configPeakOutputReverse(-1, self.kTimeoutMs)
# Set the allowable closed-loop error, Closed-Loop output will be
# neutral within this range. See Table in Section 17.2.1 for native
# units per rotation.
self.talon.configAllowableClosedloopError(0, self.kPIDLoopIdx,
self.kTimeoutMs)
# set closed loop gains in slot0, typically kF stays zero - see documentation */
self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.talon.config_kF(0, 0, self.kTimeoutMs)
self.talon.config_kP(0, 0.1, self.kTimeoutMs)
self.talon.config_kI(0, 0, self.kTimeoutMs)
self.talon.config_kD(0, 0, self.kTimeoutMs)
# zero the sensor
self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx,
self.kTimeoutMs)
def createObjects(self):
NetworkTables.initialize()
if robotmap.drivetrain_enabled:
m_l_a = WPI_TalonSRX(robotmap.port_m_l_a)
m_l_b = WPI_TalonSRX(robotmap.port_m_l_b)
m_l_c = WPI_TalonSRX(robotmap.port_m_l_c)
self.left_motors = wpilib.SpeedControllerGroup(m_l_a, m_l_b, m_l_c)
self.encoder_controller_left = m_l_a
m_r_a = WPI_TalonSRX(robotmap.port_m_r_a)
m_r_b = WPI_TalonSRX(robotmap.port_m_r_b)
m_r_c = WPI_TalonSRX(robotmap.port_m_r_c)
self.right_motors = wpilib.SpeedControllerGroup(
m_r_a, m_r_b, m_r_c)
self.encoder_controller_right = m_r_a
self.differential_drivetrain = wpilib.drive.DifferentialDrive(
self.left_motors, self.right_motors)
if robotmap.shooter_enabled:
self.shooter_motor = WPI_TalonSRX(robotmap.port_m_shooter)
if robotmap.loader_enabled:
self.loader_motor = WPI_TalonSRX(robotmap.port_m_loader)
if robotmap.navx_enabled:
self.navx = navx.AHRS.create_spi()
self.oi = DriverStation()
dashboard_utils.put_pid_values(
robotmap.dashboard_pid_drivetrain_turn,
kP=drivetrain.DrivetrainConstants.K_TURN_P,
kI=drivetrain.DrivetrainConstants.K_TURN_I,
kD=drivetrain.DrivetrainConstants.K_TURN_D)
self.robot_mode = RobotModes.MANUAL_DRIVE
def __init__(self, operator: OperatorControl):
self.operator = operator
self.motor = WPI_TalonSRX(10)
self.speed = 1
def robotInit(self):
# ctre motors defined here
self.r_motor = WPI_TalonSRX(1)
self.l_motor = WPI_TalonSRX(2)
self.stick = wpilib.Joystick(1)
def __init__(self):
Subsystem.__init__(self, 'SushiRotator')
# Motor controller object
self.sushi_motor = WPI_TalonSRX(1)
def robotInit(self):
self.timer = wpilib.Timer()
self.timer.start()
self.stick = XboxController(0)
self.dummyTalon = WPI_TalonSRX(self.ENCODER_SUM_CAN_ID)
self.leftTalonMaster = WPI_TalonSRX(self.LEFT_MASTER_CAN_ID)
self.leftTalonSlave = WPI_TalonSRX(self.LEFT_SLAVE_CAN_ID)
self.rightTalonMaster = WPI_TalonSRX(self.RIGHT_MASTER_CAN_ID)
self.rightTalonSlave = WPI_TalonSRX(self.RIGHT_SLAVE_CAN_ID)
self.leftTalonSlave.set(ControlMode.Follower, self.LEFT_MASTER_CAN_ID)
self.rightTalonSlave.set(ControlMode.Follower,
self.RIGHT_MASTER_CAN_ID)
self.drive = wpilib.RobotDrive(self.leftTalonMaster,
self.rightTalonMaster)
#self.pigeon = PigeonIMU(self.PIGEON_IMU_CAN_ID)
#if not self.isSimulation():
#self.pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, self.CAN_BUS_TIMEOUT_MS)
#else:
# print("setStatusFramePeriod() is not implmented in pyfrc simulator")
self.masterTalons = [self.leftTalonMaster, self.rightTalonMaster]
self.slaveTalons = [self.leftTalonSlave, self.rightTalonSlave]
self.leftTalons = [self.leftTalonMaster, self.leftTalonSlave]
self.rightTalons = [self.rightTalonMaster, self.rightTalonSlave]
self.talons = [
self.leftTalonMaster, self.leftTalonSlave, self.rightTalonMaster,
self.rightTalonSlave
]
'''Common configuration items common for all talons'''
for talon in self.talons:
talon.configNominalOutputForward(0.0, self.CAN_BUS_TIMEOUT_MS)
talon.configNominalOutputReverse(0.0, self.CAN_BUS_TIMEOUT_MS)
talon.configPeakOutputForward(1.0, self.CAN_BUS_TIMEOUT_MS)
talon.configPeakOutputReverse(-1.0, self.CAN_BUS_TIMEOUT_MS)
talon.enableVoltageCompensation(True)
talon.configVoltageCompSaturation(11.5, self.CAN_BUS_TIMEOUT_MS)
talon.configOpenLoopRamp(0.125, self.CAN_BUS_TIMEOUT_MS)
self.leftTalonSlave.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, self.PRIMARY_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
self.leftTalonSlave.configSelectedFeedbackCoefficient(
1.0, self.PRIMARY_PID_LOOP, self.CAN_BUS_TIMEOUT_MS)
self.leftTalonSlave.setSensorPhase(False)
self.leftTalonSlave.setStatusFramePeriod(
StatusFrame.Status_2_Feedback0, 10, self.CAN_BUS_TIMEOUT_MS)
self.rightTalonSlave.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, self.PRIMARY_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
self.rightTalonSlave.configSelectedFeedbackCoefficient(
1.0, self.PRIMARY_PID_LOOP, self.CAN_BUS_TIMEOUT_MS)
self.rightTalonSlave.setSensorPhase(False)
self.rightTalonSlave.setStatusFramePeriod(
StatusFrame.Status_2_Feedback0, 10, self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configRemoteFeedbackFilter(
self.leftTalonSlave.getDeviceID(),
RemoteSensorSource.TalonSRX_SelectedSensor, 0,
self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configRemoteFeedbackFilter(
self.rightTalonSlave.getDeviceID(),
RemoteSensorSource.TalonSRX_SelectedSensor, 1,
self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configSensorTerm(0, FeedbackDevice.RemoteSensor0,
self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configSensorTerm(1, FeedbackDevice.RemoteSensor1,
self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configSelectedFeedbackSensor(FeedbackDevice.SensorSum,
self.PRIMARY_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configSelectedFeedbackCoefficient(
1.0, self.PRIMARY_PID_LOOP, self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.setStatusFramePeriod(StatusFrame.Status_2_Feedback0,
10, self.CAN_BUS_TIMEOUT_MS)
for talon in self.leftTalons:
talon.setInverted(True)
for talon in self.rightTalons:
talon.setInverted(False)
self.leftTalonMaster.setSensorPhase(True)
if not self.isSimulation():
'''Setup the "sum" sensor as remote sensor 0'''
self.leftTalonMaster.configRemoteFeedbackFilter(
self.leftTalonSlave.getDeviceID(),
RemoteSensorSource.TalonSRX_SelectedSensor, 0,
self.CAN_BUS_TIMEOUT_MS)
'''Setup the pigeon as remote sensor 1'''
#self.leftTalonMaster.configRemoteFeedbackFilter(self.PIGEON_IMU_CAN_ID, RemoteSensorSource.Pigeon_Yaw, 1, self.CAN_BUS_TIMEOUT_MS)
else:
print(
"configRemoteFeedbackFilter() is not implemented in pyfrc simulator"
)
if not self.isSimulation():
'''Setup the "sum" sensor as remote sensor 0'''
self.rightTalonMaster.configRemoteFeedbackFilter(
self.rightTalonSlave.getDeviceID(),
RemoteSensorSource.TalonSRX_SelectedSensor, 0,
self.CAN_BUS_TIMEOUT_MS)
'''Setup the pigeon as remote sensor 1'''
#self.rightTalonMaster.configRemoteFeedbackFilter(self.PIGEON_IMU_CAN_ID, RemoteSensorSource.Pigeon_Yaw, 1, self.CAN_BUS_TIMEOUT_MS)
else:
print(
"configRemoteFeedbackFilter() is not implemented in pyfrc simulator"
)
'''
Now that the sensor sum remote sensor is setup, we can setup the master talons close-loop configuration
'''
for talon in self.masterTalons:
talon.configMotionProfileTrajectoryPeriod(
self.BASE_TRAJECTORY_PERIOD_MS)
'''
This just loads the constants into "slots".
Later we will select the slots to use for the primary and aux PID loops.
'''
talon.config_kP(self.PRIMARY_PID_LOOP_GAINS_SLOT, 0.375,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kI(self.PRIMARY_PID_LOOP_GAINS_SLOT, 0.0,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kD(self.PRIMARY_PID_LOOP_GAINS_SLOT, 0.0,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kF(self.PRIMARY_PID_LOOP_GAINS_SLOT, 0.35,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kP(self.AUX_PID_LOOP_GAINS_SLOT, 7.0,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kI(self.AUX_PID_LOOP_GAINS_SLOT, 0.0,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kD(self.AUX_PID_LOOP_GAINS_SLOT, 8.0,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kF(self.AUX_PID_LOOP_GAINS_SLOT, 0.0,
self.CAN_BUS_TIMEOUT_MS)
'''
Select the gains to use for the position control loop.
It probably doesn't matter what we select here since each individual trajectory point
has a setting for the primary and aux PID fains to use.
Selecting these gains here would be important if we were using motion magic control.
'''
talon.selectProfileSlot(self.PRIMARY_PID_LOOP_GAINS_SLOT,
self.PRIMARY_PID_LOOP)
'''This says the position control loop is allowed to command full motor output'''
talon.configClosedLoopPeakOutput(self.PRIMARY_PID_LOOP_GAINS_SLOT,
1.0, self.CAN_BUS_TIMEOUT_MS)
'''
Select the gains to use for the heading control loop.
It probably doesn't matter what we select here since each individual trajectory point
has a setting for the primary and aux PID fains to use.
Selecting these gains here would be important if we were using motion magic control.
'''
talon.selectProfileSlot(self.AUX_PID_LOOP_GAINS_SLOT,
self.AUX_PID_LOOP)
'''This says the heading control loop is allowed to command full motor output'''
talon.configClosedLoopPeakOutput(self.AUX_PID_LOOP_GAINS_SLOT, 1.0,
self.CAN_BUS_TIMEOUT_MS)
'''Select remote sensor 0 (the "sum" sensor) as the feedback for the primary PID loop (position control loop)'''
talon.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0,
self.PRIMARY_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
'''Select remote sensor 1 (the pigeon) as the feedback for the aux PID loop (heading control loop)'''
talon.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor1,
self.AUX_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
'''This sets the scale factor on the feedback sensor. For the encoders it is 1.0, this is really used for the gyro (pigeon) feedback'''
talon.configSelectedFeedbackCoefficient(1.0, self.PRIMARY_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
'''This sets the scale factor on the feedback sensor. Finally we use something other than 1.0'''
talon.configSelectedFeedbackCoefficient(
3600 / self.PIGEON_UNITS_PER_ROTATION, self.AUX_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
if not self.isSimulation():
self.leftTalonMaster.configAuxPIDPolarity(False,
self.CAN_BUS_TIMEOUT_MS)
self.rightTalonMaster.configAuxPIDPolarity(False,
self.CAN_BUS_TIMEOUT_MS)
else:
print(
"configAuxPIDPolarity() is not implemented in pyfrc simulator")
#SmartDashboard.putString("DRIVE_VALUE", "DRIVE_VALUES")
SmartDashboard.putString("LEFT_Y_VALUES", "LEFT_Y_VALUES")
SmartDashboard.putNumber("left_y_rate", 0.6)
SmartDashboard.putNumber("left_y_expo", 1.0)
SmartDashboard.putNumber("left_y_deadband", 0.1)
SmartDashboard.putNumber("left_y_power", 1.5)
SmartDashboard.putNumber("left_y_min", -0.5)
SmartDashboard.putNumber("left_y_max", 0.5)
SmartDashboard.putString("LEFT_X_VALUES", "LEFT_X_VALUES")
SmartDashboard.putNumber("left_x_rate", 0.5)
SmartDashboard.putNumber("left_x_expo", 1.0)
SmartDashboard.putNumber("left_x_deadband", 0.1)
SmartDashboard.putNumber("left_x_power", 1.5)
SmartDashboard.putNumber("left_x_min", -0.5)
SmartDashboard.putNumber("left_x_max", 0.5)
SmartDashboard.putString("RIGHT_Y_VALUES", "RIGHT_Y_VALUES")
SmartDashboard.putNumber("right_y_rate", 1.0)
SmartDashboard.putNumber("right_y_expo", 0.0)
SmartDashboard.putNumber("right_y_deadband", 0.1)
SmartDashboard.putNumber("right_y_power", 1.0)
SmartDashboard.putNumber("right_y_min", -1.0)
SmartDashboard.putNumber("right_y_max", 1.0)
SmartDashboard.putString("RIGHT_X_VALUES", "RIGHT_X_VALUES")
SmartDashboard.putNumber("right_x_rate", 0.5)
SmartDashboard.putNumber("right_x_expo", 1.0)
SmartDashboard.putNumber("right_x_deadband", 0.1)
SmartDashboard.putNumber("right_x_power", 1.5)
SmartDashboard.putNumber("right_x_min", -0.5)
SmartDashboard.putNumber("right_x_max", 0.5)