def __init__(self): super().__init__('Drivetrain') #The set motor controllers for this years robot and how motors are coded self.motor_rb = ctre.WPI_TalonSRX(1) self.motor_rf = ctre.WPI_VictorSPX(17) self.motor_lb = ctre.WPI_TalonSRX(13) self.motor_lf = ctre.WPI_VictorSPX(12) self.motor_rf.follow(self.motor_rb) self.motor_lf.follow(self.motor_lb) self.motors = [self.motor_rb, self.motor_lb, self.motor_rf, self.motor_lf] self.drive = wpilib.drive.DifferentialDrive(self.motor_rb, self.motor_lb) self.navx = navx.AHRS.create_spi() self.pdp = wpilib.PowerDistributionPanel(16) self.pdp2 = wpilib.PowerDistributionPanel(16) self.motor_lb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0) self.motor_rb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) self.navx_table = networktables.NetworkTables.getTable('/Sensor/Navx') self.leftEncoder_table = networktables.NetworkTables.getTable("/Encoder/Left") self.rightEncoder_table = networktables.NetworkTables.getTable("/Encoder/Right") self.leftError = networktables.NetworkTables.getTable("/TalonL/Error") self.rightError = networktables.NetworkTables.getTable("/TalonR/Error") self.motor_lb.setSensorPhase(True) self.motor_rb.setSensorPhase(True) self.timer = wpilib.Timer() self.timer.start() self.mode = "" self.computed_velocity = 0 self.logger = None
def __init__(self): super().__init__("Sensors") #self.navx = NavX(navx_type) # self.navx = None wpilib.LiveWindow.addSensor("Sensors", "PDP", wpilib.PowerDistributionPanel(0)) self.ultrasonic = wpilib.AnalogInput(0)
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): bl = wpilib.CANTalon(1) fl = wpilib.CANTalon(3) fr = wpilib.CANTalon(0) br = wpilib.CANTalon(2) self.talons = [bl, fl, fr, br] self.pdp = wpilib.PowerDistributionPanel()
def __init__(self): super().__init__('Drivetrain') #The set motor controllers for this years robot and how motors are coded self.motor_rb = ctre.WPI_TalonSRX(1) self.motor_rf = ctre.WPI_VictorSPX(17) self.motor_lb = ctre.WPI_TalonSRX(13) self.motor_lf = ctre.WPI_VictorSPX(12) self.motor_rf.follow(self.motor_rb) self.motor_lf.follow(self.motor_lb) self.motors = [self.motor_rb, self.motor_lb, self.motor_rf, self.motor_lf] self.drive = wpilib.drive.DifferentialDrive(self.motor_rb, self.motor_lb) self.navx = navx.AHRS.create_spi() self.pdp = wpilib.PowerDistributionPanel(16) self.motor_lb.setNeutralMode(ctre.NeutralMode.Brake) self.motor_rb.setNeutralMode(ctre.NeutralMode.Brake) self.motor_rf.setNeutralMode(ctre.NeutralMode.Brake) self.motor_lf.setNeutralMode(ctre.NeutralMode.Brake) self.motor_lb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0) self.motor_rb.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, 0) self.motor_rb.selectProfileSlot(0, 0) self.motor_lb.selectProfileSlot(0, 0) self.table = networktables.NetworkTables.getTable("/Drivetrain") self.sd_table = networktables.NetworkTables.getTable("/SmartDashboard") self.motor_lb.setSensorPhase(False) self.motor_rb.setSensorPhase(False) self.left_offset = 0 self.right_offset = 0 self.timer = wpilib.Timer() self.timer.start() self.computed_velocity = 0 self.logger = None
def __init__(self): super().__init__("HMS") self.warnVoltage = 10 self.critVoltage = 9 self.robot = team3200.getRobot() self.count = 0 self.pdp = wpilib.PowerDistributionPanel()
def __init__(self, channels): """ ``channels`` is a list of channel numbers (0 - 15) to monitor. """ self.channels = channels self.pdp = wpilib.PowerDistributionPanel() # updates every 0.8 seconds self.logStates = [LogState("PDP " + str(c), 0.8) for c in channels]
def createObjects(self): # Inputs # TODO: Update these dynamically self.stick = wpilib.Joystick(2) # self.gamepad = wpilib.XboxController(1) # self.gamepad_alt = wpilib.XboxController(2) self.gamepad = wpilib.XboxController(0) self.gamepad_alt = wpilib.XboxController(1) # Drive motors self.left_motor = wpilib.Spark(0) self.right_motor = wpilib.Spark(1) self.drive_train = wpilib.drive.DifferentialDrive( self.left_motor, self.right_motor) self.drive_train.deadband = .04 # Elevator encoder (gearbox) self.lift_encoder = ExternalEncoder(0, 1) # TODO: Fix the pid # self.lift_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION) # Drive encoders self.left_encoder = ExternalEncoder(2, 3) self.right_encoder = ExternalEncoder(4, 5, True) self.left_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION) self.right_encoder.encoder.setDistancePerPulse(WHEEL_REVOLUTION) # Elevator motors self.lift_master = CANTalon(2) self.lift_follower1 = CANTalon(3) self.lift_follower2 = CANTalon(4) self.lift_follower1.follow(self.lift_master) self.lift_follower2.follow(self.lift_master) # Intake motors self.left_intake_motor = wpilib.Spark(2) self.right_intake_motor = wpilib.Spark(3) self.right_intake_motor.setInverted(True) # Intake grabbers self.grabber_solenoid = wpilib.DoubleSolenoid(1, 0, 1) # PDP self.pdp = wpilib.PowerDistributionPanel(0) wpilib.SmartDashboard.putData("PowerDistributionPanel", self.pdp) # Misc self.navx = navx.AHRS.create_spi() self.net_table = NetworkTables.getTable("SmartDashboard") self.vision_table = NetworkTables.getTable("limelight") self.limelight_x = self.vision_table.getEntry("tx") self.limelight_valid = self.vision_table.getEntry("tv") self.dashboard_has_target = self.vision_table.getEntry("hastarget") # Launch camera server wpilib.CameraServer.launch()
def __init__(self, sensors, joystick): self.openIntake = wpilib.Solenoid(robot_map.OPEN_INTAKE) self.closeIntake = wpilib.Solenoid(robot_map.CLOSE_INTAKE) self.rightIntake = wpilib.Talon(robot_map.RIGHT_INTAKE) self.leftIntake = wpilib.Talon(robot_map.LEFT_INTAKE) self.sensors = sensors self.joystick = joystick self.pdp = wpilib.PowerDistributionPanel()
def createObjects(self): """ This is where all the components are actually created with "=" sign. Components with a parent prefix like "shooter_" will be injected. """ # SmartDashboard self.sd = NetworkTables.getTable('SmartDashboard') # Gamepad self.gamempad = wpilib.Joystick(0) self.gamempad2 = wpilib.Joystick(1) # Drive Motors self.frontLeftModule_driveMotor = ctre.WPI_VictorSPX(5) self.frontRightModule_driveMotor = ctre.WPI_VictorSPX(8) self.rearLeftModule_driveMotor = ctre.WPI_VictorSPX(4) self.rearRightModule_driveMotor = ctre.WPI_VictorSPX(9) # Rotate Motors self.frontLeftModule_rotateMotor = ctre.WPI_VictorSPX(3) self.frontRightModule_rotateMotor = ctre.WPI_VictorSPX(14) self.rearLeftModule_rotateMotor = ctre.WPI_VictorSPX(2) self.rearRightModule_rotateMotor = ctre.WPI_VictorSPX(15) # Encoders self.frontLeftModule_encoder = wpilib.AnalogInput(0) self.frontRightModule_encoder = wpilib.AnalogInput(3) self.rearLeftModule_encoder = wpilib.AnalogInput(1) self.rearRightModule_encoder = wpilib.AnalogInput(2) # Shooter self.shooter_leftShooterMotor = ctre.WPI_VictorSPX(6) self.shooter_rightShooterMotor = ctre.WPI_VictorSPX(7) self.shooter_beltMotor = ctre.WPI_VictorSPX(11) self.shooter_intakeMotor = ctre.WPI_VictorSPX(0) # Wheel of Fortune self.wof_motor = ctre.WPI_VictorSPX(13) # Climber self.climbingMotor = ctre.WPI_VictorSPX(10) self.hookMotor = ctre.WPI_VictorSPX(1) # Color Sensor self.colorSensor = color_sensor.ColorSensor() # Vision self.vision = vision.Vision() # Limit Switch self.switch = wpilib.DigitalInput(0) # PDP self.pdp = wpilib.PowerDistributionPanel(0)
def __init__(self): super().__init__('Intake') self.intake_motor_closeOpen = wpilib.VictorSP(8) self.intake_motor_rightWheel = wpilib.VictorSP(9) self.intake_motor_leftWheel = wpilib.VictorSP(7) self.limit_switch = wpilib.DigitalOutput(0) self.pdp = wpilib.PowerDistributionPanel(16) self.intake_table = networktables.NetworkTables.getTable('/Intake') self.timer = wpilib.Timer() self.timer.start() self.logger = None
def __init__(self): super().__init__() # Initialize motors. self.elevatorMotor1 = ctre.wpi_talonsrx.WPI_TalonSRX( robotMap.elevatorMotor1) self.elevatorMotor2 = ctre.wpi_talonsrx.WPI_TalonSRX( robotMap.elevatorMotor2) self.elevatorMotor2.follow(self.elevatorMotor1) self.PDP = wpilib.PowerDistributionPanel()
def __init__(self, setpoint): super().__init__(p=.003, i=0.0001, d=0.06, period=.010) self.drivetrain = self.getRobot().drivetrain self.requires(self.drivetrain) pid = self.getPIDController() pid.setInputRange(minimumInput=-180, maximumInput=180) pid.setOutputRange(minimumOutput=-1, maximumOutput=1) self.setSetpoint(setpoint) self.logger = None self.joystick = getJoystick() self.pdp = wpilib.PowerDistributionPanel(16)
def robotInit(self): #NetworkTables Init NetworkTables.initialize() self.table = NetworkTables.getTable("SmartDashboard") #Navx self.navx = navx.AHRS.create_spi() #PowerDistributionPanel self.power_board = wpilib.PowerDistributionPanel() #Motors self.leftMotor1 = wpilib.Spark( 0 ) # <-- This is what links our PWM port on the CRIO to a physical ESC. self.leftMotor2 = wpilib.Spark( 1 ) # <-- This is what links our PWM port on the CRIO to a physical ESC. self.left = wpilib.SpeedControllerGroup(self.leftMotor1, self.leftMotor2) self.rightMotor1 = wpilib.Spark(2) self.rightMotor2 = wpilib.Spark(3) self.right = wpilib.SpeedControllerGroup(self.rightMotor1, self.rightMotor2) self.liftMotor1 = wpilib.Talon(4) self.liftMotor2 = wpilib.Talon(5) self.liftMotor2.setInverted(True) self.lift = wpilib.SpeedControllerGroup(self.liftMotor1, self.liftMotor2) #User Input self.playerOne = wpilib.XboxController( 0) # <-- This is for using Xbox controllers #Drive self.robotDrive = wpilib.drive.DifferentialDrive(self.left, self.right) #Drive.py init self.drive = drive.Drive(self.robotDrive, self.navx, self.left, self.right) self.components = {'drive': self.drive, 'table': self.table} self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self): Command.getRobot = lambda x = 0: self self.driverStation = DriverStation.getInstance() self.motorHelper = motorHelper self.robotMap = robotMap.RobotMap() self.createNetworkTables() self.createControllers() self.drivetrain = subsystems.driveTrain.DriveTrain() self.loader = subsystems.loader.Loader() self.timer = wpilib.Timer() self.loaderButton = wpilib.buttons.JoystickButton(self.auxController, self.robotMap.controllerMap.auxController['loaderToggleButton']) self.loaderButton.whenPressed(commands.loaderCommand.LoaderToggle()) self.pdp = wpilib.PowerDistributionPanel() self.healthMonitor = subsystems.driveTrain.HealthMonitor() self.lifter=subsystems.lifter.Lifter() #make robot avaiable to commands wpilib.CameraServer.launch('vision.py:main') self.smartDashboard.putString('field position' ,"Enter L, R, or C,N") self.loader.setLoader(self.loader.State.kOpen)
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): # devices self.controller = wpilib.XboxController(0) self.buttonBoard = wpilib.Joystick(1) ahrs = navx.AHRS.create_spi() self.pdp = wpilib.PowerDistributionPanel(50) self.ledStrip = wpilib.PWM(0) self.ledInput = -0.99 self.superDrive = drivetrain.initDrivetrain() self.pathFollower = sea.PathFollower(self.superDrive, ahrs) # for autonomous mode self.autoScheduler = autoScheduler.AutoScheduler() self.autoScheduler.idleFunction = self.autoIdle self.autoScheduler.updateCallback = self.updateScheduler # controls the state of the robot self.controlModeMachine = sea.StateMachine() self.manualState = sea.State(self.driving) self.autoState = sea.State(self.autoScheduler.runSchedule) self.testState = sea.State(self.testing) # for shifting gear box self.compressor = wpilib.Compressor(0) self.piston1 = wpilib.DoubleSolenoid(0, 1) self.piston2 = wpilib.DoubleSolenoid(2, 3) # drive gears self.superDrive.gear = None self.driveGear = drivetrain.mediumVoltageGear self.driveMode = "velocity" self.driveSpeed = "medium" self.driveGears = \ {"voltage" : \ {"slow" : drivetrain.slowVoltageGear, "medium" : drivetrain.mediumVoltageGear, "fast" : drivetrain.fastVoltageGear}, "velocity" : \ {"slow" : drivetrain.slowVelocityGear, "medium" : drivetrain.mediumVelocityGear, "fast" : drivetrain.fastVelocityGear}, "position" : \ {"slow" : drivetrain.slowPositionGear, "medium" : drivetrain.mediumPositionGear, "fast" : drivetrain.fastPositionGear} } # testing self.testSettings = { \ "wheelNum" : 0, "motorNum" : 0, "speed" : 0 } # for dashboard motor data self.motorData = [dict() for _ in range(len(self.superDrive.motors))] # sets initial values so the dashboard doesn't break when it tries to get # them before the values are updated in self.updateMotorData for motor in range(len(self.superDrive.motors)): initAmps = round(self.superDrive.motors[motor].getOutputCurrent(), 2) initTemp = round(self.superDrive.motors[motor].getMotorTemperature(), 2) self.motorData[motor]["amps"] = initAmps self.motorData[motor]["temp"] = initTemp self.motorData[motor]["maxAmp"] = initAmps self.motorData[motor]["maxTemp"] = initTemp self.app = None sea.startDashboard(self, dashboard.CompetitionDashboard)
def create_power_distribution_panel(module=0): return wpilib.PowerDistributionPanel(module)
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ # joystick 1 on the driver station self.stick = wpilib.XboxController(0) self.driverStation = wpilib.DriverStation self.frontRight = ctre.wpi_talonsrx.WPI_TalonSRX(3) self.rearRight = ctre.wpi_talonsrx.WPI_TalonSRX(1) self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight) self.frontLeft = ctre.wpi_talonsrx.WPI_TalonSRX(4) self.rearLeft = ctre.wpi_talonsrx.WPI_TalonSRX(2) self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft) self.frontRight.setExpiration(0.2) self.rearRight.setExpiration(0.2) self.frontRight.setExpiration(0.2) self.rearLeft.setExpiration(0.2) self.drive = DifferentialDrive(self.left, self.right) # initialize motors other than drive self.intakeRight = wpilib.VictorSP(0) self.elevator = ctre.wpi_talonsrx.WPI_TalonSRX( 5) # Talon SRX controller with CAN address 5 self.intakeLeft = wpilib.VictorSP(2) self.battleAxe = wpilib.VictorSP(3) self.actuator = wpilib.Spark(4) self.axeExtender = wpilib.Spark(5) ###################################### self.encoderTicksPerInch = 1159 self.elevator.setQuadraturePosition(0, 0) self.elevator.configForwardSoftLimitThreshold( int(round(-0.1 * self.encoderTicksPerInch)), 10) self.elevator.configReverseSoftLimitThreshold( int(round(-39.5 * self.encoderTicksPerInch)), 10) self.elevator.configForwardSoftLimitEnable(True, 10) self.elevator.configReverseSoftLimitEnable(True, 10) self.elevator.configPeakOutputForward(0.8, 10) self.elevator.configPeakOutputReverse(-1, 10) self.elevator.set(ctre.ControlMode.Position, 0) self.elevator.selectProfileSlot(0, 0) self.elevator.config_kF(0, 0, 10) self.elevator.config_kP(0, 0.6, 10) self.elevator.config_kI(0, 0.003, 10) self.elevator.config_kD(0, 0, 10) self.elevator.config_IntegralZone(0, 100, 10) self.elevator.configAllowableClosedloopError( 0, int(round(0.01 * self.encoderTicksPerInch)), 10) # initialize limit switches and hall-effect sensors self.actuatorSwitchMin = wpilib.DigitalInput(0) self.actuatorSwitchMax = wpilib.DigitalInput(1) self.battleAxeSwitchUp = wpilib.DigitalInput(2) self.battleAxeSwitchDown = wpilib.DigitalInput(3) self.battleAxeExtenderSwitch = wpilib.DigitalInput(4) self.elevatorZeroSensor = wpilib.DigitalInput(5) self.powerDistributionPanel = wpilib.PowerDistributionPanel() self.powerDistributionPanel.resetTotalEnergy() # # Communicate w/navX MXP via the MXP SPI Bus. # - Alternatively, use the i2c bus. # See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details # self.navxSensor = navx.AHRS.create_spi() # self.navxSensor = navx.AHRS.create_i2c() # Items in this dictionary are available in your autonomous mode # as attributes on your autonomous object self.components = { 'drive': self.drive, 'navxSensor': self.navxSensor, 'actuator': self.actuator, 'actuatorSwitchMin': self.actuatorSwitchMin, 'actuatorSwitchMax': self.actuatorSwitchMax, 'elevator': self.elevator, 'intakeRight': self.intakeRight, 'intakeLeft': self.intakeLeft, 'gameData': self.driverStation.getInstance() } # * The first argument is the name of the package that your autonomous # modes are located in # * The second argument is passed to each StatefulAutonomous when they # start up self.automodes = AutonomousModeSelector('autonomous', self.components) NetworkTables.initialize() self.sd = NetworkTables.getTable("SmartDashboard") wpilib.CameraServer.launch('vision.py:main')
def robotInit(self): #Networktables self.netTable = NetworkTables.getTable('SmartDashboard') #Hud Data Handlers self.statUpdater = SU.StatusUpdater(self, self.netTable) #Camera Server wpilib.CameraServer.launch() #Drive Motors self.motor1 = ctre.WPI_TalonSRX(1) self.motor2 = ctre.WPI_TalonSRX(2) self.motor3 = ctre.WPI_TalonSRX(9) self.motor4 = ctre.WPI_TalonSRX(10) #Intake Motors self.stage1Left = ctre.WPI_TalonSRX(5) self.stage1Right = ctre.WPI_TalonSRX(6) self.stage2Left = ctre.WPI_TalonSRX(4) self.stage2Right = ctre.WPI_TalonSRX(7) self.stage3Left = ctre.WPI_TalonSRX(3) self.stage3Right = ctre.WPI_TalonSRX(8) #Pan Arm Controls self.leftPanArm = wpilib.PWMVictorSPX(0) self.rightPanArm = wpilib.PWMVictorSPX(1) #Shifters self.shifter = wpilib.DoubleSolenoid(1, 2) #Climb self.pto = wpilib.DoubleSolenoid(3, 4) self.climbLift = wpilib.Solenoid(5) #User Inputs self.playerOne = wpilib.XboxController(0) self.playerTwo = wpilib.XboxController(1) #Navx self.navx = navx.AHRS.create_spi() self.power_board = wpilib.PowerDistributionPanel() #User Inputs self.playerOne = wpilib.XboxController(0) self.playerTwo = wpilib.XboxController(1) #Navx self.navx = navx.AHRS.create_spi() #Points #self.points = [] #Setup Logic self.rightDriveMotors = wpilib.SpeedControllerGroup( self.motor3, self.motor4) self.leftDriveMotors = wpilib.SpeedControllerGroup( self.motor1, self.motor2) self.leftDriveMotors.setInverted(True) self.robotDrive = DifferentialDrive(self.leftDriveMotors, self.rightDriveMotors) self.lowerIntakeMotors = wpilib.SpeedControllerGroup( self.stage1Left, self.stage1Right, self.stage2Left, self.stage2Right) self.stage3 = wpilib.SpeedControllerGroup(self.stage3Left, self.stage3Right) if wpilib.SolenoidBase.getPCMSolenoidVoltageStickyFault(0) == True: wpilib.SolenoidBase.clearAllPCMStickyFaults(0) self.pto.set(wpilib.DoubleSolenoid.Value.kReverse) #Drive.py init self.drive = drive.Drive(self.robotDrive, self.navx, self.motor1, self.motor2, self.motor3, self.motor4, self.shifter) #Intake.py self.intake = intake.Intake(self.lowerIntakeMotors, self.stage3, self.leftPanArm, self.rightPanArm) #Driver Station Instance self.driverStation = wpilib.DriverStation.getInstance() #Auto mode variables self.components = {'drive': self.drive, 'intake': self.intake} self.automodes = AutonomousModeSelector('autonomous', self.components)
def robotInit(self): ''' Initialization of robot systems. ''' logging.info( '-( ͡° ͜ʖ ͡°)╯╲_-^o=o\ \"Don\'t mind me, just walking the robot.\"' ) from wpilib.drive import DifferentialDrive from ctre import WPI_TalonSRX, WPI_VictorSPX # drive train motors self.frontRightMotor = WPI_TalonSRX(4) self.rearRightMotor = WPI_TalonSRX(3) self.frontLeftMotor = WPI_TalonSRX(1) self.rearLeftMotor = WPI_TalonSRX(2) # lift encoder construction self.liftEncoder = wpilib.Encoder(8, 9) # liftArm encoder construction self.liftArmEncoder = wpilib.Encoder(5, 6) # drive train motor groups assignment self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) # drive train drive group assignment self.drive = DifferentialDrive(self.left, self.right) self.drive.setExpiration(0.1) # lift motor system initialization self.liftOne = WPI_VictorSPX(1) self.liftTwo = WPI_VictorSPX(2) self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo) # lift arm motor system initialization self.liftArmOne = WPI_VictorSPX(3) self.liftArmTwo = WPI_VictorSPX(4) self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne, self.liftArmTwo) # cargo intake motor initialization self.cargo = WPI_VictorSPX(5) # game and joystick controller construction # joystick - 0, 1 | controller - 2 self.leftStick = wpilib.Joystick(0) self.rightStick = wpilib.Joystick(1) self.xbox = wpilib.Joystick(2) self.buttonBox = wpilib.Joystick(3) # pneumatic and compressor system initialization self.Compressor = wpilib.Compressor(0) self.Compressor.setClosedLoopControl(True) self.enable = self.Compressor.getPressureSwitchValue() self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2, 3) # hatch panel claw self.DoubleSolenoidThree = wpilib.DoubleSolenoid( 4, 5) # hatch panel ejection self.Compressor.start() # Smart Dashboard and NetworkTables initialization and construction self.PDP = wpilib.PowerDistributionPanel() self.roboController = wpilib.RobotController() self.DS = wpilib.DriverStation.getInstance() # proximity detection sensors self.Hall = wpilib.DigitalInput(7) self.ultrasonic = wpilib.AnalogInput(2) self.cargoUltrasonic = wpilib.AnalogInput(3) # timer construction self.timer = wpilib.Timer() # initialization of the HTTP camera wpilib.CameraServer.launch('vision.py:main') self.sd.putString("", "Top Camera") self.sd.putString(" ", "Bottom Camera") from sensors import REV_Color_Sensor_V2 # Initialization and configuration of I2C interface with color sensor. self.colorSensor = REV_Color_Sensor_V2(wpilib.I2C.Port.kOnboard)
def robotInit(self): #Watchdog #self.watchdog = wpilib.Watchdog #self.watchdog.enable(self) #Onboard Webcam Init wpilib.CameraServer.launch() #NetworkTables Init NetworkTables.initialize() self.networkTable = NetworkTables.getTable("SmartDashboard") #PowerDistributionPanel Init self.pdp = wpilib.PowerDistributionPanel() #Drive Motor Init self.motor1 = ctre.WPI_TalonSRX(1) self.motor2 = ctre.WPI_TalonSRX(2) self.motor3 = ctre.WPI_TalonSRX(3) self.motor4 = ctre.WPI_TalonSRX(4) #Spark Max Init if not wpilib.RobotBase.isSimulation(): self.brushless1 = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.brushless2 = rev.CANSparkMax(6, rev.MotorType.kBrushless) else: self.brushless1 = ctre.WPI_TalonSRX(5) self.brushless2 = ctre.WPI_TalonSRX(6) #Intake Motor Init self.intakeMotors = ctre.WPI_TalonSRX(7) #Ramp Motor Init self.ramp = ctre.WPI_TalonSRX(8) #SpeedControllerGroups Init self.left = wpilib.SpeedControllerGroup(self.motor1, self.motor2) self.right = wpilib.SpeedControllerGroup(self.motor3, self.motor4) self.lift = wpilib.SpeedControllerGroup(self.brushless1, self.brushless2) #Ir Init self.intakeSensor = wpilib.DigitalInput(9) self.outerLeftIR = wpilib.DigitalInput(0) self.leftIR = wpilib.DigitalInput(1) self.rightIR = wpilib.DigitalInput(2) self.outerRightIR = wpilib.DigitalInput(3) #Setting Drive self.robotDrive = wpilib.drive.DifferentialDrive(self.left, self.right) #Controller Init self.playerOne = wpilib.XboxController(0) #Navx Init #self.navx = navx.AHRS.create_spi() self.navx = "Placeholder" #Sensors.py Init self.sensors = sensors.Sensors(self.robotDrive, self.navx, self.left, self.right, self.outerLeftIR, self.leftIR, self.rightIR, self.outerRightIR) #SensorState Init self.SensorState = sensors.SensorState() #Color.py Init self.color = color.PrintColor() #Drive.py Init self.drive = drive.Drive(self.robotDrive, self.navx, self.left, self.right, self.sensors, self.color) #Intake.py Init self.intake = intake.Intake(self.robotDrive, self.playerOne, self.intakeMotors, self.lift, self.intakeSensor) '''
def createObjects(self): """ Initialize robot components. """ # Joysticks self.joystick_left = wpilib.Joystick(0) self.joystick_right = wpilib.Joystick(1) self.joystick_alt = wpilib.Joystick(2) # Buttons self.btn_claw = ButtonDebouncer(self.joystick_left, 1) self.btn_forearm = ButtonDebouncer(self.joystick_right, 1) self.btn_up = JoystickButton(self.joystick_left, 3) self.btn_down = JoystickButton(self.joystick_left, 2) self.btn_climb = JoystickButton(self.joystick_right, 11) # Buttons on alternative joystick self.btn_claw_alt = ButtonDebouncer(self.joystick_alt, 1) self.btn_forearm_alt = ButtonDebouncer(self.joystick_alt, 2) self.btn_climb_alt = JoystickButton(self.joystick_alt, 3) # Buttons for toggling control options and aides self.btn_unified_control = ButtonDebouncer(self.joystick_alt, 8) self.btn_record = ButtonDebouncer(self.joystick_left, 6) self.btn_stabilize = ButtonDebouncer(self.joystick_alt, 12) self.btn_fine_movement = JoystickButton(self.joystick_right, 2) # Drive motor controllers # ID SCHEME: # 10^1: 1 = left, 2 = right # 10^0: 0 = front, 5 = rear self.lf_motor = WPI_TalonSRX(10) self.lr_motor = WPI_TalonSRX(15) self.rf_motor = WPI_TalonSRX(20) self.rr_motor = WPI_TalonSRX(25) # Follow front wheels with rear to save CAN packets self.lr_motor.follow(self.lf_motor) self.rr_motor.follow(self.rf_motor) # Drivetrain self.train = wpilib.drive.DifferentialDrive(self.lf_motor, self.rf_motor) # Winch self.winch_motors = wpilib.SpeedControllerGroup(wpilib.Victor(7), wpilib.Victor(8)) # Motion Profiling self.position_controller = motion_profile.PositionController() # Arm self.elevator = wpilib.Victor(5) self.forearm = wpilib.DoubleSolenoid(2, 3) self.claw = wpilib.DoubleSolenoid(0, 1) # NavX (purple board on top of the RoboRIO) self.navx = navx.AHRS.create_spi() self.navx.reset() # Utility self.ds = wpilib.DriverStation.getInstance() self.timer = wpilib.Timer() self.pdp = wpilib.PowerDistributionPanel(0) self.compressor = wpilib.Compressor() # Camera server wpilib.CameraServer.launch('camera/camera.py:main')
def robotInit(self): ''' Initialization of robot objects. ''' ''' Talon SRX Initialization ''' # drive train motors self.frontRightMotor = WPI_TalonSRX(4) self.rearRightMotor = WPI_TalonSRX(3) self.frontLeftMotor = WPI_TalonSRX(1) self.rearLeftMotor = WPI_TalonSRX(2) ''' Motor Groups ''' # drive train motor groups self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) # drive train drive group self.drive = DifferentialDrive(self.left, self.right) self.drive.setExpiration(0.1) ''' Victor SPX Initialization ''' # lift motors self.liftOne = WPI_VictorSPX(1) self.liftTwo = WPI_VictorSPX(2) self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo) # lift arm motors self.liftArmOne = WPI_VictorSPX(3) self.liftArmTwo = WPI_VictorSPX(4) self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne, self.liftArmTwo) # cargo intake motor self.cargo = WPI_VictorSPX(5) ''' Encoders ''' # drive train encoders self.rightEncoder = self.frontRightMotor self.leftEncoder = self.frontLeftMotor # lift encoder self.liftEncoder = wpilib.Encoder(8, 9) # liftArm encoder self.liftArmEncoder = wpilib.Encoder(5, 6, True) ''' Sensors ''' # Hall Effect Sensor self.minHall = wpilib.DigitalInput(7) self.maxHall = wpilib.DigitalInput(4) self.limitSwitch = wpilib.DigitalInput(3) self.ultrasonic = wpilib.AnalogInput(2) self.cargoUltrasonic = wpilib.AnalogInput(3) ''' Controller Initialization and Mapping ''' # joystick - 0, 1 | controller - 2 self.joystick = wpilib.Joystick(1) self.xbox = wpilib.Joystick(2) ''' Pneumatic Button Status ''' self.clawButtonStatus = Toggle(self.xbox, 2) self.gearButtonStatus = Toggle(self.joystick, 1) self.ejectorPinButtonStatus = Toggle(self.xbox, 1) self.compressorButtonStatus = Toggle(self.xbox, 9) self.liftHeightButtonStatus = Toggle(self.xbox, 3) ''' Pneumatic Initialization ''' self.Compressor = wpilib.Compressor(0) self.Compressor.setClosedLoopControl(True) self.enable = self.Compressor.getPressureSwitchValue() self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2, 3) # hatch panel claw self.DoubleSolenoidThree = wpilib.DoubleSolenoid( 4, 5) # hatch panel ejection self.Compressor.start() ''' Smart Dashboard ''' # connection for logging & Smart Dashboard logging.basicConfig(level=logging.DEBUG) self.sd = NetworkTables.getTable('SmartDashboard') NetworkTables.initialize(server='10.55.49.2') self.sd.putString(" ", "Connection") # Smart Dashboard classes self.PDP = wpilib.PowerDistributionPanel() self.roboController = wpilib.RobotController() self.DS = wpilib.DriverStation.getInstance() ''' Timer ''' self.timer = wpilib.Timer() ''' Camera ''' # initialization of the HTTP camera wpilib.CameraServer.launch('vision.py:main') self.sd.putString("", "Top Camera") self.sd.putString(" ", "Bottom Camera") ''' PID settings for lift ''' self.kP = 0.03 self.kI = 0.0 self.kD = 0.0 self.kF = 0.1 self.PIDLiftcontroller = wpilib.PIDController(self.kP, self.kI, self.kD, self.kF, self.liftEncoder, output=self) self.PIDLiftcontroller.setInputRange(0, 400) self.PIDLiftcontroller.setOutputRange(-0.5, 0.5) self.PIDLiftcontroller.setAbsoluteTolerance(1.0) self.PIDLiftcontroller.setContinuous(True) self.encoderRate = 0
def robotInit(self): """ Motors """ if not wpilib.RobotBase.isSimulation(): import ctre self.motor1 = ctre.CANTalon(1) #Drive Motors self.motor2 = ctre.CANTalon(2) self.motor3 = ctre.CANTalon(3) self.motor4 = ctre.CANTalon(4) else: self.motor1 = wpilib.Talon(1) #Drive Motors self.motor2 = wpilib.Talon(2) self.motor3 = wpilib.Talon(3) self.motor4 = wpilib.Talon(4) self.climb1 = wpilib.VictorSP(7) self.climb2 = wpilib.VictorSP(8) """ Spike Relay for LED """ self.ledRing = wpilib.Relay( 0, wpilib.Relay.Direction.kForward) #Only goes forward voltage """ Sensors """ self.navx = navx.AHRS.create_spi() #the Gyro self.psiSensor = wpilib.AnalogInput(2) self.powerBoard = wpilib.PowerDistributionPanel(0) #Might need or not self.ultrasonic = wpilib.Ultrasonic(5, 4) #trigger to echo self.ultrasonic.setAutomaticMode(True) self.encoder = wpilib.Encoder(2, 3) self.switch = wpilib.DigitalInput(6) self.servo = wpilib.Servo(0) self.joystick = wpilib.Joystick(0) #xbox controller wpilib.CameraServer.launch('misc/vision.py:main') """ Buttons """ self.visionEnable = wpilib.buttons.JoystickButton(self.joystick, 7) #X button self.gearPistonButton = wpilib.buttons.JoystickButton(self.joystick, 5) self.safetyPistonButton = wpilib.buttons.JoystickButton( self.joystick, 3) self.controlSwitch = button_debouncer.ButtonDebouncer( self.joystick, 8, period=0.5) #Controll switch init for auto lock direction self.driveControlButton = button_debouncer.ButtonDebouncer( self.joystick, 1, period=0.5) #Mecanum to tank and the other way self.climbReverseButton = wpilib.buttons.JoystickButton( self.joystick, 2) #Button for reverse out of climb """ Solenoids """ self.drivePiston = wpilib.DoubleSolenoid( 3, 4) #Changes us from mecanum to hi-grip self.gearPiston = wpilib.Solenoid(2) self.safetyPiston = wpilib.Solenoid(1) self.robodrive = wpilib.RobotDrive(self.motor1, self.motor4, self.motor3, self.motor2) self.Drive = drive.Drive(self.robodrive, self.drivePiston, self.navx, self.encoder, self.ledRing) self.climber = climb.Climb(self.climb1, self.climb2) """ All the variables that need to be defined """ self.motorWhere = True #IF IT IS IN MECANUM BY DEFAULT self.rotationXbox = 0 self.climbVoltage = 0 """ Timers """ self.timer = wpilib.Timer() self.timer.start() self.vibrateTimer = wpilib.Timer() self.vibrateTimer.start() self.vibrator = vibrator.Vibrator(self.joystick, self.vibrateTimer, .25, .15) """ The great NetworkTables part """ self.vision_table = NetworkTable.getTable('/GRIP/myContoursReport') self.alignGear = alignGear.AlignGear(self.vision_table) self.robotStats = NetworkTable.getTable('SmartDashboard') self.matchTime = matchTime.MatchTime(self.timer, self.robotStats) """ Drive Straight """ self.DS = driveStraight.driveStraight(self.timer, self.vibrator, self.Drive, self.robotStats) self.components = { 'drive': self.Drive, 'alignGear': self.alignGear, 'gearPiston': self.gearPiston, 'ultrasonic': self.ultrasonic } self.automodes = AutonomousModeSelector('autonomous', self.components) self.updater()
def __init__(self): self.board = wpilib.PowerDistributionPanel()
def createObjects(self): """ Create basic components (motor controllers, joysticks, etc.). """ # NavX self.navx = navx.AHRS.create_spi() # Initialize SmartDashboard self.sd = NetworkTable.getTable('SmartDashboard') # Joysticks self.left_joystick = wpilib.Joystick(0) self.right_joystick = wpilib.Joystick(1) self.secondary_joystick = wpilib.Joystick(2) # Triggers and buttons self.secondary_trigger = ButtonDebouncer(self.secondary_joystick, 1) # Drive motors self.fr_module = swervemodule.SwerveModule(ctre.CANTalon(30), wpilib.VictorSP(3), wpilib.AnalogInput(0), sd_prefix='fr_module', zero=1.85, has_drive_encoder=True) self.fl_module = swervemodule.SwerveModule(ctre.CANTalon(20), wpilib.VictorSP(1), wpilib.AnalogInput(2), sd_prefix='fl_module', zero=3.92, inverted=True) self.rr_module = swervemodule.SwerveModule(ctre.CANTalon(10), wpilib.VictorSP(2), wpilib.AnalogInput(1), sd_prefix='rr_module', zero=4.59) self.rl_module = swervemodule.SwerveModule(ctre.CANTalon(5), wpilib.VictorSP(0), wpilib.AnalogInput(3), sd_prefix='rl_module', zero=2.44, has_drive_encoder=True, inverted=True) # Drive control self.field_centric_button = ButtonDebouncer(self.left_joystick, 6) self.predict_position = ButtonDebouncer(self.left_joystick, 7) self.field_centric_drive = True self.field_centric_hot_switch = toggle_button.TrueToggleButton(self.left_joystick, 1) self.left_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 4) self.right_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 5) self.align_button = toggle_button.TrueToggleButton(self.right_joystick, 10) # Shooting motors self.shooter_motor = ctre.CANTalon(15) self.belt_motor = wpilib.spark.Spark(9) self.light_controller = wpilib.VictorSP(8) # Pistons for gear picker self.picker = wpilib.DoubleSolenoid(6, 7) self.pivot = wpilib.DoubleSolenoid(4, 5) self.pessure_sensor = pressure_sensor.REVAnalogPressureSensor(navx.pins.getNavxAnalogInChannel(0)) # Toggling button on secondary joystick self.pivot_toggle_button = ButtonDebouncer(self.secondary_joystick, 2) # Or, up and down buttons on right joystick self.pivot_down_button = ButtonDebouncer(self.right_joystick, 2) self.pivot_up_button = ButtonDebouncer(self.right_joystick, 3) # Climb motors self.climb_motor1 = wpilib.spark.Spark(4) self.climb_motor2 = wpilib.spark.Spark(5) # Camera gimble self.gimbal_yaw = wpilib.Servo(6) self.gimbal_pitch = wpilib.Servo(7) # PDP self.pdp = wpilib.PowerDistributionPanel(0)
def robotInit(self): ### CONSTANTS ### # normal speed scale, out of 1: self.normalScale = 0.44 # speed scale when fast button is pressed: self.fastScale = 0.9 # speed scale when slow button is pressed: self.slowScale = 0.07 self.joystickExponent = 2 self.fastJoystickExponent = .5 self.slowJoystickExponent = 4 # if the joystick direction is within this number of radians on either # side of straight up, left, down, or right, it will be rounded self.driveDirectionDeadZone = math.radians(10) # rate of increase of velocity per 1/50th of a second: accelerationRate = .04 # PIDF values for fast driving: self.fastPID = (1.0, 0.0009, 3.0, 0.0) # speed at which fast PID's should be used: self.fastPIDScale = 0.15 # PIDF values for slow driving: self.slowPID = (30.0, 0.0009, 3.0, 0.0) # speed at which slow PID's should be used: self.slowPIDScale = 0.01 pidLookBackRange = 10 ### END OF CONSTANTS ### ### FLAGS ### self.fieldOriented = True self.pidLogEnabled = False self.currentLogEnabled = True ### END OF FLAGS ### self.gamepad = Gamepad(port=0) fl = wpilib.CANTalon(2) fr = wpilib.CANTalon(1) bl = wpilib.CANTalon(0) br = wpilib.CANTalon(3) self.talons = [fl, fr, bl, br] for talon in self.talons: talon.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder) self.driveModeLog = LogState("Drive mode") self.currentPID = None self.pidLog = LogState("Drive PID") self._setPID(self.fastPID) self.driveScales = [0.0 for i in range(0, pidLookBackRange)] # 2833 ticks per wheel rotation # encoder has 100 raw ticks -- with a QuadEncoder that makes 400 ticks # the motor gear has 12 teeth and the wheel has 85 teeth # 85 / 12 * 400 = 2833.333 = ~2833 self.holoDrive = HolonomicDrive(fl, fr, bl, br, 2833) self.holoDrive.invertDrive(True) self.holoDrive.setWheelOffset(math.radians(45.0)) #angle of rollers self.filterDrive = AccelerationFilterDrive(self.holoDrive, accelerationRate) if self.fieldOriented: self.ahrs = AHRS.create_spi() # the NavX self.drive = FieldOrientedDrive(self.filterDrive, self.ahrs, offset=0) else: self.drive = self.filterDrive self.drive.setDriveMode(DriveInterface.DriveMode.POSITION) self.pdp = wpilib.PowerDistributionPanel() self.currentLog = LogState("Current")
def robotInit(self): # DEVICES self.joystick = wpilib.Joystick(0) self.buttonBoard = wpilib.Joystick(1) self.opticalSensors = [ wpilib.AnalogInput(0), wpilib.AnalogInput(1), wpilib.AnalogInput(2), wpilib.AnalogInput(3)] ahrs = navx.AHRS.create_spi() self.pdp = wpilib.PowerDistributionPanel(50) self.vision = NetworkTables.getTable('limelight') self.vision.putNumber('pipeline', 1) # SUBSYSTEMS self.superDrive = drivetrain.initDrivetrain() self.superDrive.gear = None self.multiDrive = sea.MultiDrive(self.superDrive) self.grabberArm = grabber.GrabberArm() self.climber = climber.Climber() # HELPER OBJECTS self.pathFollower = sea.PathFollower(self.superDrive, ahrs) startPosition = coordinates.startCenter.inQuadrant(1) self.pathFollower.setPosition(startPosition.x, startPosition.y, startPosition.orientation) self.autoScheduler = auto_scheduler.AutoScheduler() self.autoScheduler.updateCallback = self.updateScheduler self.autoScheduler.idleFunction = self.autoIdle self.autoScheduler.actionList.append(auto_actions.createEndAction(self, 7)) self.timingMonitor = sea.TimingMonitor() # MANUAL DRIVE STATE self.driveVoltage = False self.manualGear = None self.fieldOriented = True self.holdGear = False self.manualAuxModeMachine = sea.StateMachine() self.auxDisabledState = sea.State(self.auxDisabledMode) self.defenseState = sea.State(self.manualDefenseMode) self.hatchState = sea.State(self.manualHatchMode) self.cargoState = sea.State(self.manualCargoMode) self.climbState = sea.State(self.manualClimbMode) self.controlModeMachine = sea.StateMachine() self.autoState = sea.State(self.autoScheduler.runSchedule) self.manualState = sea.State(self.manualDriving, self.manualAuxModeMachine) # DASHBOARD self.genericAutoActions = auto_actions.createGenericAutoActions( self, self.pathFollower, self.grabberArm) self.app = None # dashboard sea.startDashboard(self, dashboard.CompetitionBotDashboard) wpilib.CameraServer.launch('camera.py:main')
def robotInit(self): ### CONSTANTS ### # if the joystick direction is within this number of radians on either # side of straight up, left, down, or right, it will be rounded self.driveDirectionDeadZone = math.radians(10) # rate of increase of velocity per 1/50th of a second: accelerationRate = 1.0 # PIDF values for fast driving: fastPID = (1.0, 0.0009, 3.0, 0.0) # speed at which fast PID's should be used: fastPIDScale = 0.09 # PIDF values for slow driving: slowPID = (30.0, 0.0009, 3.0, 0.0) # speed at which slow PID's should be used: slowPIDScale = 0.01 pidLookBackRange = 10 self.maxVelocity = 650 ### END OF CONSTANTS ### self.driverGamepad = seamonsters.gamepad.globalGamepad(port=0) fl = ctre.CANTalon(2) fr = ctre.CANTalon(1) bl = ctre.CANTalon(0) br = ctre.CANTalon(3) self.talons = [fl, fr, bl, br] for talon in self.talons: talon.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder) self.driveModeLog = LogState("Drive mode") self._setPID(fastPID) # encoder has 100 raw ticks -- with a QuadEncoder that makes 400 ticks # the motor gear has 12 teeth and the wheel has 85 teeth # 85 / 12 * 400 = 2833.333 = ~2833 ticksPerWheelRotation = 2833 self.holoDrive = HolonomicDrive(fl, fr, bl, br, ticksPerWheelRotation) self.holoDrive.invertDrive(True) self.holoDrive.setWheelOffset(math.radians(45.0)) # angle of rollers self.pidDrive = DynamicPIDDrive(self.holoDrive, self.talons, slowPID, slowPIDScale, fastPID, fastPIDScale, pidLookBackRange) self.filterDrive = AccelerationFilterDrive(self.pidDrive, accelerationRate) self.ahrs = AHRS.create_spi() # the NavX self.fieldDrive = FieldOrientedDrive(self.filterDrive, self.ahrs, offset=0) self.fieldDrive.zero() self.fieldDriveLog = LogState("Field oriented") self.pdp = wpilib.PowerDistributionPanel() self.currentLog = LogState("Drive current", logFrequency=2.0) self.encoderLog = LogState("Wheel encoders") self.speedLog = LogState("Wheel speeds") if self.pdp.getVoltage() < 12: print("Battery Level below 12 volts!!!")