def robotInit(self): """initialises robot as a mecanum drive bot w/ 2 joysticks and a camera""" #want to change this to Xbox 360 controller eventually... probably sooner rather #than later. # #This is for a USB camera. Uncomment it if we aren't using the Axis. self.camera = wpilib.USBCamera() self.camera.setExposureManual(50) self.camera.setBrightness(80) self.camera.updateSettings() self.camera.setFPS(10) self.camera.setSize(320, 240) self.camera.setWhiteBalanceAuto() #self.camera.setQuality(30) server = wpilib.CameraServer.getInstance() server.startAutomaticCapture(self.camera) self.drive = wpilib.RobotDrive(3, 1, 2, 0) self.drive.setExpiration(0.1) self.stick_left = wpilib.Joystick(0) self.stick_right = wpilib.Joystick(1) self.drive.setInvertedMotor(self.drive.MotorType.kFrontRight, True) self.drive.setInvertedMotor(self.drive.MotorType.kRearRight, True) #self.gyro = wpilib.Gyro(0) self.aux_left = wpilib.Jaguar(6) self.aux_right = wpilib.Jaguar(4) self.window_motor = wpilib.Jaguar(5) self.smart_dashboard = NetworkTable.getTable("SmartDashboard") self.mast_pot = wpilib.AnalogPotentiometer(0) self.grabba_pot = wpilib.AnalogPotentiometer(1) self.lift_pot = wpilib.AnalogPotentiometer(2) def aux_combined(output): """use for PID control""" self.aux_left.pidWrite(output) self.aux_right.pidWrite(output) self.grabba_pid = wpilib.PIDController(4, 0.07, 0, self.grabba_pot.pidGet, self.window_motor.pidWrite) self.grabba_pid.disable() self.lift_pid = wpilib.PIDController(4, 0.07, 0, self.lift_pot.pidGet, aux_combined) self.lift_pid.disable()
def __init__(self, robot): super().__init__(self.kP_real, 0, 0) # Check for simulation and update PID values # if robot.isSimulation(): # self.getPIDController().setPID(self.kP_simulation, 0, 0, 0) self.setAbsoluteTolerance(2.5) self.motor = wpilib.Victor(6) # Conversion value of potentiometer varies between the real world and simulation if robot.isReal(): self.pot = wpilib.AnalogPotentiometer(3, -270 / 5) else: self.pot = wpilib.AnalogPotentiometer(3) # defaults to degrees
def __init__(self, robot): super().__init__(7.0, 0.0, 8.0, name="Pivot") self.robot = robot self.setAbsoluteTolerance(0.005) self.getPIDController().setContinuous(False) if robot.isSimulation(): # PID is different in simulation. self.getPIDController().setPID(0.5, 0.001, 2) self.setAbsoluteTolerance(5) # Motor to move the pivot self.motor = wpilib.Victor(5) # Sensors for measuring the position of the pivot. self.upperLimitSwitch = wpilib.DigitalInput(13) self.lowerLimitSwitch = wpilib.DigitalInput(12) # 0 degrees is vertical facing up. # Angle increases the more forward the pivot goes. self.pot = wpilib.AnalogPotentiometer(1) # Put everything to the LiveWindow for testing. wpilib.LiveWindow.addSensor("Pivot", "Upper Limit Switch", self.upperLimitSwitch) wpilib.LiveWindow.addSensor("Pivot", "Lower Limit Switch", self.lowerLimitSwitch) wpilib.LiveWindow.addSensor("Pivot", "Pot", self.pot) wpilib.LiveWindow.addActuator("Pivot", "Motor", self.motor) wpilib.LiveWindow.addActuator("Pivot", "PIDSubsystem Controller", self.getPIDController())
def __init__(self, robot): super().__init__(20, 0, 0) self.robot = robot self.grabba_pot = wpilib.AnalogPotentiometer(2) self.motor = wpilib.Talon(5) self.current = wpilib.AnalogInput(3) self.setAbsoluteTolerance(.01)
def __init__(self, robot): super().__init__(self.kP_real, 0, 0) # Check for simulation and update PID values if robot.isSimulation(): self.getPIDController().setPID(self.kP_simulation, 0, 0, 0) self.setAbsoluteTolerance(2.5) self.motor = wpilib.Victor(6) # Conversion value of potentiometer varies between the real world and simulation if robot.isReal(): self.pot = wpilib.AnalogPotentiometer(3, -270 / 5) else: self.pot = wpilib.AnalogPotentiometer(3) # defaults to degrees # Let's show everything on the LiveWindow wpilib.LiveWindow.addActuator("Wrist", "Motor", self.motor) wpilib.LiveWindow.addSensor("Wrist", "Pot", self.pot) wpilib.LiveWindow.addActuator("Wrist", "PID", self.getPIDController())
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ self.open_state = .02 self.closed_state = .35 self.arm = ctre.wpi_talonsrx.WPI_TalonSRX(0) self.joystick = wpilib.Joystick(0) self.arm.setInverted(True) self.arm_pot = wpilib.AnalogPotentiometer(0) self.arm_pid = wpilib.PIDController(3, 0, 0, self.arm_pot.get, self.pid_output)
def module_init(self): self.intake_solenoid_1 = wpilib.DoubleSolenoid(0, 1) if self.DOUBLE_SOLENOID: self.intake_solenoid_2 = wpilib.DoubleSolenoid(2, 3) if self.USE_CAN: self.intake_motor = wpilib.CANTalon(6) self.intake_motor.enableControl() else: self.intake_motor = wpilib.Talon(9) self.auto_override = False self.intake_potentiometer = wpilib.AnalogPotentiometer(0) self.setpoint = 0 self.at_setpoint = 0 self.auto_intake_out = 0 self.joystick = wpilib.Joystick(1)
def __init__(self, robot): super().__init__('hatch') self.robot = robot # set up motor controllers self.carriage_motor = ctre.WPI_VictorSPX( const.CAN_MOTOR_HATCH_CARRIAGE) self.carriage_motor.setInverted(True) # set up string potentiometer self.string_pot = wpilib.AnalogPotentiometer( const.AIN_HATCH_STRING_POT, const.HATCH_STRING_POT_MULTIPLIER, const.HATCH_STRING_POT_OFFSET) # set up solenoids self.beak_piston = wpilib.DoubleSolenoid( const.CAN_PCM_A, const.PCM_HATCH_SOLENOID_BEAK_1, const.PCM_HATCH_SOLENOID_BEAK_2) self.carriage_piston = wpilib.DoubleSolenoid( const.CAN_PCM_A, const.PCM_HATCH_SOLENOID_CARRIAGE_1, const.PCM_HATCH_SOLENOID_CARRIAGE_2) # self.line_sensor_piston_1 = wpilib.DoubleSolenoid(const.CAN_PCM_B, const.PCM_LIGHT_SENSOR_SOLENOID_1, # const.PCM_LIGHT_SENSOR_SOLENOID_2) # set up carriage PID - values need to be tuned self.carriage_kP = 3 / 11 self.carriage_kI = 0.007 self.carriage_kD = 0.0 self.carriage_pid = wpilib.PIDController( self.carriage_kP, self.carriage_kI, self.carriage_kD, self.get_carriage_pid_input, self.set_carriage_pid_output, ) self.carriage_pid_output = 0 self.carriage_pid.setInputRange(const.CARRIAGE_POS_MIN, const.CARRIAGE_POS_MAX) self.carriage_pid.setOutputRange(-1, 1) self.carriage_pid.setAbsoluteTolerance(.25) self.carriage_pid.setContinuous(False) self.carriage_pid.disable()
def createObjects(self): self.leftStick = wpilib.Joystick(2) self.elevatorStick = wpilib.Joystick(1) self.rightStick = wpilib.Joystick(0) self.elevatorMotorOne = wpilib.Victor(2) self.elevatorMotorTwo = wpilib.Victor(3) self.left = wpilib.Victor(0) self.right = wpilib.Victor(1) self.myRobot = DifferentialDrive(self.left, self.right) self.elevator = wpilib.SpeedControllerGroup(self.elevatorMotorOne, self.elevatorMotorTwo) self.elevatorPot = wpilib.AnalogPotentiometer(0) self.gearshift = wpilib.DoubleSolenoid(0, 0, 1) self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5) self.gearshift.set(1)
def __init__(self, robot): super().__init__("Pivot", 7.0, 0.0, 8.0) self.robot = robot self.setAbsoluteTolerance(0.005) self.getPIDController().disableContinuousInput() if robot.isSimulation(): # PID is different in simulation. self.getPIDController().setPID(0.5, 0.001, 2) self.setAbsoluteTolerance(5) # Motor to move the pivot self.motor = wpilib.Victor(5) # Sensors for measuring the position of the pivot. self.upperLimitSwitch = wpilib.DigitalInput(13) self.lowerLimitSwitch = wpilib.DigitalInput(12) # 0 degrees is vertical facing up. # Angle increases the more forward the pivot goes. self.pot = wpilib.AnalogPotentiometer(1)
def robotInit(self): self.tankDrive = DifferentialDrive( wpilib.PWMVictorSPX(0), wpilib.PWMVictorSPX(1) ) self.leftEncoder = wpilib.Encoder(0, 1) self.rightEncoder = wpilib.Encoder(2, 3) self.elevatorMotor = wpilib.PWMVictorSPX(2) self.elevatorPot = wpilib.AnalogPotentiometer(0) # Add a 'max speed' widget to a tab named 'Configuration', using a number slider # The widget will be placed in the second column and row and will be two columns wide self.maxSpeed = ( Shuffleboard.getTab("Configuration") .add(title="Max Speed", value=1) .withWidget("Number Slider") .withPosition(1, 1) .withSize(2, 1) .getEntry() ) # Add the tank drive and encoders to a 'Drivebase' tab driveBaseTab = Shuffleboard.getTab("Drivebase") driveBaseTab.add(title="Tank Drive", value=self.tankDrive) # Put both encoders in a list layout encoders = ( driveBaseTab.getLayout(type="List Layout", title="Encoders") .withPosition(0, 0) .withSize(2, 2) ) encoders.add(title="Left Encoder", value=self.leftEncoder) encoders.add(title="Right Encoder", value=self.rightEncoder) # Add the elevator motor and potentiometer to an 'Elevator' tab elevatorTab = Shuffleboard.getTab("Elevator") elevatorTab.add(title="Motor", value=self.elevatorMotor) elevatorTab.add(title="Potentiometer", value=self.elevatorPot)
def robotInit(self): self.BUTTON_RBUMPER = 6 self.BUTTON_LBUMPER = 5 self.BUTTON_A = 1 self.BUTTON_B = 2 self.BUTTON_X = 3 self.BUTTON_Y = 4 self.LY_AXIS = 1 self.LX_AXIS = 0 self.RX_AXIS = 4 self.RY_AXIS = 5 self.R_TRIGGER = 3 self.L_TRIGGER = 2 self.LEFT_JOYSTICK_BUTTON = 9 self.RIGHT_JOYSTICK_BUTTON = 10 self.BACK_BUTTON = 7 self.START_BUTTON = 8 self.rev_per_ft = 12 / (math.pi * self.wheel_diameter) self.br_motor = ctre.wpi_talonsrx.WPI_TalonSRX(5) self.bl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(4) self.fr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(7) self.fl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(3) self.arm = ctre.wpi_talonsrx.WPI_TalonSRX(0) self.front_lift = ctre.wpi_talonsrx.WPI_TalonSRX(6) self.front_lift_slave = ctre.wpi_talonsrx.WPI_TalonSRX(50) self.front_lift_slave.follow(self.front_lift) self.back_lift = ctre.wpi_talonsrx.WPI_TalonSRX(2) self.back_lift_wheel = ctre.wpi_talonsrx.WPI_TalonSRX(1) self.fl_motor.setInverted(True) self.bl_motor.setInverted(True) self.arm.setInverted(True) self.fl_motor.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS) self.bl_motor.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS) self.fr_motor.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS) self.br_motor.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS) self.front_lift.configSelectedFeedbackSensor( ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS) self.back_lift.configSelectedFeedbackSensor( ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, MyRobot.TIMEOUT_MS) # Reverse negative encoder values self.fl_motor.setSensorPhase(True) #self.fr_motor.setSensorPhase(True) self.br_motor.setSensorPhase(True) self.front_lift.setSensorPhase(True) self.deadzone_amount = 0.15 self.control_state = "speed" self.start_navx = 0 self.previous_hyp = 0 self.js_startAngle = 0 self.rot_startNavx = 0 self.joystick = wpilib.Joystick(0) NetworkTables.addEntryListener(self.entry_listener) self.use_pid = False self.prev_pid_toggle_btn_value = False self.navx = navx.AHRS.create_spi() self.relativeGyro = RelativeGyro(self.navx) self.timer = wpilib.Timer() self.arm_pot = wpilib.AnalogPotentiometer(0) self.arm_pid = wpilib.PIDController(3, 0, 0, self.arm_pot.get, self.pid_output) self.init_time = 0 self.desired_rate = 0 self.pid_turn_rate = 0 self.prev_button1 = False self.button = False self.button_chomp = False self.front_lift_heights_index = 0 self.lift_target = 0 self.driveStates = { 'velocity': Velocty_Mode(self), 'enter_position': Enter_Position_Mode(self), 'position': Position_Mode(self), 'enter_rotation': Enter_Rotation_Mode(self), 'rotation': Rotation_Mode(self), 'leave_special': Leave_Special_Mode(self), 'lift_robot': Lift_Robot(self) } self.drive_sm = State_Machine(self.driveStates, "Drive_sm") self.drive_sm.set_state('velocity') self.liftStates = { 'fully_raised': Fully_Raised(self), 'middle': Middle(self), 'fully_lowered': Fully_Lowered(self), 'go_to_height': Go_To_Height(self) } self.lift_sm = State_Machine(self.liftStates, "lift_sm") self.lift_sm.set_state('fully_lowered') self.wheel_motors = [ self.br_motor, self.bl_motor, self.fr_motor, self.fl_motor ] wpilib.CameraServer.launch() def normalized_navx(): return self.get_normalized_angle(self.navx.getAngle()) self.angle_pid = wpilib.PIDController(self.turn_rate_p, self.turn_rate_i, self.turn_rate_d, self.relativeGyro.getAngle, self.set_pid_turn_rate) #self.turn_rate_pid. #self.turn_rate_pid. self.angle_pid.setInputRange(-self.turn_rate_pid_input_range, self.turn_rate_pid_input_range) self.angle_pid.setOutputRange(-self.turn_rate_pid_output_range, self.turn_rate_pid_output_range) self.angle_pid.setContinuous(True) self.turn_rate_values = [0] * 10
def __init__(self, robot): super().__init__(40, 0, 0) #__init__(P, I, D) self.robot = robot self.mast_pot = wpilib.AnalogPotentiometer(0) self.motor = wpilib.VictorSP(6) self.setAbsoluteTolerance(.01)
def robotInit(self): '''Robot initialization function''' self.leftMotor1 = wp.VictorSP(1) #motor initialization self.leftMotor2 = wp.VictorSP(3) self.leftMotor3 = wp.VictorSP(5) self.rightMotor1 = wp.VictorSP(2) self.rightMotor2 = wp.VictorSP(4) self.rightMotor3 = wp.VictorSP(6) self.armMotor = wp.VictorSP(7) self.leftIntakeMotor = wp.VictorSP(8) self.rightIntakeMotor = wp.VictorSP(9) self.leftSide = wp.SpeedControllerGroup( self.leftMotor1, self.leftMotor2, self.leftMotor3) #speed controller groups self.rightSide = wp.SpeedControllerGroup(self.rightMotor1, self.rightMotor2, self.rightMotor3) self.intakeMotors = wp.SpeedControllerGroup(self.leftIntakeMotor, self.rightIntakeMotor) self.myRobot = DifferentialDrive(self.leftSide, self.rightSide) self.myRobot.setExpiration(0.1) self.leftStick = wp.Joystick(0) self.rightStick = wp.Joystick(1) self.armStick = wp.XboxController(2) self.gyro = wp.ADXRS450_Gyro(0) self.rightEncd = wp.Encoder(2, 3) self.leftEncd = wp.Encoder(0, 1) self.armPot = wp.AnalogPotentiometer(0, 270, -135) self.compressor = wp.Compressor() self.shifter = wp.DoubleSolenoid(0, 1) wp.SmartDashboard.putNumber("Straight Position", 15000) wp.SmartDashboard.putNumber("leftMiddleAutoStraight", 13000) wp.SmartDashboard.putNumber("leftMiddleAutoLateral", 14000) wp.SmartDashboard.putNumber("Left Switch Pos 1", 18000) wp.SmartDashboard.putNumber("Left Switch Angle", 90) wp.SmartDashboard.putNumber("Left Switch Pos 2", 5000) wp.SmartDashboard.putNumber("Switch Score Arm Position", 70) wp.SmartDashboard.putNumber("Front Position 1", 74) wp.SmartDashboard.putNumber("Front Position 2", 16) wp.SmartDashboard.putNumber("Front Position 3", -63) wp.SmartDashboard.putNumber("lvl2 Position 1", 60) wp.SmartDashboard.putNumber("lvl2 Position 3", -50) wp.SmartDashboard.putNumber("lvl3 Position 3", -38) wp.SmartDashboard.putNumber("lvl3 Position 1", 45) wp.SmartDashboard.putBoolean("switchScore?", False) self.chooser = wp.SendableChooser() self.chooser.addDefault("None", 4) self.chooser.addObject("Straight/Enc", 1) self.chooser.addObject("Left Side Switch Auto", 2) self.chooser.addObject("Right Side Switch Auto (switch)", 3) self.chooser.addObject("Center Auto", 5) wp.SmartDashboard.putData("Choice", self.chooser) wp.CameraServer.launch("vision.py:main")
def __init__(self, robot): super().__init__('cargo') self.robot = robot self.manual_mode = False self.current_arm_speed = 0.0 # set up motor controllers self.cargo_arm_motor = ctre.WPI_VictorSPX(const.CAN_MOTOR_CARGO_ARM) self.cargo_intake_motor = ctre.WPI_VictorSPX( const.CAN_MOTOR_CARGO_INTAKE) self.cargo_arm_motor.setInverted(False) self.cargo_intake_motor.setInverted(False) # set up solenoids self.cargo_brake = wpilib.DoubleSolenoid( const.CAN_PCM_A, const.PCM_CARGO_SOLENOID_BRAKE_1, const.PCM_CARGO_SOLENOID_BRAKE_2) # set up limit switches #self.limit_1 = wpilib.DigitalInput(const.DIO_CARGO_LIMIT_1) # set up potentiometer self.pot = wpilib.AnalogPotentiometer(const.AIN_CARGO_ARM_POT, const.CARGO_ARM_POT_MULTIPLIER, const.CARGO_ARM_POT_OFFSET) # set up arm PID self.arm_kP = 0.036 self.arm_kI = 0 # is 0.0025 when doing continuous pid self.arm_kD = 0.03 self.arm_pid = wpilib.PIDController( self.arm_kP, self.arm_kI, self.arm_kD, self.get_arm_pid_input, self.set_arm_pid_output, ) self.arm_pid_output = 0 self.arm_pid.setInputRange(0, 135) self.arm_pid.setOutputRange(-1, 1) self.arm_pid.setAbsoluteTolerance( 2.5) # is 1 when doing continuous pid self.arm_pid.setContinuous(False) self.arm_pid.disable() self.climb_kP = 0.04 self.climb_kI = 0.004 self.climb_kD = 0.0 self.climb_pid = wpilib.PIDController( self.climb_kP, self.climb_kI, self.climb_kD, self.get_arm_climb_pid_input, self.set_arm_climb_pid_output, ) self.climb_pid_output = 0 self.climb_pid.setInputRange(-5, 135) self.climb_pid.setOutputRange(-1, 1) #self.climb_pid.setAbsoluteTolerance(2.5) self.climb_pid.setContinuous(False) self.climb_pid.disable() # self.robot.nt_robot.putNumber("climbP", self.arm_kP) # self.robot.nt_robot.putNumber("climbI", self.arm_kI) # self.robot.nt_robot.putNumber("climbD", self.arm_kD) # Timer for running average on intake motor current self._current_samples = [] self._last_current_value = 0.0 self.current_timer = wpilib.Timer() self.current_timer.start() self.current_timer_delay = 0.1 # times per second self.stop_arm()
def create_analog_potentiometer(channel): return wpilib.AnalogPotentiometer(channel)