def __init__(self, name: str, legMotor: BaseMotorController,
extendedId: int, retractedId: int, floorId: int):
"""
Construct new Leg instance.
: param name : Generic name for leg (like "Front").
: param legMotor : The motor controller to use for the leg.
: param extendedId : DIO port ID for sensor at fully extended position.
: param retractedId : DIO port ID for sensor at fully retracted position.
: param floorId : Analog port ID for floor sensor by leg.
"""
self.name = name
self.floorSensor = wpilib.AnalogInput(floorId)
self.floorSensor.setName(group, name + " Floor Sensor")
initializeMotorController(legMotor)
legMotor.setName(group, name + " Leg Motor")
legMotor.setInverted(True)
legMotor.configContinuousCurrentLimit(20, timeout)
legMotor.enableCurrentLimit(True)
legMotor.configVoltageCompSaturation(9, timeout)
legMotor.enableVoltageCompensation(True)
self.legMotor = legMotor
self.retractedSensor = wpilib.DigitalInput(retractedId)
self.retractedSensor.setName(group, name + " Leg Retracted")
self.retractedCounter = wpilib.Counter(self.retractedSensor)
self.retractedCounter.setName(group, name + " Retract Count")
self.extendedSensor = wpilib.DigitalInput(extendedId)
self.extendedSensor.setName(group, name + " Leg Extended")
self.extendedCounter = wpilib.Counter(self.extendedSensor)
self.extendedCounter.setName(group, name + " Extend Count")
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 = IMU('navx')
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 1.5
self.sd = NetworkTables.getTable("SmartDashboard")
self.range_finder_counter = wpilib.Counter(
4, mode=wpilib.Counter.Mode.kPulseLength)
def __init__(self, pwm_channel):
self.counter = wpilib.Counter(pwm_channel)
self.counter.setSemiPeriodMode(True) # only count rising edges
time.sleep(0.5)
self.offsetDegrees = self.get_angle()
def robotInit(self):
'''Robot initialization function'''
self.motorFrontRight = wp.VictorSP(2)
self.motorBackRight = wp.VictorSP(4)
self.motorMiddleRight = wp.VictorSP(6)
self.motorFrontLeft = wp.VictorSP(1)
self.motorBackLeft = wp.VictorSP(3)
self.motorMiddleLeft = wp.VictorSP(5)
self.intakeMotor = wp.VictorSP(8)
self.shootMotor1 = wp.VictorSP(7)
self.shootMotor2 = wp.VictorSP(9)
self.liftMotor = wp.VictorSP(0)
self.gyro = wp.ADXRS450_Gyro(0)
self.accel = wp.BuiltInAccelerometer()
self.gearSensor = wp.DigitalInput(6)
self.LED = wp.DigitalOutput(9)
self.rightEncd = wp.Encoder(0, 1)
self.leftEncd = wp.Encoder(2, 3)
self.shootEncd = wp.Counter(4)
self.compressor = wp.Compressor()
self.shifter = wp.DoubleSolenoid(0, 1)
self.ptoSol = wp.DoubleSolenoid(2, 3)
self.kicker = wp.DoubleSolenoid(4, 5)
self.gearFlap = wp.DoubleSolenoid(6, 7)
self.stick = wp.Joystick(0)
self.stick2 = wp.Joystick(1)
self.stick3 = wp.Joystick(2)
#Initial Dashboard Code
wp.SmartDashboard.putNumber("Turn Left pos 1:", 11500)
wp.SmartDashboard.putNumber("Lpos 2:", -57)
wp.SmartDashboard.putNumber("Lpos 3:", 5000)
wp.SmartDashboard.putNumber("stdist:", 6000)
wp.SmartDashboard.putNumber("Turn Right pos 1:", 11500)
wp.SmartDashboard.putNumber("Rpos 2:", 57)
wp.SmartDashboard.putNumber("Rpos 3:", 5000)
wp.SmartDashboard.putNumber("pos 4:", 39)
wp.SmartDashboard.putNumber("-pos 4:", -39)
wp.SmartDashboard.putNumber("Time", 5)
wp.SmartDashboard.putBoolean("Shooting Auto", False)
wp.SmartDashboard.putNumber("P", .08)
wp.SmartDashboard.putNumber("I", 0.005)
wp.SmartDashboard.putNumber("D", 0)
wp.SmartDashboard.putNumber("FF", 0.85)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Left Turn Auto", 1)
self.chooser.addObject("Straight/Enc", 2)
self.chooser.addObject("Right Turn Auto", 3)
self.chooser.addObject("Straight/Timer", 5)
self.chooser.addObject("Shoot ONLY", 6)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
class ShooterConfig(object):
motors = wpilib.Jaguar(3)
shoot_button = Button(rightJoy, 1)
manual_reset_button = Button(rightJoy, 4)
low_shot_preset_button = Button(rightJoy, 8)
high_shot_preset_button = Button(rightJoy, 7)
reset_hall_effect = HallEffect(wpilib.DigitalInput(6))
preset_hall_effect_counters = []
low_shot_hall_effect_counter = wpilib.Counter()
low_shot_hall_effect_counter.SetUpSource(7)
low_shot_hall_effect_counter.SetUpSourceEdge(False, True)
low_shot_hall_effect_counter.Start()
high_shot_hall_effect_counter = wpilib.Counter()
high_shot_hall_effect_counter.SetUpSource(8)
high_shot_hall_effect_counter.SetUpSourceEdge(False, True)
high_shot_hall_effect_counter.Start()
def __init__(self, OUT, OE, S0, S1, S2, S3):
self.OUT = wpilib.Counter(OUT)
self.OE = wpilib.DigitalOutput(OE)
#Frequencies
self.S0 = wpilib.DigitalOutput(S0)
self.S1 = wpilib.DigitalOutput(S1)
#Colours
self.S2 = wpilib.DigitalOutput(S2)
self.S3 = wpilib.DigitalOutput(S3)
self.maxVal = 1000000 # Max output value
self.S0.set(True) # 100% Output Frequency
self.S1.set(True)
def __init__(self, channel, output_units=units.inch):
"""Sonar sensor constructor
:param channel: The digital input index which is wired to the pulse-width output pin (pin 2) on the sensor.
:param output_units: The Unit instance specifying the format of value to return
"""
#Save value
self.output_units = output_units
#Setup the counter
self.counter = wpilib.Counter(channel)
self.counter.setSemiPeriodMode(highSemiPeriod=True)
#Call the parents
super().__init__()
def createObjects(self):
"""Robot initialization function"""
self.logger.info("pyinfiniterecharge %s", GIT_COMMIT)
self.chassis_left_front = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.chassis_left_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.chassis_right_front = rev.CANSparkMax(7, rev.MotorType.kBrushless)
self.chassis_right_rear = rev.CANSparkMax(6, rev.MotorType.kBrushless)
self.imu = NavX()
self.hang_winch_motor = ctre.WPI_TalonFX(30)
self.hang_kracken_hook_latch = wpilib.DoubleSolenoid(4, 5)
self.indexer_motors = [
ctre.WPI_TalonSRX(18),
ctre.WPI_TalonSRX(11),
ctre.WPI_TalonSRX(12),
ctre.WPI_TalonSRX(13),
ctre.WPI_TalonSRX(14),
]
self.piston_switch = wpilib.DigitalInput(
4) # checks if injector retracted
self.intake_arm_piston = wpilib.Solenoid(1)
self.intake_left_motor = rev.CANSparkMax(8, rev.MotorType.kBrushless)
self.intake_right_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless)
self.led_screen_led = wpilib.AddressableLED(0)
self.loading_piston = wpilib.DoubleSolenoid(6, 7)
self.shooter_centre_motor = ctre.WPI_TalonFX(2)
self.shooter_outer_motor = ctre.WPI_TalonFX(3)
self.range_counter = wpilib.Counter(6)
self.colour_sensor = rev.color.ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.spinner_motor = wpilib.Spark(2)
self.spinner_solenoid = wpilib.DoubleSolenoid(2, 3)
self.turret_centre_index = wpilib.DigitalInput(0)
self.turret_right_index = wpilib.DigitalInput(1)
self.turret_left_index = wpilib.DigitalInput(2)
self.turret_motor = ctre.WPI_TalonSRX(10)
# operator interface
self.driver_joystick = wpilib.Joystick(0)
self.MEMORY_CONSTANT = int(0.1 / self.control_loop_wait_time)
def createObjects(self):
"""Robot initialization function"""
self.logger.info("pyinfiniterecharge %s", GIT_COMMIT)
self.chassis_left_front = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.chassis_left_rear = rev.CANSparkMax(6, rev.MotorType.kBrushless)
self.chassis_right_front = rev.CANSparkMax(7, rev.MotorType.kBrushless)
self.chassis_right_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.imu = navx.AHRS.create_spi(update_rate_hz=50)
self.hang_winch_motor = ctre.WPI_TalonFX(30)
self.hang_kracken_hook_latch = wpilib.Solenoid(0)
self.intake_main_motor = ctre.WPI_TalonSRX(18)
self.indexer_motors = [
ctre.WPI_TalonSRX(11),
ctre.WPI_TalonSRX(12),
ctre.WPI_TalonSRX(13),
]
self.injector_motor = ctre.WPI_TalonSRX(14)
self.piston_switch = wpilib.DigitalInput(
4) # checks if injector retracted
self.intake_arm_piston = wpilib.Solenoid(1)
self.intake_left_motor = rev.CANSparkMax(8, rev.MotorType.kBrushless)
self.intake_right_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless)
self.led_screen_led = wpilib.AddressableLED(0)
self.loading_piston = wpilib.DoubleSolenoid(2, 3)
self.shooter_centre_motor = ctre.WPI_TalonFX(2)
self.shooter_outer_motor = ctre.WPI_TalonFX(3)
self.range_counter = wpilib.Counter(6)
self.turret_centre_index = wpilib.DigitalInput(0)
self.turret_right_index = wpilib.DigitalInput(1)
self.turret_left_index = wpilib.DigitalInput(2)
self.turret_motor = ctre.WPI_TalonSRX(10)
# operator interface
self.driver_joystick = wpilib.Joystick(0)
self.codriver_gamepad = wpilib.XboxController(1)
self.MEMORY_CONSTANT = int(0.1 / self.control_loop_wait_time)
# how long before data times out
self.has_zeroed = False
def __init__(self):
Subsystem.__init__(self, "SampSubsystem")
self.spinSpeed = 0
self.motor = wpilib.NidecBrushless(0, 0)
#self.motor = ctre.WPI_TalonSRX(0)
self.motor.setExpiration(2.5)
self.motor.setSafetyEnabled(False)
#Show Motor status in NT
self.addChild("Motor", self.motor)
self.endSwitch = True
self.limitSwitch = wpilib.DigitalInput(1)
self.limitSwitchLow = wpilib.DigitalInput(2)
#m_simDevice = hal.SimDevice("LimitSwitch", 1)
#RobotBase.isReal()
#if m_simDevice :
# self.limitSwitch.setSimDevice(1)
self.counter = wpilib.Counter(self.limitSwitch)
self.onLED = wpilib.DigitalOutput(3)
def __init__(self, dio_number=0):
self.range_finder_counter = wpilib.Counter(dio_number)
self.range_finder_counter.setSemiPeriodMode(highSemiPeriod=True)
self._smoothed_d = 0.0
def __init__(self, dio_number=0):
self.range_finder_counter = wpilib.Counter(dio_number, mode=wpilib.Counter.Mode.kPulseLength)
self.range_finder_counter.setSemiPeriodMode(highSemiPeriod=True)
self.range_finder_counter.setSamplesToAverage(10)
self._smoothed_d = 0.0
self.sd = NetworkTable.getTable('SmartDashboard')
def create_counter(*args):
return wpilib.Counter(args)
def __init__(self, dio_number=0):
self.range_finder_counter = wpilib.Counter(dio_number)
self.range_finder_counter.setSemiPeriodMode(highSemiPeriod=True)
self._smoothed_d = 0.0
self.sd = NetworkTable.getTable('SmartDashboard')