def create_analog_input(channel):
return wpilib.AnalogInput(channel)
def robotInit(self): self.turnEncoder = wpilib.AnalogInput(0)
def __init__(self, port):
""":param port: Analog port number"""
self.distance = wpilib.AnalogInput(1)
def createObjects(self):
# CARGO MECHANISM
self.cargo_mechanism_motor_rotator = ctre.WPI_TalonSRX(
robotmap.Ports.Cargo.rotator)
self.cargo_mechanism_servo_lock = wpilib.Servo(
robotmap.Ports.Cargo.locking_servo)
utils.configure_motor(
self.cargo_mechanism_motor_rotator, ctre.NeutralMode.Brake,
robotmap.MotorConfiguration.Cargo.peak_current,
robotmap.MotorConfiguration.Cargo.peak_current_duration)
self.cargo_mechanism_motor_rotator.setInverted(
robotmap.MotorConfiguration.Cargo.inverted)
# HATCH MECHANISM
self.hatch_grab_actuator = wpilib.DoubleSolenoid(
robotmap.Ports.Hatch.Grabber.pcm,
robotmap.Ports.Hatch.Grabber.front,
robotmap.Ports.Hatch.Grabber.back)
self.hatch_rack_actuator_left = wpilib.DoubleSolenoid(
robotmap.Ports.Hatch.Extension.pcm,
robotmap.Ports.Hatch.Extension.left_front,
robotmap.Ports.Hatch.Extension.left_back)
self.hatch_rack_actuator_right = wpilib.DoubleSolenoid(
robotmap.Ports.Hatch.Extension.pcm,
robotmap.Ports.Hatch.Extension.right_front,
robotmap.Ports.Hatch.Extension.right_back)
# DRIVETRAIN SYSTEM
# TODO fix motor port naming.
self.drivetrain_right_front = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.right_back)
self.drivetrain_right_back = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.right_bottom)
self.drivetrain_right_top = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.right_top)
self.drivetrain_left_front = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.left_front)
self.drivetrain_left_back = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.left_bottom)
self.drivetrain_left_top = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.left_top)
utils.configure_drivetrain_motors(self.drivetrain_left_back,
self.drivetrain_left_front,
self.drivetrain_left_top,
self.drivetrain_right_back,
self.drivetrain_right_front,
self.drivetrain_right_top)
self.drivetrain_right_motors = wpilib.SpeedControllerGroup(
self.drivetrain_right_back, self.drivetrain_right_front,
self.drivetrain_right_top)
self.drivetrain_left_motors = wpilib.SpeedControllerGroup(
self.drivetrain_left_back, self.drivetrain_left_front,
self.drivetrain_left_top)
self.differential_drive = wpilib.drive.DifferentialDrive(
self.drivetrain_left_motors, self.drivetrain_right_motors)
self.left_shifter_actuator = wpilib.DoubleSolenoid(
robotmap.Ports.Drivetrain.Shifters.pcm,
robotmap.Ports.Drivetrain.Shifters.left_front,
robotmap.Ports.Drivetrain.Shifters.left_back)
self.right_shifter_actuator = wpilib.DoubleSolenoid(
robotmap.Ports.Drivetrain.Shifters.pcm,
robotmap.Ports.Drivetrain.Shifters.right_front,
robotmap.Ports.Drivetrain.Shifters.right_back)
self.pressure_sensor_input = wpilib.AnalogInput(
robotmap.Ports.PressureSensor.port)
# MISC
self.oi = OI()
# run camera streaming program
wpilib.CameraServer.launch("vision.py:main")
self.current_camera = 0
self.camera_table = networktables.NetworkTables.getTable(
"/CameraPublisher")
# this is important for this year...
# self.use_teleop_in_autonomous = True
self.navx = navx.AHRS.create_spi()
def robotInit(self):
''' Initialization of robot objects. '''
''' Talon SRX Initialization '''
# drive train motors
self.frontRightMotor = WPI_TalonSRX(4)
self.rearRightMotor = WPI_TalonSRX(3)
self.frontLeftMotor = WPI_TalonSRX(1)
self.rearLeftMotor = WPI_TalonSRX(2)
''' Encoders '''
# drive train encoders
self.rightEncoder = self.frontRightMotor
self.leftEncoder = self.frontLeftMotor
# lift encoder
self.liftEncoder = wpilib.Encoder(8, 9)
# liftArm encoder
self.liftArmEncoder = wpilib.Encoder(5, 6)
''' Motor Groups '''
# drive train motor groups
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# drive train drive group
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
''' Victor SPX Initialization '''
# lift motors
self.liftOne = WPI_VictorSPX(1)
self.liftTwo = WPI_VictorSPX(2)
self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo)
# lift arm motors
self.liftArmOne = WPI_VictorSPX(3)
self.liftArmTwo = WPI_VictorSPX(4)
self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne,
self.liftArmTwo)
# cargo intake motor
self.cargo = WPI_VictorSPX(5)
''' Controller Initialization '''
# joystick - 0, 1 | controller - 2
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
self.buttonBox = wpilib.Joystick(3)
''' Button Status'''
self.buttonStatus = [False, False, False, False, False, False, False]
''' Pneumatic Initialization '''
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.enable = self.Compressor.getPressureSwitchValue()
self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting
self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2,
3) # hatch panel claw
self.DoubleSolenoidThree = wpilib.DoubleSolenoid(
4, 5) # hatch panel ejection
self.Compressor.start()
''' Smart Dashboard '''
# connection for logging & Smart Dashboard
logging.basicConfig(level=logging.DEBUG)
self.sd = NetworkTables.getTable('SmartDashboard')
NetworkTables.initialize(server='10.55.49.2')
self.sd.putString(" ", "Connection")
# Smart Dashboard classes
self.PDP = wpilib.PowerDistributionPanel()
self.roboController = wpilib.RobotController()
self.DS = wpilib.DriverStation.getInstance()
''' Sensors '''
# Hall Effect Sensor
self.Hall = wpilib.DigitalInput(7)
self.ultrasonic = wpilib.AnalogInput(2)
self.cargoUltrasonic = wpilib.AnalogInput(3)
''' Timer '''
self.timer = wpilib.Timer()
''' Camera '''
# initialization of the HTTP camera
wpilib.CameraServer.launch('vision.py:main')
self.sd.putString("", "Top Camera")
self.sd.putString(" ", "Bottom Camera")
def robotInit(self):
# Gear plate limit switch
self.gear_switch = wpilib.DigitalInput(const.ID_GEAR_SWITCH)
### Subsystems ###
# Agitator
self.agitator = subsystems.agitator.Agitator(self)
# Climber
self.climber = subsystems.climber.Climber(self)
# Drive for Xbox Controller
self.drive = subsystems.drivetrain.Drivetrain(self)
# Gear
self.gear_funnel = subsystems.gear_funnel.Gear_Funnel(self)
self.gear_lexan = subsystems.gear_lexan.Gear_Lexan(self)
self.gear_claw = subsystems.gear_claw.Gear_Claw(self)
# Ground Feeder
self.feeder = subsystems.feeder.Feeder(self)
# Shooter
self.shooter = subsystems.shooter.Shooter(self)
# Shooter Camera
self.shooter_camera = subsystems.shooter_camera.Shooter_Camera(self)
# Gear Camera
self.gear_camera = subsystems.gear_camera.Gear_Camera(self)
# Encoders
self.drive_encoder_left = wpilib.Encoder(2, 3, reverseDirection=True)
self.drive_encoder_left.setDistancePerPulse(
const.DRIVE_DISTANCE_PER_ENCODER_TICK_OMNI)
self.drive_encoder_left.reset()
self.drive_encoder_right = wpilib.Encoder(0, 1, reverseDirection=False)
self.drive_encoder_right.setDistancePerPulse(
const.DRIVE_DISTANCE_PER_ENCODER_TICK_OMNI)
self.drive_encoder_right.reset()
# Pressure sensor (200 psi)
self.pressure_sensor = wpilib.AnalogInput(const.ID_PRESSURE_SENSOR)
self._pressure_samples = []
self._last_pressure_value = 0.0
# Gyro
self.gyro = wpilib.ADXRS450_Gyro()
#self.gyro = wpilib.AnalogGyro( 0 )
#self.gyro.setSensitivity( 0.08 )
### Misc ###
# Operator Input
self.oi = oi.OI(self)
# Log to file
self.log_file = open(
os.path.join(os.path.dirname(__file__), 'log.csv'), 'w')
# Time robot object was created
self.start_time = time.time()
## Autonomous ##
self.subsystems = {
'agitator': self.agitator,
'climber': self.climber,
'drive': self.drive,
'gear_funnel': self.gear_funnel,
'gear_lexan': self.gear_lexan,
'gear_claw': self.gear_claw,
'feeder': self.feeder,
'shooter_camera': self.shooter_camera,
'gear_camera': self.gear_camera,
'shooter': self.shooter,
}
## Scheduler ##
self.scheduler = wpilib.command.Scheduler.getInstance()
## MISC ##
self.gear_is_ejecting = False
self.gear_ejecting_start_time = 0
### Logging ###
# NetworkTables
self.nt_smartdash = networktables.NetworkTable.getTable(
'SmartDashboard')
self.nt_grip_peg = networktables.NetworkTable.getTable(
'vision/peg_targets')
self.nt_grip_boiler = networktables.NetworkTable.getTable(
'vision/boiler_targets')
self.nt_vision = networktables.NetworkTable.getTable('vision')
# Timers for NetworkTables update so we don't use too much bandwidth
self.log_timer = wpilib.Timer()
self.log_timer.start()
self.log_timer_delay = 0.1 # 10 times/second
# Timer for pressure sensor's running average
self.pressure_timer = wpilib.Timer()
self.pressure_timer.start()
self.pressure_timer_delay = 1.0 # once per second
self.log()
def robotInit(self):
self.sd = wpilib.SmartDashboard
self.timer = wpilib.Timer()
self.DriveSpd = 1
self.RotationSpd = 1
self.IntakeSpd = 0
self.ShooterSpd = 1
self.Direction = -1
self.currentRotationRate = 0
self.SpeedAut = 0
self.rotateToAngle = False
self.gamedata = wpilib.DriverStation.getInstance(
).getGameSpecificMessage()
self.navx = navx.AHRS.create_spi()
self.analog = wpilib.AnalogInput(navx.getNavxAnalogInChannel(0))
self.EncoderB = wpilib.encoder.Encoder(0, 1)
self.EncoderA = wpilib.encoder.Encoder(2, 3, True)
self.frontLeft = wpilib.Spark(0)
self.rearLeft = wpilib.Spark(1)
self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
self.frontRight = wpilib.Spark(2)
self.rearRight = wpilib.Spark(3)
self.right = wpilib.SpeedControllerGroup(self.frontRight,
self.rearRight)
self.drive = DifferentialDrive(self.left, self.right)
self.stick1 = wpilib.Joystick(0)
self.stick2 = wpilib.Joystick(1)
# For Toggling Buttons
# self.toggle7 = Toggle(self.stick1, 7)
# self.toggle1 = Toggle(self.stick1, 1)
# self.toggle6 = Toggle(self.stick1, 6)
# self.toggle4 = Toggle(self.stick1, 4)
# self.toggle5 = Toggle(self.stick1, 5)
# self.toggle3 = Toggle(self.stick1, 3)
# self.toggle2 = Toggle(self.stick1, 2)
self.Lshoot = wpilib.Spark(5)
self.Rshoot = wpilib.Spark(6)
self.shoot = wpilib.SpeedControllerGroup(self.Lshoot, self.Rshoot)
self.Lintake = wpilib.Spark(4)
self.Rintake = wpilib.Spark(8)
self.intake = wpilib.SpeedControllerGroup(self.Lintake, self.Rintake)
#
# Communicate w/navX MXP via the MXP SPI Bus.
# - Alternatively, use the i2c bus.
# See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details
#
self.ahrs = AHRS.create_spi()
# self.ahrs = AHRS.create_i2c()
turnController = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.ahrs,
output=self)
turnController.setInputRange(-180.0, 180.0)
turnController.setOutputRange(-1.0, 1.0)
turnController.setAbsoluteTolerance(self.kToleranceDegrees)
turnController.setContinuous(True)
self.turnController = turnController
self.rotateToAngleRate = 0
# Add the PID Controller to the Test-mode dashboard, allowing manual */
# tuning of the Turn Controller's P, I and D coefficients. */
# Typically, only the P value needs to be modified. */
wpilib.LiveWindow.addActuator("DriveSystem", "RotateController",
turnController)
print(self.sd.getTable())
