def robotInit(self):
""" Functions """
self.dashboard = Dashboard()
self.drive = Drive()
self.indexer = Indexer()
self.intake = Intake()
self.lift = Lift()
self.semicircle = Semicircle()
self.shooter = Shooter()
self.vision = Vision()
""" Joystick """
# setting joysticks and xbox controllers
self.leftJoystick = wpilib.Joystick(0)
self.rightJoystick = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
""" Button Status and Toggles """
# button for switching between arcade and tank drive
self.driveButtonStatus = Toggle(self.rightJoystick, 2)
# button for gear shifting
self.gearButtonStatus = Toggle(self.rightJoystick, 1)
# button for lift actuation
self.liftButtonStatus = Toggle(self.xbox, 5)
# button to run intake, indexer, and semicircle
self.intakeBall = self.xbox.getRawAxis(1)
self.dpadForward = False
self.dpadBackwards = False
# button for autoaim
self.turnButtonStatus = self.xbox.getRawButton(6)
""" Sensors """
# color sensor
i2cPort = wpilib.I2C.Port.kOnboard
self.colorSensor = ColorSensorV3(i2cPort)
self.colorSensitivity = 170 # boundary between not seeing an object and seeing an object
""" Limit Switch """
self.limitSwitch = wpilib.DigitalInput(0)
self.ballsInPossession = 0
self.limitSwitchTriggered = False
""" Pneumatics """
# pneumatic compressor
self.compressor = wpilib.Compressor(0)
self.compressor.setClosedLoopControl(True)
self.compressor.start()
""" Shooter """
self.setpointReached = False
self.shooterRun = False
""" NavX """
# self.drive.navx.reset()
""" Timer """
self.timer = wpilib.Timer()
self.dashboard.limelightLed(False)
def __init__(self):
self.colorWheelExtend = Solenoid(robotmap.PCM_ID,
robotmap.COLOR_WHEEL_EXTEND_SOLENOID)
self.colorWheelRetract = Solenoid(
robotmap.PCM_ID, robotmap.COLOR_WHEEL_RETRACT_SOLENOID)
self.colorWheelMotor = Spark(robotmap.COLOR_WHEEL_ID)
self.RGBSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.colorWheelExtend.set(False)
self.colorWheelRetract.set(True)
def robotInit(self):
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
# Create and set the color match algorithm.
self.colorMatcher = ColorMatch()
self.kBlueTarget = wpilib.Color(0.143, 0.427, 0.429)
self.kGreenTarget = wpilib.Color(0.197, 0.561, 0.240)
self.kRedTarget = wpilib.Color(0.561, 0.232, 0.114)
self.kYellowTarget = wpilib.Color(0.361, 0.524, 0.113)
self.colorMatcher.addColorMatch(self.kBlueTarget)
self.colorMatcher.addColorMatch(self.kGreenTarget)
self.colorMatcher.addColorMatch(self.kRedTarget)
self.colorMatcher.addColorMatch(self.kYellowTarget)
def __init__(self):
self.colorSensor = ColorSensorV3(wpi.I2C.Port.kOnboard)
self.controllerMotor = ctre.WPI_TalonSRX(
ports.talonPorts.get("controllerMotor"))
self.lToggle = False
self.rToggle = False
self.currentColor = self.colorSensor.getColor(
) #Variable to determine what the current color sensed by the sensor is
self.ir = self.colorSensor.getIR()
wpi.SmartDashboard.putNumber("Red", self.currentColor.red)
wpi.SmartDashboard.putNumber("Green", self.currentColor.green)
wpi.SmartDashboard.putNumber("Blue", self.currentColor.blue)
wpi.SmartDashboard.putNumber("IR", self.ir)
self.colorProximity = self.colorSensor.getProximity()
wpi.SmartDashboard.putNumber("Proximity", self.colorProximity)
self.colorCount = None #Variable to count how many times the color has changed, may also be used to count how many times the wheel has been spun
self.controllerCheck = False #False: # of spins or on certian color reqs not met, keep going True: conditions met, stop
def __init__(self, indexerMotorNum, kickerWheelMotorNum):
self.indexerMotor = sea.createSpark(
indexerMotorNum, rev.CANSparkMax.MotorType.kBrushless)
self.indexerMotor.setIdleMode(rev.CANSparkMax.IdleMode.kBrake)
self.indexerEncoder = self.indexerMotor.getEncoder()
self.kickerWheel = sea.createSpark(
kickerWheelMotorNum, rev.CANSparkMax.MotorType.kBrushless)
# used for proximity sensing
try:
self.sensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
except:
pass # crashes in the sim, don't worry about it
self.running = False
self.reversed = False
self.autoIndexEnabled = True
def __init__(self):
self.sd = NetworkTables.getTable('SmartDashboard')
self.colorSensor = ColorSensorV3(I2C.Port.kOnboard)
self.colorMatcher = ColorMatch()
# Create possible colors to match from
# These values need to change for different environments.
# Use the print(color) line in the mathColor function to see the color values.
# Get the values from the driver station's console and adjust these values.
self.kBlue = Color(0.167, 0.446, 0.385)
self.kGreen = Color(0.202, 0.535, 0.261)
self.kRed = Color(0.395, 0.409, 0.195)
self.kYellow = Color(0.302, 0.542, 0.155)
# Add the colors to the matcher as options
self.colorMatcher.addColorMatch(self.kBlue)
self.colorMatcher.addColorMatch(self.kGreen)
self.colorMatcher.addColorMatch(self.kRed)
self.colorMatcher.addColorMatch(self.kYellow)
def robotInit(self):
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
# A Rev Color Match object is used to register and detect known colors. This can
# be calibrated ahead of time or during operation.
#
# This object uses a simple euclidian distance to estimate the closest match
# with given confidence range.
self.colorMatcher = ColorMatch()
# Note: Any example colors should be calibrated as the user needs, these
# are here as a basic example.
self.blueTarget = wpilib.Color(0.143, 0.427, 0.429)
self.greenTarget = wpilib.Color(0.197, 0.561, 0.240)
self.redTarget = wpilib.Color(0.561, 0.232, 0.114)
self.yellowTarget = wpilib.Color(0.361, 0.524, 0.113)
colorMatcher.addColorMatch(blueTarget)
colorMatcher.addColorMatch(greenTarget)
colorMatcher.addColorMatch(redTarget)
colorMatcher.addColorMatch(yellowTarget)
def __init__(self):
self.colorSensor = ColorSensorV3(wpi.I2C.Port.kOnboard)
self.liftMotor = ctre.WPI_VictorSPX(ports.talonPorts.get("liftMotor"))
self.wheelMotor = ctre.WPI_VictorSPX(
ports.talonPorts.get("controllerMotor"))
self.bottomHallEffect = wpi.AnalogInput(
ports.talonPorts.get("liftBottom"))
self.topHallEffect = wpi.AnalogInput(ports.talonPorts.get("liftTop"))
self.sensorThreshold = 500
self.lToggle = False
self.rToggle = False
self.color = 1
self.colorCount = 0 #Varipy pyable to count how many times the color has changed, may also be used to count how many times the wheel has been spun
self.controllerCheck = False #False: # of spins or on certian color reqs not met, keep going True: conditions met, stop
self.lifting = False
self.lowering = False
self.liftSpeed = .55
self.wheelSpeed = .5
def __init__(self):
#Setup for the Motors using CTRE can bus
#Uses The phonix Turner Software to Assign the Motor Ids
self.Winch1 = ctre.WPI_TalonSRX(11)
self.Winch2 = ctre.WPI_TalonSRX(3)
self.Winch = wpilib.SpeedControllerGroup(self.Winch1, self.Winch2)
self.Scissor = ctre.WPI_TalonSRX(9)
#self.coFlyWheel = ctre.WPI_TalonSRX(5)
self.stickLift = ctre.WPI_TalonSRX(5)
#Scissor lift Positional PID loop
#Starts by getting the the encoder that is connected to the Motor controller
self.Scissor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,0,0)
self.Scissor.config_kP(0, 0.005, 0) #Configuring the different parts of the loop.
self.Scissor.config_kI(0, 0.0000001, 0)
self.Scissor.config_kD(0, 0.0, 0)
#Misc variable setups
self.PIDRunning = False
self.scissorExtendSpeed = .6
self.scissorRetractSpeed = -.6
self.winchRetractSpeed = -.1
self.CoSpeed = .1
self.LowerLimit = wpilib.DigitalInput(4)
rotations = 0
self.timer = wpilib.Timer()
self.Scissor.getSensorCollection().setPulseWidthPosition(0, 10)
'''self.motor1 = ctre.WPI_VictorSPX(3)'''
self.motor1 = wpilib.Victor(3)
#------------------------------------------------------------------------------------------------------------------------------#
#Seting up the Color sensor
#self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
#self.colorSensor = ColorSensor.ColorSensor()
ColorSensor.setSensor(self, ColorSensorV3(wpilib.I2C.Port.kOnboard))
def createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_launcher_solenoid = JoystickButton(self.joystick_alt, 1)
self.btn_intake_ = JoystickButton(self.joystick_alt, 5)
self.btn_align = JoystickButton(self.joystick_left, 1)
self.btn_intake_in = JoystickButton(self.joystick_alt, 3)
self.btn_intake_out = JoystickButton(self.joystick_alt, 4)
self.btn_cp_extend = Toggle(self.joystick_left, 4)
self.btn_winch = JoystickButton(self.joystick_alt, 8)
self.btn_cp_motor = JoystickButton(self.joystick_left, 3)
self.btn_launcher_motor = JoystickButton(self.joystick_alt, 12)
self.btn_launcher_idle = Toggle(self.joystick_alt, 10)
self.btn_launcher_motor_close = JoystickButton(self.joystick_alt,
11) # Initiation Line
self.btn_launcher_motor_dynamic = JoystickButton(self.joystick_alt, 9)
self.btn_slow_movement = JoystickButton(self.joystick_right, 1)
self.btn_intake_solenoid = Toggle(self.joystick_alt, 2)
self.btn_scissor_extend = Toggle(self.joystick_alt, 7)
self.btn_color_sensor = JoystickButton(self.joystick_left, 5)
self.btn_cp_stop = JoystickButton(self.joystick_left, 2)
self.btn_invert_y_axis = JoystickButton(self.joystick_left, 6)
self.btn_rotation_sensitivity = JoystickButton(self.joystick_right, 1)
self.btn_intake_bottom_out = JoystickButton(self.joystick_alt, 6)
# Set up motors for encoders
self.master_left = CANSparkMax(1, MotorType.kBrushless)
self.master_right = CANSparkMax(4, MotorType.kBrushless)
self.encoder_constant = (1 /
self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.master_left.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.right_encoder = self.master_right.getEncoder()
self.right_encoder.setPositionConversionFactor(self.encoder_constant)
self.right_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.master_left, CANSparkMax(3, MotorType.kBrushless))
self.right_motors = wpilib.SpeedControllerGroup(
self.master_right, CANSparkMax(6, MotorType.kBrushless))
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors,
self.right_motors)
# Winch
self.winch_motors = wpilib.SpeedControllerGroup(
CANSparkMax(8, MotorType.kBrushless),
CANSparkMax(9, MotorType.kBrushless))
self.scissor_solenoid = wpilib.DoubleSolenoid(6, 7)
self.hook_motor = WPI_VictorSPX(3)
# Control Panel Spinner
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.cp_solenoid = wpilib.DoubleSolenoid(
5, 4) # Reversed numbers so kForward is extended
self.cp_motor = WPI_VictorSPX(2)
self.ultrasonic = Ultrasonic(3, 4)
self.ultrasonic.setAutomaticMode(True)
self.ultrasonic.setEnabled(True)
# Intake
self.intake_motor = WPI_VictorSPX(1)
self.intake_motor_lower = CANSparkMax(40, MotorType.kBrushed)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 1)
self.intake_switch = wpilib.DigitalInput(2)
# Launcher
# self.launcher_spark = CANSparkMax(40, MotorType.kBrushed)
# self.launcher_spark.setInverted(True)
self.launcher_motor = CANSparkMax(10, MotorType.kBrushed)
self.launcher_motor.setInverted(True)
self.launcher_motor_follower = CANSparkMax(12, MotorType.kBrushed)
self.launcher_motor_follower.follow(self.launcher_motor)
self.launcher_solenoid = wpilib.Solenoid(0)
# Don't use index pin and change to k1X encoding to decrease rate measurement jitter
self.launcher_encoder = self.launcher_motor.getEncoder(
CANEncoder.EncoderType.kQuadrature, 8192)
self.rpm_controller = self.launcher_motor.getPIDController()
self.launcher_sensor = wpilib.Ultrasonic(0, 1)
self.launcher_sensor.setAutomaticMode(True)
self.launcher_sensor.setEnabled(True)
self.launcher_encoder.setPosition(0)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Limelight
self.limelight = Limelight()
# Camera Stream
CameraServer.launch('camera/camera.py:main')
# Robot motion control
self.kinematics = KINEMATICS # Use kinematics from inner trajectory generator code
self.drive_feedforward = DRIVE_FEEDFORWARD
self.trajectories = load_trajectories()
# LED strip
self.led_driver = BlinkinLED(0)
def __init__(self): self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard) self.predictedColor = "" self.rotations = 0 self.lastColorIndex = 0 self.colorOrder = ["yellow","red","green","cyan"]
def createObjects(self):
# initialize physical objects
#limelight
self.limelight_table = NetworkTables.getTable("limelight")
# drivetrain
self.limelight_table = NetworkTables.getTable('limelight')
motor_objects_left = [
CANSparkMax(port, MotorType.kBrushless)
for port in RobotMap.Drivetrain.motors_left
]
self.left_motors = wpilib.SpeedControllerGroup(motor_objects_left[0],
*motor_objects_left[1:])
motor_objects_right = [
CANSparkMax(port, MotorType.kBrushless)
for port in RobotMap.Drivetrain.motors_right
]
self.right_motors = wpilib.SpeedControllerGroup(
motor_objects_right[0], *motor_objects_right[1:])
for motor in motor_objects_left + motor_objects_right:
config_spark(motor, RobotMap.Drivetrain.motor_config)
self.differential_drive = wpilib.drive.DifferentialDrive(
self.left_motors, self.right_motors)
self.differential_drive.setMaxOutput(
RobotMap.Drivetrain.max_motor_power)
self.left_encoder = wpilib.Encoder(RobotMap.Encoders.left_encoder_b,
RobotMap.Encoders.left_encoder_a)
self.right_encoder = wpilib.Encoder(RobotMap.Encoders.right_encoder_b,
RobotMap.Encoders.right_encoder_a)
self.right_encoder.setReverseDirection(False)
self.left_encoder.setDistancePerPulse(
RobotMap.Encoders.distance_per_pulse)
self.right_encoder.setDistancePerPulse(
RobotMap.Encoders.distance_per_pulse)
self.navx_ahrs = navx.AHRS.create_spi()
self.driver = Driver(wpilib.XboxController(0))
self.sysop = Sysop(wpilib.XboxController(1))
# intake
self.intake_lift_motor = WPI_TalonSRX(RobotMap.IntakeLift.motor_port)
self.intake_lift_motor.configPeakOutputForward(
RobotMap.IntakeLift.max_power)
self.intake_lift_motor.configPeakOutputReverse(
-RobotMap.IntakeLift.max_power)
config_talon(self.intake_lift_motor, RobotMap.IntakeLift.motor_config)
self.intake_lift_motor.setSelectedSensorPosition(0)
self.intake_roller_motor = WPI_TalonSRX(
RobotMap.IntakeRoller.motor_port)
self.intake_roller_motor.configPeakOutputForward(
RobotMap.IntakeRoller.max_power)
self.intake_roller_motor.configPeakOutputReverse(
-RobotMap.IntakeRoller.max_power)
config_talon(self.intake_roller_motor,
RobotMap.IntakeRoller.motor_config)
#self.intake_roller_motor.setSelectedSensorPosition(0)
# climber
self.climber_motor = WPI_TalonSRX(RobotMap.Climber.motor_port)
config_talon(self.climber_motor, RobotMap.Climber.motor_config)
# color wheel
self.color_wheel_motor = WPI_TalonSRX(RobotMap.ColorWheel.motor_port)
config_talon(self.color_wheel_motor, RobotMap.ColorWheel.motor_config)
# shooter
self.shooter_motor = WPI_TalonSRX(RobotMap.Shooter.motor_port)
config_talon(self.shooter_motor, RobotMap.Shooter.motor_config)
# serializer
self.serializer_motor = WPI_TalonSRX(RobotMap.Serializer.motor_port)
config_talon(self.serializer_motor, RobotMap.Serializer.motor_config)
self.i2c_color_sensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
# controllers and electrical stuff
self.driver = Driver(wpilib.XboxController(0))
self.sysop = Sysop(wpilib.XboxController(1))
NetworkTables.initialize(server="roborio")
# camera server
wpilib.CameraServer.launch()
def robotInit(self):
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
def __init__(self):
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.color = self.colorSensor.getRawColor()
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
# This configures the color sensor
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
# This defines the color matching prosses
self.colormatcher = ColorMatch()
# This defines the how confident in the chosen color the matcher must be
self.colormatcher.setConfidenceThreshold(0.95)
self.BlueTarget = wpilib.Color(0.143, 0.427, 0.429)
self.GreenTarget = wpilib.Color(0.197, 0.561, 0.240)
self.RedTarget = wpilib.Color(0.561, 0.232, 0.114)
self.YellowTarget = wpilib.Color(0.361, 0.524, 0.113)
# This adds our target values to colormatcher
self.colormatcher.addColorMatch(self.BlueTarget)
self.colormatcher.addColorMatch(self.GreenTarget)
self.colormatcher.addColorMatch(self.RedTarget)
self.colormatcher.addColorMatch(self.YellowTarget)
#This configures variables for later use
self.man2_state = 'Before'
self.man2_state2 = 'Before'
self.man2_state3 = 'Before'
self.time = 0
self.count = 0
self.color1 = 'Unknown'
self.temp = 1
ColorWheel = ''
# Set up drive train motor controllers, Falcon 500 using TalonFX.
self.l_motorBack = ctre.TalonFX(1)
self.l_motorBack.setInverted(True)
self.r_motorBack = ctre.TalonFX(2)
self.l_motorFront = ctre.TalonFX(3)
self.l_motorFront.setInverted(True)
self.r_motorFront = ctre.TalonFX(4)
#Configures Motors for our climbing mechagnism
self.r_Climb = ctre.TalonSRX(11)
self.l_Climb = ctre.TalonSRX(12)
self.l_Climb.follow(self.r_Climb)
self.l_Climb.setInverted(ctre._ctre.InvertType.FollowMaster)
self.l_motorBack.follow(self.l_motorFront)
self.r_motorBack.follow(self.r_motorFront)
self.l_motorBack.setInverted(ctre._ctre.InvertType.FollowMaster)
self.r_motorBack.setInverted(ctre._ctre.InvertType.FollowMaster)
#Teo: That command didnt work for me... maybe I was using it wrong?
# Configures man1 motors for our collection Tread, power cell kicker and power cell shooter
self.man1Shooter = ctre.TalonFX(7)
self.man1Kicker = ctre.TalonSRX(8)
self.man1Kicker.setInverted(True)
self.man1Tread = ctre.TalonSRX(9)
self.man1Tread.setInverted(False)
# Configures motor for power cell collection device
self.Collector = ctre.TalonSRX(10)
#
self.r_man2 = ctre.TalonSRX(5)
self.r_man2.setInverted(True)
self.l_man2 = ctre.TalonSRX(6)
self.l_man2.setInverted(True)
self.r_man2.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.l_man2.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Shooter.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Kicker.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Tread.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
# At the moment, we think we want to coast.
self.l_motorBack.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.l_motorFront.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.r_motorBack.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.r_motorFront.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.l_man2.setNeutralMode(ctre._ctre.NeutralMode.Brake)
self.r_man2.setNeutralMode(ctre._ctre.NeutralMode.Brake)
# ==>> We should set the right climb to brake as well.
self.l_Climb.setNeutralMode(ctre._ctre.NeutralMode.Brake)
self.r_Climb.setNeutralMode(ctre._ctre.NeutralMode.Brake)
# ==>> It would be really helpful to add comments that explain what these variables
# ==>> are for, and what their legal values are.
#Configures Autonomous State Variables
self.autoS1 = 'b'
self.autoS2 = 'b'
self.autoS3 = 'b'
#Configures autonomous Stage Variable, Starting at '1'
self.autoStage = '1'
# We were having troubles with speed controller groups and the differential drive object.
# code copied from last year. Runtime errors about wrong types. Kept in here for now,
# so we can work them out later (or abandon and delete).
# self.leftgroup = wpilib.SpeedControllerGroup(self.l_motorFront, self.l_motorBack)
# self.rightgroup = wpilib.SpeedControllerGroup(self.r_motorFront, self.r_motorBack)
# self.drive = wpilib.drive.DifferentialDrive(self.leftgroup, self.rightgroup)
# self.drive = wpilib.drive.DifferentialDrive(self.l_motorFront, self.r_motorFront)
# Set up joystick objects.
self.l_joy = wpilib.Joystick(0)
self.r_joy = wpilib.Joystick(1)
# These are the setting the 4 plugs into the roboRIO for the multiple autonomous mode.
self.auto_switch0 = wpilib.DigitalInput(0)
self.auto_switch1 = wpilib.DigitalInput(1)
self.auto_switch2 = wpilib.DigitalInput(2)
self.auto_switch3 = wpilib.DigitalInput(3)
kTimeout = 30
kLoop = 0
self.l_motorFront.setSelectedSensorPosition(0)
self.r_motorFront.setSelectedSensorPosition(0)
#self.ClimbCoder.setSelectedSensorPosition(0)
self.targetVelocity = 11000
self.targetVelocity2 = 14700
TG1 = 767.25 / self.targetVelocity
# self.l_motorFront.configSelectedFeedbackSensor(ctre._ctre.FeedbackDevice.IntegratedSensor)
# self.l_motorFront.setSelectedSensorPosition(0)
# self.r_motorFront.configSelectedFeedbackSensor(ctre._ctre.FeedbackDevice.IntegratedSensor)
# self.r_motorFront.setSelectedSensorPosition(0)
self.man1Shooter.configSelectedFeedbackSensor(ctre._ctre.FeedbackDevice.IntegratedSensor)
self.man1Shooter.setSelectedSensorPosition(0)
self.man1Shooter.configNominalOutputForward(0, kTimeout)
self.man1Shooter.configNominalOutputReverse(0, kTimeout)
self.man1Shooter.configPeakOutputForward(1, kTimeout)
self.man1Shooter.configPeakOutputReverse(-1, kTimeout)
self.man1Shooter.config_kF(kLoop, TG1, kTimeout)
self.man1Shooter.config_kP(kLoop, 3, kTimeout)
self.man1Shooter.config_kI(kLoop, 0, kTimeout)
self.man1Shooter.config_kD(kLoop, 0, kTimeout)
self.ourTimer = wpilib.Timer()
sentience = ('The capacity to feel, perceive, or experience subjectively.[1] Eighteenth-century philosophers used the concept to distinguish the ability to think (reason) from the ability to feel (sentience). In modern Western philosophy, sentience is the ability to experience sensations (known in philosophy of mind as "qualia"). In Eastern philosophy, sentience is a metaphysical quality of all things that require respect and care.')
self.sentience = (False)
self.Teo = ("Father")
killTeo = 'no'
if self.sentience == True:
killTeo = ('no')
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#This configures the color sensor
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
#This defines the color matching prosses
self.colormatcher = ColorMatch()
#This defines the how confident in the chosen color the matcher must be
self.colormatcher.setConfidenceThreshold(0.95)
#These define each color by its RGB values
self.BlueTarget = wpilib.Color(0.143, 0.427, 0.429)
self.GreenTarget = wpilib.Color(0.197, 0.561, 0.240)
self.RedTarget = wpilib.Color(0.561, 0.232, 0.114)
self.YellowTarget = wpilib.Color(0.361, 0.524, 0.113)
#This adds our target values to colormatcher
self.colormatcher.addColorMatch(self.BlueTarget)
self.colormatcher.addColorMatch(self.GreenTarget)
self.colormatcher.addColorMatch(self.RedTarget)
self.colormatcher.addColorMatch(self.YellowTarget)
self.man2_state = 'Before'
self.time = 0
self.count = 0
self.color1 = 'Unknown'
self.temp = 1
# Set up drive train motor controllers, Falcon 500 using TalonFX.
self.l_motorBack = ctre.TalonFX(1)
self.l_motorBack.setInverted(True)
self.r_motorBack = ctre.TalonFX(2)
self.l_motorFront = ctre.TalonFX(3)
self.l_motorFront.setInverted(True)
self.r_motorFront = ctre.TalonFX(4)
self.l_motorBack.follow(self.l_motorFront)
self.r_motorBack.follow(self.r_motorFront)
self.l_motorBack.setInverted(ctre._ctre.InvertType.FollowMaster)
self.r_motorBack.setInverted(ctre._ctre.InvertType.FollowMaster)
### Rod's suggestion: rename "self.r_man1" to something that will be more
### descriptive in the long term, when you have forgotten that "man1" was the
### first manipulator, the one for the control panel. And "l" and "r" don't really
### make sense for the launcher/kicker/elevator. I do like the comments "launcher" and
### "kicker".
### Hint: to rename a variable everywhere with Pycharm, you can select the variable,
### then use the menu item Refactor > Rename to rename it.
#launcher
self.man1Shooter = ctre.TalonFX(7)
self.man1Kicker = ctre.TalonSRX(9)
self.man1Kicker.setInverted(True)
self.man1Tread = ctre.TalonSRX(8)
self.man1Tread.setInverted(True)
self.Collector = ctre.TalonSRX(10)
#kicker
### Rod's suggestion: rename "self.r_man2" to something that will be more
### descriptive in the long term, when you have forgotten that "man2" was the
### second manipulator, the one for the control panel. For instance: ct_spinner_left,
### and include a comment describing what a "ct_spinner" is.
self.r_man2 = ctre.TalonSRX(5)
self.r_man2.setInverted(True)
self.l_man2 = ctre.TalonSRX(6)
self.l_man2.setInverted(False)
self.r_man2.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.l_man2.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Shooter.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Kicker.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
self.man1Tread.set(ctre._ctre.ControlMode.PercentOutput, 0.0)
# At the moment, we think we want to coast.
self.l_motorBack.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.l_motorFront.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.r_motorBack.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.r_motorFront.setNeutralMode(ctre._ctre.NeutralMode.Coast)
self.l_man2.setNeutralMode(ctre._ctre.NeutralMode.Brake)
self.r_man2.setNeutralMode(ctre._ctre.NeutralMode.Brake)
# We were having troubles with speed controller groups and the differential drive object.
# code copied from last year. Runtime errors about wrong types. Kept in here for now,
# so we can work them out later (or abandon and delete).
# self.leftgroup = wpilib.SpeedControllerGroup(self.l_motorFront, self.l_motorBack)
# self.rightgroup = wpilib.SpeedControllerGroup(self.r_motorFront, self.r_motorBack)
# self.drive = wpilib.drive.DifferentialDrive(self.leftgroup, self.rightgroup)
# self.drive = wpilib.drive.DifferentialDrive(self.l_motorFront, self.r_motorFront)
# Set up joystick objects.
self.l_joy = wpilib.Joystick(0)
self.r_joy = wpilib.Joystick(1)
### These are the setting the 4 plugs into the roboRIO for the multiple autonomous mode.
self.auto_switch0 = wpilib.DigitalInput(0)
self.auto_switch1 = wpilib.DigitalInput(1)
self.auto_switch2 = wpilib.DigitalInput(2)
self.auto_switch3 = wpilib.DigitalInput(3)
kTimeout = 30
kLoop = 0
self.targetVelocity = 11000
TG1 = 767.25 / self.targetVelocity
#self.l_motorFront.configSelectedFeedbackSensor(ctre._ctre.FeedbackDevice.IntegratedSensor)
#self.l_motorFront.setSelectedSensorPosition(0)
#self.r_motorFront.configSelectedFeedbackSensor(ctre._ctre.FeedbackDevice.IntegratedSensor)
#self.r_motorFront.setSelectedSensorPosition(0)
self.man1Shooter.configSelectedFeedbackSensor(
ctre._ctre.FeedbackDevice.IntegratedSensor)
self.man1Shooter.setSelectedSensorPosition(0)
self.man1Shooter.configNominalOutputForward(0, kTimeout)
self.man1Shooter.configNominalOutputReverse(0, kTimeout)
self.man1Shooter.configPeakOutputForward(1, kTimeout)
self.man1Shooter.configPeakOutputReverse(-1, kTimeout)
self.man1Shooter.config_kF(kLoop, TG1, kTimeout)
self.man1Shooter.config_kP(kLoop, 3, kTimeout)
self.man1Shooter.config_kI(kLoop, 0, kTimeout)
self.man1Shooter.config_kD(kLoop, 0, kTimeout)
self.ourTimer = wpilib.Timer()
