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()