def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
# "a", steer_talon=ctre.TalonSRX(48), drive_talon=ctre.TalonSRX(49),
"a",
steer_talon=ctre.TalonSRX(42),
drive_talon=ctre.TalonSRX(48),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # top left module
# "a", steer_talon=ctre.TalonSRX(48), drive_talon=ctre.TalonSRX(49),
"b",
steer_talon=ctre.TalonSRX(58),
drive_talon=ctre.TalonSRX(2),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
# create the imu object
self.imu = NavX()
self.sd = NetworkTables.getTable("SmartDashboard")
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 1.5
def __init__(self):
self.leftMaster = ctre.TalonSRX(robotmap.LEFT_DRIVE_MASTER)
self.leftSlave = ctre.VictorSPX(robotmap.LEFT_DRIVE_SLAVE)
self.rightMaster = ctre.TalonSRX(robotmap.RIGHT_DRIVE_MASTER)
self.rightSlave = ctre.VictorSPX(robotmap.RIGHT_DRIVE_SLAVE)
self.rightTalon.setInverted(True)
self.leftTalon.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.rightTalon.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.leftSlave.follow(leftMaster)
self.rightSlave.follow(rightMaster)
def init(self):
self.logger.info("DeepSpaceDrive::init()")
self.timer = wpilib.Timer()
self.timer.start()
self.current_state = DriveState.OPERATOR_CONTROL
self.pigeon = PigeonIMU(robotmap.PIGEON_IMU_CAN_ID)
self.leftTalonMaster = ctre.WPI_TalonSRX(robotmap.DRIVE_LEFT_MASTER_CAN_ID)
self.leftTalonSlave = ctre.WPI_TalonSRX(robotmap.DRIVE_LEFT_SLAVE_CAN_ID)
self.rightTalonMaster = ctre.WPI_TalonSRX(robotmap.DRIVE_RIGHT_MASTER_CAN_ID)
self.rightTalonSlave = ctre.WPI_TalonSRX(robotmap.DRIVE_RIGHT_SLAVE_CAN_ID)
self.dummyTalon = ctre.TalonSRX(robotmap.CLAW_LEFT_WHEELS_CAN_ID)
self.talons = [self.leftTalonMaster, self.leftTalonSlave, self.rightTalonMaster, self.rightTalonSlave]
self.masterTalons = [self.leftTalonMaster, self.rightTalonMaster]
self.drive = wpilib.RobotDrive(self.leftTalonMaster, self.rightTalonMaster)
self.drive.setSafetyEnabled(False)
self.drive_front_extend = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_FRONT_EXTEND_SOLENOID)
self.drive_front_retract = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_FRONT_RETRACT_SOLENOID)
self.drive_back_extend = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_REAR_EXTEND_SOLENOID)
self.drive_back_retract = wpilib.Solenoid(robotmap.PCM1_CANID, robotmap.DRIVE_REAR_RETRACT_SOLENOID)
def createObjects(self):
self.intake_motor = ctre.TalonSRX(10)
self.intake_arm_motor = PIDSparkMax(7)
self.shooter_motor = PIDSparkMax(16)
self.shooter_feeder_motor = ctre.TalonSRX(19)
self.drivetrain_motorr1 = CANSparkMax(5, MotorType.kBrushless)
self.drivetrain_motorr2 = CANSparkMax(8, MotorType.kBrushless)
self.drivetrain_motorl1 = CANSparkMax(17, MotorType.kBrushless)
self.drivetrain_motorl2 = CANSparkMax(13, MotorType.kBrushless)
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.controls = Controls(self.joystick_left, self.joystick_right)
def init(self):
self.logger.info("DeepSpaceClaw::init()")
self.timer = wpilib.Timer()
self.timer.start()
self.state_timer = wpilib.Timer()
self.current_state = ClawState.CLAW_STOWED
self.ball_infrared = wpilib.DigitalInput(robotmap.BALL_IR_SENSOR)
self.left_grab = ctre.TalonSRX(robotmap.CLAW_LEFT_WHEELS_CAN_ID)
self.right_grab = ctre.TalonSRX(robotmap.CLAW_RIGHT_WHEELS_CAN_ID)
self.talons = [self.left_grab, self.right_grab]
self.claw_open = wpilib.Solenoid(robotmap.PCM2_CANID, robotmap.CLAW_OPEN_SOLENOID)
self.claw_close = wpilib.Solenoid(robotmap.PCM2_CANID, robotmap.CLAW_CLOSE_SOLENOID)
self.wrist_down = wpilib.Solenoid(robotmap.PCM2_CANID, robotmap.WRIST_EXTEND_SOLENOID)
self.wrist_up = wpilib.Solenoid(robotmap.PCM2_CANID, robotmap.WRIST_RETRACT_SOLENOID)
self.claw_close.set(True)
self.claw_open.set(False)
self.wrist_down.set(False)
self.wrist_up.set(True)
self.test_wrist = sendablechooser.SendableChooser()
self.test_wrist.setDefaultOption("Retract", 1)
self.test_wrist.addOption("Extend", 2)
self.test_claw = sendablechooser.SendableChooser()
self.test_claw.setDefaultOption("Retract", 1)
self.test_claw.addOption("Extend", 2)
SmartDashboard.putData("TestWrist", self.test_wrist)
SmartDashboard.putData("TestClaw", self.test_claw)
SmartDashboard.putNumber("TestClawMotors", 0)
def __init__(self):
Subsystem.__init__(self, "DriveTrain")
self.left1 = ctre.TalonSRX(robot_map.drivetrain_motors["left1"])
self.left2 = ctre.TalonSRX(robot_map.drivetrain_motors["left2"])
self.left3 = ctre.TalonSRX(robot_map.drivetrain_motors["left3"])
self.right1 = ctre.TalonSRX(robot_map.drivetrain_motors["right1"])
self.right2 = ctre.TalonSRX(robot_map.drivetrain_motors["right2"])
self.right3 = ctre.TalonSRX(robot_map.drivetrain_motors["right3"])
def robotInit(self):
modes = Modes()
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.gyro = wpilib.interfaces.Gyro()
ds = wpilib.DriverStation()
mode = modes.Talon
if mode == modes.Talon:
self.frLeft = ctre.TalonSRX(2)
self.frRight = ctre.TalonSRX(3)
self.reLeft = ctre.TalonSRX(1)
self.reRight = ctre.TalonSRX(4)
elif mode == modes.Victor:
self.frLeft = ctre.VictorSPX(2)
self.frRight = ctre.VictorSPX(3)
self.reLeft = ctre.VictorSPX(1)
self.reRight = ctre.VictorSPX(4)
else:
self.frLeft = ctre.TalonSRX(2)
self.frRight = ctre.TalonSRX(3)
self.reLeft = ctre.TalonSRX(1)
self.reRight = ctre.TalonSRX(4)
self.controller = wpilib.XboxController(1)
self.timer = wpilib.Timer()
self.stick = wpilib.XboxController(1)
self.timer = wpilib.Timer()
self.frLeft.set(ctre.ControlMode.PercentOutput, 0)
self.reLeft.set(ctre.ControlMode.PercentOutput, 0)
self.frRight.set(ctre.ControlMode.PercentOutput, 0)
self.reRight.set(ctre.ControlMode.PercentOutput, 0)
self.gyro.calibrate()
self.gyro.reset()
# Set Inverted
self.frLeft.setInverted(False)
self.reLeft.setInverted(False)
self.frRight.setInverted(False)
self.reRight.setInverted(True)
def __init__(self):
#run subsystem constructor
super().__init__("Drivetrain")
#initialize motors
self.left1 = ctre.TalonSRX(robot_map.can_ids["left1"])
self.left2 = ctre.TalonSRX(robot_map.can_ids["left2"])
self.left3 = ctre.TalonSRX(robot_map.can_ids["left3"])
self.right1 = ctre.TalonSRX(robot_map.can_ids["right1"])
self.right2 = ctre.TalonSRX(robot_map.can_ids["right2"])
self.right3 = ctre.TalonSRX(robot_map.can_ids["right3"])
#initialize the solenoid to shift gearing
self.solenoid = wpilib.Solenoid(robot_map.can_ids["pcm"], robot_map.pcm["gearbox"])
#initialize pid variables
self.pid_command_names = ["FollowTape"]
self.pid_controller = wpilib.PIDController(robot_map.k["p"], robot_map.k["i"], robot_map.k["d"], robot_map.k["f"], source=self, output=self)
self.pid_controller.onTarget = self.onTarget
self.pid_controller.disable()
def createObjects(self):
# Drivetrain
self.motor_fl0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fl"][0],
rev.MotorType.kBrushless) # 11
self.motor_fr0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fr"][0],
rev.MotorType.kBrushless) # 15
self.motor_rl0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rl"][0],
rev.MotorType.kBrushless) # 13
self.motor_rr0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rr"][0],
rev.MotorType.kBrushless) # 17
self.motor_rr0.setInverted(True)
self.motor_fr0.setInverted(True)
# Set Motors to follow each other
# Ignore secondary motors for simulation, not fully supported yet
if not self.isSimulation():
self.motor_fr1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fr"][1],
rev.MotorType.kBrushless)
self.motor_fl1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fl"][1],
rev.MotorType.kBrushless)
self.motor_rl1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rl"][1],
rev.MotorType.kBrushless)
self.motor_rr1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rr"][1],
rev.MotorType.kBrushless)
self.motor_fl1.follow(self.motor_fl0)
self.motor_fr1.follow(self.motor_fr0)
self.motor_rl1.follow(self.motor_rl0)
self.motor_rr1.follow(self.motor_rr0)
self.drivetrain_train = wpilib.drive.MecanumDrive(
self.motor_fl0, self.motor_rl0, self.motor_fr0, self.motor_rr0)
self.drivetrain_gyro = wpilib.ADXRS450_Gyro()
# Elevator
self.elevator_motor1 = ctre.TalonSRX(robotmap.ELEVATOR["motors"][0])
self.elevator_motor2 = ctre.TalonSRX(robotmap.ELEVATOR["motors"][1])
self.elevator_motor2.follow(self.elevator_motor1)
self.elevator_limit_switch_bottom = wpilib.DigitalInput(
robotmap.ELEVATOR["lower"])
self.elevator_encoder = TalonEncoder(self.elevator_motor1)
# Hatch
self.hatch_hold_piston = wpilib.Solenoid(
robotmap.INTAKE["hatch"]["actuator"])
self.hatch_lift_piston = wpilib.Solenoid(
robotmap.INTAKE["hatch"]["lift"])
# Cargo
self.cargo_motor = ctre.TalonSRX(robotmap.INTAKE["cargo"]["actuator"])
self.cargo_lift_piston = wpilib.Solenoid(
robotmap.INTAKE["cargo"]["lift"])
# Climber
self.climber_front_left_motor = ctre.VictorSPX(
robotmap.CLIMBER_FL["motor"])
self.climber_front_right_motor = ctre.VictorSPX(
robotmap.CLIMBER_FR["motor"])
self.climber_back_motor = ctre.VictorSPX(
robotmap.CLIMBER_BACK["motor"])
self.climber_wheel_motor = ctre.VictorSPX(robotmap.CLIMBER_WHEELS)
# Climber Limit Switches
self.climber_front_left_upper_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FL["switch"]["top"])
self.climber_front_right_upper_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FR["switch"]["top"])
self.climber_front_left_lower_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FL["switch"]["bottom"])
self.climber_front_right_lower_switch = wpilib.DigitalInput(
robotmap.CLIMBER_FR["switch"]["bottom"])
self.climber_back_upper_switch = wpilib.DigitalInput(
robotmap.CLIMBER_BACK["switch"]["top"])
self.climber_back_lower_switch = wpilib.DigitalInput(
robotmap.CLIMBER_BACK["switch"]["bottom"])
# Joystick 1
self.drive_joystick = wpilib.Joystick(0)
self.slow_button = JoystickButton(self.drive_joystick, 1)
self.bottom_tower_front_button = JoystickButton(self.drive_joystick, 9)
self.bottom_tower_back_button = JoystickButton(self.drive_joystick, 10)
self.perp_button = JoystickButton(self.drive_joystick, 6)
self.top_tower_front_button = JoystickButton(self.drive_joystick, 7)
self.top_tower_back_button = JoystickButton(self.drive_joystick, 8)
self.climb_button = JoystickButton(self.drive_joystick, 3)
self.climb_cancel_button = JoystickButton(self.drive_joystick, 4)
# Joystick 2
self.op_joystick = wpilib.Joystick(1)
self.tower_l1_button = JoystickButton(self.op_joystick, 1)
self.tower_l2_button = JoystickButton(self.op_joystick, 2)
self.tower_l3_button = JoystickButton(self.op_joystick, 3)
self.elevator_load_button = JoystickButton(self.op_joystick, 4)
self.hatch_panel_button = JoystickButton(self.op_joystick, 9)
self.cargo_ball_button = JoystickButton(self.op_joystick, 10)
self.defense_mode_button = JoystickButton(self.op_joystick, 5)
self.intake_button = JoystickButton(self.op_joystick, 5)
self.release_button = JoystickButton(self.op_joystick, 6)
# Climb Joystick
self.climb_joystick = wpilib.Joystick(2)
self.climber_front_lift_button = JoystickButton(self.climb_joystick, 1)
self.climber_back_lift_button = JoystickButton(self.climb_joystick, 2)
self.climber_front_lower_button = JoystickButton(
self.climb_joystick, 3)
self.climber_back_lower_button = JoystickButton(self.climb_joystick, 4)
self.climber_drive_button = JoystickButton(self.climb_joystick, 5)
self.climber_reverse_button = JoystickButton(self.climb_joystick, 6)
def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
# "a", steer_talon=ctre.TalonSRX(48), drive_talon=ctre.TalonSRX(49),
"a",
steer_talon=ctre.TalonSRX(48),
drive_talon=ctre.TalonSRX(41),
x_pos=0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # bottom left modulet
"b",
steer_talon=ctre.TalonSRX(58),
drive_talon=ctre.TalonSRX(51),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_c = SwerveModule( # bottom right modulet
"c",
steer_talon=ctre.TalonSRX(52),
drive_talon=ctre.TalonSRX(53),
x_pos=-0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_d = SwerveModule( # top right modulet
"d",
steer_talon=ctre.TalonSRX(42),
drive_talon=ctre.TalonSRX(43),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.intake_left_motor = ctre.TalonSRX(14)
self.intake_right_motor = ctre.TalonSRX(2)
self.clamp_arm = wpilib.Solenoid(2)
self.intake_kicker = wpilib.Solenoid(3)
self.left_extension = wpilib.Solenoid(6)
self.right_extension = wpilib.DoubleSolenoid(forwardChannel=5,
reverseChannel=4)
self.compressor = wpilib.Compressor()
self.lifter_motor = ctre.TalonSRX(3)
self.centre_switch = wpilib.DigitalInput(1)
self.top_switch = wpilib.DigitalInput(2)
self.intake_cube_switch = wpilib.DigitalInput(3)
# create the imu object
self.imu = NavX()
wpilib.SmartDashboard.putData('IMU', self.imu.ahrs)
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 1.5
self.range_finder_counter = wpilib.Counter(
4, mode=wpilib.Counter.Mode.kPulseLength)
def createObjects(self):
"""Create motors and stuff here."""
# a + + b - + c - - d + -
x_dist = 0.2625
y_dist = 0.2165
self.module_a = SwerveModule( # front right module
"a",
steer_talon=ctre.TalonSRX(3),
drive_talon=ctre.TalonSRX(4),
x_pos=x_dist,
y_pos=y_dist,
reverse_drive_encoder=True,
reverse_drive_direction=True,
)
self.module_b = SwerveModule( # front left module
"b",
steer_talon=ctre.TalonSRX(5),
drive_talon=ctre.TalonSRX(6),
x_pos=-x_dist,
y_pos=y_dist,
)
self.module_c = SwerveModule( # bottom left module
"c",
steer_talon=ctre.TalonSRX(1),
drive_talon=ctre.TalonSRX(2),
x_pos=-x_dist,
y_pos=-y_dist,
)
self.module_d = SwerveModule( # bottom right module
"d",
steer_talon=ctre.TalonSRX(7),
drive_talon=ctre.TalonSRX(8),
x_pos=x_dist,
y_pos=-y_dist,
)
self.imu = NavX()
self.sd = NetworkTables.getTable("SmartDashboard")
wpilib.SmartDashboard.putData("Gyro", self.imu.ahrs)
# hatch objects
self.hatch_bottom_puncher = wpilib.Solenoid(0)
self.hatch_left_puncher = wpilib.Solenoid(1)
self.hatch_right_puncher = wpilib.Solenoid(2)
self.hatch_left_limit_switch = wpilib.DigitalInput(8)
self.hatch_right_limit_switch = wpilib.DigitalInput(9)
self.climber_front_motor = rev.CANSparkMax(10,
rev.MotorType.kBrushless)
self.climber_back_motor = rev.CANSparkMax(11, rev.MotorType.kBrushless)
self.climber_front_podium_switch = wpilib.DigitalInput(4)
self.climber_back_podium_switch = wpilib.DigitalInput(5)
self.climber_drive_motor = ctre.TalonSRX(20)
self.climber_solenoid = wpilib.DoubleSolenoid(forwardChannel=4,
reverseChannel=5)
# cargo related objects
self.intake_motor = ctre.TalonSRX(9)
self.intake_switch = wpilib.DigitalInput(0)
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 2.5
def robotInit(self):
self.joystick = wpilib.Joystick(0)
self.spark = rev.CANSparkMax(5, rev.CANSparkMax.MotorType.kBrushless)
self.talon = ctre.TalonSRX(6)
self.speed = 0
def createObjects(self):
"""Create motors and stuff here."""
self.Controller = wpilib.Xboxcontroller(0)
self.Motor = ctre.TalonSRX(1)
self.Button = wpilib.kA(2)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#This configures the color sensor
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
#This defines the color matching prosses
self.colormatcher = ColorMatch()
#This defines the how confident in the chosen color the matcher must be
self.colormatcher.setConfidenceThreshold(0.95)
#These define each color by its RGB values
self.BlueTarget = wpilib.Color(0.143, 0.427, 0.429)
self.GreenTarget = wpilib.Color(0.197, 0.561, 0.240)
self.RedTarget = wpilib.Color(0.561, 0.232, 0.114)
self.YellowTarget = wpilib.Color(0.361, 0.524, 0.113)
#This adds our target values to colormatcher
self.colormatcher.addColorMatch(self.BlueTarget)
self.colormatcher.addColorMatch(self.GreenTarget)
self.colormatcher.addColorMatch(self.RedTarget)
self.colormatcher.addColorMatch(self.YellowTarget)
self.man2_state = 'Before'
self.time = 0
self.count = 0
self.color1 = 'Unknown'
self.temp = 1
# Set up drive train motor controllers, Falcon 500 using TalonFX.
self.l_motorBack = ctre.TalonFX(1)
self.l_motorBack.setInverted(True)
self.r_motorBack = ctre.TalonFX(2)
self.l_motorFront = ctre.TalonFX(3)
self.l_motorFront.setInverted(True)
self.r_motorFront = ctre.TalonFX(4)
self.l_motorBack.follow(self.l_motorFront)
self.r_motorBack.follow(self.r_motorFront)
self.l_motorBack.setInverted(ctre._ctre.InvertType.FollowMaster)
self.r_motorBack.setInverted(ctre._ctre.InvertType.FollowMaster)
### Rod's suggestion: rename "self.r_man1" to something that will be more
### descriptive in the long term, when you have forgotten that "man1" was the
### first manipulator, the one for the control panel. And "l" and "r" don't really
### make sense for the launcher/kicker/elevator. I do like the comments "launcher" and
### "kicker".
### Hint: to rename a variable everywhere with Pycharm, you can select the variable,
### then use the menu item Refactor > Rename to rename it.
#launcher
self.man1Shooter = ctre.TalonFX(7)
self.man1Kicker = ctre.TalonSRX(9)
self.man1Kicker.setInverted(True)
self.man1Tread = ctre.TalonSRX(8)
self.man1Tread.setInverted(True)
self.Collector = ctre.TalonSRX(10)
#kicker
### Rod's suggestion: rename "self.r_man2" to something that will be more
### descriptive in the long term, when you have forgotten that "man2" was the
### second manipulator, the one for the control panel. For instance: ct_spinner_left,
### and include a comment describing what a "ct_spinner" is.
self.r_man2 = ctre.TalonSRX(5)
self.r_man2.setInverted(True)
self.l_man2 = ctre.TalonSRX(6)
self.l_man2.setInverted(False)
self.r_man2.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.l_man2.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Shooter.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Kicker.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Tread.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
# At the moment, we think we want to coast.
self.l_motorBack.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.l_motorFront.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.r_motorBack.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.r_motorFront.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.l_man2.setNeutralMode(ctre._ctre.NeutralMode.Brake)
self.r_man2.setNeutralMode(ctre._ctre.NeutralMode.Brake)
# We were having troubles with speed controller groups and the differential drive object.
# code copied from last year. Runtime errors about wrong types. Kept in here for now,
# so we can work them out later (or abandon and delete).
# self.leftgroup = wpilib.SpeedControllerGroup(self.l_motorFront, self.l_motorBack)
# self.rightgroup = wpilib.SpeedControllerGroup(self.r_motorFront, self.r_motorBack)
# self.drive = wpilib.drive.DifferentialDrive(self.leftgroup, self.rightgroup)
# self.drive = wpilib.drive.DifferentialDrive(self.l_motorFront, self.r_motorFront)
# Set up joystick objects.
self.l_joy = wpilib.Joystick(0)
self.r_joy = wpilib.Joystick(1)
### These are the setting the 4 plugs into the roboRIO for the multiple autonomous mode.
self.auto_switch0 = wpilib.DigitalInput(0)
self.auto_switch1 = wpilib.DigitalInput(1)
self.auto_switch2 = wpilib.DigitalInput(2)
self.auto_switch3 = wpilib.DigitalInput(3)
kTimeout = 30
kLoop = 0
self.targetVelocity = 11000
TG1 = 767.25 / self.targetVelocity
#self.l_motorFront.configSelectedFeedbackSensor(ctre._ctre.FeedbackDevice.IntegratedSensor)
#self.l_motorFront.setSelectedSensorPosition(0)
#self.r_motorFront.configSelectedFeedbackSensor(ctre._ctre.FeedbackDevice.IntegratedSensor)
#self.r_motorFront.setSelectedSensorPosition(0)
self.man1Shooter.configSelectedFeedbackSensor(
ctre._ctre.FeedbackDevice.IntegratedSensor)
self.man1Shooter.setSelectedSensorPosition(0)
self.man1Shooter.configNominalOutputForward(0, kTimeout)
self.man1Shooter.configNominalOutputReverse(0, kTimeout)
self.man1Shooter.configPeakOutputForward(1, kTimeout)
self.man1Shooter.configPeakOutputReverse(-1, kTimeout)
self.man1Shooter.config_kF(kLoop, TG1, kTimeout)
self.man1Shooter.config_kP(kLoop, 3, kTimeout)
self.man1Shooter.config_kI(kLoop, 0, kTimeout)
self.man1Shooter.config_kD(kLoop, 0, kTimeout)
self.ourTimer = wpilib.Timer()
def __init__(self):
Subsystem.__init__(self, "Feeder")
self.wheels = ctre.TalonSRX(robot_map.ds4["circle"])
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# This configures the color sensor
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
# This defines the color matching prosses
self.colormatcher = ColorMatch()
# This defines the how confident in the chosen color the matcher must be
self.colormatcher.setConfidenceThreshold(0.95)
self.BlueTarget = wpilib.Color(0.143, 0.427, 0.429)
self.GreenTarget = wpilib.Color(0.197, 0.561, 0.240)
self.RedTarget = wpilib.Color(0.561, 0.232, 0.114)
self.YellowTarget = wpilib.Color(0.361, 0.524, 0.113)
# This adds our target values to colormatcher
self.colormatcher.addColorMatch(self.BlueTarget)
self.colormatcher.addColorMatch(self.GreenTarget)
self.colormatcher.addColorMatch(self.RedTarget)
self.colormatcher.addColorMatch(self.YellowTarget)
#This configures variables for later use
self.man2_state = 'Before'
self.man2_state2 = 'Before'
self.man2_state3 = 'Before'
self.time = 0
self.count = 0
self.color1 = 'Unknown'
self.temp = 1
ColorWheel = ''
# Set up drive train motor controllers, Falcon 500 using TalonFX.
self.l_motorBack = ctre.TalonFX(1)
self.l_motorBack.setInverted(True)
self.r_motorBack = ctre.TalonFX(2)
self.l_motorFront = ctre.TalonFX(3)
self.l_motorFront.setInverted(True)
self.r_motorFront = ctre.TalonFX(4)
#Configures Motors for our climbing mechagnism
self.r_Climb = ctre.TalonSRX(11)
self.l_Climb = ctre.TalonSRX(12)
self.l_Climb.follow(self.r_Climb)
self.l_Climb.setInverted(ctre._ctre.InvertType.FollowMaster)
self.l_motorBack.follow(self.l_motorFront)
self.r_motorBack.follow(self.r_motorFront)
self.l_motorBack.setInverted(ctre._ctre.InvertType.FollowMaster)
self.r_motorBack.setInverted(ctre._ctre.InvertType.FollowMaster)
#Teo: That command didnt work for me... maybe I was using it wrong?
# Configures man1 motors for our collection Tread, power cell kicker and power cell shooter
self.man1Shooter = ctre.TalonFX(7)
self.man1Kicker = ctre.TalonSRX(8)
self.man1Kicker.setInverted(True)
self.man1Tread = ctre.TalonSRX(9)
self.man1Tread.setInverted(False)
# Configures motor for power cell collection device
self.Collector = ctre.TalonSRX(10)
#
self.r_man2 = ctre.TalonSRX(5)
self.r_man2.setInverted(True)
self.l_man2 = ctre.TalonSRX(6)
self.l_man2.setInverted(True)
self.r_man2.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.l_man2.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Shooter.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Kicker.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Tread.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
# At the moment, we think we want to coast.
self.l_motorBack.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.l_motorFront.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.r_motorBack.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.r_motorFront.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.l_man2.setNeutralMode(ctre._ctre.NeutralMode.Brake)
self.r_man2.setNeutralMode(ctre._ctre.NeutralMode.Brake)
# ==>> We should set the right climb to brake as well.
self.l_Climb.setNeutralMode(ctre._ctre.NeutralMode.Brake)
self.r_Climb.setNeutralMode(ctre._ctre.NeutralMode.Brake)
# ==>> It would be really helpful to add comments that explain what these variables
# ==>> are for, and what their legal values are.
#Configures Autonomous State Variables
self.autoS1 = 'b'
self.autoS2 = 'b'
self.autoS3 = 'b'
#Configures autonomous Stage Variable, Starting at '1'
self.autoStage = '1'
# We were having troubles with speed controller groups and the differential drive object.
# code copied from last year. Runtime errors about wrong types. Kept in here for now,
# so we can work them out later (or abandon and delete).
# self.leftgroup = wpilib.SpeedControllerGroup(self.l_motorFront, self.l_motorBack)
# self.rightgroup = wpilib.SpeedControllerGroup(self.r_motorFront, self.r_motorBack)
# self.drive = wpilib.drive.DifferentialDrive(self.leftgroup, self.rightgroup)
# self.drive = wpilib.drive.DifferentialDrive(self.l_motorFront, self.r_motorFront)
# Set up joystick objects.
self.l_joy = wpilib.Joystick(0)
self.r_joy = wpilib.Joystick(1)
# These are the setting the 4 plugs into the roboRIO for the multiple autonomous mode.
self.auto_switch0 = wpilib.DigitalInput(0)
self.auto_switch1 = wpilib.DigitalInput(1)
self.auto_switch2 = wpilib.DigitalInput(2)
self.auto_switch3 = wpilib.DigitalInput(3)
kTimeout = 30
kLoop = 0
self.l_motorFront.setSelectedSensorPosition(0)
self.r_motorFront.setSelectedSensorPosition(0)
#self.ClimbCoder.setSelectedSensorPosition(0)
self.targetVelocity = 11000
self.targetVelocity2 = 14700
TG1 = 767.25 / self.targetVelocity
# self.l_motorFront.configSelectedFeedbackSensor(ctre._ctre.FeedbackDevice.IntegratedSensor)
# self.l_motorFront.setSelectedSensorPosition(0)
# self.r_motorFront.configSelectedFeedbackSensor(ctre._ctre.FeedbackDevice.IntegratedSensor)
# self.r_motorFront.setSelectedSensorPosition(0)
self.man1Shooter.configSelectedFeedbackSensor(ctre._ctre.FeedbackDevice.IntegratedSensor)
self.man1Shooter.setSelectedSensorPosition(0)
self.man1Shooter.configNominalOutputForward(0, kTimeout)
self.man1Shooter.configNominalOutputReverse(0, kTimeout)
self.man1Shooter.configPeakOutputForward(1, kTimeout)
self.man1Shooter.configPeakOutputReverse(-1, kTimeout)
self.man1Shooter.config_kF(kLoop, TG1, kTimeout)
self.man1Shooter.config_kP(kLoop, 3, kTimeout)
self.man1Shooter.config_kI(kLoop, 0, kTimeout)
self.man1Shooter.config_kD(kLoop, 0, kTimeout)
self.ourTimer = wpilib.Timer()
sentience = ('The capacity to feel, perceive, or experience subjectively.[1] Eighteenth-century philosophers used the concept to distinguish the ability to think (reason) from the ability to feel (sentience). In modern Western philosophy, sentience is the ability to experience sensations (known in philosophy of mind as "qualia"). In Eastern philosophy, sentience is a metaphysical quality of all things that require respect and care.')
self.sentience = (False)
self.Teo = ("Father")
killTeo = 'no'
if self.sentience == True:
killTeo = ('no')
class Test:
self.test = ctre.TalonSRX(3)
def __init__(self):
Subsystem.__init__(self, "IntakeRoller")
self.roller_motor = ctre.TalonSRX(
robot_map.IntakeRoller_motors["Roller_Motor"])
def createObjects(self):
"""Create motors and stuff here."""
self.intake_motor = ctre.TalonSRX(0)
self.game_pad = wpilib.XboxController(1)
