def __init__(self) -> None:
super().__init__()
# The Romi has the left and right motors set to
# PWM channels 0 and 1 respectively
self.leftMotor = wpilib.Spark(0)
self.rightMotor = wpilib.Spark(1)
# The Romi has onboard encoders that are hardcoded
# to use DIO pins 4/5 and 6/7 for the left and right
self.leftEncoder = wpilib.Encoder(4, 5)
self.rightEncoder = wpilib.Encoder(6, 7)
# Set up the differential drive controller
self.drive = wpilib.drive.DifferentialDrive(self.leftMotor, self.rightMotor)
# Set up the RomiGyro
self.gyro = RomiGyro()
# Set up the BuiltInAccelerometer
self.accelerometer = wpilib.BuiltInAccelerometer()
# Use inches as unit for encoder distances
self.leftEncoder.setDistancePerPulse(
(math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
)
self.rightEncoder.setDistancePerPulse(
(math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution
)
self.resetEncoders()
def __init__(self):
super().__init__('Drive')
self.debug = False
self.yawZero = 0
self.navx = navx.ahrs.AHRS.create_spi()
self.navx.setName("Drive", "NavX")
self.accel = wpilib.BuiltInAccelerometer()
self.accel.setName("Drive", "Accel")
self.left : Tread = Tread("Left", \
robotmap.kCanDriveLeft0, robotmap.kCanDriveLeft1, robotmap.kCanDriveLeft2, True, \
robotmap.kDioDriveLeftEncA, robotmap.kDioDriveLeftEncB, kLeftConv)
self.right : Tread = Tread("Right", \
robotmap.kCanDriveRight0, robotmap.kCanDriveRight1, robotmap.kCanDriveRight2, False, \
robotmap.kDioDriveRightEncA, robotmap.kDioDriveRightEncB, kRightConv)
self.left.debug = self.debug
self.right.debug = self.debug
self.drive: DifferentialDrive = DifferentialDrive(
self.left.motor, self.right.motor)
self.drive.setName("Drive", "Differential")
self.drive.setSafetyEnabled(False)
self.drive.setDeadband(0.025)
self.drive.setRightSideInverted(False)
self.periodic()
self.zero()
def robotInit(self):
'''Robot initialization function'''
self.motorFrontRight = wp.VictorSP(2)
self.motorBackRight = wp.VictorSP(4)
self.motorMiddleRight = wp.VictorSP(6)
self.motorFrontLeft = wp.VictorSP(1)
self.motorBackLeft = wp.VictorSP(3)
self.motorMiddleLeft = wp.VictorSP(5)
self.intakeMotor = wp.VictorSP(8)
self.shootMotor1 = wp.VictorSP(7)
self.shootMotor2 = wp.VictorSP(9)
self.liftMotor = wp.VictorSP(0)
self.gyro = wp.ADXRS450_Gyro(0)
self.accel = wp.BuiltInAccelerometer()
self.gearSensor = wp.DigitalInput(6)
self.LED = wp.DigitalOutput(9)
self.rightEncd = wp.Encoder(0, 1)
self.leftEncd = wp.Encoder(2, 3)
self.shootEncd = wp.Counter(4)
self.compressor = wp.Compressor()
self.shifter = wp.DoubleSolenoid(0, 1)
self.ptoSol = wp.DoubleSolenoid(2, 3)
self.kicker = wp.DoubleSolenoid(4, 5)
self.gearFlap = wp.DoubleSolenoid(6, 7)
self.stick = wp.Joystick(0)
self.stick2 = wp.Joystick(1)
self.stick3 = wp.Joystick(2)
#Initial Dashboard Code
wp.SmartDashboard.putNumber("Turn Left pos 1:", 11500)
wp.SmartDashboard.putNumber("Lpos 2:", -57)
wp.SmartDashboard.putNumber("Lpos 3:", 5000)
wp.SmartDashboard.putNumber("stdist:", 6000)
wp.SmartDashboard.putNumber("Turn Right pos 1:", 11500)
wp.SmartDashboard.putNumber("Rpos 2:", 57)
wp.SmartDashboard.putNumber("Rpos 3:", 5000)
wp.SmartDashboard.putNumber("pos 4:", 39)
wp.SmartDashboard.putNumber("-pos 4:", -39)
wp.SmartDashboard.putNumber("Time", 5)
wp.SmartDashboard.putBoolean("Shooting Auto", False)
wp.SmartDashboard.putNumber("P", .08)
wp.SmartDashboard.putNumber("I", 0.005)
wp.SmartDashboard.putNumber("D", 0)
wp.SmartDashboard.putNumber("FF", 0.85)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Left Turn Auto", 1)
self.chooser.addObject("Straight/Enc", 2)
self.chooser.addObject("Right Turn Auto", 3)
self.chooser.addObject("Straight/Timer", 5)
self.chooser.addObject("Shoot ONLY", 6)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def createObjects(self):
# Define Driving Motors
self.rightDrive = wpilib.VictorSP(0)
self.leftDrive = wpilib.VictorSP(1)
# Create Robot Drive
self.robotDrive = wpilib.drive.DifferentialDrive(
self.rightDrive, self.leftDrive)
# Create Shifter Pneumatics
self.shifterSolenoid = wpilib.DoubleSolenoid(0, 0, 1)
# Joysticks and Controllers
self.driveJoystick = wpilib.Joystick(0)
self.driveController = XboxController(0)
self.subsystemJoystick = wpilib.Joystick(1)
self.subsystemController = XboxController(1)
# Create NavX and Accel
self.navX = navx.AHRS.create_spi()
self.accel = wpilib.BuiltInAccelerometer()
# Set Drivespeed
self.driveSpeed = 0
# Intake Motors
self.intakeMotor = wpilib.VictorSP(2)
# Intake Lifter
self.intakeLifter = wpilib.Spark(6)
# Create Cube Intake Pneumatics
self.intakeSolenoid = wpilib.Solenoid(0, 2)
# Create Ramp Motors
self.rightRamp = wpilib.Spark(5)
self.leftRamp = wpilib.Spark(4)
# Create Ramp Deploy Pneumatics
self.rampSolenoid = wpilib.Solenoid(0, 3)
# Create Timer (For Making Timed Events)
self.timer = wpilib.Timer()
# Initialize Compressor
self.compressor = wpilib.Compressor()
# Create CameraServer
wpilib.CameraServer.launch("common/multipleCameras.py:main")
# Set Gear in Dashboard
wpilib.SmartDashboard.putString("Shift State", "Low Gear")
wpilib.SmartDashboard.putString("Cube State", "Unclamped")
def robotInit(self):
'''Robot initialization function'''
#positions = [0,-625,-1210,-1815,-2425,-2635]
positions = [0, -325, -650, -975, -1300, -1395]
scorePos = [0, 0, -275, -545, -805, -1075]
self.motorFrontRight = wp.TalonSRX(frontRightPort)
self.motorBackRight = wp.TalonSRX(backRightPort)
self.motorFrontLeft = wp.TalonSRX(frontLeftPort)
self.motorBackLeft = wp.TalonSRX(backLeftPort)
self.stick = joy.happyHappyJoyJoy(leftDriveJoyPort)
self.stick2 = joy.happyHappyJoyJoy(rightDriveJoyPort)
self.cop = joy.happyHappyJoyJoy(coPilotJoyPort)
self.smartDs = wp.SmartDashboard() #the smart dashboard communication
self.accel = wp.BuiltInAccelerometer()
self.driveEncd = wp.Encoder(driveEncdAPort, driveEncdBPort)
self.feeders = feed.feedMe(feederLeftPort, feederRightPort,
feedUpChannel, feedDownChannel, 0.35)
self.rightSwitch = wp.DigitalInput(rightToteDetPort)
self.leftSwitch = wp.DigitalInput(leftToteDetPort)
self.autoSwitch = wp.DigitalInput(autoSwitchPort)
self.comp = wp.Compressor()
self.Vator = ele.elevatorObj(liftMotPort, liftEncdAPort, liftEncdBPort,
positions, scorePos, 0.75, 30)
try:
self.camServ = wp.CameraServer()
self.usbCam = wp.USBCamera()
self.usbCam.setExposureManual(50)
self.usbCam.setBrightness(80)
self.usbCam.updateSettings() # force update before we start thread
self.camServ.startAutomaticCapture(self.usbCam)
except:
pass
def robotInit(self):
'''Robot initialization function'''
self.person = robotuser.RobotUser()
self.accel = wpilib.BuiltInAccelerometer(1)
self.joystickChannel = 0 # usb number in DriverStation
# channels for motors
self.leftMotorChannel = 1
self.rightMotorChannel = 0
self.leftRearMotorChannel = 3
self.rightRearMotorChannel = 2
self.left = wpilib.SpeedControllerGroup(
wpilib.Talon(self.leftMotorChannel),
wpilib.Talon(self.leftRearMotorChannel))
self.right = wpilib.SpeedControllerGroup(
wpilib.Talon(self.rightMotorChannel),
wpilib.Talon(self.rightRearMotorChannel))
self.myRobot = DifferentialDrive(self.left, self.right)
self.joystick = wpilib.Joystick(self.joystickChannel)
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.gyro = wpilib.AnalogGyro(0)
self.gyro.reset()
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
NetworkTables.initialize(server='127.0.0.1')
self.smartdash = NetworkTables.getTable('SmartDashboard')
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
wpilib.CameraServer.launch("vision.py:main")
self.ll = NetworkTables.getTable("limelight")
self.ll.putNumber('ledStatus', 1)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(2)
self.rightStick = wpilib.Joystick(1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.smartdash.putNumber('tx', 1)
self.gearshift.set(1)
self.pdp = wpilib.PowerDistributionPanel(0)
self.accelerometer = wpilib.BuiltInAccelerometer()
self.gyro.initSendable
self.myRobot.initSendable
self.gearshift.initSendable
self.pdp.initSendable
self.accelerometer.initSendable
self.acc = wpilib.AnalogAccelerometer(3)
self.setpoint = 90.0
self.P = .3
self.I = 0
self.D = 0
self.integral = 0
self.previous_error = 0
self.rcw = 0
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
"""
1. Chute
2. Loader
3. Climber
"""
# Configure shooter motor controller.
#self.Gyro = wpilib.ADXRS450_Gyro()
self.Chute_l = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.Chute_r = ctre.wpi_talonsrx.WPI_TalonSRX(
5) # Create a CANTalon object.
# Choose an encoder as a feedback device. The default should be QuadEncoder already, but might as well make sure.
# I thought the encoder was 20 pulses per revolution per phase, which would require "80" as an argument below, but after trying it, it looks like there are 12.
# Setting this parameter changes things so getPosition() returns decimal revolutions, and getSpeed() returns RPM.
#self.shooter.configEncoderCodesPerRev(48)
# resets shooter position on startup
#self.unloader.setQuadraturePosition(0, 0)
#self.unloader.setNeutralMode(ctre.wpilib.Spark.NeutralMode.Coast)# This should change between brake and coast modes.
self.l_motor1 = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.l_motor2 = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.l_motor1.setInverted(False)
self.l_motor2.setInverted(False)
self.r_motor1 = ctre.wpi_talonsrx.WPI_TalonSRX(0)
self.r_motor2 = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.r_motor1.setInverted(False)
self.r_motor2.setInverted(False)
# Configure shooter motor controller.
# Create a CANTalon object.
self.l_motor1.setNeutralMode(
ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
#self.l_motor1.configSelectedFeedbackSensor(ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0)
self.l_motor2.setNeutralMode(
ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
self.l_motor1.setNeutralMode(
ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
#self.r_motor1.configSelectedFeedbackSensor(ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0)
self.l_motor2.setNeutralMode(
ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast)
# Choose an encoder as a feedback device. The default should be QuadEncoder already, but might as well make sure.
# I thought the encoder was 20 pulses per revolution per phase, which would require "80" as an argument below, but after trying it, it looks like there are 12.
# Setting this parameter changes things so getPosition() returns decimal revolutions, and getSpeed() returns RPM.
#self.l_motor1.configEncoderCodesPerRev(48)
#self.l_motor2.configEncoderCodesPerRev(48)
#self.r_motor1.configEncoderCodesPerRev(48)
#self.r_motor2.configEncoderCodesPerRev(48)
# resets shooter position on startup
self.l_motor1.setQuadraturePosition(0, 0)
self.r_motor1.setQuadraturePosition(0, 0)
#self.stick = wpilib.Joystick(0)
self.l_joy = wpilib.Joystick(0)
self.r_joy = wpilib.Joystick(1)
self.loader_l = wpilib.Spark(1)
self.loader_r = wpilib.Spark(0)
self.climber = wpilib.Spark(3)
self.left = wpilib.SpeedControllerGroup(self.l_motor1, self.l_motor2)
self.right = wpilib.SpeedControllerGroup(self.r_motor1, self.r_motor2)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.accelerometer = wpilib.BuiltInAccelerometer(
wpilib.BuiltInAccelerometer.Range.k2G)
self.vel = 0
self.pos = 0
self.grav = 9.82
self.counter = 0
def robotInit(self):
'''Robot-wide Initialization code'''
self.robotnumber = wpilib.DigitalInput(9)
#Initializing networktables
self.smart_dashboard = NetworkTables.getTable("SmartDashboard")
self.smart_dashboard.putNumber('robot_speed', 1)
self.smart_dashboard.putString('robot_name',
"Zil1" if self.robotnumber else "Zil2")
# initialize and launch the camera
wpilib.CameraServer.launch()
# initialize the left and right encoders.
self.encoder_wheel_left = wpilib.Encoder(
0, 1, True, wpilib.Encoder.EncodingType.k4X)
self.encoder_wheel_left.reset()
self.encoder_wheel_right = wpilib.Encoder(
2, 3, False, wpilib.Encoder.EncodingType.k4X)
self.encoder_wheel_right.reset()
self.encoder_wheel_left.setDistancePerPulse(self.kDistancePerPulse)
self.encoder_wheel_right.setDistancePerPulse(self.kDistancePerPulse)
#Initalizing drive motors
if self.robotnumber:
self.drive_l_motor = wpilib.Victor(portmap.motors.left_drive)
self.drive_r_motor = wpilib.Victor(portmap.motors.right_drive)
else:
self.drive_l_motor = wpilib.Spark(portmap.motors.left_drive)
self.drive_r_motor = wpilib.Spark(portmap.motors.right_drive)
self.claw_l_motor = wpilib.Victor(portmap.motors.left_claw)
self.claw_r_motor = wpilib.Victor(portmap.motors.right_claw)
self.lift_motor = wpilib.Victor(portmap.motors.lift)
# initialize drive (differential drive is tank drive)
self.drive = wpilib.drive.DifferentialDrive(self.drive_l_motor,
self.drive_r_motor)
self.drive.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
self.left_stick = wpilib.Joystick(portmap.joysticks.left_joystick)
self.right_stick = wpilib.Joystick(portmap.joysticks.right_joystick)
# initialize gyro
self.gyro = wpilib.AnalogGyro(wpilib.AnalogInput(1))
self.gyro.calibrate()
# initialize the ultra sonic
self.range = wpilib.AnalogInput(0)
# initialize Accelerometer
self.accelerometer = wpilib.BuiltInAccelerometer(
Accelerometer.Range.k4G)
# initialize autonomous components
self.components = {
'drive': self.drive,
'drive_r_motor': self.drive_r_motor,
'drive_l_motor': self.drive_l_motor,
'claw_r_motor': self.claw_r_motor,
'claw_l_motor': self.claw_r_motor,
'lift_motor': self.lift_motor,
'encoder_wheel_right': self.encoder_wheel_right,
'encoder_wheel_left': self.encoder_wheel_left,
'gyro': self.gyro,
'accelerometer': self.accelerometer,
'range': self.range
}
self.automodes = AutonomousModeSelector('autonomous', self.components)
self.logger.log(logging.INFO, "robot initialization complete.")
def create_built_in_accelerometer(range=2):
return wpilib.BuiltInAccelerometer(range)
def robotInit(self):
# NetworkTables
self.nt_robot = networktables.NetworkTables.getTable('Robot')
### Subsystems ###
# Drivetrain
self.drive = subsystems.drivetrain.Drivetrain(self)
# Driverstation
self.driverstation = wpilib.DriverStation.getInstance()
# Power Distribution Panel
self.pdp = wpilib.PowerDistributionPanel()
# Robot Components -- subsystems
self.hatch = subsystems.hatch.Hatch(self)
self.cargo = subsystems.cargo.Cargo(self)
self.climber = subsystems.climber.Climber(self)
# Limelight / camera
self.limelight = subsystems.limelight.Limelight(self)
# wpilib.CameraServer.launch()
# Arduino / line sensors
self.arduino = subsystems.arduino.Arduino(self)
# Gyro
# self.gyro = wpilib.ADXRS450_Gyro()
# Ultrasonics
# self.ultrasonic_front = wpilib.AnalogInput(const.AIN_ULTRASONIC_FRONT)
# self.ultrasonic_rear = wpilib.AnalogInput(const.AIN_ULTRASONIC_REAR)
self.match_time = 9999
# Operator Input
self.oi = oi.OI(self)
# Encoders -- change after encoders are decided and added
self.use_enc_correction = False
self.drive_encoder_left = wpilib.Encoder(
const.DIO_DRIVE_ENC_LEFT_1,
const.DIO_DRIVE_ENC_LEFT_2,
reverseDirection=const.DRIVE_ENCODER_LEFT_REVERSED)
self.drive_encoder_right = wpilib.Encoder(
const.DIO_DRIVE_ENC_RIGHT_1,
const.DIO_DRIVE_ENC_RIGHT_2,
reverseDirection=const.DRIVE_ENCODER_RIGHT_REVERSED)
self.drive_encoder_left.setDistancePerPulse(1 /
const.DRIVE_TICKS_PER_FOOT)
self.drive_encoder_right.setDistancePerPulse(
1 / const.DRIVE_TICKS_PER_FOOT)
self.climber.climber_motor_1.setSelectedSensorPosition(0, 0, 10)
self.climber.climber_motor_2.setSelectedSensorPosition(0, 0, 10)
# Built in Accelerometer
self.accel = wpilib.BuiltInAccelerometer()
self.accel.setRange(0) # Sets range from -2g to 2g
# Array for average accel.
self.accel_samples = []
self.last_accel_value = 0.0
# Pressure Sensor (200 psi)
# self.pressure_sensor = wpilib.AnalogInput(const.AIN_PRESSURE_SENSOR)
# Timer for pressure sensor's running average
# self._pressure_samples = []
# self._last_pressure_value = 0.0
# self.pressure_timer = wpilib.Timer()
# self.pressure_timer.start()
# self.pressure_timer_delay = 1.0 # times per second
# Time robot object was created
self.start_time = time.time()
## Scheduler ##
self.scheduler = wpilib.command.Scheduler.getInstance()
### LOGGING ###
# 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.25 # 4 times/second
# Disable LW telemetry before comp to improve loop times
wpilib.LiveWindow.disableAllTelemetry()
self.limelight.set_startup_modes()
