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)
