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