class OI(): __instances = None DEADZONE = 1 / 128 def __init__(self): self.driverController = XboxController(0) self.manipulatorController = XboxController(1) self.aButtonDriver = JoystickButton(self.driverController, 0) self.aButtonDriver.whenPressed(DriveStraightCommand(10)) @staticmethod def getInstance() -> OI: if (OI.__instances == None): OI.__instances = OI() return OI.__instances def __deadzone(self, input, deadzone=DEADZONE) -> Number: absValue = math.fabs(input) # no signum ;-; return 0 if abs < deadzone else ((absValue - deadzone) / (1.0 - deadzone) * math.copysign(1, input)) def getLeftY(self): return self.__deadzone( self.driverController.getY(GenericHID.Hand.kLeft)) def getRightY(self): return self.__deadzone( self.driverController.getY(GenericHID.Hand.kRight))
def createObjects(self): # Joysticks self.joystick = wpilib.Joystick(0) # Drive motor controllers # Dig | 0/1 # 2^1 | Left/Right # 2^0 | Front/Rear #self.lf_motor = wpilib.Victor(0b00) # => 0 #self.lr_motor = wpilib.Victor(0b01) # => 1 #self.rf_motor = wpilib.Victor(0b10) # => 2 #self.rr_motor = wpilib.Victor(0b11) # => 3 # TODO: This is not in any way an ideal numbering system. # The PWM ports should be redone to use the old layout above. self.lf_motor = wpilib.Victor(9) self.lr_motor = wpilib.Victor(8) self.rf_motor = wpilib.Victor(7) self.rr_motor = wpilib.Victor(6) self.drivetrain = wpilib.drive.DifferentialDrive( wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor), wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor)) self.btn_sarah = ButtonDebouncer(self.joystick, 2) self.sarah = False # Intake self.intake_wheel_left = wpilib.Spark(5) self.intake_wheel_right = wpilib.Spark(4) self.intake_wheels = wpilib.SpeedControllerGroup( self.intake_wheel_left, self.intake_wheel_right) self.intake_wheels.setInverted(True) self.btn_pull = JoystickButton(self.joystick, 3) self.btn_push = JoystickButton(self.joystick, 1)
def createObjects(self): # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # Buttons self.btn_intake_in = JoystickButton(self.joystick_alt, 2) self.btn_intake_out = JoystickButton(self.joystick_alt, 1) # Set up Speed Controller Groups self.left_motors = wpilib.SpeedControllerGroup( VictorSP(0), VictorSP(1), ) self.right_motors = wpilib.SpeedControllerGroup( VictorSP(2), VictorSP(3), ) # Drivetrain self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors) # Intake self.intake_motor = VictorSP(4)
def createObjects(self): # Joysticks self.joystick = wpilib.Joystick(0) # Drive motor controllers # Dig | 0/1 # 2^1 | Left/Right # 2^0 | Front/Rear self.lf_motor = wpilib.Victor(0b00) # =>0 self.lr_motor = wpilib.Victor(0b01) # =>1 self.rf_motor = wpilib.Victor(0b10) # =>2 self.rr_motor = wpilib.Victor(0b11) # =>3 self.drivetrain = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor), wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor)) self.btn_sarah = ButtonDebouncer(self.joystick, 2) self.sarah = False # Intake self.intake_wheel_left = wpilib.Spark(5) self.intake_wheel_right = wpilib.Spark(4) self.intake_wheels = wpilib.SpeedControllerGroup(self.intake_wheel_left, self.intake_wheel_right) self.intake_wheels.setInverted(True) self.btn_pull = JoystickButton(self.joystick, 3) self.btn_push = JoystickButton(self.joystick, 1)
def __init__(self): self.driverController = XboxController(0) self.manipulatorController = XboxController(1) self.aButtonDriver = JoystickButton(self.driverController, 0) self.aButtonDriver.whenPressed(DriveStraightCommand(10))
class Bot(magicbot.MagicRobot): drive: drive.Drive intake: intake.Intake def createObjects(self): # Joysticks self.joystick = wpilib.Joystick(0) # Drive motor controllers # Dig | 0/1 # 2^1 | Left/Right # 2^0 | Front/Rear self.lf_motor = wpilib.Victor(0b00) # =>0 self.lr_motor = wpilib.Victor(0b01) # =>1 self.rf_motor = wpilib.Victor(0b10) # =>2 self.rr_motor = wpilib.Victor(0b11) # =>3 self.drivetrain = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor), wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor)) self.btn_sarah = ButtonDebouncer(self.joystick, 2) self.sarah = False # Intake self.intake_wheel_left = wpilib.Spark(5) self.intake_wheel_right = wpilib.Spark(4) self.intake_wheels = wpilib.SpeedControllerGroup(self.intake_wheel_left, self.intake_wheel_right) self.intake_wheels.setInverted(True) self.btn_pull = JoystickButton(self.joystick, 3) self.btn_push = JoystickButton(self.joystick, 1) def autonomous(self): super().autonomous() def disabledPeriodic(self): pass def disabledInit(self): pass def teleopInit(self): pass def teleopPeriodic(self): # Normal joysticks self.drive.move(-self.joystick.getY(),self.joystick.getX()) # Corrections for aviator joystick #self.drive.move(-2*(self.joystick.getY()+.5), # 2*(self.joystick.getX()+.5)+ROT_COR, # sarah=self.sarah) if self.btn_sarah: self.sarah = not self.sarah if self.btn_pull.get(): self.intake.pull() elif self.btn_push.get(): self.intake.push()
def __init__(self, robot): self.robot = robot self.driver_joystick = wpilib.Joystick(0) self.copilot_joystick = wpilib.Joystick(1) self.driver_a_button = JoystickButton(self.driver_joystick, ctrlmap.BUTTON_A) self.driver_b_button = JoystickButton(self.driver_joystick, ctrlmap.BUTTON_B) self.driver_a_button.whenPressed(dcmds.SetDriveSpeedMult(robot, 1)) self.driver_b_button.whenPressed(dcmds.SetDriveSpeedMult(robot, 0.5))
def __init__(self, fl, rl, fr, rr, navx, stick): self.fl = fl self.rl = rl self.fr = fr self.rr = rr self.drive = wpilib.drive.MecanumDrive(fl, rl, fr, rr) self.navx = navx self.stick = stick self.foctoggle = JoystickButton(self.stick, 11) self.focenabled = False self.foctoggledown = False
def getJoystick(): """ Assign commands to button actions, and publish your joysticks so you can read values from them later. """ joystick = Joystick(0) trigger = JoystickButton(joystick, Joystick.ButtonType.kTriggerButton) trigger.whenPressed(Crash()) return joystick
def init(): global joystick, A, B, X, Y, bumper_L, bumper_R, back, start, stick_L, stick_R joystick = Joystick(robotmap.joystick) A = JoystickButton(joystick, 1) B = JoystickButton(joystick, 2) X = JoystickButton(joystick, 3) Y = JoystickButton(joystick, 4) bumper_L = JoystickButton(joystick, 5) bumper_R = JoystickButton(joystick, 6) back = JoystickButton(joystick, 7) start = JoystickButton(joystick, 8) stick_L = JoystickButton(joystick, 9) stick_R = JoystickButton(joystick, 10)
def init(): global logger, joystick, A, B, X, Y, bumper_L, bumper_R, back, start, stick_L, stick_R logger = logging.getLogger("OI") joystick = Joystick(robotmap.joystick) A = JoystickButton(joystick, 1) B = JoystickButton(joystick, 2) X = JoystickButton(joystick, 3) Y = JoystickButton(joystick, 4) bumper_L = JoystickButton(joystick, 5) bumper_R = JoystickButton(joystick, 6) back = JoystickButton(joystick, 7) start = JoystickButton(joystick, 8) stick_L = JoystickButton(joystick, 9) stick_R = JoystickButton(joystick, 10) logger.debug("OI initialized")
def initPortcullis(self): s = self.aimStick self.pcUpButton = JoystickButton(s, OI.k_pcUpButton) self.pcUpButton.whenPressed(PCmds.MoveUp(self.robot)) self.pcDownButton = JoystickButton(s, OI.k_pcDownButton) self.pcDownButton.whenPressed(PCmds.MoveDown(self.robot)) self.pcBarInButton = JoystickButton(s, OI.k_pcBarInButton) self.pcBarInButton.whenPressed(PCmds.BarIn(self.robot)) self.pcBarOutButton = JoystickButton(s, OI.k_pcBarOutButton) self.pcBarOutButton.whenPressed(PCmds.BarOut(self.robot)) self.pcBarStopButton = JoystickButton(s, OI.k_pcBarStopButton) self.pcBarStopButton.whenPressed(PCmds.BarStop(self.robot))
class Robot(MagicRobot): def createObjects(self): self.joystick = wpilib.Joystick(2) self.winch_button = JoystickButton(self.joystick, 6) self.cp_extend_button = JoystickButton(self.joystick, 5) self.cp_solenoid = wpilib.DoubleSolenoid(4, 5) self.cp_motor = CANSparkMax(10, MotorType.kBrushed) self.solenoid = wpilib.Solenoid(0) self.btn_solenoid = JoystickButton(self.joystick, 1) self.intake = WPI_VictorSPX(1) self.cp_motor_button = JoystickButton(self.joystick, 7) self.winch_motor = CANSparkMax(8, MotorType.kBrushless) self.six = JoystickButton(self.joystick, 12) self.shooter_motor = CANSparkMax(7, MotorType.kBrushed) self.shooter_motor.restoreFactoryDefaults() self.shooter_motor.setOpenLoopRampRate(0) self.intake_in = JoystickButton(self.joystick, 3) self.intake_out = JoystickButton(self.joystick, 4) def teleopPeriodic(self): if self.winch_button.get(): self.winch_motor.set(1) else: self.winch_motor.set(0) if self.six.get(): # 75% is good for initiation line self.shooter_motor.set(0.75) else: self.shooter_motor.set(0) self.solenoid.set(self.btn_solenoid.get()) if self.intake_in.get(): self.intake.set(-1) elif self.intake_out.get(): self.intake.set(1) else: self.intake.set(0) if self.cp_motor_button.get(): self.cp_motor.set(0.7) else: self.cp_motor.set(0) if self.cp_extend_button.get( ) and self.cp_solenoid.get() != wpilib.DoubleSolenoid.Value.kReverse: self.cp_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse) if not self.cp_extend_button.get( ) and self.cp_solenoid.get() != wpilib.DoubleSolenoid.Value.kForward: self.cp_solenoid.set(wpilib.DoubleSolenoid.Value.kForward)
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)
class OI: """ Operator interface of the robot. Manages commands to run based on driver input. """ def __init__(self, robot): self.robot = robot self.driver_joystick = wpilib.Joystick(0) self.copilot_joystick = wpilib.Joystick(1) self.driver_a_button = JoystickButton(self.driver_joystick, ctrlmap.BUTTON_A) self.driver_b_button = JoystickButton(self.driver_joystick, ctrlmap.BUTTON_B) self.driver_a_button.whenPressed(dcmds.SetDriveSpeedMult(robot, 1)) self.driver_b_button.whenPressed(dcmds.SetDriveSpeedMult(robot, 0.5))
def createObjects(self): """ Initialize all wpilib motors & sensors """ # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) self.btn_fine_movement = JoystickButton(self.joystick_right, 2) # Drive motor controllers # ID SCHEME: # 10^1: 1 = left, 2 = right # 10^0: 0 = front, 5 = rear self.lf_motor = WPI_TalonSRX(10) self.lr_motor = WPI_VictorSPX(15) self.rf_motor = WPI_TalonSRX(20) self.rr_motor = WPI_VictorSPX(25) self.lf_motor.configSelectedFeedbackSensor( WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute ) self.rf_motor.configSelectedFeedbackSensor( WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute ) # Following masters self.lr_motor.follow(self.lf_motor) self.rr_motor.follow(self.rf_motor) # Drive init self.train = wpilib.drive.DifferentialDrive( self.lf_motor, self.rf_motor ) # Arm self.arm_motor = WPI_TalonSRX(0) self.arm_motor.configSelectedFeedbackSensor( WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute ) self.arm_limit = wpilib.DigitalInput(4) # Gyro self.gyro = navx.AHRS.create_spi() self.gyro.reset() self.control_loop_wait_time = 0.02
def __init__(self, hs1, hs2, as1, as2, stick): self.hatchsolenoid1 = hs1 self.hatchsolenoid2 = hs2 self.armsolenoid1 = as1 self.armsolenoid2 = as2 # the corresponding solenoids for each subsystem must be in opposite # states. self.hatchsolenoid1.set(False) self.hatchsolenoid2.set(True) self.armsolenoid1.set(False) self.armsolenoid2.set(True) # Currently trigger and front button. # TODO separate controls out into separate config class. self.hatchbutton = JoystickButton(stick, 2) self.armbutton = JoystickButton(stick, 1) self.hatchenabled = False self.armenabled = False
class Robot(magicbot.MagicRobot): drive: Drive intake: Intake def createObjects(self): # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # Buttons self.btn_intake_in = JoystickButton(self.joystick_alt, 2) self.btn_intake_out = JoystickButton(self.joystick_alt, 1) # Set up Speed Controller Groups self.left_motors = wpilib.SpeedControllerGroup( VictorSP(0), VictorSP(1), ) self.right_motors = wpilib.SpeedControllerGroup( VictorSP(2), VictorSP(3), ) # Drivetrain self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors) # Intake self.intake_motor = VictorSP(4) def teleopPeriodic(self): self.drive.move(-self.joystick_left.getY(), self.joystick_right.getX()) # Intake if self.btn_intake_out.get(): if self.joystick_right.getX() >= -0.1 and self.joystick_right.getX( ) <= 0.1: self.intake.spin(1) elif self.btn_intake_in.get(): self.intake.spin(-1)
def __init__(self, robot): # driver mappings self.driver_joystick = wpilib.XboxController(0) JoystickButton(self.driver_joystick, wpilib.XboxController.Button.kX).whenPressed( TurnByGyro(robot, -90.0)) JoystickButton(self.driver_joystick, wpilib.XboxController.Button.kY).whenPressed( TurnByGyro(robot, 90.0)) JoystickButton(self.driver_joystick, wpilib.XboxController.Button.kBack).whenPressed( ClimbByGyro(robot)) JoystickButton(self.driver_joystick, wpilib.XboxController.Button.kStart).whenPressed( ToggleCompressor(robot)) # operator mappings self.operator_joystick = wpilib.XboxController(1) JoystickButton(self.operator_joystick, wpilib.XboxController.Button.kA).whenPressed( DeployHatch(robot)) JoystickButton(self.operator_joystick, wpilib.XboxController.Button.kB).whenPressed( DropCargo(robot)) JoystickButton(self.operator_joystick, wpilib.XboxController.Button.kY).whenPressed( ToggleLED(robot)) JoystickButton(self.operator_joystick, wpilib.XboxController.Button.kY).whileHeld( AlignByCamera(robot)) JoystickButton(self.operator_joystick, wpilib.XboxController.Button.kY).whenReleased( ToggleLED(robot)) JoystickButton(self.operator_joystick, wpilib.XboxController.Button.kStart).whenPressed( ToggleCompressor(robot))
class OI: def __init__(self, robot): self.robot = robot self.xbox = wpilib.Joystick(0) self.l_trigger = self.xbox.getRawAxis(2) self.r_trigger = self.xbox.getRawAxis(3) self.l_bumper = JoystickButton(self.xbox, 6) self.r_bumper = JoystickButton(self.xbox, 5) self.a_button = JoystickButton(self.xbox, 1) self.b_button = JoystickButton(self.xbox, 2) self.steer = self.xbox.getRawAxis(0) self.speed = self.l_trigger - self.r_trigger self.a_button.whileHeld(Pop(self.robot)) self.r_bumper.whileHeld(LiftFront(self.robot)) self.l_bumper.whileHeld(LiftRear(self.robot)) #self.a_button.toggleWhenPressed(DriveBot(self.robot)) #self.b_button.toggleWhenPressed(Circles(self.robot)) def getSteer(self): return 0.8 * (self.xbox.getRawAxis(0)**3) def getSpeed(self): l_trigger = self.xbox.getRawAxis(2) r_trigger = self.xbox.getRawAxis(3) return 0.8 * ((l_trigger - r_trigger)**3) def getIntakeSpeed(self): return self.xbox.getRawAxis(5)
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, robot): self.joystick = wpilib.Joystick(0) JoystickButton(self.joystick, 12).whenPressed(LowGoal(robot)) JoystickButton(self.joystick, 10).whenPressed(Collect(robot)) JoystickButton(self.joystick, 11).whenPressed(SetPivotSetpoint(robot, Pivot.SHOOT)) JoystickButton(self.joystick, 9).whenPressed(SetPivotSetpoint(robot, Pivot.SHOOT_NEAR)) DoubleButton(self.joystick, 2, 3).whenActive(Shoot(robot)) # SmartDashboard Buttons SmartDashboard.putData("Drive Forward", DriveForward(robot, 2.25)) SmartDashboard.putData("Drive Backward", DriveForward(robot, -2.25)) SmartDashboard.putData("Start Rollers", SetCollectionSpeed(robot, Collector.FORWARD)) SmartDashboard.putData("Stop Rollers", SetCollectionSpeed(robot, Collector.STOP)) SmartDashboard.putData("Reverse Rollers", SetCollectionSpeed(robot, Collector.REVERSE))
def teleopPeriodic(self): """Called when operation control mode is enabled""" while self.isEnabled(): self.drive.driveCartesian( -self.conditonAxis(self.lstick.getX(), 0.05, 0.85, 0.6, 1.5, -1, 1), self.conditonAxis(self.lstick.getY(), 0.05, 0.85, 0.6, 1.5, -1, 1), -self.conditonAxis(self.rstick.getX(), 0.25, 0.85, 0.6, 0.5, -1, 1), self.gyro.getYaw()) if self.timer.get() > 1: if (JoystickButton(self.lstick, 3).get()): self.timer.reset() self.COMPRESSOR_STATE = not self.COMPRESSOR_STATE if self.COMPRESSOR_STATE: self.compressor.start() else: self.compressor.stop() #pilotstick 1 if JoystickButton(self.lstick, 5).get(): self.BallDoorOpen() if JoystickButton(self.lstick, 4).get(): self.BallDoorClose() if JoystickButton(self.lstick, 6).get(): self.BallGrabStart() if JoystickButton(self.lstick, 7).get(): self.BallGrabStop() #pilotstick 2 if JoystickButton(self.rstick, 3).get(): self.activate(self.GearDoorRaise, self.GearDoorDrop, self.GEAR_DOOR_STATE) if JoystickButton(self.rstick, 5).get(): self.activate(self.GearAdjustExtend, self.GearAdjustRetract, self.GEAR_ADJUST_STATE) if JoystickButton(self.rstick, 4).get(): self.activate(self.GearPusherExtend, self.GearPusherRetract, self.GEAR_PUSHER_STATE)
def teleopPeriodic(self): """Called when operation control mode is enabled""" while self.isEnabled(): ''' self.logger.info("FrontLeft: " + str(self.frontLeftMotor.getMotorOutputVoltage())) self.logger.info("RearLeft: " + str(self.rearLeftMotor.getMotorOutputVoltage())) self.logger.info("FrontRight: " + str(self.frontRightMotor.getMotorOutputVoltage())) self.logger.info("RearRight: " + str(self.rearRightMotor.getMotorOutputVoltage())) ''' self.drive.driveCartesian( -self.conditonAxis(self.lstick.getX(), 0.05, 0.85, 0.6, 1.5, -1, 1), self.conditonAxis(self.lstick.getY(), 0.05, 0.85, 0.6, 1.5, -1, 1), -self.conditonAxis(self.rstick.getX(), 0.25, 0.85, 0.6, 0.5, -1, 1) #self.gyro.getYaw() ) if self.solenoid_timer.hasPeriodPassed(0.5): if JoystickButton(self.lstick, 3).get(): self.COMPRESSOR_STATE = not self.COMPRESSOR_STATE self.activate(self.compressor.start, self.compressor.stop, self.COMPRESSOR_STATE) if JoystickButton(self.lstick, 2).get(): self.TICKLER_STATE = not self.TICKLER_STATE self.activate(self.BallDoorOpen, self.BallDoorClose, self.TICKLER_STATE) if JoystickButton(self.lstick, 1).get(): self.GRABBER_STATE = not self.GRABBER_STATE self.activate(self.BallGrabStart, self.BallGrabStop, self.GRABBER_STATE) #pilotstick 2 if JoystickButton(self.rstick, 3).get(): self.GEAR_DOOR_STATE = not self.GEAR_DOOR_STATE self.activate(self.GearDoorRaise, self.GearDoorDrop, self.GEAR_DOOR_STATE) if JoystickButton(self.rstick, 5).get(): self.GEAR_ADJUST_STATE = not self.GEAR_ADJUST_STATE self.activate(self.GearAdjustExtend, self.GearAdjustRetract, self.GEAR_ADJUST_STATE) if JoystickButton(self.rstick, 4).get(): self.GEAR_PUSHER_STATE = not self.GEAR_PUSHER_STATE self.activate(self.GearPusherExtend, self.GearPusherRetract, self.GEAR_PUSHER_STATE) wpilib.Timer.delay(0.04)
def __init__(self, port): super().__init__(port) self.timer = wpilib.Timer() self.timer.start() self.btnX = JoystickButton(self, robotmap.XBOX_X) self.btnY = JoystickButton(self, robotmap.XBOX_Y) self.btnA = JoystickButton(self, robotmap.XBOX_A) self.btnB = JoystickButton(self, robotmap.XBOX_B) self.btnLB = JoystickButton(self, robotmap.XBOX_LEFT_BUMPER) self.btnRB = JoystickButton(self, robotmap.XBOX_RIGHT_BUMPER) self.btnStart = JoystickButton(self, robotmap.XBOX_START) self.btnBack = JoystickButton(self, robotmap.XBOX_BACK) self.btnDpadUp = XboxDPadButton(self, DPAD_BUTTON.DPAD_UP) self.btnDpadRight = XboxDPadButton(self, DPAD_BUTTON.DPAD_RIGHT) self.btnDpadDown = XboxDPadButton(self, DPAD_BUTTON.DPAD_DOWN) self.btnDpadLeft = XboxDPadButton(self, DPAD_BUTTON.DPAD_LEFT)
def robotInit(self): # Robot initialization function # VictorSPX = Motor Controllers self.frontLeftMotor = ctre.WPI_VictorSPX(0) self.rearLeftMotor = ctre.WPI_VictorSPX(1) self.frontRightMotor = ctre.WPI_VictorSPX(4) self.rearRightMotor = ctre.WPI_VictorSPX(5) self.basketMotor = ctre.WPI_VictorSPX(3) # Digital Inputs (Limit Switch) self.basketLimitSwitch = wpilib.DigitalInput(0) # Motor controller groups for each side of the robot self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) # Differential drive with left and right motor controller groups self.myRobot = DifferentialDrive(self.left, self.right) self.myRobot.setExpiration(0.1) self.direction = -1 # Declare gamepad self.gamepad = wpilib.Joystick(0) # Declare buttons # Controller mapping (1-10): B (East), A (South), Y (North), X (West), Right Bumper, Left Bumper, ?, ?, ?, ? self.toggleHatchButton = JoystickButton(self.gamepad, 1) self.toggleBasketButton = JoystickButton(self.gamepad, 2) self.toggleDirectionButton = JoystickButton(self.gamepad, 3) self.speedUpButton = JoystickButton(self.gamepad, 4) self.raiseBasketButton = JoystickButton(self.gamepad, 5) self.lowerBasketButton = JoystickButton(self.gamepad, 6) # Determine if already acted on self.hatchActed = False self.basketActed = False self.directionActed = False # Solenoids self.hatchSolenoid = wpilib.DoubleSolenoid(0, 1) self.basketSolenoid = wpilib.DoubleSolenoid(2, 3) # Compressor self.compressor = wpilib.Compressor(0) # Camera Server wpilib.CameraServer.launch()
def __init__(self, port: int = robotmap.joystick): super().__init__(port) buttons = { 'a': 1, 'b': 2, 'x': 3, 'y': 4, 'bumper_l': 5, 'bumper_r': 6, 'back': 7, 'start': 8, 'l_3': 9, 'r_3': 10 } for button, number in buttons.items(): self.__dict__[str(button)] = JoystickButton(self, number)
def robotInit(self): Command.getRobot = lambda x=0: self self.driveSubsystem = DriveSubsystem() self.controller = Joystick(0) self.forward = JoystickButton(self.controller, PS4Button["TRIANGLE"]) self.backward = JoystickButton(self.controller, PS4Button["CROSS"]) self.right = JoystickButton(self.controller, PS4Button["CIRCLE"]) self.left = JoystickButton(self.controller, PS4Button["SQUARE"]) self.forward.whenPressed(MoveForward()) self.forward.whenReleased(Stop()) self.backward.whenPressed(MoveBackward()) self.backward.whenReleased(Stop()) self.right.whenPressed(TurnRight()) self.right.whenReleased(Stop()) self.left.whenPressed(TurnLeft()) self.left.whenReleased(Stop())
def __init__(self, robot): self.robot = robot self.xbox = wpilib.Joystick(0) self.l_trigger = self.xbox.getRawAxis(2) self.r_trigger = self.xbox.getRawAxis(3) self.l_bumper = JoystickButton(self.xbox, 6) self.r_bumper = JoystickButton(self.xbox, 5) self.a_button = JoystickButton(self.xbox, 1) self.b_button = JoystickButton(self.xbox, 2) self.steer = self.xbox.getRawAxis(0) self.speed = self.l_trigger - self.r_trigger self.a_button.whileHeld(Pop(self.robot)) self.r_bumper.whileHeld(LiftFront(self.robot)) self.l_bumper.whileHeld(LiftRear(self.robot))
def initLauncher(self): aS = self.aimStick dS = self.driveStick self.kickBallButton = JoystickButton(aS, OI.k_kickBallButton) self.kickBallButton.whenPressed(ILCmds.LaunchBallCommandGroup(self.robot)) # TODO: Java code had two buttons for stop wheels self.mechStopWheelsButton = JoystickButton(aS, OI.k_stopIntakeWheelsButton) self.mechStopWheelsButton.whenPressed(ILCmds.StopWheelsCommand(self.robot)) self.grabBallButton = JoystickButton(dS, OI.k_intakeBallButton) self.grabBallButton.whenPressed(ILCmds.IntakeBallCommandGroup(self.robot)) self.spinIntakeWheelsOutButton = JoystickButton(aS, OI.k_spinIntakeWheelsOutButton) self.spinIntakeWheelsOutButton.whenPressed(ILCmds.SpinIntakeWheelsCommand(self.robot, "out")) self.driveLauncherJumpToIntakeButton = JoystickButton(dS, OI.k_aimIntakeButton) self.driveLauncherJumpToIntakeButton.whenPressed(ILCmds.LauncherGoToIntakeCommand(self.robot)) # TODO: Java code had two buttons for neutral self.driveLauncherJumpToNeutralButton = JoystickButton(aS, OI.k_aimNeutralButton) self.driveLauncherJumpToNeutralButton.whenPressed(ILCmds.LauncherGoToNeutralCommand(self.robot))
def __init__(self, robot): self.driver = Joystick(0) JoystickButton(self.driver, 3).whenPressed(OpenGripper(robot)) JoystickButton(self.driver, 4).whenPressed(CloseGripper(robot)) JoystickButton(self.driver, 1).whenPressed(ToggleGripper(robot))
def __init__(self, robot): """Double joysticks WOOT""" print("Initializing joysticks...") #Initialise the stick and the smart dashboard (in case we need stuff for auton): self.stick = wpilib.Joystick(0) self.setpointStick = wpilib.Joystick(1) self.smart_dashboard = NetworkTable.getTable("SmartDashboard") #Main stick POV. #----------------------------------------------------------------------- drive_north = POVButton(self.stick, 0) drive_northeast = POVButton(self.stick, 45) drive_east = POVButton(self.stick, 90) drive_southeast = POVButton(self.stick, 135) drive_south = POVButton(self.stick, 180) drive_southwest = POVButton(self.stick, 225) drive_west = POVButton(self.stick, 270) drive_northwest = POVButton(self.stick, 315) #Setpoint stick button mapping. #----------------------------------------------------------------------- drive_trigger = JoystickButton(self.stick, 1) drive_thumb = JoystickButton(self.stick, 2) drive_bottom_left = JoystickButton(self.stick, 3) drive_bottom_right = JoystickButton(self.stick, 4) drive_top_left = JoystickButton(self.stick, 5) drive_top_right = JoystickButton(self.stick, 6) #Goes from front to back. outer_base is the outer ring of buttons on #the base, inner_base is the inner ring of buttons on the base. #----------------------------------------------------------------------- drive_outer_base_one = JoystickButton(self.stick, 7) drive_inner_base_one = JoystickButton(self.stick, 8) drive_outer_base_two = JoystickButton(self.stick, 9) drive_inner_base_two = JoystickButton(self.stick, 10) drive_outer_base_three = JoystickButton(self.stick, 11) drive_inner_base_three = JoystickButton(self.stick, 12) #Hat switch POV stuff. #----------------------------------------------------------------------- pov_north = POVButton(self.setpointStick, 0) pov_northeast = POVButton(self.setpointStick, 45) pov_east = POVButton(self.setpointStick, 90) pov_southeast = POVButton(self.setpointStick, 135) pov_south = POVButton(self.setpointStick, 180) pov_southwest = POVButton(self.setpointStick, 225) pov_west = POVButton(self.setpointStick, 270) pov_northwest = POVButton(self.setpointStick, 315) #Setpoint stick button mapping. #----------------------------------------------------------------------- bad_trigger = JoystickButton(self.setpointStick, 1) thumb = JoystickButton(self.setpointStick, 2) bottom_left = JoystickButton(self.setpointStick, 3) bottom_right = JoystickButton(self.setpointStick, 4) top_left = JoystickButton(self.setpointStick, 5) top_right = JoystickButton(self.setpointStick, 6) #Goes from front to back. outer_base is the outer ring of buttons on #the base, inner_base is the inner ring of buttons on the base. #----------------------------------------------------------------------- seven = JoystickButton(self.setpointStick, 7) eight = JoystickButton(self.setpointStick, 8) nine = JoystickButton(self.setpointStick, 9) ten = JoystickButton(self.setpointStick, 10) eleven = JoystickButton(self.setpointStick, 11) twelve = JoystickButton(self.setpointStick, 12) #----------------------------------------------------------------------- #Mapping of buttons. bad_trigger.whileHeld(HatButton(robot, 1)) thumb.whileHeld(TiltButton(robot)) pov_north.whileHeld(Intake(robot, .45, .3)) pov_south.whileHeld(Intake(robot, -.5, -.5)) bottom_left.whileHeld(EarsButton(robot, 1)) bottom_right.whileHeld(EarsButton(robot, .4)) top_left.whileHeld(Intake(robot, -.5, -.5)) drive_thumb.whileHeld(Intake(robot, -.5, -.5)) #Give the message to confirm initialisation print("Joysticks initialized")
def __init__(self, robot): self.joy = wpilib.Joystick(0) # Put Some buttons on the SmartDashboard SmartDashboard.putData("Elevator Bottom", SetElevatorSetpoint(robot, 0)); SmartDashboard.putData("Elevator Platform", SetElevatorSetpoint(robot, 0.2)); SmartDashboard.putData("Elevator Top", SetElevatorSetpoint(robot, 0.3)); SmartDashboard.putData("Wrist Horizontal", SetWristSetpoint(robot, 0)); SmartDashboard.putData("Raise Wrist", SetWristSetpoint(robot, -45)); SmartDashboard.putData("Open Claw", OpenClaw(robot)); SmartDashboard.putData("Close Claw", CloseClaw(robot)); SmartDashboard.putData("Deliver Soda", Autonomous(robot)); # Create some buttons d_up = JoystickButton(self.joy, 5) d_right = JoystickButton(self.joy, 6) d_down = JoystickButton(self.joy, 7) d_left = JoystickButton(self.joy, 8) l2 = JoystickButton(self.joy, 9) r2 = JoystickButton(self.joy, 10) l1 = JoystickButton(self.joy, 11) r1 = JoystickButton(self.joy, 12) # Connect the buttons to commands d_up.whenPressed(SetElevatorSetpoint(robot, 0.2)) d_down.whenPressed(SetElevatorSetpoint(robot, -0.2)) d_right.whenPressed(CloseClaw(robot)) d_left.whenPressed(OpenClaw(robot)) r1.whenPressed(PrepareToPickup(robot)) r2.whenPressed(Pickup(robot)) l1.whenPressed(Place(robot)) l2.whenPressed(Autonomous(robot))
class OI: """ OI defines the operator interface for Ares. - two joysticks with many associated buttons - smartdashboard widgets """ # Joysticks k_driveStickPort = 0 k_launcherStickPort = 1 # DriveStick Commands - controls for driver: # move, turn, throttle, intake, drive, intake ball k_intakeBallButton = 3 k_aimIntakeButton = 4 k_driveStopIntakeWheelsButton = 5 k_aimNeutralButton = 6 k_turnThrottleButton = 8 k_driveStrightButton = 11 # experimental # LauncherStick Commands - controls for mechanism operator: # aim, shoot, portcullis k_lightSwitchButton = 2 k_kickBallButton = 3 k_spinIntakeWheelsOutButton = 4 k_stopIntakeWheelsButton = 5 k_pcUpButton = 6 # Pc is portcullis k_pcDownButton = 7 k_pcBarStopButton = 9 k_pcBarInButton = 10 k_pcBarOutButton = 11 # Auto positions are numbered (1-5), position 1 is special k_LowBarPosition = 1 # Auto barrier k_LowBarBarrier = 0 k_MoatBarrier = 1 k_RoughTerrainBarrier = 2 k_RockWallBarrier = 3 k_PortcullisBarrier = 4 k_RampartsBarrier = 5 # Auto strategy k_NoMoveStrategy = 0 k_CrossStrategy = 1 k_CrossBlindShotStrategy = 2 k_CrossVisionShotStrategy = 3 def __init__(self, robot): self.robot = robot self.driveStick = wpilib.Joystick(OI.k_driveStickPort) self.aimStick = wpilib.Joystick(OI.k_launcherStickPort) self.initDriveTrain() self.initLauncher() self.initPortcullis() self.initAutonomous() self.initVision() self.initSmartDashboard() def initDriveTrain(self): ds = self.driveStick self.dtTurnThrottleButton = JoystickButton(ds, OI.k_turnThrottleButton) # TODO: convert whileHeld/whenRealsed pair to a single command self.dtTurnThrottleButton.whileHeld(DTCmds.TurnSpeedSlow(self.robot)) self.dtTurnThrottleButton.whenReleased(DTCmds.TurnSpeedFast(self.robot)) def initLauncher(self): aS = self.aimStick dS = self.driveStick self.kickBallButton = JoystickButton(aS, OI.k_kickBallButton) self.kickBallButton.whenPressed(ILCmds.LaunchBallCommandGroup(self.robot)) # TODO: Java code had two buttons for stop wheels self.mechStopWheelsButton = JoystickButton(aS, OI.k_stopIntakeWheelsButton) self.mechStopWheelsButton.whenPressed(ILCmds.StopWheelsCommand(self.robot)) self.grabBallButton = JoystickButton(dS, OI.k_intakeBallButton) self.grabBallButton.whenPressed(ILCmds.IntakeBallCommandGroup(self.robot)) self.spinIntakeWheelsOutButton = JoystickButton(aS, OI.k_spinIntakeWheelsOutButton) self.spinIntakeWheelsOutButton.whenPressed(ILCmds.SpinIntakeWheelsCommand(self.robot, "out")) self.driveLauncherJumpToIntakeButton = JoystickButton(dS, OI.k_aimIntakeButton) self.driveLauncherJumpToIntakeButton.whenPressed(ILCmds.LauncherGoToIntakeCommand(self.robot)) # TODO: Java code had two buttons for neutral self.driveLauncherJumpToNeutralButton = JoystickButton(aS, OI.k_aimNeutralButton) self.driveLauncherJumpToNeutralButton.whenPressed(ILCmds.LauncherGoToNeutralCommand(self.robot)) def initVision(self): s = self.aimStick self.lightSwitchButton = JoystickButton(s, OI.k_lightSwitchButton) self.lightSwitchButton.whenPressed(VCmds.LightSwitchCmd(self.robot)) def initPortcullis(self): s = self.aimStick self.pcUpButton = JoystickButton(s, OI.k_pcUpButton) self.pcUpButton.whenPressed(PCmds.MoveUp(self.robot)) self.pcDownButton = JoystickButton(s, OI.k_pcDownButton) self.pcDownButton.whenPressed(PCmds.MoveDown(self.robot)) self.pcBarInButton = JoystickButton(s, OI.k_pcBarInButton) self.pcBarInButton.whenPressed(PCmds.BarIn(self.robot)) self.pcBarOutButton = JoystickButton(s, OI.k_pcBarOutButton) self.pcBarOutButton.whenPressed(PCmds.BarOut(self.robot)) self.pcBarStopButton = JoystickButton(s, OI.k_pcBarStopButton) self.pcBarStopButton.whenPressed(PCmds.BarStop(self.robot)) def initAutonomous(self): ch = SendableChooser() ch.addDefault("1: Low Bar", self.k_LowBarPosition) ch.addObject("2", 2) ch.addObject("3", 3) ch.addObject("4", 4) ch.addObject("5", 5) self.startingFieldPosition = ch ch = SendableChooser() ch.addDefault("Low Bar", self.k_LowBarBarrier) ch.addObject("Moat", self.k_MoatBarrier) ch.addObject("Rough Terrain", self.k_RoughTerrainBarrier) ch.addObject("Rock Wall", self.k_RockWallBarrier) ch.addObject("Portcullis", self.k_PortcullisBarrier) ch.addObject("Ramparts", self.k_RampartsBarrier) self.barrierType = ch ch = SendableChooser() ch.addDefault("None", self.k_NoMoveStrategy) ch.addObject("Cross Only", self.k_CrossStrategy) ch.addObject("Cross, Blind Shot", self.k_CrossBlindShotStrategy) ch.addObject("Cross, Vision Shot",self.k_CrossVisionShotStrategy) self.strategy = ch def initSmartDashboard(self): SmartDashboard.putData("AutoFieldPosition", self.startingFieldPosition) SmartDashboard.putData("AutoBarrierType", self.barrierType) SmartDashboard.putData("AutoStrategy", self.strategy)
def initVision(self): s = self.aimStick self.lightSwitchButton = JoystickButton(s, OI.k_lightSwitchButton) self.lightSwitchButton.whenPressed(VCmds.LightSwitchCmd(self.robot))
def __init__(self, robot): """Warning: Metric tonnes of code here. May need to be tidied up a wee bit.""" self.stick_left = wpilib.Joystick(0) self.pad = wpilib.Joystick(1) self.smart_dashboard = NetworkTable.getTable("SmartDashboard") #Buttons? Aw, man, I love buttons! *bleep bloop* #----------------------------------------------- # Create some buttons on the left stick (which is really not, but I don't wanna disturb the preexisting code). left_trigger = JoystickButton(self.stick_left, 1) left_thumb = JoystickButton(self.stick_left, 2) left_three = JoystickButton(self.stick_left, 3) left_four = JoystickButton(self.stick_left, 4) left_five = JoystickButton(self.stick_left, 5) left_six = JoystickButton(self.stick_left, 6) left_seven = JoystickButton(self.stick_left, 7) left_eight = JoystickButton(self.stick_left, 8) left_nine = JoystickButton(self.stick_left, 9) left_ten = JoystickButton(self.stick_left, 10) left_eleven = JoystickButton(self.stick_left, 11) left_twelve = JoystickButton(self.stick_left, 12) #Create some POV stuff on the left stick, based on angles and the hat switch left_north = POVButton(self.stick_left, 0) left_northeast = POVButton(self.stick_left, 45) left_east = POVButton(self.stick_left, 90) left_southeast = POVButton(self.stick_left, 135) left_south = POVButton(self.stick_left, 180) left_southwest = POVButton(self.stick_left, 225) left_west = POVButton(self.stick_left, 270) left_northwest = POVButton(self.stick_left, 315) #Keypad Buttons g0 = JoystickButton(self.pad, 1) g1 = KeyButton(self.smart_dashboard, 10) g2 = KeyButton(self.smart_dashboard, 11) g3 = KeyButton(self.smart_dashboard, 12) g4 = KeyButton(self.smart_dashboard, 13) g5 = KeyButton(self.smart_dashboard, 14) g6 = KeyButton(self.smart_dashboard, 15) g7 = KeyButton(self.smart_dashboard, 16) g8 = KeyButton(self.smart_dashboard, 17) g9 = KeyButton(self.smart_dashboard, 18) g10 = KeyButton(self.smart_dashboard, 19) g11 = KeyButton(self.smart_dashboard, 20) g12 = KeyButton(self.smart_dashboard, 21) g13 = KeyButton(self.smart_dashboard, 22) g14 = KeyButton(self.smart_dashboard, 23) g15 = KeyButton(self.smart_dashboard, 24) g16 = KeyButton(self.smart_dashboard, 25) g17 = KeyButton(self.smart_dashboard, 26) g18 = KeyButton(self.smart_dashboard, 27) g19 = KeyButton(self.smart_dashboard, 28) g20 = KeyButton(self.smart_dashboard, 29) g21 = KeyButton(self.smart_dashboard, 30) g22 = KeyButton(self.smart_dashboard, 31) topshift = KeyButton(self.smart_dashboard, 32) bottomshift = KeyButton(self.smart_dashboard, 33) # Connect buttons & commands #--------------------------- #Bump commands left_south.whenPressed(DriveStraight(robot, 0, .25, timeout = .25)) left_north.whenPressed(DriveStraight(robot, 0, -.25, timeout = .25)) left_east.whenPressed(DriveStraight(robot, .25, 0, timeout = .35)) left_west.whenPressed(DriveStraight(robot, -.25, 0, timeout = .35)) #Mast control left_thumb.whileHeld(Shaker(robot)) left_five.whileHeld(MastButton(robot, .38)) left_six.whileHeld(MastButton(robot, -.38)) #SuperStrafe left_seven.whenPressed(SuperStrafe64(robot, SuperStrafe64.kForward)) left_eight.whenPressed(SuperStrafe64(robot, SuperStrafe64.kBack)) left_nine.whenPressed(SuperStrafe64(robot, SuperStrafe64.kLeft)) left_ten.whenPressed(SuperStrafe64(robot, SuperStrafe64.kRight)) #Lift presets g1.whileHeld(LiftGoToLevelShift(robot, 1, topshift, bottomshift)) g2.whileHeld(LiftGoToLevelShift(robot, 2, topshift, bottomshift)) g3.whileHeld(LiftGoToLevelShift(robot, 4, topshift, bottomshift)) g4.whileHeld(LiftGoToLevelShift(robot, 0, topshift, bottomshift)) g5.whenPressed(OpenClaw(robot)) g6.whenPressed(GrabTote(robot)) g7.whenPressed(GrabCan(robot)) #Never, under ANY circumstances, run these during a match. #g9.whenPressed(RecordMacro(robot, "macro.csv")) #g10.whenPressed(PlayMacro(robot, "macro.csv")) #RecordMacro(robot, "macro.csv") #PlayMacro(robot, "macro.csv") g11.whenPressed(RecordMacro(robot, "macro_1.csv")) g12.whenPressed(PlayMacro(robot, "macro_1.csv")) #RecordMacro(robot, "macro_1.csv") #PlayMacro(robot, "macro_1.csv") #g13.whenPressed(RecordMacro(robot, "autonomous.csv")) #g14.whenPressed(PlayMacro(robot, "autonomous.csv")) #RecordMacro(robot, "autonomous.csv") #PlayMacro(robot, "autonomous.csv") #g15.whenPressed(RecordMacro(robot, "macro_3.csv")) #g16.whenPressed(PlayMacro(robot, "macro_3.csv")) #Winchy thing g17.whileHeld(WinchButton(robot, .5)) g18.whileHeld(WinchButton(robot, -.5)) g19.whenPressed(GrabSpecialTote(robot)) #Mast g20.whileHeld(MastBack(robot)) g21.whileHeld(MastLevel(robot)) g22.whileHeld(MastForward(robot))
class MyRobot(wpilib.IterativeRobot): def robotInit(self): Command.getRobot = lambda x=0: self self.driveSubsystem = DriveSubsystem() self.controller = Joystick(0) self.forward = JoystickButton(self.controller, PS4Button["TRIANGLE"]) self.backward = JoystickButton(self.controller, PS4Button["CROSS"]) self.right = JoystickButton(self.controller, PS4Button["CIRCLE"]) self.left = JoystickButton(self.controller, PS4Button["SQUARE"]) self.forward.whenPressed(MoveForward()) self.forward.whenReleased(Stop()) self.backward.whenPressed(MoveBackward()) self.backward.whenReleased(Stop()) self.right.whenPressed(TurnRight()) self.right.whenReleased(Stop()) self.left.whenPressed(TurnLeft()) self.left.whenReleased(Stop()) def autonomousInit(self): '''Called only at the beginning of autonomous mode''' pass def autonomousPeriodic(self): '''Called every 20ms in autonomous mode''' pass def disabledInit(self): '''Called only at the beginning of disabled mode''' pass def disabledPeriodic(self): '''Called every 20ms in disabled mode''' pass def teleopInit(self): '''Called only at the beginning of teleoperated mode''' pass def teleopPeriodic(self): '''Called every 20ms in teleoperated mode''' Scheduler.getInstance().run()
class Robot(magicbot.MagicRobot): # Order of components determines order that execute methods are run # State Machines panel_spinner: PanelSpinner auto_launcher: AutoShoot # Other components align: Align odometry: Odometry follower: Follower intake: Intake control_panel: ControlPanel launcher: Launcher drive: Drive flipped = tunable(False) ntSolenoid_state = ntproperty("/robot/ntSolenoid_state", 0) ds_velocity_setpoint = ntproperty('/components/launcher/DS_VEL_SETPOINT', 2100) limelight_stream_mode = ntproperty('/limelight/stream', 2) WHEEL_DIAMETER = 0.1524 # Units: Meters ENCODER_PULSES_PER_REV = 2048 # Ignore quadrature decoding (4x, 8192) LAUNCHER_GEARING = 1 # No gearing since encoder is on shaft GEARING = 7.56 # 7.56:1 gear ratio def createObjects(self): # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # Buttons self.btn_launcher_solenoid = JoystickButton(self.joystick_alt, 1) self.btn_intake_ = JoystickButton(self.joystick_alt, 5) self.btn_align = JoystickButton(self.joystick_left, 1) self.btn_intake_in = JoystickButton(self.joystick_alt, 3) self.btn_intake_out = JoystickButton(self.joystick_alt, 4) self.btn_cp_extend = Toggle(self.joystick_left, 4) self.btn_winch = JoystickButton(self.joystick_alt, 8) self.btn_cp_motor = JoystickButton(self.joystick_left, 3) self.btn_launcher_motor = JoystickButton(self.joystick_alt, 12) self.btn_launcher_idle = Toggle(self.joystick_alt, 10) self.btn_launcher_motor_close = JoystickButton(self.joystick_alt, 11) # Initiation Line self.btn_launcher_motor_dynamic = JoystickButton(self.joystick_alt, 9) self.btn_slow_movement = JoystickButton(self.joystick_right, 1) self.btn_intake_solenoid = Toggle(self.joystick_alt, 2) self.btn_scissor_extend = Toggle(self.joystick_alt, 7) self.btn_color_sensor = JoystickButton(self.joystick_left, 5) self.btn_cp_stop = JoystickButton(self.joystick_left, 2) self.btn_invert_y_axis = JoystickButton(self.joystick_left, 6) self.btn_rotation_sensitivity = JoystickButton(self.joystick_right, 1) self.btn_intake_bottom_out = JoystickButton(self.joystick_alt, 6) # Set up motors for encoders self.master_left = CANSparkMax(1, MotorType.kBrushless) self.master_right = CANSparkMax(4, MotorType.kBrushless) self.encoder_constant = (1 / self.GEARING) * self.WHEEL_DIAMETER * math.pi self.left_encoder = self.master_left.getEncoder() self.left_encoder.setPositionConversionFactor(self.encoder_constant) self.left_encoder.setVelocityConversionFactor(self.encoder_constant / 60) self.right_encoder = self.master_right.getEncoder() self.right_encoder.setPositionConversionFactor(self.encoder_constant) self.right_encoder.setVelocityConversionFactor(self.encoder_constant / 60) self.left_encoder.setPosition(0) self.right_encoder.setPosition(0) # Set up Speed Controller Groups self.left_motors = wpilib.SpeedControllerGroup( self.master_left, CANSparkMax(3, MotorType.kBrushless)) self.right_motors = wpilib.SpeedControllerGroup( self.master_right, CANSparkMax(6, MotorType.kBrushless)) # Drivetrain self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors) # Winch self.winch_motors = wpilib.SpeedControllerGroup( CANSparkMax(8, MotorType.kBrushless), CANSparkMax(9, MotorType.kBrushless)) self.scissor_solenoid = wpilib.DoubleSolenoid(6, 7) self.hook_motor = WPI_VictorSPX(3) # Control Panel Spinner self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard) self.cp_solenoid = wpilib.DoubleSolenoid( 5, 4) # Reversed numbers so kForward is extended self.cp_motor = WPI_VictorSPX(2) self.ultrasonic = Ultrasonic(3, 4) self.ultrasonic.setAutomaticMode(True) self.ultrasonic.setEnabled(True) # Intake self.intake_motor = WPI_VictorSPX(1) self.intake_motor_lower = CANSparkMax(40, MotorType.kBrushed) self.intake_solenoid = wpilib.DoubleSolenoid(2, 1) self.intake_switch = wpilib.DigitalInput(2) # Launcher # self.launcher_spark = CANSparkMax(40, MotorType.kBrushed) # self.launcher_spark.setInverted(True) self.launcher_motor = CANSparkMax(10, MotorType.kBrushed) self.launcher_motor.setInverted(True) self.launcher_motor_follower = CANSparkMax(12, MotorType.kBrushed) self.launcher_motor_follower.follow(self.launcher_motor) self.launcher_solenoid = wpilib.Solenoid(0) # Don't use index pin and change to k1X encoding to decrease rate measurement jitter self.launcher_encoder = self.launcher_motor.getEncoder( CANEncoder.EncoderType.kQuadrature, 8192) self.rpm_controller = self.launcher_motor.getPIDController() self.launcher_sensor = wpilib.Ultrasonic(0, 1) self.launcher_sensor.setAutomaticMode(True) self.launcher_sensor.setEnabled(True) self.launcher_encoder.setPosition(0) # NavX (purple board on top of the RoboRIO) self.navx = navx.AHRS.create_spi() self.navx.reset() # Limelight self.limelight = Limelight() # Camera Stream CameraServer.launch('camera/camera.py:main') # Robot motion control self.kinematics = KINEMATICS # Use kinematics from inner trajectory generator code self.drive_feedforward = DRIVE_FEEDFORWARD self.trajectories = load_trajectories() # LED strip self.led_driver = BlinkinLED(0) def autonomous(self): self.right_motors.setInverted(True) super().autonomous() self.limelight.changePipeline(0) def disabledInit(self): self.navx.reset() self.limelight_stream_mode = 0 def disabledPeriodic(self): self.limelight.averagePose() def teleopInit(self): self.right_motors.setInverted(False) self.drive.squared_inputs = True self.drive.speed_constant = 1.05 self.limelight.changePipeline(0) self.drive.rotational_constant = 0.5 self.inverse = 1 def teleopPeriodic(self): # if self.btn_invert_y_axis.get(): # self.flipped = True # self.inverse *= -1 # else: # self.flipped = False # self.logger.info(self.ultrasonic.getRangeInches()) if self.btn_invert_y_axis.get(): self.inverse = 1 else: self.inverse = -1 if self.btn_rotation_sensitivity.get(): self.drive.rotational_constant = 0.1 self.drive.move(self.inverse * self.joystick_left.getY(), self.joystick_right.getX()) # Align (Overrides self.drive.move() because it's placed after) if self.btn_align.get() and self.limelight.targetExists(): self.drive.set_target(self.limelight.getYaw(), relative=True) if self.btn_align.get(): self.limelight.TurnLightOn(True) self.drive.align() else: self.limelight.TurnLightOn(False) self.drive.set_target(None) if self.btn_slow_movement.get(): # 10% of original values self.drive.rotational_constant = 0.54 self.drive.speed_constant = 0.5 # Control Panel Spinner self.control_panel.set_solenoid(self.btn_cp_extend.get()) if self.btn_cp_extend.get(): self.ntSolenoid_state = True else: self.ntSolenoid_state = False if self.btn_scissor_extend.get(): self.scissor_solenoid.set(wpilib.DoubleSolenoid.Value.kForward) else: self.scissor_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse) # Color Sensor if self.btn_color_sensor.get(): self.panel_spinner.spin_to() if self.btn_cp_motor.get(): self.control_panel.spin(0.5) # Launcher if self.btn_launcher_motor.get(): self.launcher.setVelocity(4630) elif self.btn_launcher_motor_close.get(): self.launcher.setVelocity(3900) elif self.btn_launcher_motor_dynamic.get(): # self.limelight.TurnLightOn(True) self.launcher.setVelocity(self.ds_velocity_setpoint) # if self.limelight.targetExists(): # self.launcher.setVelocityFromDistance(self.limelight.pitch_angle, 4670) elif self.btn_launcher_idle.get(): self.launcher.setPercentOutput(1) if self.btn_launcher_solenoid.get(): self.auto_launcher.fire_when_ready() if self.btn_cp_stop.get(): self.panel_spinner.done() # Intake if self.btn_intake_out.get(): self.intake.spin_inside(1) elif self.btn_intake_in.get(): self.intake.spin(-1, 0.4) elif self.btn_intake_bottom_out.get(): self.intake.spin_lower(-0.4) if self.btn_intake_solenoid.get(): self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse) else: self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kForward) # Winch if self.btn_winch.get(): self.winch_motors.set(0.65) else: self.winch_motors.set( 0 ) # Must use set(0) when not pressed because there is no component # Hook if self.joystick_alt.getPOV(0) == 90: self.hook_motor.set(0.5) elif self.joystick_alt.getPOV(0) == 270: self.hook_motor.set(-0.5) else: self.hook_motor.set(0) if self.joystick_alt.getPOV(0) == 0: self.launcher.fire()
def createObjects(self): # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # Buttons self.btn_launcher_solenoid = JoystickButton(self.joystick_alt, 1) self.btn_intake_ = JoystickButton(self.joystick_alt, 5) self.btn_align = JoystickButton(self.joystick_left, 1) self.btn_intake_in = JoystickButton(self.joystick_alt, 3) self.btn_intake_out = JoystickButton(self.joystick_alt, 4) self.btn_cp_extend = Toggle(self.joystick_left, 4) self.btn_winch = JoystickButton(self.joystick_alt, 8) self.btn_cp_motor = JoystickButton(self.joystick_left, 3) self.btn_launcher_motor = JoystickButton(self.joystick_alt, 12) self.btn_launcher_idle = Toggle(self.joystick_alt, 10) self.btn_launcher_motor_close = JoystickButton(self.joystick_alt, 11) # Initiation Line self.btn_launcher_motor_dynamic = JoystickButton(self.joystick_alt, 9) self.btn_slow_movement = JoystickButton(self.joystick_right, 1) self.btn_intake_solenoid = Toggle(self.joystick_alt, 2) self.btn_scissor_extend = Toggle(self.joystick_alt, 7) self.btn_color_sensor = JoystickButton(self.joystick_left, 5) self.btn_cp_stop = JoystickButton(self.joystick_left, 2) self.btn_invert_y_axis = JoystickButton(self.joystick_left, 6) self.btn_rotation_sensitivity = JoystickButton(self.joystick_right, 1) self.btn_intake_bottom_out = JoystickButton(self.joystick_alt, 6) # Set up motors for encoders self.master_left = CANSparkMax(1, MotorType.kBrushless) self.master_right = CANSparkMax(4, MotorType.kBrushless) self.encoder_constant = (1 / self.GEARING) * self.WHEEL_DIAMETER * math.pi self.left_encoder = self.master_left.getEncoder() self.left_encoder.setPositionConversionFactor(self.encoder_constant) self.left_encoder.setVelocityConversionFactor(self.encoder_constant / 60) self.right_encoder = self.master_right.getEncoder() self.right_encoder.setPositionConversionFactor(self.encoder_constant) self.right_encoder.setVelocityConversionFactor(self.encoder_constant / 60) self.left_encoder.setPosition(0) self.right_encoder.setPosition(0) # Set up Speed Controller Groups self.left_motors = wpilib.SpeedControllerGroup( self.master_left, CANSparkMax(3, MotorType.kBrushless)) self.right_motors = wpilib.SpeedControllerGroup( self.master_right, CANSparkMax(6, MotorType.kBrushless)) # Drivetrain self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors) # Winch self.winch_motors = wpilib.SpeedControllerGroup( CANSparkMax(8, MotorType.kBrushless), CANSparkMax(9, MotorType.kBrushless)) self.scissor_solenoid = wpilib.DoubleSolenoid(6, 7) self.hook_motor = WPI_VictorSPX(3) # Control Panel Spinner self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard) self.cp_solenoid = wpilib.DoubleSolenoid( 5, 4) # Reversed numbers so kForward is extended self.cp_motor = WPI_VictorSPX(2) self.ultrasonic = Ultrasonic(3, 4) self.ultrasonic.setAutomaticMode(True) self.ultrasonic.setEnabled(True) # Intake self.intake_motor = WPI_VictorSPX(1) self.intake_motor_lower = CANSparkMax(40, MotorType.kBrushed) self.intake_solenoid = wpilib.DoubleSolenoid(2, 1) self.intake_switch = wpilib.DigitalInput(2) # Launcher # self.launcher_spark = CANSparkMax(40, MotorType.kBrushed) # self.launcher_spark.setInverted(True) self.launcher_motor = CANSparkMax(10, MotorType.kBrushed) self.launcher_motor.setInverted(True) self.launcher_motor_follower = CANSparkMax(12, MotorType.kBrushed) self.launcher_motor_follower.follow(self.launcher_motor) self.launcher_solenoid = wpilib.Solenoid(0) # Don't use index pin and change to k1X encoding to decrease rate measurement jitter self.launcher_encoder = self.launcher_motor.getEncoder( CANEncoder.EncoderType.kQuadrature, 8192) self.rpm_controller = self.launcher_motor.getPIDController() self.launcher_sensor = wpilib.Ultrasonic(0, 1) self.launcher_sensor.setAutomaticMode(True) self.launcher_sensor.setEnabled(True) self.launcher_encoder.setPosition(0) # NavX (purple board on top of the RoboRIO) self.navx = navx.AHRS.create_spi() self.navx.reset() # Limelight self.limelight = Limelight() # Camera Stream CameraServer.launch('camera/camera.py:main') # Robot motion control self.kinematics = KINEMATICS # Use kinematics from inner trajectory generator code self.drive_feedforward = DRIVE_FEEDFORWARD self.trajectories = load_trajectories() # LED strip self.led_driver = BlinkinLED(0)
def initDriveTrain(self): ds = self.driveStick self.dtTurnThrottleButton = JoystickButton(ds, OI.k_turnThrottleButton) # TODO: convert whileHeld/whenRealsed pair to a single command self.dtTurnThrottleButton.whileHeld(DTCmds.TurnSpeedSlow(self.robot)) self.dtTurnThrottleButton.whenReleased(DTCmds.TurnSpeedFast(self.robot))