def robotInit(self):
self.timer = wpilib.Timer()
self.m_imu = ADIS16470_IMU()
self.m_yawSelected = KYAW_DEFAULT
self.m_runCal = False
self.m_configCal = False
self.m_reset = False
self.m_setYawAxis = False
self.m_yawActiveAxis = ADIS16470_IMU.IMUAxis.kZ
self.m_autoChooser = wpilib.SendableChooser()
self.m_autoChooser.AddOption(KAUTONAME_CUSTOM, KAUTONAME_CUSTOM)
self.m_yawChooser = wpilib.SendableChooser()
self.m_yawChooser.SetDefaultOption(KYAW_DEFAULT, KYAW_DEFAULT)
self.m_yawChooser.AddOption(kYawXAxis, kYawXAxis)
self.m_yawChooser.AddOption(kYawYAxis, kYawYAxis)
wpilib.SmartDashboard.putData("Auto Modes", m_autoChooser)
wpilib.SmartDashboard.putData("IMUYawAxis", m_yawChooser)
wpilib.SmartDashboard.putBoolean("RunCal", False)
wpilib.SmartDashboard.putBoolean("ConfigCal", False)
wpilib.SmartDashboard.putBoolean("Reset", False)
wpilib.SmartDashboard.putBoolean("SetYawAxis", False)
def robotInit(self):
#Joysticks
self.Joystick = wpilib.Joystick(self.Joystick_Channel)
self.Sec_Joystick = wpilib.Joystick(self.Sec_Joystick_Channel)
#Magnetic Limit Switches
self.switch1 = wpilib.DigitalInput(0)
#Mechanisms
self.Elevator = ctre.WPI_TalonSRX(self.Elevator_Motor)
self.LF_Intake = ctre.WPI_TalonSRX(self.Left_Front_Intake)
self.RF_Intake = ctre.WPI_TalonSRX(self.Right_Front_Intake)
#PWM Mechanisms
self.Climber = wpilib.Talon(self.Climber_Motor)
#Drive Motors
self.LMF = wpilib.Talon(self.Left_Motor_Front)
self.RMF = wpilib.Talon(self.Right_Motor_Front)
self.LMB = wpilib.Talon(self.Left_Motor_Back)
self.RMB = wpilib.Talon(self.Right_Motor_Back)
self.Left = wpilib.SpeedControllerGroup(self.LMF, self.LMB)
self.Right = wpilib.SpeedControllerGroup(self.RMF, self.RMB)
#This is not the built-in robot drive
self.My_Robot = DifferentialDrive(self.Left, self.Right)
self.My_Robot.setExpiration(0.1)
#SmartDashboard
self.spChooser = wpilib.SendableChooser()
self.spChooser.addDefault("Left", 1)
self.spChooser.addObject("Middle", 2)
self.spChooser.addObject("Right", 3)
wpilib.SmartDashboard.putData('StartingPosition', self.spChooser)
self.amChooser = wpilib.SendableChooser()
self.amChooser.addDefault("Scale", 1)
self.amChooser.addObject("Other Lever", 2)
self.amChooser.addObject("Line", 3)
self.amChooser.addObject("None", 0)
wpilib.SmartDashboard.putData('AutoMode', self.amChooser)
self.writeAutoChooser = wpilib.SendableChooser()
self.writeAutoChooser.addDefault("Read Auto", 0)
self.writeAutoChooser.addObject("Write Auto", 1)
wpilib.SmartDashboard.putData('WriteAuto', self.writeAutoChooser)
self.infoChooser = wpilib.SendableChooser()
self.infoChooser.addDefault("0", 0)
for line in customParsing.read():
self.infoChooser.addObject(line, line)
wpilib.SmartDashboard.putData("InfoChooserChannel", self.infoChooser)
def robotInit(self):
print("[Robot] Initializing")
logging.basicConfig(level=logging.DEBUG)
print("[Robot] Starting network tables server")
NetworkTables.initialize()
print("[Robot] Setting up systems")
self.cargosystem = CargoSystem(self)
self.climbsystem = ClimbSystem(self)
self.drivetrain = DriveTrain(self)
self.hatchsystem = HatchSystem(self)
self.oi = OI(self)
# setup autonomous type
print("[Robot] Setting up autonomous")
scAutonomousType = wpilib.SendableChooser()
scAutonomousType.setDefaultOption("1. Driver Controlled", 100)
scAutonomousType.addOption ("2. Timed Just Drive Forward", 200)
scAutonomousType.addOption ("3. Motion Profile Just Drive Forward", 300)
scAutonomousType.addOption ("4. Total Autonomous", 400)
wpilib.SmartDashboard.putData ("Anonomous Type", scAutonomousType)
# setup robot positions
scRobotPosition = wpilib.SendableChooser()
scRobotPosition.setDefaultOption("1. Upper Left" , 1)
scRobotPosition.addOption ("2. Lower Left" , 2)
scRobotPosition.addOption ("3. Centre" , 3)
scRobotPosition.addOption ("4. Upper Right", 4)
scRobotPosition.addOption ("5. Lower Right", 5)
wpilib.SmartDashboard.putData ("Robot Position", scRobotPosition)
# setup hatch positions
scHatchPosition = wpilib.SendableChooser()
scHatchPosition.setDefaultOption("1. Back" , 10)
scHatchPosition.addOption ("2. Middle" , 20)
scHatchPosition.addOption ("3. Front" , 30)
wpilib.SmartDashboard.putData ("Hatch Position", scHatchPosition)
# wpilib.CameraServer.launch("d:\\jet\\projects\\python\\RobotPy\\src\\vision.py:main")
# wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putNumber("H-Lower", 0)
wpilib.SmartDashboard.putNumber("H-Upper", 180)
wpilib.SmartDashboard.putNumber("S-Lower", 0)
wpilib.SmartDashboard.putNumber("S-Upper", 255)
wpilib.SmartDashboard.putNumber("V-Lower", 0)
wpilib.SmartDashboard.putNumber("V-Upper", 255)
def robotInit(self):
'''
This is a good place to set up your subsystems and anything else that
you will need to access later.
'''
subsystems.init()
self.autoChooser = wpilib.SendableChooser()
# self.autoChooser.addDefault('No Auto', NoAuto())
self.autoChooser.addDefault('Center Switch Auto', CenterSwitchAuto())
self.autoChooser.addObject('Left Switch Auto', LeftSwitchAuto())
# self.autoChooser.addObject('Center Switch Auto', CenterSwitchAuto())
self.autoChooser.addObject('Right Switch Auto', RightSwitchAuto())
self.autoChooser.addObject('Cross Line Auto', CrossLineAuto())
self.autoChooser.addObject('Left Switch Safe', LeftSwitchSafe())
self.autoChooser.addObject('Right Switch Safe', RightSwitchSafe())
wpilib.SmartDashboard.putData('Auto Mode', self.autoChooser)
self.autonomousCommand = None
'''
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
'''
OI.init()
# Initialize the camera server
wpilib.CameraServer.launch('vision.py:main')
def robotInit(self):
"""
This is a good place to set up your subsystems and anything else that
you will need to access later.
"""
# Define Robot getter
Command.getRobot = lambda: self
# Shuffleboard options
self.driveSwitcher = wpilib.SendableChooser()
self.driveSwitcher.setDefaultOption("WPI Mecanum",
self.DriveSwitcher.WPI_MECANUM)
self.driveSwitcher.addOption("Field Oriented",
self.DriveSwitcher.FIELD_ORIENTED)
self.driveSwitcher.addOption("Morpheus", self.DriveSwitcher.MORPHEUS)
self.driveSwitcher.addOption("No Joystick", self.DriveSwitcher.NO_JOY)
# Create subsystems
self.mastyBoi = MastBoi()
self.suplex = Flipper()
self.spootnikDrives = SpootnikDrives()
self.hatchEffector = HatchEffector()
self.cargoEffector = CargoEffector()
# Autonomous commands
self.autonomousCommand = DriveToBaseLine()
"""
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
"""
self.oi = OI()
def robotInit(self):
self.pegChooser = wpilib.SendableChooser()
self.pegChooser.addObject('Left', 'left')
self.pegChooser.addObject('Right', 'right')
self.pegChooser.addObject('Middle', 'middle')
wpilib.SmartDashboard.putData('ChooseYourPeg', self.pegChooser)
#test = sd.getNumber("someNumber")
#self.logger.info("Test = " + str(test))
self.logger.info("Robot Starting up...")
self.logger.info("Camera started")
self.mctype = wpilib.Spark
self.logger.info("Defined motor controller type")
self.cam_horizontal = wpilib.Servo(6)
self.cam_vertical = wpilib.Servo(7)
self.cam_vertical_value = 0.2
self.cam_horizontal_value = 0.5
self.logger.info("Defined Camera Servos and Respective Values")
self.cam_horizontal.set(self.cam_horizontal_value)
self.cam_vertical.set(self.cam_vertical_value)
self.logger.info("Set Camera Servos to Halfway")
self.pdp = wpilib.PowerDistributionPanel()
self.logger.info("defined PowerDistributionPanel")
self.left = self.mctype(LEFT_MOTOR)
self.right = self.mctype(RIGHT_MOTOR)
self.winchMotor = self.mctype(WINCH_MOTOR)
self.logger.info("Defined FL, BL, FR, BR motors")
self.controls = controlstemplate.Controls(
wpilib.Joystick(JOYSTICK_PORT), self.isTest)
self.logger.info("Defined Control scheme")
self.timer = wpilib.Timer()
self.logger.info("Defined Timer")
self.logger.info("Robot On")
def robotInit(self):
constants.load_control_config()
wpilib.CameraServer.launch('driver_vision.py:main')
self.autoPositionSelect = wpilib.SendableChooser()
self.autoPositionSelect.addDefault('Middle', 'Middle')
self.autoPositionSelect.addObject('Left', 'Left')
self.autoPositionSelect.addObject('Right', 'Right')
wpilib.SmartDashboard.putData('Robot Starting Position',
self.autoPositionSelect)
self.control_stick = wpilib.Joystick(0)
self.drivetrain = swerve.SwerveDrive(constants.chassis_length,
constants.chassis_width,
constants.swerve_config)
self.lift = lift.RD4BLift(constants.lift_ids['left'],
constants.lift_ids['right'])
self.winch = winch.Winch(constants.winch_id)
# self.claw = lift.Claw(
# constants.claw_id
# )
self.imu = IMU(wpilib.SPI.Port.kMXP)
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 initialize(self):
if self.initialized:
return
# initializing auto chooser
self.preferredElement = wpilib.SendableChooser()
self.preferredElement.addObject('crossLine', 0)
self.preferredElement.addObject('switch', 1)
self.preferredElement.addObject('scale', 2)
self.startingPosition = wpilib.SendableChooser()
self.startingPosition.addObject('left', 0)
self.startingPosition.addObject('middle', 1)
self.startingPosition.addObject('right', 2)
SmartDashboard.putData("Preferred Element", self.preferredElement)
SmartDashboard.putData("Starting Position", self.startingPosition)
print("AutoManager: Initialized")
self.initialized = True
def __init__(self):
self.chooser = wpilib.SendableChooser()
self.chooser.addDefault('Dumb Auto', 1)
self.chooser.addObject('Left Auto', 2)
self.chooser.addObject('Right Auto', 3)
self.chooser.addObject('Not Lambot Auto', 4)
self.chooser.addObject('Right MonarchE Auto', 5)
self.chooser.addObject('Left MonarchE Auto', 6)
wpilib.SmartDashboard.putData('Autonomous Mode', self.chooser)
def robotInit(self):
self.timer = wpilib.Timer()
#self.gyro = ADIS16470_IMU()
self.m_yawSelected = kYaw_default
self.m_runCal = False
self.m_configCal = False
self.m_reset = False
# self.m_yawActiveAxis = self.gyro.IMUAxis.kz
# Adds Options. Very Helpful.
self.m_autoChooser = wpilib.SendableChooser()
self.m_autoChooser.AddOption(kautoname_custom, kautoname_custom)
self.m_yawChooser = wpilib.SendableChooser()
self.m_yawChooser.SetDefaultOption(self.kYawDefault, self.kYawDefault)
self.m_yawchooser.AddOption(kYaw_xaxis, kYaw_xAxis)
self.m_yawchooser.AddOption(kYaw_yAxis, kYaw_yAxis)
wpilib.SmartDashboard.putBoolean("Config Calibration", False)
wpilib.SmartDashboard.putBoolean("Run Calibration", False)
wpilib.SmartDashboard.putBoolean("Reset Gyro", False)
wpilib.SmartDashboard.putBoolean("Set Current Axis", False)
def robotInit(self):
"""Init is called once when the robot is turned on."""
self.efacing = 1
"""efacing is used to invert our controls."""
self.CarEncoder = wpilib.Encoder(0, 1)
#self.HatchEncoder = wpilib.Encoder(3, 4)
self.chooser = wpilib.SendableChooser()
self.chooser.addObject('cargo', '1')
self.chooser.addObject('hatch panel', '2')
self.left = wpilib.VictorSP(0)
self.right = wpilib.VictorSP(1)
"""Motors used for driving"""
self.myRobot = DifferentialDrive(self.left, self.right)
"""DifferentialDrive is the main object that deals with driving"""
self.RotServo = wpilib.Servo(2)
self.TiltServo = wpilib.Servo(3)
"""Our two servos will rotate (RotServo) and tilt (TiltServo) our
vision cameras."""
self.motor1 = wpilib.Talon(5)
self.motor2 = wpilib.Talon(6)
"""I mostly just added these motor controllers anticipating some sort
of intake system that uses motors."""
self.Punch = wpilib.DoubleSolenoid(0, 1)
self.DPunch = wpilib.DoubleSolenoid(3, 2)
"""The punching mechanism for removal of the hatch panels can use a
DoubleSolenoid or regular Solenoid. The Solenoid only needs the channel
it's plugged into (4) whereas the Double Solenoid needs the module
number, forward channel number, and reverse channel order in that
order."""
self.XBox0 = wpilib.XboxController(0)
self.XBox1 = wpilib.XboxController(1)
"""Xbox controllers 1 and 2 on the driver station."""
self.myRobot.setExpiration(0.1)
"""If communication is cut off between the driver station and the robot
def robotInit(self):
'''
self.auto_goal = 0
self.auto_state = 0
self.sd.putBoolean("Center Lane", False)
self.sd.putBoolean("Left lane", False)
self.sd.putBoolean("Right lane", False)
self.sd.putBoolean("right goal", False)
self.sd.putBoolean("left goal", False)
'''
self.sd = wpi.SmartDashboard()
self.sd.putNumber("team",
wpi.DriverStation.getInstance().getAlliance())
self.dump_mode = True
self.dump_chooser = wpi.SendableChooser()
self.dump_chooser.addDefault("Yes", True)
self.dump_chooser.addObject("No", False)
self.sd.putData("Dump?", self.dump_chooser)
self.cc = ntu.ChooserControl("Dump?", None, self.set_dump_mode)
wpi.CameraServer.launch("vision/vision.py:run")
self.gripper_sole = wpi.DoubleSolenoid(0, 2)
self.dump_sole = wpi.Solenoid(1)
self.lift = wpi.Spark(5)
self.frontLeftMotor = wpi.Spark(2)
self.rearLeftMotor = wpi.Spark(3)
self.frontRightMotor = wpi.Spark(1)
self.rearRightMotor = wpi.Spark(0)
self.left = wpi.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpi.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
self.drive = drive.DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
self.joystick = wpi.XboxController(0)
self.timer = wpi.Timer()
def robotInit(self):
'''Robot initialization function'''
self.leftMotor1 = ctre.WPI_VictorSPX(1) #motor initialization
self.leftMotor2 = ctre.WPI_VictorSPX(3)
self.leftMotor3 = ctre.WPI_VictorSPX(5)
self.rightMotor1 = ctre.WPI_VictorSPX(0)
self.rightMotor2 = ctre.WPI_VictorSPX(2)
self.rightMotor3 = ctre.WPI_VictorSPX(4)
self.armMotor = ctre.WPI_VictorSPX(6)
self.leftIntakeMotor = ctre.WPI_VictorSPX(7)
self.rightIntakeMotor = ctre.WPI_VictorSPX(8)
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.Joystick(2)
self.gyro = wp.ADXRS450_Gyro(0)
self.rightEncd = wp.Encoder(0, 1)
self.leftEncd = wp.Encoder(2, 3)
self.compressor = wp.Compressor()
wp.SmartDashboard.putNumber("Straight Position", 4000)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Straight/Enc", 1)
self.chooser.addObject("Left Turn Auto", 2)
self.chooser.addObject("Right Turn Auto", 3)
self.chooser.addObject("Straight/Timer", 5)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def robotInit(self):
self.drivetrain = drivetrain.Drivetrain(self)
self.intake = intake.Intake(self)
self.lift = lift.Lift(self)
self.oi = OI(self)
self.logger = logging.getLogger("robot")
self.autonomousCommand = None
wpilib.CameraServer.launch('vision.py:main')
self.chooser = wpilib.SendableChooser()
self.chooser.addObject("Left", 1)
self.chooser.addDefault("Center", 2)
self.chooser.addObject("Right", 3)
wpilib.SmartDashboard.putData("Auto Chooser", self.chooser)
def robotInit(self):
'''Initialize all subsystems.'''
self.drivetrain = DriveTrain(self)
self.shooter = Shooter(self)
self.intake_sub = Intake_Sub(self)
self.shifter = Shifter(self)
self.blocker = Blocker(self)
self.climbmotors = Climbmotors(self)
self.climbpistons = Climbpistons(self)
self.agitator = Agitator(self)
self.autoChooser = wpilib.SendableChooser()
self.autoChooser.setDefaultOption("Drive", DriveAutonomous(self))
# The "front" of the robot (which end is facing forward)
self.front = -1
self.oi = OI(self)
def robotInit(self):
""" Robot Initialization. Runs on turn-on. """
### Xnabled? ###
self.true_run = wpilib.SendableChooser()
self.true_run.addDefault("On", True)
self.true_run.addObject("Off", False)
### Setup Camera ###
wpilib.CameraServer.launch("vision.py:main")
### Drive Train Initialization ###
# Motors
self.pwm = [None, None, None, None, None, None, None, None, None, None]
self.pwm[0] = wpilib.Victor(0)
self.pwm[1] = wpilib.Victor(1)
self.pwm[2] = wpilib.Spark(2)
self.pwm[3] = wpilib.Spark(3)
self.pwm[7] = wpilib.Victor(7)
self.pwm[8] = wpilib.Victor(8)
self.pwm[9] = wpilib.Victor(9)
# Drive Train
self.driveLeft = wpilib.SpeedControllerGroup(self.pwm[0], self.pwm[2])
self.driveRight = wpilib.SpeedControllerGroup(self.pwm[1], self.pwm[3])
self.driveTrain = wpilib.drive.DifferentialDrive(
self.driveLeft, self.driveRight)
### Initialize Gamepad ###
self.gamepad = wpilib.XboxController(0)
### Other Inputs ###
self.finderA = wpilib.AnalogInput(0)
self.finderB = wpilib.AnalogInput(1)
### Variables ###
self.launchTick = 0
self.allignment = True
self.direction = -1
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.drivetrain = drivetrain.Drivetrain()
self.sensors = sensor_handler.sensorHandler()
self.joystick = joystick_handler.JoystickHandler()
self.elevator = elevator.Elevator()
self.intakeHandler = intake.Intake(self.sensors, self.joystick)
self.auto = Autonomous_mode_handler.AutonomousModeHandler(
self.drivetrain, self.sensors, self.elevator, self.intakeHandler)
self.autoPath = auto_path.AutoPath(self.sensors, self.drivetrain)
self.chooser = wpilib.SendableChooser()
self.chooser.addObject('center', 'robot_constant.CENTER_POS')
self.chooser.addObject('left', 'robot_constant.LEFT_POS')
self.chooser.addObject('right', 'robot_constant.RIGHT_POS')
self.chooser.addObject('default', 'robto_constant.DEFAULT')
wpilib.SmartDashboard.putData('Auto Mode', self.chooser)
logging.basicConfig(filename='robotLogs.log', level=logging.DEBUG)
logging.info("Init robot")
def __init__(self) -> None:
# The driver's controller
# self.driverController = wpilib.XboxController(constants.kDriverControllerPort)
self.driverController = wpilib.Joystick(constants.kDriverControllerPort)
# The robot's subsystems
self.drive = DriveSubsystem()
self.hatch = HatchSubsystem()
# Autonomous routines
# A simple auto routine that drives forward a specified distance, and then stops.
self.simpleAuto = DriveDistance(
constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, self.drive
)
# A complex auto routine that drives forward, drops a hatch, and then drives backward.
self.complexAuto = ComplexAuto(self.drive, self.hatch)
# Chooser
self.chooser = wpilib.SendableChooser()
# Add commands to the autonomous command chooser
self.chooser.setDefaultOption("Simple Auto", self.simpleAuto)
self.chooser.addOption("Complex Auto", self.complexAuto)
# Put the chooser on the dashboard
wpilib.SmartDashboard.putData("Autonomous", self.chooser)
self.configureButtonBindings()
# set up default drive command
self.drive.setDefaultCommand(
DefaultDrive(
self.drive,
lambda: -self.driverController.getY(GenericHID.Hand.kLeftHand),
lambda: self.driverController.getX(GenericHID.Hand.kLeftHand),
)
)
def robotInit(self):
constants.load_control_config()
wpilib.CameraServer.launch('driver_vision.py:main')
self.autoPositionSelect = wpilib.SendableChooser()
self.autoPositionSelect.addDefault('Middle-Baseline',
'Middle-Baseline')
self.autoPositionSelect.addObject('Middle-Placement',
'Middle-Placement') # noqa: E501
self.autoPositionSelect.addObject('Left', 'Left')
self.autoPositionSelect.addObject('Right', 'Right')
wpilib.SmartDashboard.putData('Robot Starting Position',
self.autoPositionSelect)
self.drivetrain = swerve.SwerveDrive(constants.chassis_length,
constants.chassis_width,
constants.swerve_config)
self.drivetrain.load_config_values()
self.lift = lift.ManualControlLift(constants.lift_ids['left'],
constants.lift_ids['right'],
constants.lift_limit_channel,
constants.start_limit_channel)
self.winch = winch.Winch(constants.winch_id)
self.throttle = wpilib.Joystick(1)
self.claw = lift.Claw(constants.claw_id, constants.claw_follower_id)
self.imu = IMU(wpilib.SPI.Port.kMXP)
self.sd_update_timer = wpilib.Timer()
self.sd_update_timer.reset()
self.sd_update_timer.start()
def robotInit(self):
"""
This function is run when the robot is first started up and should be
used for any initialization code.
"""
# Initialize the subsystems
self.drivetrain = DriveTrain(self)
self.collector = Collector(self)
self.shooter = Shooter(self)
self.pneumatics = Pneumatics(self)
self.pivot = Pivot(self)
wpilib.SmartDashboard.putData(self.drivetrain)
wpilib.SmartDashboard.putData(self.collector)
wpilib.SmartDashboard.putData(self.shooter)
wpilib.SmartDashboard.putData(self.pneumatics)
wpilib.SmartDashboard.putData(self.pivot)
# This MUST be here. If the OI creates Commands (which it very likely
# will), constructing it during the construction of CommandBase (from
# which commands extend), subsystems are not guaranteed to be
# yet. Thus, their requires() statements may grab null pointers. Bad
# news. Don't move it.
self.oi = OI(self)
#instantiate the command used for the autonomous period
self.autoChooser = wpilib.SendableChooser()
self.autoChooser.addDefault("Drive and Shoot",
DriveAndShootAutonomous(self))
self.autoChooser.addObject("Drive Forward", DriveForward(self))
wpilib.SmartDashboard.putData("Auto Mode", self.autoChooser)
self.autonomousCommand = None
# Pressurize the pneumatics
self.pneumatics.start()
def robotInit(self):
#Motor controllers, joysticks, and setting up the drive type
self.motor1=wpilib.Jaguar(0)
self.motor2=wpilib.Jaguar(1)
self.slide_motor=wpilib.Jaguar(2)
self.non_existant_motor=wpilib.Jaguar(4)
self.tryit=wpilib.Jaguar(6)
self.red=wpilib.Jaguar(9)
self.blue=wpilib.Jaguar(8)
self.green=wpilib.Jaguar(7)
self.robot_drive = wpilib.RobotDrive(self.motor2,self.motor1)
self.stick1 = wpilib.Joystick(0)
self.joystick_button=wpilib.buttons.JoystickButton(self.stick1, 1)
self.lights=wpilib.buttons.JoystickButton(self.stick1, 2)
#All our clicky switches
self.clicky1=wpilib.DigitalInput(9)
self.clicky2=wpilib.DigitalInput(8)
self.clicky3=wpilib.DigitalInput(7)
wpilib.SmartDashboard.putBoolean("Clicky1",self.clicky1.get())
self.chooser=wpilib.SendableChooser()
self.chooser.addObject("Auto 1","1")
wpilib.SmartDashboard.putData('What Automous', self.chooser)
self.counter=0
def __init__(self) -> None:
# The robot's subsystems and commands are defined here...
self.drivetrain = Drivetrain()
self.onboardIO = OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT)
# Assumes a gamepad plugged into channnel 0
self.controller = wpilib.Joystick(0)
# Create SmartDashboard chooser for autonomous routines
self.chooser = wpilib.SendableChooser()
# NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay"
# that is specified when launching the wpilib-ws server on the Romi raspberry pi.
# By default, the following are available (listed in order from inside of the board to outside):
# - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
# - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
# - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
# - PWM 2 (mapped to Arduino Pin 21)
# - PWM 3 (mapped to Arduino Pin 22)
#
# Your subsystem configuration should take the overlays into account
self._configureButtonBindings()
def __init__(self, default=ledstrip.LEDStrip.Command.RED):
'''
Constructor
'''
self.command = default
self.chooser = wpilib.SendableChooser()
self.chooser.addDefault("Red", ledstrip.LEDStrip.Command.RED)
self.chooser.addObject("Blue", ledstrip.LEDStrip.Command.BLUE)
self.chooser.addObject("Green", ledstrip.LEDStrip.Command.GREEN)
self.chooser.addObject("Rainbow", ledstrip.LEDStrip.Command.RAINBOW)
self.chooser.addObject("Theater Blue",
ledstrip.LEDStrip.Command.THEATER_BLUE)
self.chooser.addObject("Theater Red",
ledstrip.LEDStrip.Command.THEATER_RED)
self.chooser.addObject("Theater Rainbow",
ledstrip.LEDStrip.Command.THEATER_RAINBOW)
self.chooser.addObject("Off", ledstrip.LEDStrip.Command.OFF)
SmartDashboard.putData("LEDs", self.chooser)
self.r = 0
self.g = 0
self.b = 0
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, autonomous_pkgname, *args, **kwargs):
'''
:param autonomous_pkgname: Module to load autonomous modes from
:param args: Args to pass to created autonomous modes
:param kwargs: Keyword args to pass to created autonomous modes
'''
self.ds = wpilib.DriverStation.getInstance()
self.modes = {}
self.active_mode = None
logger.info("Begin initializing autonomous mode switcher")
# load all modules in specified module
autonomous_pkg = importlib.import_module(autonomous_pkgname)
modules_path = os.path.dirname(os.path.abspath(
autonomous_pkg.__file__))
modules = glob(os.path.join(modules_path, '*.py'))
for module_filename in modules:
module_name = os.path.basename(module_filename[:-3])
if module_name in ['__init__', 'manager']:
continue
try:
module = importlib.import_module('.' + module_name,
autonomous_pkgname)
#module = imp.load_source('.' + module_name, module_filename)
except:
if not self.ds.isFMSAttached():
raise
#
# Find autonomous mode classes in the modules that are present
# -> note that we actually create the instance of the objects here,
# so that way we find out about any errors *before* we get out
# on the field..
for name, obj in inspect.getmembers(module, inspect.isclass):
if hasattr(obj, 'MODE_NAME'):
# don't allow the driver to select this mode
if hasattr(obj, 'DISABLED') and obj.DISABLED:
logger.warn("autonomous mode %s is marked as disabled",
obj.MODE_NAME)
continue
try:
instance = obj(*args, **kwargs)
except:
if not self.ds.isFMSAttached():
raise
else:
continue
if instance.MODE_NAME in self.modes:
if not self.ds.isFMSAttached():
raise RuntimeError(
"Duplicate name %s in %s" %
(instance.MODE_NAME, module_filename))
logger.error(
"Duplicate name %s specified by object type %s in module %s",
instance.MODE_NAME, name, module_filename)
self.modes[name + '_' + module_filename] = instance
else:
self.modes[instance.MODE_NAME] = instance
# now that we have a bunch of valid autonomous mode objects, let
# the user select one using the SmartDashboard.
# SmartDashboard interface
self.chooser = wpilib.SendableChooser()
default_modes = []
logger.info("Loaded autonomous modes:")
for k, v in sorted(self.modes.items()):
if hasattr(v, 'DEFAULT') and v.DEFAULT == True:
logger.info(" -> %s [Default]", k)
self.chooser.addDefault(k, v)
default_modes.append(k)
else:
logger.info(" -> %s", k)
self.chooser.addObject(k, v)
if len(self.modes) == 0:
logger.warn("-- no autonomous modes were loaded!")
self.chooser.addObject('None', None)
if len(default_modes) == 0:
self.chooser.addDefault('None', None)
elif len(default_modes) != 1:
if not self.ds.isFMSAttached():
raise RuntimeError(
"More than one autonomous mode was specified as default! (modes: %s)"
% (', '.join(default_modes)))
# must PutData after setting up objects
wpilib.SmartDashboard.putData('Autonomous Mode', self.chooser)
logger.info("Autonomous switcher initialized")
def createObjects(self):
#navx
self.navx = AHRS.create_spi()
self.navx.setPIDSourceType(PIDSource.PIDSourceType.kDisplacement)
#Drivetrain
self.left_talon0 = ctre.CANTalon(0)
self.left_talon1 = ctre.CANTalon(1)
self.right_talon0 = ctre.CANTalon(2)
self.right_talon1 = ctre.CANTalon(3)
#Set up talon slaves
self.left_talon1.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.left_talon1.set(self.left_talon0.getDeviceID())
self.right_talon1.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.right_talon1.set(self.right_talon0.getDeviceID())
#Set talon feedback device
self.left_talon0.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.right_talon0.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
#Set the Ticks per revolution in the talons
self.left_talon0.configEncoderCodesPerRev(1440)
self.right_talon0.configEncoderCodesPerRev(1440)
#Reverse left talon
self.left_talon0.setInverted(True)
self.right_talon0.setInverted(False)
#Climber
self.climber_motor = wpilib.Spark(0)
self.climber_2 = wpilib.Spark(1)
#Sensors
self.left_enc = encoder.Encoder(self.left_talon0)
self.right_enc = encoder.Encoder(self.right_talon0, True)
#Controls
self.left_joystick = wpilib.Joystick(0)
self.right_joystick = wpilib.Joystick(1)
self.climber_joystick = wpilib.Joystick(2)
self.buttons = unifiedjoystick.UnifiedJoystick(
[self.left_joystick, self.right_joystick])
self.last_button_state = False
#Bling
self.leds = ledstrip.LEDStrip()
#Autonomous Placement
self.auto_positions = wpilib.SendableChooser()
self.auto_positions.addDefault("Position 1", 1)
self.auto_positions.addObject("Position 2", 2)
self.auto_positions.addObject("Position 3", 3)
SmartDashboard.putData("auto_position", self.auto_positions)
#SD variables
SmartDashboard.putNumber("Vision/Turn", 0)
SmartDashboard.putBoolean("Reversed", True)
#LiveWindow
LiveWindow.addActuator("Drive", "Left Master Talon", self.left_talon0)
LiveWindow.addActuator("Drive", "Right Master Talon",
self.right_talon0)
def __init__(self, components):
''''''
self.ds = wpilib.DriverStation.GetInstance()
self.modes = {}
self.active_mode = None
print("AutonomousModeManager::__init__() Begins")
# load all modules in the current directory
modules_path = os.path.dirname(os.path.abspath(__file__))
sys.path.append(modules_path)
modules = glob(os.path.join(modules_path, '*.py'))
for module_filename in modules:
module_name = os.path.basename(module_filename[:-3])
if module_name in ['__init__', 'manager']:
continue
try:
module = imp.load_source(module_name, module_filename)
except:
if not self.ds.IsFMSAttached():
raise
#
# Find autonomous mode classes in the modules that are present
# -> note that we actually create the instance of the objects here,
# so that way we find out about any errors *before* we get out
# on the field..
for name, obj in inspect.getmembers(module, inspect.isclass):
if hasattr(obj, 'MODE_NAME'):
# don't allow the driver to select this mode
if hasattr(obj, 'DISABLED') and obj.DISABLED:
print(
"Warning: autonomous mode %s is marked as disabled"
% obj.MODE_NAME)
continue
try:
instance = obj(components)
except:
if not self.ds.IsFMSAttached():
raise
else:
continue
if instance.MODE_NAME in self.modes:
if not self.ds.IsFMSAttached():
raise RuntimeError(
"Duplicate name %s in %s" %
(instance.MODE_NAME, module_filename))
print(
"ERROR: Duplicate name %s specified by object type %s in module %s"
% (instance.MODE_NAME, name, module_filename))
self.modes[name + '_' + module_filename] = instance
else:
self.modes[instance.MODE_NAME] = instance
# now that we have a bunch of valid autonomous mode objects, let
# the user select one using the SmartDashboard.
# SmartDashboard interface
sd = wpilib.SmartDashboard
self.chooser = wpilib.SendableChooser()
default_modes = []
print("Loaded autonomous modes:")
for k, v in sorted(self.modes.items()):
if hasattr(v, 'DEFAULT') and v.DEFAULT == True:
print(" -> %s [Default]" % k)
self.chooser.AddDefault(k, v)
default_modes.append(k)
else:
print(" -> %s" % k)
self.chooser.AddObject(k, v)
if len(self.modes) == 0:
print("-- no autonomous modes were loaded!")
# provide a none option
self.chooser.AddObject('None', None)
if len(default_modes) == 0:
self.chooser.AddDefault('None', None)
elif len(default_modes) != 1:
if not self.ds.IsFMSAttached():
raise RuntimeError(
"More than one autonomous mode was specified as default! (modes: %s)"
% (', '.join(default_modes)))
# must PutData after setting up objects
sd.PutData('Autonomous Mode', self.chooser)
print("AutonomousModeManager::__init__() Done")
def __init__(self):
# Single Time Initialization
Subsystem.__init__(self, "Gyroscope")
# Default values
# self.gyro = ADIS16470_IMU()
self.gyro = ADXRS450_Gyro
self.accel = ADXL345_SPI
# self.m_yawSelected = kYaw_default
self.m_runCal = False
self.m_configCal = False
self.m_reset = False
# self.m_yawActiveAxis = self.gyro.IMUAxis.kz
# Adds Options. Very Helpful.
self.m_autoChooser = wpilib.SendableChooser()
self.m_autoChooser.addOption(kautoname_custom, kautoname_custom)
self.m_yawChooser = wpilib.SendableChooser()
self.m_yawChooser.setDefaultOption(self.kYaw_default,
self.kYaw_default)
# self.m_yawchooser.addOption(kYaw_xaxis, kYaw_xAxis)
self.m_yawchooser.addOption(kYaw_yAxis, kYaw_yAxis)
# SmartDashBoard Statistics
wpilib.SmartDashboard.putBoolean("Config Calibration", False)
wpilib.SmartDashboard.putBoolean("Run Calibration", False)
wpilib.SmartDashboard.putBoolean("Reset Gyro", False)
wpilib.SmartDashboard.putBoolean("Set Current Axis", False)
def gyroControls(self):
# wpilib.SmartDashboard.putNumber("Yaw/Z Angle", self.m_imu.getAngle())
# wpilib.SmartDashboard.putNumber("X Comp Angle", self.m_imu.getXComplimentaryAngle())
# wpilib.SmartDashboard.putNumber("Y Comp Angle", self.m_imu.getYComplimentaryAngle())
wpilib.SmartDashboard.putNumber("Angle", self.gyro.GetAngle())
# self.m_yawSelected = kYawDefault
wpilib.SmartDashboard.putNumber("Yaw/Z Angle",
self.m_imu.getAngle())
wpilib.SmartDashboard.putNumber(
"X Comp Angle", self.m_imu.getXComplimentaryAngle())
wpilib.SmartDashboard.putNumber(
"Y Comp Angle", self.m_imu.getYComplimentaryAngle())
self.m_yawSelected = kYaw_default
# Set IMU Settings
if (self.m_configCal):
# self.configCalTime(self.imu._8s)
self.m_configCal = wpilib.SmartDashboard.putBoolean(
"Config Calibration", False)
if (self.m_reset):
# self.m_imu.reset()
self.m_reset = wpilib.SmartDashboard.putBoolean(
"Reset Gyro", False)
if (self.m_runCal):
# self.m_imu.Calibrate()
self.m_runCal = wpilib.Smartdashboard.putBoolean(
"Run Calibration", False)
# Read the Axis you want to read
# Sendable Chooser allows you to change the value, hence changing what is displayed.
if (self.m_yawSelected == "X-Axis"):
self.m_yawActiveAxis = self.m_imu.IMUAxis.kX
elif (self.m_yawSelected == "Y-Axis"):
self.m_yawActiveAxis = self.m_imu.IMUAxis.kY
else:
# self.m_yawActiveAxis = self.m_imu.IMUAxis.kZ
pass
# Set the Axis you want to read
if (self.m_setYawAxis):
self.m_setYawAxis = wpilib.SmartDashboard.putBoolean(
"setYawAxis", False)
"""
def robotInit(self):
"""
WPILIB method on initialization
"""
# instansiate a getter method so you can do 'import robot;
# robot.get_robot()'
global get_robot
try:
wpilib.CameraServer.launch('cameraservant.py:main')
except:
print("Could not find module cscore")
get_robot = self.get_self
self.num_loops = 0
subsystems.init()
self.autonomousProgram = PulseMotor()
self.chooser = wpilib.SendableChooser()
#self.chooser.addDefault("SEQUENCE", Sequence())
self.chooser.addObject("Go 1 meter forward", DriveToDistance(1, 1))
self.chooser.addObject("Turn 90 Degrees Clockwise", TurnDrive(90))
# self.chooser.addObject("Turn Arm Horizontal", LiftToProportion(robotmap.measures.ROBOT_ARM_HORIZONTAL))
self.chooser.addObject("Do Nothing Auto", DoNothing(15))
self.chooser.addObject("Grabber(True)", Grabber(True))
self.chooser.addObject("Grabber(False)", Grabber(False))
#self.chooser.addObject("ParametricLine", ParametricDrive(lambda t: .1 * t, lambda t: .4 * t, 5))
# The Auto Line is 10 ft (~3.048 meters) from the start point. May have to be tweaked a bit.
self.chooser.addObject("Drive Forward", DriveToDistance(3.048, 3.048))
self.chooser.addObject("Switch Back (inplace)", SwitchBack_inplace())
self.chooser.addObject("COMP: Left", "l")
self.chooser.addObject("COMP: Middle", "m")
self.chooser.addObject("COMP: Right", "r")
self.chooser.addDefault("COMP: Minimal Auto",
DriveToDistance(3.048, 3.048))
self.chooser.addObject("COMP: Do Nothing", DoNothing(15))
self.auto_data = None
#self.chooser.addObject('PulseMotor', PulseMotor())
wpilib.SmartDashboard.putData('Autonomous Program', self.chooser)
self.teleopProgram = wpilib.command.CommandGroup()
self.teleopProgram.addParallel(PIDTankDriveJoystick())
#self.teleopProgram.addParallel(TankDriveJoystick())
# self.teleopProgram.addParallel(ArmExtender())
self.teleopProgram.addParallel(ArmRotate())
#self.teleopProgram.addParallel(NavXCommand())
#self.teleopProgram.addParallel(CorrectTip())
oi.init()
