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()
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)
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)
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()
class TestRobot(magicbot.MagicRobot): shooter_speed = tunable(0, writeDefault=False) time = tunable(0) voltage = tunable(0) rotation = tunable(0) def createObjects(self): self.launcher_motor = CANSparkMax(7, MotorType.kBrushed) self.launcher_motor.restoreFactoryDefaults() self.launcher_motor.setOpenLoopRampRate(0) self.intake = WPI_VictorSPX(1) self.solenoid = wpilib.Solenoid(0) self.encoder = self.launcher_motor.getEncoder() # self.shooter_running = False self.logger.addFilter(PeriodicFilter(1, bypass_level=logging.WARN)) # 2000rpm is a good setting # set up joystick buttons self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # self.left_button = Toggle(self.joystick_alt, 10) # self.middle_button = Toggle(self.joystick_alt, 11) # self.right_button = Toggle(self.joystick_alt, 12) self.one = JoystickButton(self.joystick_alt, 7) self.two = JoystickButton(self.joystick_alt, 8) self.three = JoystickButton(self.joystick_alt, 9) self.four = JoystickButton(self.joystick_alt, 10) self.five = JoystickButton(self.joystick_alt, 11) self.six = JoystickButton(self.joystick_alt, 12) self.intake_in = JoystickButton(self.joystick_alt, 3) self.intake_out = JoystickButton(self.joystick_alt, 4) self.btn_solenoid = JoystickButton(self.joystick_alt, 1) # Set up Speed Controller Groups self.left_motors = wpilib.SpeedControllerGroup(CANSparkMax(1, MotorType.kBrushless), CANSparkMax(2, MotorType.kBrushless), CANSparkMax(3, MotorType.kBrushless)) self.right_motors = wpilib.SpeedControllerGroup(CANSparkMax(4, MotorType.kBrushless), CANSparkMax(5, MotorType.kBrushless), CANSparkMax(6, MotorType.kBrushless)) # Drivetrain self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors) # self.compressor = wpilib.Compressor() def disabledInit(self): self.launcher_motor.set(0) self.intake.set(0) self.solenoid.set(False) def teleopInit(self): self.train.setDeadband(0.1) # self.compressor.start() def teleopPeriodic(self): self.logger.info(self.encoder.getVelocity()) self.shooter_speed = self.encoder.getVelocity() # if self.left_button.get(): # self.launcher_motor.set(-0.2) # elif self.middle_button.get(): # self.launcher_motor.set(-0.5) # elif self.right_button.get(): # self.launcher_motor.set(-0.7) # else: # self.launcher_motor.set(0) if self.intake_in.get(): self.intake.set(-1) elif self.intake_out.get(): self.intake.set(1) else: self.intake.set(0) # BUTTON SETTINGS if self.one.get(): self.launcher_motor.set(0) elif self.two.get(): self.launcher_motor.set(-0.2) elif self.three.get(): self.launcher_motor.set(-0.4) elif self.four.get(): self.launcher_motor.set(-0.6) elif self.five.get(): self.launcher_motor.set(-0.8) elif self.six.get(): self.launcher_motor.set(-1) else: # self.launcher_motor.set(-self.joystick_alt.getAxis(3)) self.launcher_motor.set(0) # SLIDER SETTINGS # self.launcher_motor.set(-self.joystick_alt.getAxis(3)) self.solenoid.set(self.btn_solenoid.get()) self.train.arcadeDrive(-self.joystick_left.getY(), self.joystick_right.getX())
class DeepSpaceRobot(magicbot.MagicRobot): # state machines climb3: Climb3 # components cargo: Cargo climber: Climber drivetrain: Drivetrain elevator: Elevator hatch: Hatch drive_joystick: wpilib.Joystick 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 teleopInit(self): self.elevator.recalculate_ratio() self.elevator.cargo_mode() def teleopPeriodic(self): # Standard Mecanum Driving forward = -self.drive_joystick.getY() strafe = self.drive_joystick.getX() turn = self.drive_joystick.getRawAxis(3) slow = self.slow_button.get() gyro_manually_set = False # Auto Rotation if self.top_tower_front_button.get(): self.drivetrain.rotate_to_angle( self.drivetrain.find_nearest_angle(-30)) gyro_manually_set = True elif self.bottom_tower_front_button.get(): self.drivetrain.rotate_to_angle( self.drivetrain.find_nearest_angle(30)) gyro_manually_set = True elif self.bottom_tower_back_button.get(): self.drivetrain.rotate_to_angle( self.drivetrain.find_nearest_angle(150)) gyro_manually_set = True elif self.top_tower_back_button.get(): self.drivetrain.rotate_to_angle( self.drivetrain.find_nearest_angle(-150)) gyro_manually_set = True elif self.perp_button.get(): self.drivetrain.rotate_to_angle(self.drivetrain.nearest_90) gyro_manually_set = True HEIGHTS = [0, 0, 0] if self.hatch_panel_button.get(): self.hatch.lower() self.cargo.lift() self.elevator.hatch_mode() elif self.cargo_ball_button.get(): self.cargo.lower() self.hatch.lift() self.elevator.cargo_mode() elif self.defense_mode_button.get(): self.cargo.lift() self.hatch.lift() self.elevator.defense_mode() if self.elevator.height_for_hatch: HEIGHTS = ROCKET_HATCH_HEIGHTS elif self.elevator.height_for_cargo: HEIGHTS = ROCKET_CARGO_HEIGHTS if self.tower_l1_button.get(): self.elevator.move_to_setpoint(HEIGHTS[0]) elif self.tower_l2_button.get(): self.elevator.move_to_setpoint(HEIGHTS[1]) elif self.tower_l3_button.get(): self.elevator.move_to_setpoint(HEIGHTS[2]) elif self.elevator_load_button.get(): self.elevator.move_to_setpoint(ROCKET_HATCH_HEIGHTS[0] if self. elevator.height_for_hatch else 0) if self.climber_front_lift_button.get(): self.climber.front_up() if self.climber_back_lift_button.get(): self.climber.rear_up() if self.climber_front_lower_button.get(): self.climber.front_down() if self.climber_back_lower_button.get(): self.climber.rear_down() if self.climber_drive_button.get(): self.climber.drive_forward() if self.climber_reverse_button.get(): self.climber.drive_backwards() if self.climb_button.get(): self.climb3.climb() if self.climb_cancel_button.get(): self.climb3.stop() if self.elevator.height_for_hatch: if self.intake_button.get(): self.hatch.hold() if self.release_button.get(): self.hatch.release() if self.elevator.height_for_cargo: if self.intake_button.get(): self.cargo.intake() if self.release_button.get(): self.cargo.outtake() self.drivetrain.move( forward, strafe, turn, gyro=True, driver=True, slow=slow, gyro_manually_set=gyro_manually_set, ) def disabledInit(self): # Set everything to the state it should be in for being disabled self.elevator.recalculate_ratio() self.drivetrain.move(0, 0, 0) self.elevator.stop()
class MyRobot(wpilib.TimedRobot): # Called once on robot startup. # Declare and initialize the joystick and all of the subsystems. # More documentation on each of these constructors is present in their file. 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") # Called repeatedly in a loop while the robot is disabled. def disabledPeriodic(self): try: if wpilib.DriverStation.getInstance().getAlliance( ) == wpilib.DriverStation.Alliance.Blue: self.ledController.setState(LEDController.STATE_BLUE_STANDBY) elif wpilib.DriverStation.getInstance().getAlliance( ) == wpilib.DriverStation.Alliance.Red: self.ledController.setState(LEDController.STATE_RED_STANDBY) else: self.ledController.setState(LEDController.STATE_DISABLED) except: self.ledController.setState(LEDController.STATE_DISABLED) self.lift.log() self.arm.log() self.base.log() self.ballintake.log() self.roller.log() if self.testButton.get(): if self.testvar == 17: self.testvar = 0 self.ledController.setState(self.testvar) self.testvar += 1 self.ledController.update() wpilib.SmartDashboard.putNumber( "visionerror", self.visiontable.getNumber("heading_error", 0)) # Called once on teleop start. # Rezero lift encoder and rezero navx heading. def teleopInit(self): self.lift.initSensor() self.base.navx.reset() self.roller.state = "init" self.ledController.update() # Called repeatedly during teleop. # Update all of the subsystems and log new data to SmartDashboard. def teleopPeriodic(self): self.base.update() self.lift.update() self.arm.update() self.ballintake.update() self.roller.update() self.ledController.setState(11) self.ledController.update() self.lift.log() self.arm.log() self.base.log() self.ballintake.log() self.roller.log() self.ledController.log() autonomousPeriodic = teleopPeriodic autonomousInit = teleopInit
class Arm(): # Initialize a new instance of Arm. # @param hs1, hs2 - the two solenoids for the hatch grabber. # @param as1, as2 - the two solenoids for the arm. # @param stick - the joystick instance. 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 # Toggle the state of the hatch by inverting both hatch solenoids. def toggleHatch(self): self.hatchsolenoid1.set(not self.hatchsolenoid1.get()) self.hatchsolenoid2.set(not self.hatchsolenoid2.get()) # Toggle the state of the arm by inverting both arm solenoids. def toggleArm(self): self.armsolenoid1.set(not self.armsolenoid1.get()) self.armsolenoid2.set(not self.armsolenoid2.get()) # Update the arm and the hatch via two simple state machines that will # toggle the hatch and the arm solenoids depending on the state of the # joystick buttons. def update(self): if self.hatchbutton.get() and not self.hatchenabled: self.hatchenabled = True self.toggleHatch() LEDController.getInstance().setState(LEDController.STATE_HATCH) elif not self.hatchbutton.get() and self.hatchenabled: self.hatchenabled = False if self.armbutton.get() and not self.armenabled: self.armenabled = True self.toggleArm() elif not self.armbutton.get() and self.armenabled: self.armenabled = False # log arm subsystem-related information to SmartDashboard def log(self): wpilib.SmartDashboard.putBoolean("hatchsolenoid1_status", self.hatchsolenoid1.get()) wpilib.SmartDashboard.putBoolean("hatchsolenoid2_status", self.hatchsolenoid2.get()) wpilib.SmartDashboard.putBoolean("armsolenoid1_status", self.armsolenoid1.get()) wpilib.SmartDashboard.putBoolean("armsolenoid2_status", self.armsolenoid2.get()) wpilib.SmartDashboard.putBoolean("arm_enabled", self.armenabled) wpilib.SmartDashboard.putBoolean("hatch_enabled", self.hatchenabled) wpilib.SmartDashboard.putBoolean("hatch_button", self.hatchbutton.get()) wpilib.SmartDashboard.putBoolean("arm_button", self.armbutton.get())
class MyRobot(MagicRobot): # # Define components here # drive = drive.Drive arm = arm.Arm # # Define constants here # # Robot attributes WHEEL_DIAMETER = 0.5 # 6 inches ENCODER_COUNTS_PER_REV = 360 # Pathfinder constants MAX_VELOCITY = 5 # ft/s MAX_ACCELERATION = 6 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 autonomous(self): """ Prepare for and start autonomous mode. """ self.drive.reset() self.drive.squared_inputs = False self.drive.rotational_constant = 0.5 # Call autonomous super().autonomous() def teleopPeriodic(self): """ Place code here that does things as a result of operator actions """ self.drive.move( -self.joystick_left.getY(), self.joystick_right.getX(), self.btn_fine_movement.get(), )
class Panthera(magicbot.MagicRobot): drive: drive.Drive winch: winch.Winch arm: arm.Arm recorder: recorder.Recorder time = tunable(0) plates = tunable('') voltage = tunable(0) rotation = tunable(0) unified_control = tunable(False) recording = tunable(False) stabilize = tunable(False) stabilizer_threshold = tunable(30) stabilizer_aggression = tunable(5) def createObjects(self): """ Initialize robot components. """ # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # Buttons self.btn_claw = ButtonDebouncer(self.joystick_left, 1) self.btn_forearm = ButtonDebouncer(self.joystick_right, 1) self.btn_up = JoystickButton(self.joystick_left, 3) self.btn_down = JoystickButton(self.joystick_left, 2) self.btn_climb = JoystickButton(self.joystick_right, 11) # Buttons on alternative joystick self.btn_claw_alt = ButtonDebouncer(self.joystick_alt, 1) self.btn_forearm_alt = ButtonDebouncer(self.joystick_alt, 2) self.btn_climb_alt = JoystickButton(self.joystick_alt, 3) # Buttons for toggling control options and aides self.btn_unified_control = ButtonDebouncer(self.joystick_alt, 8) self.btn_record = ButtonDebouncer(self.joystick_left, 6) self.btn_stabilize = ButtonDebouncer(self.joystick_alt, 12) 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_TalonSRX(15) self.rf_motor = WPI_TalonSRX(20) self.rr_motor = WPI_TalonSRX(25) # Follow front wheels with rear to save CAN packets self.lr_motor.follow(self.lf_motor) self.rr_motor.follow(self.rf_motor) # Drivetrain self.train = wpilib.drive.DifferentialDrive(self.lf_motor, self.rf_motor) # Winch self.winch_motors = wpilib.SpeedControllerGroup(wpilib.Victor(7), wpilib.Victor(8)) # Motion Profiling self.position_controller = motion_profile.PositionController() # Arm self.elevator = wpilib.Victor(5) self.forearm = wpilib.DoubleSolenoid(2, 3) self.claw = wpilib.DoubleSolenoid(0, 1) # NavX (purple board on top of the RoboRIO) self.navx = navx.AHRS.create_spi() self.navx.reset() # Utility self.ds = wpilib.DriverStation.getInstance() self.timer = wpilib.Timer() self.pdp = wpilib.PowerDistributionPanel(0) self.compressor = wpilib.Compressor() # Camera server wpilib.CameraServer.launch('camera/camera.py:main') def robotPeriodic(self): """ Executed periodically regardless of mode. """ self.time = int(self.timer.getMatchTime()) self.voltage = self.pdp.getVoltage() self.rotation = self.navx.getAngle() % 360 def autonomous(self): """ Prepare for and start autonomous mode. """ self.compressor.stop() self.drive.squared_inputs = False self.drive.rotational_constant = 0.5 self.plates = '' wpilib.Timer.delay(1) # Read data on plate colors from FMS. # 3.10: "The FMS provides the ALLIANCE color assigned to each PLATE to the Driver Station software. Immediately following the assignment of PLATE color prior to the start of AUTO." # Will fetch a string of three characters ('L' or 'R') denoting position of the current alliance's on the switches and scale, with the nearest structures first. # More information: http://wpilib.screenstepslive.com/s/currentCS/m/getting_started/l/826278-2018-game-data-details self.plates = self.ds.getGameSpecificMessage() # Call autonomous super().autonomous() def disabledInit(self): """ Executed once right away when robot is disabled. """ # Reset Gyro to 0 self.navx.reset() def disabledPeriodic(self): """ Executed periodically while robot is disabled. Useful for testing. """ pass def teleopInit(self): """ Executed when teleoperated mode begins. """ self.compressor.start() self.drive.squared_inputs = True self.drive.rotational_constant = 0.5 def teleopPeriodic(self): """ Executed periodically while robot is in teleoperated mode. """ # Read from joysticks and move drivetrain accordingly self.drive.move(-self.joystick_left.getY(), self.joystick_right.getX(), self.btn_fine_movement.get()) if self.stabilize: if abs(self.navx.getPitch()) > self.stabilizer_threshold: self.drive.move(self.navx.getPitch() / 180 * self.stabilizer_aggression, 0) if self.btn_stabilize.get(): self.stabilize = not self.stabilize if self.btn_unified_control.get(): self.unified_control = not self.unified_control # Winch if (self.btn_climb.get() and self.unified_control) or self.btn_climb_alt.get(): self.winch.run() # Arm if (self.btn_claw.get() and self.unified_control) or self.btn_claw_alt.get(): self.arm.actuate_claw() if (self.btn_forearm.get() and self.unified_control) or self.btn_forearm_alt.get(): self.arm.actuate_forearm() self.arm.move(-self.joystick_alt.getY()) if self.unified_control: if self.btn_up.get(): self.arm.up() if self.btn_down.get(): self.arm.down() if self.btn_record.get(): if self.recording: self.recording = False self.recorder.stop() else: self.recording = True self.recorder.start(self.voltage) if self.recording: self.recorder.capture((self.joystick_left, self.joystick_right, self.joystick_alt))
class Roller(): POSITION_TOP = 3100 def __init__(self, roller_motor, pivot, stick, initial_state): self.roller_motor = roller_motor self.pivot = pivot self.stick = stick self.rollerenabler = JoystickButton(self.stick, 14) self.pivotupbutton = JoystickButton(self.stick, 5) self.pivotdownbutton = JoystickButton(self.stick, 10) self.pistonoutbutton = JoystickButton(self.stick, 8) self.pistoninbutton = JoystickButton(self.stick, 7) #self.ratchetswitchbutton = JoystickButton(self.stick, 15) #self.ratchetbtnpressed = False #self.pivotstopbutton = JoystickButton(self.stick, 14) #self.pivotinitbutton = JoystickButton(self.stick, 13) self.rollerout = JoystickButton(self.stick, 9) self.rollerin = JoystickButton(self.stick, 6) self.state = initial_state self.pivot.selectProfileSlot(0, 0) self.switch = wpilib.DigitalInput(2) self.count = 0 self.sol2 = wpilib.Solenoid(5, 3) self.sol1 = wpilib.Solenoid(5, 0) self.sol1.set(False) self.sol2.set(True) self.ratchet = wpilib.Solenoid(5, 6) self.ratchetEnabled = False self.ratchet.set(not self.ratchetEnabled) def update(self): if (self.state is not "init") and (not self.rollerenabler.get()): self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, 0) self.roller_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0) self.state = "manual" self.sol1.set(False) self.sol2.set(True) return if self.state == "manual": # if self.pivot.getSelectedSensorPosition() >= self.POSITION_TOP and self.stick.getY() >0: # self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, 0) # else: # self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, self.stick.getY()) #pass if self.pivotupbutton.get(): #self.pivot.setQuadraturePosition(0) self.state = "up" elif self.pivotdownbutton.get(): self.state = "down" if self.pistonoutbutton.get(): self.sol1.set(True) self.sol2.set(False) if self.pistoninbutton.get(): self.sol1.set(False) self.sol2.set(True) #elif self.pivotstopbutton.get(): #self.pivot.setQuadraturePosition(0) #self.state = "manual" #elif self.pivotinitbutton.get(): # self.state = "init" self.state_table[self.state](self) if self.rollerin.get(): self.roller_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, -1) #elif self.rollerout.get(): # self.roller_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, -1) else: self.roller_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0) def log(self): wpilib.SmartDashboard.putString("roller_state", self.state) wpilib.SmartDashboard.putNumber("roller_position", self.pivot.getSelectedSensorPosition()) def state_init(self): self.ratchet.set(not self.ratchetEnabled) if self.rollerenabler.get(): self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, 0) self.pivot.setQuadraturePosition(0) self.state = 'manual' return if self.switch.get(): self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, -.3) else: self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, 0) self.pivot.setQuadraturePosition(0) self.ratchet.set(self.ratchetEnabled) self.state = 'manual' def state_manual(self): pass def state_up(self): self.pivot.set(WPI_TalonSRX.ControlMode.Position, self.POSITION_TOP) #if self.pivot.getSelectedSensorPosition() > self.POSITION_TOP + 500: # self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, 0) # self.state = 'manual' if self.pistonoutbutton.get(): self.sol1.set(True) self.sol2.set(False) if self.pistoninbutton.get(): self.sol1.set(False) self.sol2.set(True) self.ratchet.set(self.ratchetEnabled) def state_down(self): self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, .1) self.ratchet.set(not self.ratchetEnabled) if self.count == 0: self.state = "post_state_down" self.count += 1 def post_state_down(self): self.count = 0 self.ratchet.set(not self.ratchetEnabled) self.pivot.set(WPI_TalonSRX.ControlMode.PercentOutput, -.4) if self.pistonoutbutton.get(): self.sol1.set(True) self.sol2.set(False) if self.pistoninbutton.get(): self.sol1.set(False) self.sol2.set(True) state_table = { "post_state_down": post_state_down, "manual": state_manual, "up": state_up, "down": state_down, "init": state_init }
class Robot(MagicRobot): drivetrain = Drivetrain climber = Climber elevator = Elevator intake = Intake mode = RobotMode.switch rumbling = False 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) def up_mode(self): self.mode += 1 def down_mode(self): self.mode -= 1 def autonomous(self): self.intake.reset_wrist() super().autonomous() def teleopInit(self): self.intake.reset_wrist_up() def teleopPeriodic(self): self.right = -self.right_drive_joystick.getRawAxis(1) self.left = -self.left_drive_joystick.getRawAxis(1) self.right = 0 if abs(self.right) < 0.1 else self.right self.left = 0 if abs(self.left) < 0.1 else self.left self.drivetrain.tank(math.copysign(self.right**2, self.right), math.copysign(self.left**2, self.left)) # outtake if self.operator_joystick.getRawAxis(3) > 0.01: self.intake.set_speed( self.operator_joystick.getRawAxis(3) * 0.8 + 0.2) elif self.operator_joystick.getRawButton(3): self.intake.intake() else: self.intake.hold() elevator_speed = -self.operator_joystick.getY(0) if self.down_button.get(): self.down_mode() elif self.up_button.get(): self.up_mode() joystick_val = -self.operator_joystick.getRawAxis(1) if joystick_val > 0.2: joystick_val -= 0.2 elif joystick_val < -0.2: joystick_val += 0.2 else: joystick_val = 0.0 self.elevator.move_setpoint(joystick_val / 50.0 * 20) if self.right_bumper_button.get(): self.intake.wrist_down() self.intake.intake() self.intake.open_arm() if self.intake.has_cube(): self.rumbling = True else: self.intake.close_arm() if self.left_bumper_button.get(): self.climber.set_speed(-self.operator_joystick.getRawAxis(5)) else: wrist_speed = self.operator_joystick.getRawAxis(5) wrist_speed = 0 if abs(wrist_speed) < 0.2 else wrist_speed self.intake.move_wrist_setpoint(wrist_speed) if self.has_cube_button.get(): self.intake.toggle_has_cube() self.operator_joystick.setRumble( wpilib.Joystick.RumbleType.kRightRumble, 1 if self.rumbling else 0) self.operator_joystick.setRumble( wpilib.Joystick.RumbleType.kLeftRumble, 1 if self.rumbling else 0) self.rumbling = False def disabledPeriodic(self): self.drivetrain._update_odometry() self.elevator.reset_position() def testPeriodic(self): self.wrist_motor.set(self.operator_joystick.getRawAxis(5) * 0.6) self.elevator_winch.set(self.operator_joystick.getRawAxis(1)) self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse) self.left_drive_motor.set(0) self.right_drive_motor.set(0)
class Base(): # Initialize a new instance of Base. # @param fl, rl, fr, rr - the front-left, rear-left, front-right, rear-right # motors of the drive base, respectively. # @param navx - the NavX instance. # @param stick - the Joystick instance. 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 # Ignore all joystick inputs that are below a certain threshold. def deadband(self, val): if abs(val) < .2: val = 0 return val # Update the mecanum drive and FOC status. def update(self): if wpilib.DriverStation.getInstance().getAlliance( ) == wpilib.DriverStation.Alliance.Blue: LEDController.getInstance().setState( LEDController.STATE_BLUE_MOTION) elif wpilib.DriverStation.getInstance().getAlliance( ) == wpilib.DriverStation.Alliance.Red: LEDController.getInstance().setState( LEDController.STATE_RED_MOTION) else: LEDController.getInstance().setState(LEDController.STATE_DISABLED) # You *cannot* use a simple if statement without the helper focenabled # variable to toggle FOC state. if self.foctoggle.get() and not self.foctoggledown: self.focenabled = not self.focenabled self.foctoggledown = True elif not self.foctoggle.get() and self.foctoggledown: self.foctoggledown = False scaleval = (1 - self.stick.getThrottle()) * 0.8 + 0.1 # Based on FOC state, decide whether or not to pass NavX heading. if self.focenabled: self.drive.driveCartesian( self.deadband(self.stick.getX()) * scaleval * 2, -self.deadband(self.stick.getY()) * scaleval, self.stick.getZ() * 0.25 * scaleval, self.navx.getAngle()) else: self.drive.driveCartesian( self.deadband(self.stick.getX()) * scaleval * 2, -self.deadband(self.stick.getY()) * scaleval, self.stick.getZ() * 0.25 * scaleval) # Log as much data as possible to SmartDashboard for debug purposes. def log(self): wpilib.SmartDashboard.putBoolean("foc_enabled", self.focenabled) wpilib.SmartDashboard.putNumber("fl_velocity", self.fl.get()) wpilib.SmartDashboard.putNumber("fr_velocity", self.fr.get()) wpilib.SmartDashboard.putNumber("rl_velocity", self.rl.get()) wpilib.SmartDashboard.putNumber("rr_velocity", self.rr.get()) wpilib.SmartDashboard.putNumber("fl_temperature", self.fl.getMotorTemperature()) wpilib.SmartDashboard.putNumber("fr_temperature", self.fr.getMotorTemperature()) wpilib.SmartDashboard.putNumber("rl_temperature", self.rl.getMotorTemperature()) wpilib.SmartDashboard.putNumber("rr_temperature", self.rr.getMotorTemperature()) wpilib.SmartDashboard.putNumber("navx_heading", self.navx.getAngle()) wpilib.SmartDashboard.putNumber("navx_velocity_x", self.navx.getVelocityX()) wpilib.SmartDashboard.putNumber("navx_velocity_y", self.navx.getVelocityY()) wpilib.SmartDashboard.putNumber("navx_accel_x", self.navx.getWorldLinearAccelX()) wpilib.SmartDashboard.putNumber("navx_accel_y", self.navx.getWorldLinearAccelY()) wpilib.SmartDashboard.putNumber("navx_temp", self.navx.getTempC()) wpilib.SmartDashboard.putNumber("foc_button", self.foctoggle.get()) wpilib.SmartDashboard.putNumber("stick_x", self.stick.getX()) wpilib.SmartDashboard.putNumber("stick_y", self.stick.getY()) wpilib.SmartDashboard.putNumber("stick_z", self.stick.getZ())
class Robot(magicbot.MagicRobot): # Automations # TODO: bad name seek_target: seek_target.SeekTarget # Controllers # recorder: recorder.Recorder # Components follower: trajectory_follower.TrajectoryFollower drive: drive.Drive lift: lift.Lift hatch_manipulator: hatch_manipulator.HatchManipulator cargo_manipulator: cargo_manipulator.CargoManipulator climber: climber.Climber ENCODER_PULSE_PER_REV = 1024 WHEEL_DIAMETER = 0.5 """ manual_lift_control = tunable(True) stabilize = tunable(False) stabilizer_threshold = tunable(30) stabilizer_aggression = tunable(5) """ """ time = tunable(0) voltage = tunable(0) yaw = tunable(0) """ def createObjects(self): """ Initialize robot components. """ # For using teleop in autonomous self.robot = self # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # Buttons self.button_strafe_left = JoystickButton(self.joystick_left, 4) self.button_strafe_right = JoystickButton(self.joystick_left, 5) self.button_strafe_forward = JoystickButton(self.joystick_left, 3) self.button_strafe_backward = JoystickButton(self.joystick_left, 2) self.button_slow_rotation = JoystickButton(self.joystick_right, 4) self.button_lift_actuate = ButtonDebouncer(self.joystick_alt, 2) self.button_manual_lift_control = ButtonDebouncer(self.joystick_alt, 6) self.button_hatch_kick = JoystickButton(self.joystick_alt, 1) self.button_cargo_push = JoystickButton(self.joystick_alt, 5) self.button_cargo_pull = JoystickButton(self.joystick_alt, 3) self.button_cargo_pull_lightly = JoystickButton(self.joystick_alt, 4) self.button_climb_front = JoystickButton(self.joystick_right, 3) self.button_climb_back = JoystickButton(self.joystick_right, 2) self.button_target = JoystickButton(self.joystick_right, 8) # 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_TalonSRX(15) self.rf_motor = WPI_TalonSRX(20) self.rr_motor = WPI_TalonSRX(25) encoder_constant = ((1 / self.ENCODER_PULSE_PER_REV) * self.WHEEL_DIAMETER * math.pi) self.r_encoder = wpilib.Encoder(0, 1) self.r_encoder.setDistancePerPulse(encoder_constant) self.l_encoder = wpilib.Encoder(2, 3) self.l_encoder.setDistancePerPulse(encoder_constant) self.l_encoder.setReverseDirection(True) # Drivetrain self.train = wpilib.drive.MecanumDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor) # Functional motors self.lift_motor = WPI_TalonSRX(40) self.lift_motor.setSensorPhase(True) self.lift_switch = wpilib.DigitalInput(4) self.lift_solenoid = wpilib.DoubleSolenoid(2, 3) self.hatch_solenoid = wpilib.DoubleSolenoid(0, 1) self.left_cargo_intake_motor = WPI_TalonSRX(35) # TODO: electricians soldered one motor in reverse. # self.left_cargo_intake_motor.setInverted(True) self.right_cargo_intake_motor = WPI_TalonSRX(30) """ self.cargo_intake_motors = wpilib.SpeedControllerGroup(self.left_cargo_intake_motor, self.right_cargo_intake_motor) """ self.right_cargo_intake_motor.follow(self.left_cargo_intake_motor) self.front_climb_piston = wpilib.DoubleSolenoid(4, 5) self.back_climb_piston = wpilib.DoubleSolenoid(6, 7) # Tank Drivetrain """ self.tank_train = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor), wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor)) """ # Load trajectories self.generated_trajectories = load_trajectories() # NavX self.navx = navx.AHRS.create_spi() self.navx.reset() # Utility # self.ds = wpilib.DriverStation.getInstance() # self.timer = wpilib.Timer() self.pdp = wpilib.PowerDistributionPanel(0) self.compressor = wpilib.Compressor() # Camera server wpilib.CameraServer.launch('camera/camera.py:main') wpilib.LiveWindow.disableAllTelemetry() def robotPeriodic(self): """ Executed periodically regardless of mode. """ # self.time = int(self.timer.getMatchTime()) # self.voltage = self.pdp.getVoltage() # self.yaw = self.navx.getAngle() % 360 pass def autonomous(self): """ Prepare for and start autonomous mode. """ # Call autonomous super().autonomous() def disabledInit(self): """ Executed once right away when robot is disabled. """ # Reset Gyro to 0 self.navx.reset() def disabledPeriodic(self): """ Executed periodically while robot is disabled. Useful for testing. """ pass def teleopInit(self): """ Executed when teleoperated mode begins. """ self.lift.zero = self.lift_motor.getSelectedSensorPosition() self.lift.current_position = 5000000 self.compressor.start() def teleopPeriodic(self): """ Executed periodically while robot is in teleoperated mode. """ # Read from joysticks and move drivetrain accordingly self.drive.move(x=-self.joystick_left.getY(), y=self.joystick_left.getX(), rot=self.joystick_right.getX(), real=True, slow_rot=self.button_slow_rotation.get()) """ self.drive.strafe(self.button_strafe_left.get(), self.button_strafe_right.get(), self.button_strafe_forward.get(), self.button_strafe_backward.get()) """ """ for button in range(7, 12 + 1): if self.joystick_alt.getRawButton(button): self.lift.target(button) """ # if self.manual_lift_control: self.lift.move(-self.joystick_alt.getY()) """ else: # self.lift.correct(-self.joystick_alt.getY()) # self.lift.approach() pass """ """ if self.button_manual_lift_control: # self.manual_lift_control = not self.manual_lift_control pass """ if self.button_hatch_kick.get(): self.hatch_manipulator.extend() else: self.hatch_manipulator.retract() if self.button_target.get(): self.seek_target.seek() if self.button_lift_actuate.get(): self.lift.actuate() if self.button_cargo_push.get(): self.cargo_manipulator.push() elif self.button_cargo_pull.get(): self.cargo_manipulator.pull() elif self.button_cargo_pull_lightly.get(): self.cargo_manipulator.pull_lightly() if self.button_climb_front.get(): self.climber.extend_front() else: self.climber.retract_front() if self.button_climb_back.get(): self.climber.extend_back() else: self.climber.retract_back() """
class Lift(): # Encoder constants for the lift. # TODO recalibrate. POSITION_MAX = -6.624 * (10**4) POSITION_HATCH_TOP = -3.265 * (10**4) POSITION_HATCH_MIDDLE = -3.265 * (10**4) POSITION_HATCH_BOTTOM = 0.000 POSITION_BALL_TOP = -6.500 * (10**4) POSITION_BALL_MIDDLE = -3.184 * (10**4) POSITION_BALL_BOTTOM = -0.000 * (10**4) POSITION_HUMAN_PLAYER = -3.993 * (10**4) # Initialize a new Lift instance. # @param lift_talon - the motor for the lift. # @param upper_switch - the upper limit switch. # @param lower_switch - the lower limit switch. # @param stick - the Joystick instance. def __init__(self, lift_talon, upper_switch, lower_switch, stick): self.lift_motor = lift_talon self.upper_switch = upper_switch self.lower_switch = lower_switch # I don't think there's a better way to do this. Using an array will # sacrifice too much readability for not much more conciseness. self.manual_up = JoystickButton(stick, 4) self.manual_down = JoystickButton(stick, 3) self.ball_bottom = JoystickButton(stick, 5) self.ball_middle = JoystickButton(stick, 6) self.ball_top = JoystickButton(stick, 7) self.hatch_bottom = JoystickButton(stick, 10) self.hatch_middle = JoystickButton(stick, 9) self.hatch_top = JoystickButton(stick, 8) self.lift_disabler = JoystickButton(stick, 14) self.state = "manual_stop" def getTalon(self): return self.lift_motor # Rezero lift_motor encoder and tell it to use the 1st PID slot def initSensor(self): self.lift_motor.selectProfileSlot(0, 0) self.lift_motor.setQuadraturePosition(0) def state_manual_up(self): self.lift_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, -.8) LEDController.getInstance().setState(LEDController.STATE_LIFT_UP) def state_manual_down(self): self.lift_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, .45) LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN) def state_manual_stop(self): self.lift_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, -0.05) def state_hatch_bottom(self): self.lift_motor.set(WPI_TalonSRX.ControlMode.Position, self.POSITION_HATCH_BOTTOM) if abs(self.lift_motor.getSelectedSensorPosition() - self.POSITION_HATCH_BOTTOM) > 10: return elif self.lift_motor.getSelectedSensorPosition( ) < self.POSITION_HATCH_BOTTOM: LEDController.getInstance().setState(LEDController.STATE_LIFT_UP) elif self.lift_motor.getSelectedSensorPosition( ) > self.POSITION_HATCH_BOTTOM: LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN) def state_hatch_middle(self): self.lift_motor.set(WPI_TalonSRX.ControlMode.Position, self.POSITION_HATCH_MIDDLE) if abs(self.lift_motor.getSelectedSensorPosition() - self.POSITION_HATCH_MIDDLE) > 10: return elif self.lift_motor.getSelectedSensorPosition( ) < self.POSITION_HATCH_MIDDLE: LEDController.getInstance().setState(LEDController.STATE_LIFT_UP) elif self.lift_motor.getSelectedSensorPosition( ) > self.POSITION_HATCH_MIDDLE: LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN) def state_hatch_top(self): self.lift_motor.set(WPI_TalonSRX.ControlMode.Position, self.POSITION_HATCH_TOP) if abs(self.lift_motor.getSelectedSensorPosition() - self.POSITION_HATCH_TOP) > 10: return elif self.lift_motor.getSelectedSensorPosition( ) < self.POSITION_HATCH_TOP: LEDController.getInstance().setState(LEDController.STATE_LIFT_UP) elif self.lift_motor.getSelectedSensorPosition( ) > self.POSITION_HATCH_TOP: LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN) def state_ball_bottom(self): self.lift_motor.set(WPI_TalonSRX.ControlMode.Position, self.POSITION_BALL_BOTTOM) if abs(self.lift_motor.getSelectedSensorPosition() - self.POSITION_BALL_BOTTOM) > 10: return elif self.lift_motor.getSelectedSensorPosition( ) < self.POSITION_BALL_BOTTOM: LEDController.getInstance().setState(LEDController.STATE_LIFT_UP) elif self.lift_motor.getSelectedSensorPosition( ) > self.POSITION_BALL_BOTTOM: LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN) def state_ball_middle(self): self.lift_motor.set(WPI_TalonSRX.ControlMode.Position, self.POSITION_BALL_MIDDLE) if abs(self.lift_motor.getSelectedSensorPosition() - self.POSITION_BALL_MIDDLE) > 10: return elif self.lift_motor.getSelectedSensorPosition( ) < self.POSITION_BALL_MIDDLE: LEDController.getInstance().setState(LEDController.STATE_LIFT_UP) elif self.lift_motor.getSelectedSensorPosition( ) > self.POSITION_BALL_MIDDLE: LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN) def state_ball_top(self): self.lift_motor.set(WPI_TalonSRX.ControlMode.Position, self.POSITION_BALL_TOP) if abs(self.lift_motor.getSelectedSensorPosition() - self.POSITION_BALL_TOP) > 10: return elif self.lift_motor.getSelectedSensorPosition( ) < self.POSITION_BALL_TOP: LEDController.getInstance().setState(LEDController.STATE_LIFT_UP) elif self.lift_motor.getSelectedSensorPosition( ) > self.POSITION_BALL_TOP: LEDController.getInstance().setState(LEDController.STATE_LIFT_DOWN) # Large table with all of the states. This is a dictionary that will let # you look up functions by the name of the state. state_table = { "manual_up": state_manual_up, "manual_down": state_manual_down, "manual_stop": state_manual_stop, "hatch_bottom": state_hatch_bottom, "hatch_middle": state_hatch_middle, "hatch_top": state_hatch_top, "ball_bottom": state_ball_bottom, "ball_middle": state_ball_middle, "ball_top": state_ball_top } # update the lift. def update(self): if self.lift_disabler.get(): return if self.ball_bottom.get(): self.state = "ball_bottom" elif self.ball_middle.get(): self.state = "ball_middle" elif self.ball_top.get(): self.state = "ball_top" elif self.hatch_bottom.get(): self.state = "hatch_bottom" elif self.hatch_middle.get(): self.state = "hatch_middle" elif self.hatch_top.get(): self.state = "hatch_top" # Manual up and down MUST OVERRIDE all of the auto lift states. if self.manual_up.get(): self.state = "manual_up" elif self.manual_down.get(): self.state = "manual_down" elif self.state is "manual_up" or self.state is "manual_down": self.state = "manual_stop" # limit switch checks. If a limit switch is pressed, rezero the # encoder and go into manual_stop state. if not self.upper_switch.get(): self.lift_motor.setQuadraturePosition(int(self.POSITION_MAX)) if self.lift_motor.get() < -0.07 or self.state is "manual_up": self.lift_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0) self.state = "manual_stop" return if not self.lower_switch.get(): self.lift_motor.setQuadraturePosition(0) if self.lift_motor.get() > 0.07 or self.state is "manual_down": self.lift_motor.set(WPI_TalonSRX.ControlMode.PercentOutput, 0) self.state = "manual_stop" return # execute the state. self.state_table[self.state](self) # Log as much data as possible to SmartDashboard. def log(self): wpilib.SmartDashboard.putString("lift_state", self.state) wpilib.SmartDashboard.putNumber( "lift_position", self.lift_motor.getSelectedSensorPosition()) wpilib.SmartDashboard.putNumber("lift_velocity", self.lift_motor.get()) wpilib.SmartDashboard.putBoolean("manual_up_button", self.manual_up.get()) wpilib.SmartDashboard.putBoolean("manual_down_button", self.manual_down.get()) wpilib.SmartDashboard.putBoolean("ball_bottom_button", self.ball_bottom.get()) wpilib.SmartDashboard.putBoolean("ball_middle_button", self.ball_middle.get()) wpilib.SmartDashboard.putBoolean("ball_top_button", self.ball_top.get()) wpilib.SmartDashboard.putBoolean("hatch_bottom_button", self.hatch_bottom.get()) wpilib.SmartDashboard.putBoolean("hatch_middle_button", self.hatch_middle.get()) wpilib.SmartDashboard.putBoolean("hatch_top_button", self.hatch_top.get()) wpilib.SmartDashboard.putBoolean("upper_limit", self.upper_switch.get()) wpilib.SmartDashboard.putBoolean("lower_limit", self.lower_switch.get())