def robotInit(self):
self.vib1 = wpilib.DigitalOutput(6)
self.vib2 = wpilib.DigitalOutput(7)
self.vib3 = wpilib.DigitalOutput(8)
self.vib4 = wpilib.DigitalOutput(9)
self.timer = wpilib.Timer()
def __init__(self, dio1: ChannelMode, dio2: ChannelMode) -> None:
"""Constructor.
:param dio1: Mode for DIO 1 (input = Button B, output = green LED)
:param dio2: Mode for DIO 2 (input = Button C, output = red LED)
"""
super().__init__()
self.nextMessageTime = 0
self.buttonA = wpilib.DigitalInput(0)
self.buttonB: typing.Optional[wpilib.DigitalInput] = None
self.buttonC: typing.Optional[wpilib.DigitalInput] = None
self.yellowLed = wpilib.DigitalOutput(3)
self.greenLed: typing.Optional[wpilib.DigitalOutput] = None
self.redLed: typing.Optional[wpilib.DigitalOutput] = None
if dio1 == ChannelMode.INPUT:
self.buttonB = wpilib.DigitalInput(1)
else:
self.greenLed = wpilib.DigitalOutput(1)
if dio2 == ChannelMode.INPUT:
self.buttonC = wpilib.DigitalInput(2)
else:
self.redLed = wpilib.DigitalOutput(2)
def __init__(self, OUT, OE, S0, S1, S2, S3):
self.OUT = wpilib.Counter(OUT)
self.OE = wpilib.DigitalOutput(OE)
#Frequencies
self.S0 = wpilib.DigitalOutput(S0)
self.S1 = wpilib.DigitalOutput(S1)
#Colours
self.S2 = wpilib.DigitalOutput(S2)
self.S3 = wpilib.DigitalOutput(S3)
self.maxVal = 1000000 # Max output value
self.S0.set(True) # 100% Output Frequency
self.S1.set(True)
def robotInit(self):
self.dNaught = 18.0416
self.aNaught = 5946
self.joy = wpilib.Joystick(0)
self.turret = Turret()
self.visionTime = False
wpilib.DigitalOutput(0).set(0)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pot = wp.AnalogInput(0)
self.led = wp.DigitalOutput(0)
self.led.enablePWM(self.pot.getVoltage() / 5)
def createObjects(self):
'''Create motors and stuff here'''
# Objects that are created here are shared with all classes
# that declare them. For example, if I had:
# self.elevator_motor = wpilib.TalonSRX(2)
# here, then I could have
# class Elevator:
# elevator_motor = wpilib.TalonSRX
# and that variable would be available to both the MyRobot
# class and the Elevator class. This "variable injection"
# is especially useful if you want to certain objects with
# multiple different classes.
# create the imu object
self.bno055 = BNO055()
# the "logger" - allows you to print to the logging screen
# of the control computer
self.logger = logging.getLogger("robot")
# the SmartDashboard network table allows you to send
# information to a html dashboard. useful for data display
# for drivers, and also for plotting variables over time
# while debugging
self.sd = NetworkTable.getTable('SmartDashboard')
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = XboxController(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
self.drive_motor_a = CANTalon(2)
self.drive_motor_b = CANTalon(5)
self.drive_motor_c = CANTalon(4)
self.drive_motor_d = CANTalon(3)
self.gear_alignment_motor = CANTalon(14)
self.winch_motor = CANTalon(11)
self.winch_motor.setInverted(True)
self.rope_lock_solenoid = wpilib.DoubleSolenoid(forwardChannel=0,
reverseChannel=1)
# self.rope_lock_solenoid = wpilib.DoubleSolenoid(forwardChannel=3,
# reverseChannel=4)
self.gear_push_solenoid = wpilib.Solenoid(2)
self.gear_drop_solenoid = wpilib.Solenoid(3)
# self.gear_push_solenoid = wpilib.DoubleSolenoid(forwardChannel=1, reverseChannel=2)
# self.gear_drop_solenoid = wpilib.Solenoid(0)
self.test_trajectory = generate_trapezoidal_trajectory(
0, 0, 3, 0, Chassis.max_vel, 1, -1, Chassis.motion_profile_freq)
self.throttle = 1.0
self.direction = 1.0
self.led_dio = wpilib.DigitalOutput(1)
self.compressor = wpilib.Compressor()
def robotInit(self):
'''Robot initialization function'''
self.motorFrontRight = wp.VictorSP(2)
self.motorBackRight = wp.VictorSP(4)
self.motorMiddleRight = wp.VictorSP(6)
self.motorFrontLeft = wp.VictorSP(1)
self.motorBackLeft = wp.VictorSP(3)
self.motorMiddleLeft = wp.VictorSP(5)
self.intakeMotor = wp.VictorSP(8)
self.shootMotor1 = wp.VictorSP(7)
self.shootMotor2 = wp.VictorSP(9)
self.liftMotor = wp.VictorSP(0)
self.gyro = wp.ADXRS450_Gyro(0)
self.accel = wp.BuiltInAccelerometer()
self.gearSensor = wp.DigitalInput(6)
self.LED = wp.DigitalOutput(9)
self.rightEncd = wp.Encoder(0, 1)
self.leftEncd = wp.Encoder(2, 3)
self.shootEncd = wp.Counter(4)
self.compressor = wp.Compressor()
self.shifter = wp.DoubleSolenoid(0, 1)
self.ptoSol = wp.DoubleSolenoid(2, 3)
self.kicker = wp.DoubleSolenoid(4, 5)
self.gearFlap = wp.DoubleSolenoid(6, 7)
self.stick = wp.Joystick(0)
self.stick2 = wp.Joystick(1)
self.stick3 = wp.Joystick(2)
#Initial Dashboard Code
wp.SmartDashboard.putNumber("Turn Left pos 1:", 11500)
wp.SmartDashboard.putNumber("Lpos 2:", -57)
wp.SmartDashboard.putNumber("Lpos 3:", 5000)
wp.SmartDashboard.putNumber("stdist:", 6000)
wp.SmartDashboard.putNumber("Turn Right pos 1:", 11500)
wp.SmartDashboard.putNumber("Rpos 2:", 57)
wp.SmartDashboard.putNumber("Rpos 3:", 5000)
wp.SmartDashboard.putNumber("pos 4:", 39)
wp.SmartDashboard.putNumber("-pos 4:", -39)
wp.SmartDashboard.putNumber("Time", 5)
wp.SmartDashboard.putBoolean("Shooting Auto", False)
wp.SmartDashboard.putNumber("P", .08)
wp.SmartDashboard.putNumber("I", 0.005)
wp.SmartDashboard.putNumber("D", 0)
wp.SmartDashboard.putNumber("FF", 0.85)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Left Turn Auto", 1)
self.chooser.addObject("Straight/Enc", 2)
self.chooser.addObject("Right Turn Auto", 3)
self.chooser.addObject("Straight/Timer", 5)
self.chooser.addObject("Shoot ONLY", 6)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def robotInit(self):
self.pop = Camara2.main()
wpilib.CameraServer.launch()
# NetworkTables.startClientTeam(5716)
logging.basicConfig(level=logging.DEBUG)
NetworkTables.initialize()
self.pc = NetworkTables.getTable("SmartDashboard")
# self.cond = threading.Condition()
# self.notified = [False]
#NetworkTables.initialize(server='roborio-5716-frc.local')
#NetworkTables.initialize()
#self.sd = NetworkTables.getTable('SmartDashboard')
# wpilib.CameraServer.launch()
# cap = cv2.VideoCapture(0)
# self.Video = VideoRecorder()
# wpilib.CameraServer.launch()
# self.chasis_controller = wpilib.Joystick(0)
self.timer = wpilib.Timer()
self.left_cannon_motor = wpilib.Talon(5)
self.right_cannon_motor = wpilib.Talon(6)
self.sucker = wpilib.Talon(7)
self.front_left_motor = wpilib.Talon(3)
self.rear_left_motor = wpilib.Talon(1)
self.front_right_motor = wpilib.Talon(4)
self.rear_right_motor = wpilib.Talon(2)
self.front_left_motor.setInverted(True)
self.color_wheel = wpilib.Talon(8)
self.drive = MecanumDrive(
self.front_left_motor,
self.rear_left_motor,
self.front_right_motor,
self.rear_right_motor)
#led
self.green_led = wpilib.DigitalOutput(0)
#cannon pneumatic
self.Compressor = wpilib.Compressor(0)
self.PSV = self.Compressor.getPressureSwitchValue()
self.cannon_piston = wpilib.Solenoid(0,5)
self.hook1 = wpilib.Solenoid(0,0)
self.hook2 = wpilib.Solenoid(0,7)
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 autonomousPeriodic(self):
#we should put something here at some point
circumference = 4 * math.pi
goDistanceY = 36 #inches
encoderPointsPerRev = 18
revsForFeeder = 500
x, y = self.drive.averageWheelPosition()
if y < (goDistanceY / circumference) * encoderPointsPerRev:
self.drive.move(0, .15, 0)
else:
self.drive.stationary()
if self.feeder.getPosition() < revsForFeeder * encoderPointsPerRev:
wpilib.DigitalOutput(0).set(1)
self.feeder.feed(self.feederSpeed)
self.turret.aim(
sd.getEntry('targetFittedHeight').getDouble(0),
sd.getEntry('targetYaw').getDouble(0))
else:
wpilib.DigitalOutput(0).set(0)
self.feeder.stop()
self.turret.flywheelManual(0)
def teleopPeriodic(self):
self.area = (sd.getEntry('targetFittedWidth').getDouble(0)) * (
sd.getEntry('targetFittedHeight').getDouble(0))
try:
self.distance = (self.aNaught / self.area) * self.dNaught
except:
print("no area")
self.distance = 1
print('the yaw is ' + str(sd.getEntry('targetYaw').getDouble(0)))
print('the pitch is ' + str(sd.getEntry('targetPitch').getDouble(0)))
#print('the distance is ' + str(self.distance*self.dNaught))
#print('turret position at ' + str(360*(wpilib.AnalogInput(4).getValue()/2522)))
if self.joy.getRawButton(7):
print('vision time')
wpilib.DigitalOutput(0).set(1)
self.turret.turretAlign(sd.getEntry('targetYaw').getDouble(0))
else:
wpilib.DigitalOutput(0).set(0)
self.turret.stopTurret()
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#traceback.print_stack()
m1 = wpilib.TalonSRX(MOTOR1)
m2 = wpilib.TalonSRX(MOTOR2)
self.robot_drive = wpilib.RobotDrive(m1, m2)
self.stick = wpilib.Joystick(0)
print(self.stick)
#pprint (vars(self.stick))
#self.button1 = wpilib.buttons.JoystickButton(self.stick, 0)
self.s1 = wpilib.DigitalOutput(SOLENOID)
self.CannonIsFiring = False
def robotInit(self):
self.navx = AHRS.create_spi()
wpilib.CameraServer.launch()
wpilib.DigitalOutput(0).set(0)
self.joystick = wpilib.Joystick(0)
self.auxiliary = wpilib.XboxController(1)
self.drive = DriveTrain()
self.climb = Climb(16) #climb ID
self.feeder = Feeder(9) #feeder ID
self.intake = Intake(11, 10) #intake ID, half moon ID
self.turret = Turret(15, 14) #rotate ID, flywheel ID
self.drive.zeroEncoders()
self.climb.coast()
self.intake.coast()
#constants
self.joystickDeadband = .2
self.scaling = .55
self.intakeSpeed = .35
self.halfMoonSpeed = .8
self.feederSpeed = .85
self.turretSpeed = .2
self.climbExtend = .35
self.climbContract = .75
self.manualFlywheelSpeed = 8.5 #volts
self.navx.reset()
self.kP = 0.0016
self.kI = 0.0003
self.kD = 0
self.driveAngleController = wpilib.controller.PIDController(
self.kP, self.kI, self.kD)
self.driveAngleController.enableContinuousInput(-180, 180)
wpilib.SmartDashboard.putNumber("Shooter RPM", 0)
def __init__(self):
Subsystem.__init__(self, "SampSubsystem")
self.spinSpeed = 0
self.motor = wpilib.NidecBrushless(0, 0)
#self.motor = ctre.WPI_TalonSRX(0)
self.motor.setExpiration(2.5)
self.motor.setSafetyEnabled(False)
#Show Motor status in NT
self.addChild("Motor", self.motor)
self.endSwitch = True
self.limitSwitch = wpilib.DigitalInput(1)
self.limitSwitchLow = wpilib.DigitalInput(2)
#m_simDevice = hal.SimDevice("LimitSwitch", 1)
#RobotBase.isReal()
#if m_simDevice :
# self.limitSwitch.setSimDevice(1)
self.counter = wpilib.Counter(self.limitSwitch)
self.onLED = wpilib.DigitalOutput(3)
def robotInit(self):
self.fan1 = wpilib.DigitalOutput(3)
self.fan2 = wpilib.DigitalOutput(4)
self.fan3 = wpilib.DigitalOutput(5)
self.timer = wpilib.Timer()
def __init__(self):
super().__init__('Intake')
self.intake_motor_closeOpen = wpilib.VictorSP(8)
self.intake_motor_rightWheel = wpilib.VictorSP(7)
self.intake_motor_leftWheel = wpilib.VictorSP(9)
self.limit_switch = wpilib.DigitalOutput(1)
def create_digital_output(channel):
return wpilib.DigitalOutput(channel)
def __init__(self):
super().__init__("LEDs")
self._led_red = wpilib.DigitalOutput(DIO_LED_RED)
self._led_green = wpilib.DigitalOutput(DIO_LED_GREEN)
class variableInitalize():
#Joysticks
stick = wpilib.Joystick(1)
#Visctors
DriveL1 = wpilib.Victor(2)
DriveL2 = wpilib.Victor(4)
DriveR1 = wpilib.Victor(1)
DriveR2 = wpilib.Victor(3)
shooterRoller = wpilib.Victor(9)
shooterNeck = wpilib.Victor(6)
bridgeWedge = wpilib.Victor(10)
elevator = wpilib.Victor(5)
frontFeed = wpilib.Victor(7)
backFeed = wpilib.Victor(8)
#Encoders
neckEncode = wpilib.Encoder(5, 6, True)
leftDriveEncode = wpilib.Encoder(3, 4, False)
rightDriveEncode = wpilib.Encoder(1, 2, False)
turretroller = wpilib.Encoder(7, 8, False)
#Relays
camLights = wpilib.Relay(1)
targetInd = wpilib.Relay(4)
compLights = wpilib.Relay(2)
#Timers
neckTimer = wpilib.Timer()
ballTimer = wpilib.Timer()
feedTimer = wpilib.Timer()
neckcTimer = wpilib.Timer()
rpmTime = wpilib.Timer()
rpmlockedTimer = wpilib.Timer()
autonTimer = wpilib.Timer()
rpmLockFlash = wpilib.Timer()
rpmTimer = wpilib.Timer()
#Digital Input
frontFeedDI = wpilib.DigitalInput(9)
backFeedDI = wpilib.DigitalInput(10)
bottomNeckDI = wpilib.DigitalInput(11)
topNeckDI = wpilib.DigitalInput(12)
#Digital Output
#trackingLight = wpilib.DigitalOutput(12)
ledNotification = wpilib.DigitalOutput(13)
#Analong
rpmPot = wpilib.AnalogChannel(1)
bridgePot = wpilib.AnalogChannel(2)
#Solinoid
breakRelay = wpilib.Solenoid(1)
#drive station
ds = wpilib.DriverStationLCD.GetInstance()
rateOfPot = potRate(rpmPot, rpmTimer)
def Wait(self, waitTime):
wpilib.Wait(waitTime)
def SimpleRobot(self):
return wpilib.SimpleRobot
def testInit(self):
self.drive.coast()
self.navx.reset()
print("Starting tests")
wpilib.DigitalOutput(0).set(1)
def checkSwitches(self):
#buttons list
intakeIn = self.joystick.getRawButton(2)
intakeOut = self.auxiliary.getXButton()
climbUp = self.auxiliary.getYButton()
climbDown = self.auxiliary.getAButton()
turretClockwise = self.auxiliary.getStickButton(
wpilib.interfaces.GenericHID.Hand.kRightHand)
turretCounterclockwise = self.auxiliary.getStickButton(
wpilib.interfaces.GenericHID.Hand.kLeftHand)
autoAim = self.auxiliary.getBumper(
wpilib.interfaces.GenericHID.Hand.kRightHand)
manualFlywheel = self.auxiliary.getBumper(
wpilib.interfaces.GenericHID.Hand.kLeftHand)
feederIn = self.auxiliary.getBButton()
manualFlywheelSpeed = wpilib.SmartDashboard.getNumber(
"Shooter RPM", 4000)
#intake control
if intakeIn:
self.intake.collect(
self.intakeSpeed,
self.halfMoonSpeed) #intake speed, half moon speed
elif intakeOut:
self.intake.expel(
self.intakeSpeed,
self.halfMoonSpeed) #intake speed, half moon speed
else:
self.intake.stop()
#climb control
if climbUp:
self.climb.extend(self.climbExtend)
elif climbDown:
self.climb.contract(self.climbContract)
else:
self.climb.stop()
if autoAim:
wpilib.DigitalOutput(0).set(1)
#,sd.getEntry('isValid').getBoolean()
self.turret.aim(
sd.getEntry('targetFittedHeight').getDouble(0),
sd.getEntry('targetYaw').getDouble(0))
wpilib.SmartDashboard.putNumber(
"rec height",
sd.getEntry('targetFittedHeight').getDouble(0))
if feederIn:
self.feeder.feed(self.feederSpeed)
else:
self.feeder.stop()
else:
#manual turret rotation
wpilib.DigitalOutput(0).set(0)
if turretClockwise:
self.turret.turretManual(
self.turretSpeed) #turret speed percent
elif turretCounterclockwise:
self.turret.turretManual(
-self.turretSpeed) #turret speed percent
else:
self.turret.turretManual(0)
#manual flywheel
if manualFlywheel:
self.turret.flywheelRPM(manualFlywheelSpeed)
if feederIn:
self.feeder.feed(self.feederSpeed)
else:
self.feeder.stop()
else:
self.turret.flywheelManual(0)
self.feeder.stop()
