Beispiel #1
0
    def __init__(self):
        super().__init__()
        self.logger = logging.getLogger(self.getName())

        # Configure motors
        self.leftMasterTalon = WPI_TalonSRX(RobotMap.LEFT_MASTER_TALON)
        self.leftSlaveTalon = WPI_TalonSRX(RobotMap.LEFT_SLAVE_TALON)
        self.rightMasterTalon = WPI_TalonSRX(RobotMap.RIGHT_MASTER_TALON)
        self.rightSlaveTalon = WPI_TalonSRX(RobotMap.RIGHT_SLAVE_TALON)

        self.leftSlaveTalon.follow(self.leftMasterTalon)
        self.rightSlaveTalon.follow(self.rightMasterTalon)

        self.leftMasterTalon.configSelectedFeedbackSensor(
            FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.TIMEOUT_MS)
        self.rightMasterTalon.configSelectedFeedbackSensor(
            FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.TIMEOUT_MS)
        self.leftMasterTalon.setInverted(True)
        self.leftSlaveTalon.setInverted(True)

        self.configureDrivePID()

        # Configure shift solenoid
        self.shiftSolenoid = DoubleSolenoid(RobotMap.SHIFT_IN_SOLENOID,
                                            RobotMap.SHIFT_OUT_SOLENOID)
Beispiel #2
0
    def __init__(self, robot):
        super().__init__()
        self.robot = robot

        # Map the CIM motors to the TalonSRX's
        self.rightTalon = WPI_TalonSRX(INTAKE_RIGHT_MOTOR)
        self.leftTalon = WPI_TalonSRX(INTAKE_LEFT_MOTOR)
Beispiel #3
0
    def __init__(self, robot):
        super().__init__()
        self.robot = robot

        # Map the CIM motors to the TalonSRX's
        self.frontLeft = WPI_TalonSRX(DRIVETRAIN_FRONT_LEFT_MOTOR)
        self.leftTalon = WPI_TalonSRX(DRIVETRAIN_REAR_LEFT_MOTOR)
        self.frontRight = WPI_TalonSRX(DRIVETRAIN_FRONT_RIGHT_MOTOR)
        self.rightTalon = WPI_TalonSRX(DRIVETRAIN_REAR_RIGHT_MOTOR)

        # Set the front motors to be the followers of the rear motors
        self.frontLeft.set(WPI_TalonSRX.ControlMode.Follower,
                           DRIVETRAIN_REAR_LEFT_MOTOR)
        self.frontRight.set(WPI_TalonSRX.ControlMode.Follower,
                            DRIVETRAIN_REAR_RIGHT_MOTOR)

        # Add the motors to the speed controller groups and create the differential drivetrain
        self.leftDrive = SpeedControllerGroup(self.frontLeft, self.leftTalon)
        self.rightDrive = SpeedControllerGroup(self.frontRight,
                                               self.rightTalon)
        self.diffDrive = DifferentialDrive(self.leftDrive, self.rightDrive)

        # Setup the default motor controller setup
        self.initControllerSetup()

        # Map the pigeon.  This will be connected to an unused Talon.
        self.talonPigeon = WPI_TalonSRX(DRIVETRAIN_PIGEON)
        self.pigeonIMU = PigeonIMU(self.talonPigeon)
Beispiel #4
0
    def createObjects(self):
        """
        Initialize testbench components.
        """
        self.joystick = wpilib.Joystick(0)

        self.brushless = wpilib.NidecBrushless(9, 9)
        self.spark = wpilib.Spark(4)

        self.lf_victor = wpilib.Victor(0)
        self.lr_victor = wpilib.Victor(1)
        self.rf_victor = wpilib.Victor(2)
        self.rr_victor = wpilib.Victor(3)

        self.lf_talon = WPI_TalonSRX(5)
        self.lr_talon = WPI_TalonSRX(10)
        self.rf_talon = WPI_TalonSRX(15)
        self.rr_talon = WPI_TalonSRX(20)

        self.drive = wpilib.drive.DifferentialDrive(
            wpilib.SpeedControllerGroup(self.lf_victor, self.lr_victor,
                                        self.lf_talon, self.lr_talon),
            wpilib.SpeedControllerGroup(self.rf_victor, self.rr_victor,
                                        self.rf_talon, self.rr_talon))

        wpilib.CameraServer.launch()
