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!!!")
