def __init__(self, robot):
super().__init__("shooter")
self.robot = robot
self.counter = 0 # used for updating the log
self.feed_forward = 3.5 # default volts to give the flywheel to get close to setpoint, optional
# motor controllers
self.sparkmax_flywheel = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.spark_hood = Talon(5)
self.spark_feed = Talon(7)
# encoders and PID controllers
self.hood_encoder = Encoder(
4,
5) # generic encoder - we'll have to install one on the 775 motor
self.hood_encoder.setDistancePerPulse(1 / 1024)
self.hood_controller = wpilib.controller.PIDController(Kp=0.005,
Ki=0,
Kd=0.0)
self.hood_setpoint = 5
self.hood_controller.setSetpoint(self.hood_setpoint)
self.flywheel_encoder = self.sparkmax_flywheel.getEncoder(
) # built-in to the sparkmax/neo
self.flywheel_controller = self.sparkmax_flywheel.getPIDController(
) # built-in PID controller in the sparkmax
# limit switches, use is TBD
self.limit_low = DigitalInput(6)
self.limit_high = DigitalInput(7)
def __init__(self):
self.clockwise_limit_switch = DigitalInput(
TURRET_CLOCKWISE_LIMIT_SWITCH)
self.counterclockwise_limit_switch = DigitalInput(
TURRET_COUNTERCLOCKWISE_LIMIT_SWITCH)
self.turn_motor = SparkMax(TURRET_TURN_MOTOR)
self.turn_pid = PIDController(0.4, 0.001, 0.02)
self.shoot_motor_1 = Falcon(TURRET_SHOOT_MOTORS[0])
self.shoot_motor_2 = Falcon(TURRET_SHOOT_MOTORS[1])
self.timer = Timer()
self.limelight = Limelight()
def init():
"""
Initialize switch objects.
"""
global gear_mech_switch
gear_mech_switch = DigitalInput(robotmap.switches.gear_switch_channel)
def init():
global drive_left_encoder, drive_right_encoder, elevator_encoder, gyro, back_photosensor, side_photosensor, pdp
drive_left_encoder = Encoder(6, 7)
drive_left_encoder.setDistancePerPulse(_drive_encoder_scale())
drive_right_encoder = Encoder(4, 5)
drive_right_encoder.setDistancePerPulse(_drive_encoder_scale())
elevator_encoder = Encoder(0, 1, True)
elevator_encoder.setDistancePerPulse(_elevator_encoder_scale())
gyro = Gyro(0)
back_photosensor = DigitalInput(8)
side_photosensor = DigitalInput(3)
pdp = PowerDistributionPanel()
def __init__(self, robot):
super().__init__("Arm")
self.robot = robot
self.peakCurrentLimit = 30
self.PeaKDuration = 50
self.continuousLimit = 15
motor = {}
for name in self.robot.RobotMap.motorMap.motors:
motor[name] = self.robot.Creator.createMotor(self.robot.RobotMap.motorMap.motors[name])
self.motors = motor
for name in self.motors:
self.motors[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted'])
# drive current limit
self.motors[name].configPeakCurrentLimit(self.peakCurrentLimit, 10)
self.motors[name].configPeakCurrentDuration(self.PeaKDuration, 10)
self.motors[name].configContinuousCurrentLimit(self.continuousLimit, 10)
self.motors[name].enableCurrentLimit(True)
self.AEnc = Encoder(4, 5, False, Encoder.EncodingType.k4X)
self.Zero = DigitalInput(6)
self.kp = 0.00035
self.ki = 0.00000000001
self.kd = 0.0000001
self.ArmPID = PID(self.kp, self.ki, self.kd)
def __init__(self):
self.left_fly = CANTalon(motor_map.left_fly_motor)
self.right_fly = CANTalon(motor_map.right_fly_motor)
self.intake_main = CANTalon(motor_map.intake_main_motor)
self.intake_mecanum = Talon(motor_map.intake_mecanum_motor)
self.ball_limit = DigitalInput(sensor_map.ball_limit)
self.intake_mecanum.setInverted(True)
self.left_fly.reverseOutput(True)
self.left_fly.enableBrakeMode(False)
self.right_fly.enableBrakeMode(False)
self.left_fly.setControlMode(CANTalon.ControlMode.Speed)
self.right_fly.setControlMode(CANTalon.ControlMode.Speed)
self.left_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.right_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I,
sensor_map.shoot_D, sensor_map.shoot_F,
sensor_map.shoot_Izone, sensor_map.shoot_RR,
sensor_map.shoot_Profile)
self.left_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.right_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising)
self.left_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
self.right_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev)
def __init__(self, pin):
self._value = 0
# setup gpiozero to call increment on each when_activated
encoder = DigitalInput(5)
encoder.when_activated = self._increment
encoder.when_deactivated = self._increment
def __init__(self, robot):
super().__init__()
# Capslock because its a constant?
# Added to current angle to account for max angle recalibration
# Motors
self.arm_motors = Arm_Motors()
#limit = Back_Switch()
#self.back_switch = limit.back_switch
# Encoders
#self.l_arm_encoder = My_Arm_Encoder(0,1)
self.l_arm_encoder = My_Arm_Encoder(9, 10)
self.limit_switch = DigitalInput(8)
#self.l_arm_encoder = wpilib.Encoder(0, 1)
# By empirical test
self.max_click_rate = 318.0
# For getting encoder and accounting for limits and error
# Arm starts in downward position
self.arm_min_or_max = 0
#XXX experimentally tested
self.max_ticks = 434
self.last_encoder = 0
# For creating the profile (arm profile)
self.x_axis_ticks = []
# XXX angle max may be wrong
self.angle_max = 150
# Slope of profile
self.max_accel = 0.536
self.min_decel = 0.536
self.time = 10
def __init__(self, channel, module=1, reverse=False):
"""
Initializes the switch on some digital channel and module.
Normally assumes switches are active low.
"""
super().__init__()
self.s = DigitalInput(module, channel)
self.reverse = reverse
def __init__(self, robot):
super().__init__("Climb")
self.robot = robot
self.winchMotor = Spark(robotmap.CLIMBWINCH)
self.armUpSolenoid = Solenoid(robotmap.CLIMBARM_UP)
self.armDownSolenoid = Solenoid(robotmap.CLIMBARM_DOWN)
self.lockCloseSolenoid = Solenoid(robotmap.CLIMBCLAMPLOCK_CLOSE)
self.lockOpenSolenoid = Solenoid(robotmap.CLIMBCLAMPLOCK_OPEN)
self.hookLimit = DigitalInput(robotmap.LIMIT_SWITCH_HOOK)
def __init__(self):
super().__init__()
self._l_motor = Talon(constants.motor_intake_l)
self._r_motor = Talon(constants.motor_intake_r)
self._intake_piston = Solenoid(constants.solenoid_intake)
self._photosensor = DigitalInput(constants.far_photosensor)
self._left_pwm = 0
self._right_pwm = 0
self._open = False
def __init__(self, Robot):
self._intake = Robot.intake
self._drive = Robot.drive
self._ramp = 0
self._rampSpeed = 6
self._LiftMotorLeft = SyncGroup(LIFT_MOTOR_LEFT_IDS, WPI_VictorSPX)
self._LiftMotorRight = SyncGroup(LIFT_MOTOR_RIGHT_IDS, WPI_VictorSPX)
self._LiftMotorLeft.setInverted(True)
self._liftEncoder = Encoder(LIFT_ENCODER_A, LIFT_ENCODER_B, False,
Encoder.EncodingType.k4X)
self._liftEncoder.setDistancePerPulse(1)
self._liftHallaffect = DigitalInput(HALL_DIO)
self.zeroEncoder()
self._liftPID = MiniPID(*LIFT_PID)
self._liftPID.setOutputLimits(-0.45, 1)
self.disableSpeedLimit = False
def __init__(self, operator: OperatorControl):
self.operator = operator
self.motor = WPI_VictorSPX(7)
self.encoder = Encoder(0, 1, False, Encoder.EncodingType.k4X)
self.encoder.setIndexSource(2)
self.limit = DigitalInput(4)
self.dashboard = NetworkTables.getTable("SmartDashboard")
self.totalValues = 2048 # * self.encoder.getEncodingScale() - 1
self.targetValue = 10
self.realValue = 0
self.allowedError = 10
self.hole = 4
self.holeOffset = 36 - 15
self.maxspeed = .25
self.minspeed = .1
self.speedmult = 1/300
self.speed = .1
def __init__(self, robot):
Subsystem.__init__(self, 'Conveyor')
self.robot = robot
self.blaser = DigitalInput(3)
motors = {}
self.map = self.robot.botMap
for name in self.map.motorMap.motors:
motors[name] = self.robot.Creator.createMotor(
self.map.motorMap.motors[name])
self.cMotors = motors
for name in self.cMotors:
self.cMotors[name].setInverted(
self.map.motorMap.motors[name]['inverted'])
self.cMotors[name].setNeutralMode(ctre.NeutralMode.Coast)
def __init__(self):
self.encoderUnit = 4096
self.gearRatio = 93.33
self.armSpeedMultiplier = 1
self.armBottomLimit = 5
self.armUpperLimit = 200
self.resetValue = 0
self.bottomLimitSwitch = DigitalInput(ArmLimitSwitch)
self.armMotor1 = CANSparkMax(armBaseMotor1,MotorType.kBrushless)
self.armMotor2 = CANSparkMax(armBaseMotor2,MotorType.kBrushless)
self.armEncoder1 = self.armMotor1.getEncoder()
self.armEncoder2 = self.armMotor2.getEncoder()
self.currentArmPower = 0
self.isOverride = False
def __init__(self):
super().__init__()
self._motor = SyncGroup(Talon, constants.motor_elevator)
self._position_encoder = Encoder(*constants.encoder_elevator)
self._photosensor = DigitalInput(constants.photosensor)
self._stabilizer_piston = Solenoid(constants.solenoid_dropper)
self._position_encoder.setDistancePerPulse(
(PITCH_DIAMETER * math.pi) / TICKS_PER_REVOLUTION)
# Trajectory controlling stuff
self._follower = TrajectoryFollower()
self.assure_tote = CircularBuffer(5)
self.auto_stacking = True # Do the dew
self._tote_count = 0 # Keep track of totes!
self._has_bin = False # Do we have a bin on top?
self._new_stack = True # starting a new stack?
self._tote_first = False # Override bin first to grab totes before anything else
self._should_drop = False # Are we currently trying to get a bin ?
self._close_stabilizer = True # Opens the stabilizer manually
self.force_stack = False # manually actuates the elevator down and up
self._follower.set_goal(Setpoints.BIN) # Base state
self._follower_thread = Thread(target=self.update_follower)
self._follower_thread.start()
self._auton = False
quickdebug.add_tunables(Setpoints,
["DROP", "STACK", "BIN", "TOTE", "FIRST_TOTE"])
quickdebug.add_printables(
self,
[('position', self._position_encoder.getDistance),
('photosensor', self._photosensor.get), "has_bin", "tote_count",
"tote_first", "at_goal", "has_game_piece", "auto_stacking"])
def __init__(self):
self.rightSensor = DigitalInput(0)
self.leftSensor = DigitalInput(1)
def __init__(self):
Events.__init__(self)
self.elevator_motor = WPI_TalonSRX(5)
self.elevator_bottom_switch = DigitalInput(9)
self.carriage_motor = WPI_TalonSRX(3)
self.carriage_bottom_switch = DigitalInput(1)
self.carriage_top_switch = DigitalInput(2)
self._create_events([
LifterComponent.EVENTS.on_control_move,
LifterComponent.EVENTS.on_manual_move
])
self._is_reset = False
# configure elevator motor and encoder
self.elevator_motor.setNeutralMode(NeutralMode.Brake)
self.elevator_motor.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.setSensorPhase(True)
self.elevator_motor.setInverted(True)
self.elevator_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configAllowableClosedloopError(
0, LifterComponent.ELEVATOR_ALLOWABLE_ERROR,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configForwardSoftLimitThreshold(
int(LifterComponent.ELEVATOR_MAX_HEIGHT /
LifterComponent.ELEVATOR_CONV_FACTOR), 0)
self.elevator_motor.config_kF(0, LifterComponent.ELEVATOR_kF,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kP(0, LifterComponent.ELEVATOR_kP,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kI(0, LifterComponent.ELEVATOR_kI,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kD(0, LifterComponent.ELEVATOR_kD,
LifterComponent.TIMEOUT_MS)
# self.elevator_motor.setCurrentLimit()
# self.elevator_motor.EnableCurrentLimit()
# self.elevator_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS)
# configure carriage motor and encoder
self.carriage_motor.setNeutralMode(NeutralMode.Brake)
self.carriage_motor.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.setSensorPhase(True)
self.carriage_motor.setInverted(True)
self.carriage_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configAllowableClosedloopError(
0, LifterComponent.CARRIAGE_ALLOWABLE_ERROR,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configForwardSoftLimitThreshold(
int(LifterComponent.CARRIAGE_MAX_HEIGHT /
LifterComponent.CARRIAGE_CONV_FACTOR), 0)
self.carriage_motor.config_kF(0, LifterComponent.ELEVATOR_kF,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kP(0, LifterComponent.ELEVATOR_kP,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kI(0, LifterComponent.ELEVATOR_kI,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kD(0, LifterComponent.ELEVATOR_kD,
LifterComponent.TIMEOUT_MS)
def initialize(self, robot):
self.state = -1
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.usingNeo = True
self.frontRetractStart = 0
self.wheelsStart = 0
self.wheelsStart2 = 0
if self.usingNeo:
# NOTE: If using Spark Max in PWM mode to control Neo brushless
# motors you MUST configure the speed controllers manually
# using a USB cable and the Spark Max client software!
self.frontLift: Spark = Spark(map.frontLiftPwm)
self.backLift: Spark = Spark(map.backLiftPwm)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
if map.robotId == map.astroV1:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.backLift.setNeutralMode(2)
self.frontLift.setNeutralMode(2)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
if not self.usingNeo:
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10,
0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backFloor)
self.frontSwitch = DigitalInput(map.frontFloor)
self.switchTopBack = DigitalInput(map.backTopSensor)
self.switchTopFront = DigitalInput(map.frontTopSensor)
self.switchBottomBack = DigitalInput(map.backBottomSensor)
self.switchBottomFront = DigitalInput(map.frontBottomSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = -1 #degrees
self.climbSpeed = 0.9
self.wheelSpeed = 0.9
self.backHold = -0.10 #holds back stationary if extended ADJUST**
self.frontHold = -0.10 #holds front stationary if extended
self.kP = 0.35 #proportional gain for angle to power
self.autoClimbIndicator = False
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False
def initialize(self, robot):
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
if map.robotId == map.astroV1:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10, 0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backBottomSensor)
self.frontSwitch = DigitalInput(map.frontBottomSensor)
self.upSwitch = DigitalInput(map.upSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = 0 #degrees
self.climbSpeed = 0.5
self.wheelSpeed = 0.5
self.backHold = -0.15 #holds back stationary if extended ADJUST**
self.frontHold = -0.1 #holds front stationary if extended
self.kP = 0.4 #proportional gain for angle to power
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False