Beispiel #5
0
 def createObjects(self):
     self.joystick_left = wpilib.Joystick(0)
     self.joystick_right = wpilib.Joystick(1)
     self.lf_motor = WPI_TalonSRX(10)
     self.lr_motor = WPI_TalonSRX(15)
     self.rf_motor = WPI_TalonSRX(20)
     self.rr_motor = WPI_TalonSRX(25)
     # Drivetrain
     self.train = wpilib.drive.DifferentialDrive(
         wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
         wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
    def __init__(self):
        super().__init__(p=0.015, i=0.0001, d=0.0)
        self._deadband = 0.1
        self._turnOutput = 0.0

        # Configure motors
        motors = [
            WPI_TalonSRX(i)
            for i in [RM.DRIVE_LF, RM.DRIVE_LB, RM.DRIVE_RF, RM.DRIVE_RB]
        ]
        for i, motor in enumerate(motors):
            # motor.configFactoryDefault()
            motor.enableVoltageCompensation(True)
            motor.configOpenLoopRamp(1.4, 10)
            motor.setNeutralMode(NeutralMode.Brake)

            # Invert left side motors?
            # if i <= 1:
            #     motor.setInverted(True)

            # Set up PIDSubsystem parameters
            self.setInputRange(0.0, 360.0)
            self.pidController = self.getPIDController()
            self.pidController.setContinuous(True)
            self.pidController.setAbsoluteTolerance(1.0)
            self.setSetpoint(0.0)
            # Enable this is you use the PID functionality
            # self.pidController.enable()

        self.drive = MecanumDrive(*motors)
        self.drive.setExpiration(1)
        self.drive.setSafetyEnabled(False)
        self.drive.setDeadband(0.1)
Beispiel #7
0
    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
Beispiel #8
0
    def __init__(self, robot):
        super().__init__()
        self.robot = robot

        # Map the CIM motors to the TalonSRX's
        self.talon = WPI_TalonSRX(BOOM_MOTOR)

        # Setup the default motor controller setup
        self.initOpenLoop()
        self.initClosedLoop()
Beispiel #9
0
    def __init__(self):
        super().__init__()
        self.leftIntakeTalon = WPI_TalonSRX(RobotMap.LEFT_INTAKE_TALON)
        self.rightIntakeTalon = WPI_TalonSRX(RobotMap.RIGHT_INTAKE_TALON)
        self.intakeVictor = WPI_VictorSPX(RobotMap.END_EFFECTOR_VICTOR)

        self.intakeSolenoid = DoubleSolenoid(RobotMap.INTAKE_IN_SOLENOID,
                                             RobotMap.INTAKE_OUT_SOLENOID)
        self.placingSolenoid = DoubleSolenoid(RobotMap.PLACE_IN_SOLENOID,
                                              RobotMap.PLACE_OUT_SOLENOID)

        self.leftIntakeTalon.configContinuousCurrentLimit(
            6, Constants.TIMEOUT_MS)
        self.rightIntakeTalon.configContinuousCurrentLimit(
            6, Constants.TIMEOUT_MS)

        self.setPosition(IntakeOutput.Position.UP)
        self.setArmPower(0)

        self.mode = None  # This cannot be set here because the buttons do not yet exist.
    def __init__(self):
        super().__init__("Drivetrain")

        self.leftleader = WPI_TalonSRX(CAN_LEFT_LEADER)
        set_motor(self.leftleader, WPI_TalonSRX.NeutralMode.Brake, False)

        self.leftfollower = WPI_VictorSPX(CAN_LEFT_FOLLOWER)
        set_motor(self.leftfollower, WPI_VictorSPX.NeutralMode.Brake, False)
        self.leftfollower.follow(self.leftleader)

        self.rightleader = WPI_TalonSRX(CAN_RIGHT_LEADER)
        set_motor(self.rightleader, WPI_TalonSRX.NeutralMode.Brake, True)

        self.rightfollower = WPI_VictorSPX(CAN_RIGHT_FOLLOWER)
        set_motor(self.rightfollower, WPI_VictorSPX.NeutralMode.Brake, True)
        self.rightfollower.follow(self.rightleader)

        self.DS = wpilib.DoubleSolenoid(0, 1)

        self.drive = DifferentialDrive(self.leftleader, self.rightleader)
        self.drive.maxOutput = 1.0
Beispiel #11
0
    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)
Beispiel #12
0
    def __init__(self):
        super().__init__()
        self.liftDrive = WPI_TalonSRX(RobotMap.BACK_DRIVE_TALON)

        self.liftSpark = CANSparkMax(RobotMap.LIFT_SPARK, MotorType.kBrushless)
        self.liftSpark.restoreFactoryDefaults()
        self.liftSpark.setIdleMode(IdleMode.kCoast)
        self.liftSpark.setSmartCurrentLimit(40)
        self.liftEncoder = self.liftSpark.getEncoder()
        self.liftPID = self.liftSpark.getPIDController()

        self.setLiftPIDConstants()

        self.elevatorSpark = CANSparkMax(RobotMap.ELEVATOR_SPARK,
                                         MotorType.kBrushless)
        self.elevatorSpark.restoreFactoryDefaults()
        self.elevatorSpark.setIdleMode(IdleMode.kCoast)
        self.elevatorSpark.setInverted(True)
        self.liftSpark.setSmartCurrentLimit(60)
        self.elevatorEncoder = self.elevatorSpark.getEncoder()
        self.elevatorPID = self.elevatorSpark.getPIDController()

        self.setElevatorPIDConstants()
Beispiel #13
0
    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')
Beispiel #14
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()