def robotInit(self):
self.joystick = wpilib.Joystick(0)
self.motors = wpilib.SpeedControllerGroup(WPI_VictorSPX(2),
WPI_VictorSPX(3))
self.priorAutospeed = 0
# TODO: Check if we need IdleMode.kBrake
# self.motor.setIdleMode(IdleMode.kBrake);
self.encoder = wpilib.Encoder(
8, 7, True, encodingType=wpilib.Encoder.EncodingType.k1X)
# //
# // Configure encoder related functions -- getDistance and getrate should
# // return units and units/s
# //
# % if units == 'Degrees':
# double encoderConstant = (1 / GEARING) * 360
# % elif units == 'Radians':
# double encoderConstant = (1 / GEARING) * 2. * Math.PI;
# % elif units == 'Rotations':
self.encoderConstant = (1 / (self.ENCODER_PULSES_PER_REV))
self.encoder.setDistancePerPulse(self.encoderConstant)
self.encoder.reset()
NetworkTablesInstance.getDefault().setUpdateRate(0.010)
class IntakeMotor:
def __init__(self, _id: int, _invert: bool):
self.motor = WPI_VictorSPX(_id)
self.motor.setInverted(_invert)
def set(self, value: float):
self.motor.set(ControlMode.PercentOutput, value)
def __init__(self, robot):
"""Save the robot object, and assign and save hardware ports
connected to the drive motors."""
super().__init__(name="agitator")
self.robot = robot
self.motor = WPI_VictorSPX(11)
class Agitator(Subsystem):
def __init__(self, robot):
"""Save the robot object, and assign and save hardware ports
connected to the drive motors."""
super().__init__(name="agitator")
self.robot = robot
self.motor = WPI_VictorSPX(11)
def agitate(self):
self.motor.set(1.0)
def calm(self):
self.motor.set(0.0)
def __init__(self, operator: OperatorControl):
self.operator = operator
self.motor = WPI_VictorSPX(7)
self.encoder = Encoder(0, 1, False, Encoder.EncodingType.k4X)
self.encoder.setIndexSource(2)
self.limit = DigitalInput(4)
self.dashboard = NetworkTables.getTable("SmartDashboard")
self.totalValues = 2048 # * self.encoder.getEncodingScale() - 1
self.targetValue = 10
self.realValue = 0
self.allowedError = 10
self.hole = 4
self.holeOffset = 36 - 15
self.maxspeed = .25
self.minspeed = .1
self.speedmult = 1/300
self.speed = .1
def createObjects(self):
self.joystick = wpilib.Joystick(2)
self.winch_button = JoystickButton(self.joystick, 6)
self.cp_extend_button = JoystickButton(self.joystick, 5)
self.cp_solenoid = wpilib.DoubleSolenoid(4, 5)
self.cp_motor = CANSparkMax(10, MotorType.kBrushed)
self.solenoid = wpilib.Solenoid(0)
self.btn_solenoid = JoystickButton(self.joystick, 1)
self.intake = WPI_VictorSPX(1)
self.cp_motor_button = JoystickButton(self.joystick, 7)
self.winch_motor = CANSparkMax(8, MotorType.kBrushless)
self.six = JoystickButton(self.joystick, 12)
self.shooter_motor = CANSparkMax(7, MotorType.kBrushed)
self.shooter_motor.restoreFactoryDefaults()
self.shooter_motor.setOpenLoopRampRate(0)
self.intake_in = JoystickButton(self.joystick, 3)
self.intake_out = JoystickButton(self.joystick, 4)
def createObjects(self):
self.launcher_motor = CANSparkMax(7, MotorType.kBrushed)
self.launcher_motor.restoreFactoryDefaults()
self.launcher_motor.setOpenLoopRampRate(0)
self.intake = WPI_VictorSPX(1)
self.solenoid = wpilib.Solenoid(0)
self.encoder = self.launcher_motor.getEncoder()
# self.shooter_running = False
self.logger.addFilter(PeriodicFilter(1, bypass_level=logging.WARN))
# 2000rpm is a good setting
# set up joystick buttons
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# self.left_button = Toggle(self.joystick_alt, 10)
# self.middle_button = Toggle(self.joystick_alt, 11)
# self.right_button = Toggle(self.joystick_alt, 12)
self.one = JoystickButton(self.joystick_alt, 7)
self.two = JoystickButton(self.joystick_alt, 8)
self.three = JoystickButton(self.joystick_alt, 9)
self.four = JoystickButton(self.joystick_alt, 10)
self.five = JoystickButton(self.joystick_alt, 11)
self.six = JoystickButton(self.joystick_alt, 12)
self.intake_in = JoystickButton(self.joystick_alt, 3)
self.intake_out = JoystickButton(self.joystick_alt, 4)
self.btn_solenoid = JoystickButton(self.joystick_alt, 1)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(CANSparkMax(1, MotorType.kBrushless), CANSparkMax(2, MotorType.kBrushless), CANSparkMax(3, MotorType.kBrushless))
self.right_motors = wpilib.SpeedControllerGroup(CANSparkMax(4, MotorType.kBrushless), CANSparkMax(5, MotorType.kBrushless), CANSparkMax(6, MotorType.kBrushless))
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors)
class Robot(MagicRobot):
def createObjects(self):
self.joystick = wpilib.Joystick(2)
self.winch_button = JoystickButton(self.joystick, 6)
self.cp_extend_button = JoystickButton(self.joystick, 5)
self.cp_solenoid = wpilib.DoubleSolenoid(4, 5)
self.cp_motor = CANSparkMax(10, MotorType.kBrushed)
self.solenoid = wpilib.Solenoid(0)
self.btn_solenoid = JoystickButton(self.joystick, 1)
self.intake = WPI_VictorSPX(1)
self.cp_motor_button = JoystickButton(self.joystick, 7)
self.winch_motor = CANSparkMax(8, MotorType.kBrushless)
self.six = JoystickButton(self.joystick, 12)
self.shooter_motor = CANSparkMax(7, MotorType.kBrushed)
self.shooter_motor.restoreFactoryDefaults()
self.shooter_motor.setOpenLoopRampRate(0)
self.intake_in = JoystickButton(self.joystick, 3)
self.intake_out = JoystickButton(self.joystick, 4)
def teleopPeriodic(self):
if self.winch_button.get():
self.winch_motor.set(1)
else:
self.winch_motor.set(0)
if self.six.get():
# 75% is good for initiation line
self.shooter_motor.set(0.75)
else:
self.shooter_motor.set(0)
self.solenoid.set(self.btn_solenoid.get())
if self.intake_in.get():
self.intake.set(-1)
elif self.intake_out.get():
self.intake.set(1)
else:
self.intake.set(0)
if self.cp_motor_button.get():
self.cp_motor.set(0.7)
else:
self.cp_motor.set(0)
if self.cp_extend_button.get(
) and self.cp_solenoid.get() != wpilib.DoubleSolenoid.Value.kReverse:
self.cp_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
if not self.cp_extend_button.get(
) and self.cp_solenoid.get() != wpilib.DoubleSolenoid.Value.kForward:
self.cp_solenoid.set(wpilib.DoubleSolenoid.Value.kForward)
class Climbmotors(Subsystem):
"""Raise and lower the robot's arm."""
def __init__(self, robot):
"""Assign ports and save them for use in the move and stop methods."""
super().__init__(name = "climbers")
self.motor1 = WPI_VictorSPX(5)
self.motor2 = WPI_VictorSPX(6)
def climb(self, xboxcontroller):
self.motor2.follow(self.motor1)
self.motor1.set(xboxcontroller.getTriggerAxis(GenericHID.Hand.kRightHand)-xboxcontroller.getTriggerAxis(GenericHID.Hand.kLeftHand))
def stop(self):
self.motor2.follow(self.motor1)
self.motor1.set(0.0)
class Indexer():
def __init__(self, operator: OperatorControl):
self.operator = operator
self.motor = WPI_VictorSPX(7)
self.encoder = Encoder(0, 1, False, Encoder.EncodingType.k4X)
self.encoder.setIndexSource(2)
self.limit = DigitalInput(4)
self.dashboard = NetworkTables.getTable("SmartDashboard")
self.totalValues = 2048 # * self.encoder.getEncodingScale() - 1
self.targetValue = 10
self.realValue = 0
self.allowedError = 10
self.hole = 4
self.holeOffset = 36 - 15
self.maxspeed = .25
self.minspeed = .1
self.speedmult = 1/300
self.speed = .1
def update(self):
self.realValue = self.encoder.get()
offset = (self.targetValue - self.realValue) % self.totalValues - (self.totalValues / 2)
self.speed = clamp(abs(offset) * self.speedmult, self.minspeed, self.maxspeed)
self.dashboard.putString("Indexer", "{} offset".format(offset))
if (offset > self.allowedError):
self.motor.set(ControlMode.PercentOutput, -self.speed)
elif (offset < -self.allowedError):
self.motor.set(ControlMode.PercentOutput, self.speed)
else:
self.motor.set(ControlMode.PercentOutput, 0)
if (abs(offset) < 15):
if (self.operator.getIndexUp()):
self.hole = (self.hole + 1) % 5
elif (self.operator.getIndexDown()):
self.hole = (self.hole + 4) % 5
self.setRotation(self.hole * 72 + self.holeOffset)
def getRotation(self) -> float:
return self.realValue / self.totalValues * 360
def setRotation(self, degrees):
self.targetValue = clamp(degrees/360*self.totalValues,
0, self.totalValues)
class Intake_Sub(Subsystem):
"""Operate the drivetrain."""
def __init__(self, robot):
"""Save the robot object, and assign and save hardware ports
connected to the drive motors."""
super().__init__(name="intake")
self.robot = robot
self.motor = WPI_VictorSPX(7)
def intake(self):
self.motor.set(1.0)
def stop(self):
self.motor.set(0)
def eject(self):
self.motor.set(-.5)
def __init__(self, robot):
super().__init__('Drive')
SmartDashboard.putNumber("DriveStraight_P", 0.075)
SmartDashboard.putNumber("DriveStraight_I", 0.0)
SmartDashboard.putNumber("DriveStraight_D", 0.42)
# OLD GAINS 0.075, 0, 0.42
SmartDashboard.putNumber("DriveAngle_P", 0.009)
SmartDashboard.putNumber("DriveAngle_I", 0.0)
SmartDashboard.putNumber("DriveAngle_D", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_P", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_I", 0.0)
SmartDashboard.putNumber("DriveStraightAngle_D", 0.01)
self.driveStyle = "Tank"
SmartDashboard.putString("DriveStyle", self.driveStyle)
#SmartDashboard.putData("Mode", self.mode)
self.robot = robot
self.lime = self.robot.limelight
self.nominalPID = 0.15
self.nominalPIDAngle = 0.22 # 0.11 - v2
self.preferences = Preferences.getInstance()
timeout = 0
TalonLeft = Talon(map.driveLeft1)
TalonRight = Talon(map.driveRight1)
leftInverted = True
rightInverted = False
TalonLeft.setInverted(leftInverted)
TalonRight.setInverted(rightInverted)
VictorLeft1 = Victor(map.driveLeft2)
VictorLeft2 = Victor(map.driveLeft3)
VictorLeft1.follow(TalonLeft)
VictorLeft2.follow(TalonLeft)
VictorRight1 = Victor(map.driveRight2)
VictorRight2 = Victor(map.driveRight3)
VictorRight1.follow(TalonRight)
VictorRight2.follow(TalonRight)
for motor in [VictorLeft1, VictorLeft2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(leftInverted)
for motor in [VictorRight1, VictorRight2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(rightInverted)
for motor in [TalonLeft, TalonRight]:
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.clearStickyFaults(timeout) #Clears sticky faults
motor.configContinuousCurrentLimit(40, timeout) #15 Amps per motor
motor.configPeakCurrentLimit(
70, timeout) #20 Amps during Peak Duration
motor.configPeakCurrentDuration(
300, timeout) #Peak Current for max 100 ms
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(12,
timeout) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
motor.configOpenLoopRamp(0.2,
timeout) #number of seconds from 0 to 1
self.left = TalonLeft
self.right = TalonRight
self.navx = navx.AHRS.create_spi()
self.leftEncoder = Encoder(map.leftEncoder[0], map.leftEncoder[1])
self.leftEncoder.setDistancePerPulse(self.leftConv)
self.leftEncoder.setSamplesToAverage(10)
self.rightEncoder = Encoder(map.rightEncoder[0], map.rightEncoder[1])
self.rightEncoder.setDistancePerPulse(self.rightConv)
self.rightEncoder.setSamplesToAverage(10)
self.zero()
#PID for Drive
self.TolDist = 0.1 #feet
[kP, kI, kD, kF] = [0.027, 0.00, 0.20, 0.00]
if wpilib.RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.25, 0.00, 1.00, 0.00]
distController = PIDController(kP,
kI,
kD,
kF,
source=self.__getDistance__,
output=self.__setDistance__)
distController.setInputRange(0, 50) #feet
distController.setOutputRange(-0.6, 0.6)
distController.setAbsoluteTolerance(self.TolDist)
distController.setContinuous(False)
self.distController = distController
self.distController.disable()
'''PID for Angle'''
self.TolAngle = 2 #degrees
[kP, kI, kD, kF] = [0.025, 0.00, 0.01, 0.00]
if RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.005, 0.0, 0.01, 0.00]
angleController = PIDController(kP,
kI,
kD,
kF,
source=self.__getAngle__,
output=self.__setAngle__)
angleController.setInputRange(-180, 180) #degrees
angleController.setOutputRange(-0.5, 0.5)
angleController.setAbsoluteTolerance(self.TolAngle)
angleController.setContinuous(True)
self.angleController = angleController
self.angleController.disable()
self.k = 1
self.sensitivity = 1
SmartDashboard.putNumber("K Value", self.k)
SmartDashboard.putNumber("sensitivity", self.sensitivity)
self.prevLeft = 0
self.prevRight = 0
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)
class Intake: def __init__(self): self.lBallObtainer = WPI_VictorSPX(LBALL_OBTAINER_VICTOR) self.rBallObtainer = WPI_VictorSPX(RBALL_OBTAINER_VICTOR) self.ballAngle = WPI_TalonSRX(BALL_ANGLE_TALON) self.autonTimer = wpilib.Timer() self.timerStart = False def setSpeed(self, speed: float) -> None: self.lBallObtainer.set(speed) self.rBallObtainer.set(speed) def setAngle(self, speed: float) -> None: self.ballAngle.set(speed) def getDistance(self) -> float: return self.ballAngle.getSelectedSensorPosition(0) / 10000 def getAngleVelocity(self) -> float: return self.ballAngle.get() def resetEncoders(self) -> None: self.ballAngle.setSelectedSensorPosition(0.0) def encoderWrite(self, angleDistance: float) -> None: if (fabs(angleDistance - self.getDistance()) > 2 and self.getDistance() < angleDistance): self.ballAngle.set(-.5) elif (fabs(angleDistance - self.getDistance()) > 2 and self.getDistance() > angleDistance): self.ballAngle.set(.5) else: self.ballAngle.set(0) def autonAngle(self, angleDistance: float) -> bool: if (fabs(angleDistance - self.getDistance()) > 2 and self.getDistance() < angleDistance): self.ballAngle.set(-.5) return False elif (fabs(angleDistance - self.getDistance()) > 2 and self.getDistance() > angleDistance): self.ballAngle.set(.5) return False else: self.ballAngle.set(0) return True def autonInOut(self, speed: float, seconds: float) -> bool: if not self.timerStart: self.autonTimer.start() self.timerStart = True self.lBallObtainer.set(speed) self.rBallObtainer.set(speed) if autonTimer.get() > seconds: self.autonTimer.reset() self.timerStart = False return True else: return False def autonTimerPrep(self) -> None: self.timerStart = False def getEncoder2Distance(self) -> float: pass
def __init__(self, robot):
"""Assign ports and save them for use in the move and stop methods."""
super().__init__(name="climbers")
self.motor1 = WPI_VictorSPX(5)
self.motor2 = WPI_VictorSPX(6)
def __init__(self): self.lBallObtainer = WPI_VictorSPX(LBALL_OBTAINER_VICTOR) self.rBallObtainer = WPI_VictorSPX(RBALL_OBTAINER_VICTOR) self.ballAngle = WPI_TalonSRX(BALL_ANGLE_TALON) self.autonTimer = wpilib.Timer() self.timerStart = False
class TestRobot(magicbot.MagicRobot):
shooter_speed = tunable(0, writeDefault=False)
time = tunable(0)
voltage = tunable(0)
rotation = tunable(0)
def createObjects(self):
self.launcher_motor = CANSparkMax(7, MotorType.kBrushed)
self.launcher_motor.restoreFactoryDefaults()
self.launcher_motor.setOpenLoopRampRate(0)
self.intake = WPI_VictorSPX(1)
self.solenoid = wpilib.Solenoid(0)
self.encoder = self.launcher_motor.getEncoder()
# self.shooter_running = False
self.logger.addFilter(PeriodicFilter(1, bypass_level=logging.WARN))
# 2000rpm is a good setting
# set up joystick buttons
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# self.left_button = Toggle(self.joystick_alt, 10)
# self.middle_button = Toggle(self.joystick_alt, 11)
# self.right_button = Toggle(self.joystick_alt, 12)
self.one = JoystickButton(self.joystick_alt, 7)
self.two = JoystickButton(self.joystick_alt, 8)
self.three = JoystickButton(self.joystick_alt, 9)
self.four = JoystickButton(self.joystick_alt, 10)
self.five = JoystickButton(self.joystick_alt, 11)
self.six = JoystickButton(self.joystick_alt, 12)
self.intake_in = JoystickButton(self.joystick_alt, 3)
self.intake_out = JoystickButton(self.joystick_alt, 4)
self.btn_solenoid = JoystickButton(self.joystick_alt, 1)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(CANSparkMax(1, MotorType.kBrushless), CANSparkMax(2, MotorType.kBrushless), CANSparkMax(3, MotorType.kBrushless))
self.right_motors = wpilib.SpeedControllerGroup(CANSparkMax(4, MotorType.kBrushless), CANSparkMax(5, MotorType.kBrushless), CANSparkMax(6, MotorType.kBrushless))
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors, self.right_motors)
# self.compressor = wpilib.Compressor()
def disabledInit(self):
self.launcher_motor.set(0)
self.intake.set(0)
self.solenoid.set(False)
def teleopInit(self):
self.train.setDeadband(0.1)
# self.compressor.start()
def teleopPeriodic(self):
self.logger.info(self.encoder.getVelocity())
self.shooter_speed = self.encoder.getVelocity()
# if self.left_button.get():
# self.launcher_motor.set(-0.2)
# elif self.middle_button.get():
# self.launcher_motor.set(-0.5)
# elif self.right_button.get():
# self.launcher_motor.set(-0.7)
# else:
# self.launcher_motor.set(0)
if self.intake_in.get():
self.intake.set(-1)
elif self.intake_out.get():
self.intake.set(1)
else:
self.intake.set(0)
# BUTTON SETTINGS
if self.one.get():
self.launcher_motor.set(0)
elif self.two.get():
self.launcher_motor.set(-0.2)
elif self.three.get():
self.launcher_motor.set(-0.4)
elif self.four.get():
self.launcher_motor.set(-0.6)
elif self.five.get():
self.launcher_motor.set(-0.8)
elif self.six.get():
self.launcher_motor.set(-1)
else:
# self.launcher_motor.set(-self.joystick_alt.getAxis(3))
self.launcher_motor.set(0)
# SLIDER SETTINGS
# self.launcher_motor.set(-self.joystick_alt.getAxis(3))
self.solenoid.set(self.btn_solenoid.get())
self.train.arcadeDrive(-self.joystick_left.getY(),
self.joystick_right.getX())
def __init__(self, _id: int, _invert: bool):
self.motor = WPI_VictorSPX(_id)
self.motor.setInverted(_invert)
class Gemini(wpilib.TimedRobot):
def __init__(self):
""" Initialization of internal class variables and software-bases only """
super().__init__()
# global button status list construction
self.buttonToggleStatus = [
False, False, False, False, False, False, False
]
from networktables import NetworkTables
# connection for logging & Smart Dashboard
self.sd = NetworkTables.getTable('SmartDashboard')
NetworkTables.initialize(server='10.55.49.2')
self.sd.putString(" ", "Connection")
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 autonomousInit(self):
''' Executed each time the robot enters autonomous. '''
# pre-auto timer configuration
self.timer.reset()
self.timer.start()
# drive train encoder reset
self.frontRightMotor.setQuadraturePosition(0, 0)
self.frontLeftMotor.setQuadraturePosition(0, 0)
self.liftEncoder.reset()
def autonomousPeriodic(self):
'''' Called periodically during autonomous. '''
if self.DS.getGameSpecificMessage():
# Test Methods
if self.DS.getGameSpecificMessage() is 'encoder_test':
# Drives robot set encoder distance away
self.rightPos = fabs(
self.frontRightMotor.getQuadraturePosition())
self.leftPos = fabs(
self.frontLeftMotor.getQuadraturePosition())
self.distIn = ((
(self.leftPos + self.rightPos) / 2) / 4096) * 18.84955
if 0 <= self.distIn <= 72:
self.drive.tankDrive(0.5, 0.5)
else:
self.drive.tankDrive(0, 0)
if self.DS.getGameSpecificMessage() is 'diagnostics':
# Smart Dashboard Tests
self.sd.putNumber("Temperature: ", self.PDP.getTemperature())
self.sd.putNumber("Battery Voltage: ",
self.roboController.getBatteryVoltage())
self.sd.putBoolean(" Browned Out?",
self.roboController.isBrownedOut)
# Smart Dashboard diagnostics
self.sd.putNumber(
"Right Encoder Speed: ",
abs(self.frontRightMotor.getQuadratureVelocity()))
self.sd.putNumber(
"Left Encoder Speed: ",
abs(self.frontLeftMotor.getQuadratureVelocity()))
self.sd.putNumber("Lift Encoder: ",
self.liftEncoder.getDistance())
if self.DS.getGameSpecificMessage() is 'pressure':
self.Compressor.start()
elif self.Compressor.enabled() is True:
self.Compressor.stop()
if not self.DS.getGameSpecificMessage(
) is 'pressure' and not self.DS.getGameSpecificMessage(
) is 'encoder_test':
# begin normal periodic
# get all required data once per frame
# toggle button management per frame
if self.buttonBox.getRawButtonPressed(1):
self.buttonToggleStatus = [
not self.buttonToggleStatus[0], False, False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(2):
self.buttonToggleStatus = [
False, not self.buttonToggleStatus[1], False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(3):
self.buttonToggleStatus = [
False, False, not self.buttonToggleStatus[2], False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(4):
self.buttonToggleStatus = [
False, False, False, not self.buttonToggleStatus[3], False,
False, False
]
elif self.buttonBox.getRawButtonPressed(5):
self.buttonToggleStatus = [
False, False, False, False, not self.buttonToggleStatus[4],
False, False
]
elif self.buttonBox.getRawButtonPressed(6):
self.buttonToggleStatus = [
False, False, False, False, False,
not self.buttonToggleStatus[5], False
]
elif self.buttonBox.getRawButtonPressed(7):
self.buttonToggleStatus = [
False, False, False, False, False, False,
not self.buttonToggleStatus[6]
]
liftTicks = self.liftEncoder.get()
hallState = self.Hall.get()
compressorState = self.Compressor.enabled()
solenoidStateOne = self.DoubleSolenoidOne.get()
solenoidStateTwo = self.DoubleSolenoidTwo.get()
solenoidStateThree = self.DoubleSolenoidThree.get()
# robot ultrasonic
self.ultraValue = self.ultrasonic.getVoltage()
# cargo ultrasonic
self.cargoUltraValue = self.cargoUltrasonic.getVoltage()
# xbox value states
xboxButtonStates = [
self.xbox.getRawButton(1),
self.xbox.getRawButton(2),
self.xbox.getRawButton(3),
self.xbox.getRawButton(4),
self.xbox.getRawButton(5),
self.xbox.getRawButton(6),
self.xbox.getRawButton(7),
self.xbox.getRawButton(8),
self.xbox.getRawButton(9),
self.xbox.getRawButton(10)
]
xboxAxisStates = [
self.xbox.getRawAxis(1),
self.xbox.getRawAxis(2),
self.xbox.getRawAxis(3),
self.xbox.getRawAxis(4),
self.xbox.getRawAxis(5)
]
# joystick value states
rJoystickButtonStates = [self.rightStick.getRawButton(1)]
rJoystickAxisStates = [
self.rightStick.getRawAxis(1),
self.rightStick.getRawAxis(2),
self.rightStick.getRawAxis(3)
]
lJoystickButtonStates = [self.leftStick.getRawButton(1)]
lJoystickAxisStates = [
self.leftStick.getRawAxis(1),
self.leftStick.getRawAxis(2),
self.leftStick.getRawAxis(3)
]
# define lift stages
def cargo_one():
if liftTicks <= 133: # Cargo 1
self.lift.set(0.5)
elif liftTicks > 133:
self.lift.set(0.05)
def cargo_two():
if liftTicks <= 270: # Cargo 2
self.lift.set(0.5)
elif liftTicks > 270:
self.lift.set(0.05)
def cargo_three():
if liftTicks <= 415: # Cargo 3
self.lift.set(0.5)
elif liftTicks > 415:
self.lift.set(0.05)
def hatch_one():
if liftTicks <= 96: # Hatch 1
self.lift.set(0.5)
elif liftTicks > 96:
self.lift.set(0.05)
def hatch_two():
if liftTicks <= 237: # Hatch 2
self.lift.set(0.5)
elif liftTicks > 237:
self.lift.set(0.05)
def hatch_three():
if liftTicks <= 378: # Hatch 3
self.lift.set(0.5)
elif liftTicks > 378:
self.lift.set(0.05)
def lift_encoder_reset():
self.lift.set(0.01)
if hallState is True:
self.liftEncoder.reset()
if self.buttonToggleStatus[0] is True:
cargo_three()
elif self.buttonToggleStatus[1] is True:
hatch_three()
elif self.buttonToggleStatus[2] is True:
cargo_two()
elif self.buttonToggleStatus[3] is True:
hatch_two()
elif self.buttonToggleStatus[4] is True:
cargo_one()
elif self.buttonToggleStatus[5] is True:
hatch_one()
elif self.buttonToggleStatus[6] is True:
lift_encoder_reset()
# compressor state
self.sd.putString(
"Compressor Status: ",
"Enabled" if compressorState is True else "Disabled")
# gear state
self.sd.putString(
"Gear Shift: ",
"High Speed" if solenoidStateOne is 1 else "Low Speed")
# ejector state
self.sd.putString(
"Ejector Pins: ",
"Ejected" if solenoidStateThree is 2 else "Retracted")
# claw state
self.sd.putString("Claw: ",
"Open" if solenoidStateTwo is 2 else "Closed")
# hatch station range state
self.sd.putString(
"PLAYER STATION RANGE: ",
"YES!!!!" if 0.142 <= self.ultraValue <= 0.146 else "NO!")
# self.sd.putNumber("Ultrasonic Voltage: ", self.ultraValue)
# hatch spaceship range
self.sd.putString(
"HATCH RANGE: ", "HATCH IN RANGE"
if 0.7 <= self.cargoUltraValue <= 1.56 else "NOT IN RANGE")
# compressor
if xboxButtonStates[8]:
self.Compressor.stop()
elif xboxButtonStates[9]:
self.Compressor.start()
elif rJoystickButtonStates[0]: # shift right
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kForward)
elif lJoystickButtonStates[0]: # shift left
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[2]: # open claw
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[1]: # close claw
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[3]: # eject
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[0]: # retract
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kReverse)
# lift control
if True in self.buttonToggleStatus is False:
if xboxButtonStates[4]: # hold
self.lift.set(0.05)
elif xboxAxisStates[2] > 0.1: # up
self.lift.set(xboxAxisStates[2] / 1.5)
elif xboxAxisStates[1] > 0.1: # down
self.lift.set(-xboxAxisStates[1] * 0.25)
else:
self.lift.set(0)
# four-bar control
if xboxButtonStates[5]:
self.liftArm.set(0.05)
elif not xboxButtonStates[5]:
self.liftArm.set(-xboxAxisStates[0] / 4.0)
else:
self.liftArm.set(0)
# cargo intake control
if xboxButtonStates[6]:
self.cargo.set(0.12)
elif xboxAxisStates[4]: # take in
self.cargo.set(xboxAxisStates[4] * 0.75)
# controller mapping for tank steering
rightAxis = rJoystickAxisStates[0]
leftAxis = lJoystickAxisStates[0]
# drives drive system using tank steering
if solenoidStateOne is 1: # if on high gear
divisor = 1.2 # then 90% of high speed
elif solenoidStateOne is 2: # if on low gear
divisor = 1.2 # then normal slow speed
else:
divisor = 1.0
leftSign = leftAxis / fabs(leftAxis) if leftAxis != 0 else 0
rightSign = rightAxis / fabs(rightAxis) if rightAxis != 0 else 0
self.drive.tankDrive(-leftSign * (1 / divisor) * (leftAxis**2),
-rightSign * (1 / divisor) * (rightAxis**2))
def teleopInit(self):
''' Executed at the start of teleop mode. '''
self.drive.setSafetyEnabled(True)
# drive train encoder reset
self.frontRightMotor.setQuadraturePosition(0, 0)
self.frontLeftMotor.setQuadraturePosition(0, 0)
# lift encoder rest
self.liftEncoder.reset()
# compressor
self.Compressor.start()
def teleopPeriodic(self):
''' Periodically executes methods during the teleop mode. '''
# begin normal periodic
# get all required data once per frame
# toggle button management per frame
if self.buttonBox.getRawButtonPressed(1):
self.buttonToggleStatus = [
not self.buttonToggleStatus[0], False, False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(2):
self.buttonToggleStatus = [
False, not self.buttonToggleStatus[1], False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(3):
self.buttonToggleStatus = [
False, False, not self.buttonToggleStatus[2], False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(4):
self.buttonToggleStatus = [
False, False, False, not self.buttonToggleStatus[3], False,
False, False
]
elif self.buttonBox.getRawButtonPressed(5):
self.buttonToggleStatus = [
False, False, False, False, not self.buttonToggleStatus[4],
False, False
]
elif self.buttonBox.getRawButtonPressed(6):
self.buttonToggleStatus = [
False, False, False, False, False,
not self.buttonToggleStatus[5], False
]
elif self.buttonBox.getRawButtonPressed(7):
self.buttonToggleStatus = [
False, False, False, False, False, False,
not self.buttonToggleStatus[6]
]
liftTicks = self.liftEncoder.get()
hallState = self.Hall.get()
compressorState = self.Compressor.enabled()
solenoidStateOne = self.DoubleSolenoidOne.get()
solenoidStateTwo = self.DoubleSolenoidTwo.get()
solenoidStateThree = self.DoubleSolenoidThree.get()
# robot ultrasonic
self.ultraValue = self.ultrasonic.getVoltage()
# cargo ultrasonic
self.cargoUltraValue = self.cargoUltrasonic.getVoltage()
# xbox value states
xboxButtonStates = [
self.xbox.getRawButton(1),
self.xbox.getRawButton(2),
self.xbox.getRawButton(3),
self.xbox.getRawButton(4),
self.xbox.getRawButton(5),
self.xbox.getRawButton(6),
self.xbox.getRawButton(7),
self.xbox.getRawButton(8),
self.xbox.getRawButton(9),
self.xbox.getRawButton(10)
]
xboxAxisStates = [
self.xbox.getRawAxis(1),
self.xbox.getRawAxis(2),
self.xbox.getRawAxis(3),
self.xbox.getRawAxis(4),
self.xbox.getRawAxis(5)
]
# joystick value states
rJoystickButtonStates = [self.rightStick.getRawButton(1)]
rJoystickAxisStates = [
self.rightStick.getRawAxis(1),
self.rightStick.getRawAxis(2),
self.rightStick.getRawAxis(3)
]
lJoystickButtonStates = [self.leftStick.getRawButton(1)]
lJoystickAxisStates = [
self.leftStick.getRawAxis(1),
self.leftStick.getRawAxis(2),
self.leftStick.getRawAxis(3)
]
# define lift stages
def cargo_one():
if liftTicks <= 133: # Cargo 1
self.lift.set(0.5)
elif liftTicks > 133:
self.lift.set(0.05)
def cargo_two():
if liftTicks <= 270: # Cargo 2
self.lift.set(0.5)
elif liftTicks > 270:
self.lift.set(0.05)
def cargo_three():
if liftTicks <= 415: # Cargo 3
self.lift.set(0.5)
elif liftTicks > 415:
self.lift.set(0.05)
def hatch_one():
if liftTicks <= 96: # Hatch 1
self.lift.set(0.5)
elif liftTicks > 96:
self.lift.set(0.05)
def hatch_two():
if liftTicks <= 237: # Hatch 2
self.lift.set(0.5)
elif liftTicks > 237:
self.lift.set(0.05)
def hatch_three():
if liftTicks <= 378: # Hatch 3
self.lift.set(0.5)
elif liftTicks > 378:
self.lift.set(0.05)
def lift_encoder_reset():
self.lift.set(0.01)
if hallState is True:
self.liftEncoder.reset()
if self.buttonToggleStatus[0] is True:
cargo_three()
elif self.buttonToggleStatus[1] is True:
hatch_three()
elif self.buttonToggleStatus[2] is True:
cargo_two()
elif self.buttonToggleStatus[3] is True:
hatch_two()
elif self.buttonToggleStatus[4] is True:
cargo_one()
elif self.buttonToggleStatus[5] is True:
hatch_one()
elif self.buttonToggleStatus[6] is True:
lift_encoder_reset()
# compressor state
self.sd.putString("Compressor Status: ",
"Enabled" if compressorState is True else "Disabled")
# gear state
self.sd.putString(
"Gear Shift: ",
"High Speed" if solenoidStateOne is 1 else "Low Speed")
# ejector state
self.sd.putString(
"Ejector Pins: ",
"Ejected" if solenoidStateThree is 2 else "Retracted")
# claw state
self.sd.putString("Claw: ",
"Open" if solenoidStateTwo is 2 else "Closed")
# hatch station range state
self.sd.putString(
"PLAYER STATION RANGE: ",
"YES!!!!" if 0.142 <= self.ultraValue <= 0.146 else "NO!")
# self.sd.putNumber("Ultrasonic Voltage: ", self.ultraValue)
# hatch spaceship range
self.sd.putString(
"HATCH RANGE: ", "HATCH IN RANGE"
if 0.7 <= self.cargoUltraValue <= 1.56 else "NOT IN RANGE")
# compressor
if xboxButtonStates[8]:
self.Compressor.stop()
elif xboxButtonStates[9]:
self.Compressor.start()
elif rJoystickButtonStates[0]: # shift right
self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kForward)
elif lJoystickButtonStates[0]: # shift left
self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[2]: # open claw
self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[1]: # close claw
self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[3]: # eject
self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[0]: # retract
self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kReverse)
# lift control
if True in self.buttonToggleStatus is False:
if xboxButtonStates[4]: # hold
self.lift.set(0.05)
elif xboxAxisStates[2] > 0.1: # up
self.lift.set(xboxAxisStates[2] / 1.5)
elif xboxAxisStates[1] > 0.1: # down
self.lift.set(-xboxAxisStates[1] * 0.25)
else:
self.lift.set(0)
# four-bar control
if xboxButtonStates[5]:
self.liftArm.set(0.05)
elif not xboxButtonStates[5]:
self.liftArm.set(-xboxAxisStates[0] / 4.0)
else:
self.liftArm.set(0)
# cargo intake control
if xboxButtonStates[6]:
self.cargo.set(0.12)
elif xboxAxisStates[4]: # take in
self.cargo.set(xboxAxisStates[4] * 0.75)
# controller mapping for tank steering
rightAxis = rJoystickAxisStates[0]
leftAxis = lJoystickAxisStates[0]
# drives drive system using tank steering
if solenoidStateOne is 1: # if on high gear
divisor = 1.2 # then 90% of high speed
elif solenoidStateOne is 2: # if on low gear
divisor = 1.2 # then normal slow speed
else:
divisor = 1.0
leftSign = leftAxis / fabs(leftAxis) if leftAxis != 0 else 0
rightSign = rightAxis / fabs(rightAxis) if rightAxis != 0 else 0
self.drive.tankDrive(-leftSign * (1 / divisor) * (leftAxis**2),
-rightSign * (1 / divisor) * (rightAxis**2))