def __init__(self):
super().__init__()
self.dog = self.GetWatchdog()
self.stick1 = wpilib.Joystick(1)
self.stick2 = wpilib.Joystick(2)
self.stick3 = wpilib.Joystick(3)
self.leftArmStick = wpilib.KinectStick(1)
self.rightArmStick = wpilib.KinectStick(2)
self.motor1 = wpilib.CANJaguar(1)
self.motor2 = wpilib.CANJaguar(2)
self.leftArm = wpilib.Servo(1)
self.rightArm = wpilib.Servo(2)
self.leftLeg = wpilib.Servo(3)
self.rightLeg = wpilib.Servo(4)
self.spinner = wpilib.Servo(5)
self.compressor = wpilib.Compressor(1, 1)
self.compressor.Start()
self.relayMotor = wpilib.Relay(2)
self.solenoid1 = wpilib.Solenoid(1)
self.solenoid2 = wpilib.Solenoid(2)
def __init__(self):
print('CargoGrab: init called')
super().__init__('CargoGrab')
self.logPrefix = "CargoGrab: "
self.leftservo = wpilib.Servo(robotmap.cargograb.leftServoPort)
self.rightservo = wpilib.Servo(robotmap.cargograb.rightServoPort)
def __init__(self):
print('CargoGrab: init called')
super().__init__('CargoGrab')
self.logPrefix = "CargoGrab: "
try:
self.rightServo = wpilib.Servo(robotmap.cargograb.rightServoPort)
except Exception as e:
print("{}Exception caught instantiating right servo. {}".format(
self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.leftServo = wpilib.Servo(robotmap.cargograb.leftServoPort)
except Exception as e:
print("{}Exception caught instantiating left servo. {}".format(
self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
self.cargoToggle = False
self.openAngleRight = robotmap.cargograb.openAngleRight
self.closeAngleRight = robotmap.cargograb.closeAngleRight
self.openAngleLeft = robotmap.cargograb.openAngleLeft
self.closeAngleLeft = robotmap.cargograb.closeAngleLeft
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):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#Initialize Networktables
self.sd = NetworkTables.getTable('SmartDashboard')
#Set up motors to drive robot
self.M2 = wpilib.VictorSP(2)
self.M3 = wpilib.VictorSP(3)
#self.M2.setInverted(True)
#self.M3.setInverted(True)
self.left = wpilib.SpeedControllerGroup(self.M2, self.M3)
self.M0 = wpilib.VictorSP(0)
self.M1 = wpilib.VictorSP(1)
self.right = wpilib.SpeedControllerGroup(self.M0, self.M1)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.stick = wpilib.Joystick(1)
self.timer = wpilib.Timer()
#Camera
wpilib.CameraServer.launch()
#Servo
self.SV1 = wpilib.Servo(9)
self.SV2 = wpilib.Servo(8)
#Dashboard
NetworkTables.initialize(server='10.61.62.2')
#Switches
self.SW0 = wpilib.DigitalInput(0)
self.SW1 = wpilib.DigitalInput(1)
#Elevator
self.E = wpilib.VictorSP(5)
self.prepareCubeFlag = 0
self.grabCubeFlag = 0
self.deliverCubeFlag = 0
self.adjustLeftFlag = 0
self.adjustRightFlag = 0
self.driveFlag = 0
#Gyro
self.gyro = wpilib.ADXRS450_Gyro(0)
self.gyro.reset()
#All possible autonomous routines in a sendable chooser
'''
self.chooser = wpilib.SendableChooser()
self.chooser.addDefault("None", '4')
self.chooser.addObject("left-LeftScale", '1')
self.chooser.addObject("Middle-LeftScale", '2')
self.chooser.addObject("Right-LeftScale", '3')
self.chooser.addObject("Left-RightScale", '5')
'''
#wpilib.SmartDashboard.putData('Choice', self.chooser)
#Encoders
self.EC1 = wpilib.Encoder(2, 3)
self.EC2 = wpilib.Encoder(4, 5)
self.EC1.reset()
self.EC2.reset()
def robotInit(self):
# Construct the Network Tables Object
self.sd = NetworkTables.getTable('SmartDashboard')
self.sd.putNumber('RobotSpeed', .5)
self.leftArm = wpilib.Servo(self.leftArm)
self.leftArm.setBounds(2.0, 1.8, 1.5, 1.2, 1.0)
self.rightArm = wpilib.Servo(self.rightArm)
self.rightArm.setBounds(2.0, 1.8, 1.5, 1.2, 1.0)
self.stick = wpilib.XboxController(self.joystickChannel)
self.robotSpeed= self.sd.getNumber('RobotSpeed', .5)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.robot_drive = wpilib.RobotDrive(0,1,2,3)
self.stick = wpilib.Joystick(0)
self.Motor1 = wpilib.VictorSP(4)
self.Motor2 = wpilib.VictorSP(5)
self.Switch1 = wpilib.DigitalInput(0)
self.Switch2 = wpilib.DigitalInput(1)
self.Servo1 = wpilib.Servo(6)
self.Servo2 = wpilib.Servo(7)
def __init__(self):
self.enabled = False
NetworkTables.initialize(server='roborio-190-frc.local')
self.sd = NetworkTables.getTable('/SmartDashboard')
#servo
self.leftServo = wpilib.Servo(0)
self.rightServo = wpilib.Servo(1)
self.leftServo.set(0)
self.rightServo.set(0)
#camera stuff
self.leftCam = self.sd.getAutoUpdateValue('LeftCamera', False)
self.rightCam = self.sd.getAutoUpdateValue('RightCamera', False)
def robotInit(self):
self.sd = NetworkTables.getTable('SmartDashboard')
wpilib.CameraServer.launch('vision.py:camSrv1')
wpilib.CameraServer.launch('vision.py:camSrv2')
#Joystick/gamepad setup
self.stick1 = wpilib.Joystick(1) #Right
self.stick2 = wpilib.Joystick(2) #Left
self.gamepad = wpilib.Joystick(3) #Operator Controller
#Servo setup
self.rightServo = wpilib.Servo(1) #Right
self.leftServo = wpilib.Servo(2) #Left
#Drive Train Motor Setup
self.rightFrontMotor = wpilib.Talon(7) #Right front
self.rightBackMotor = wpilib.Talon(6) #Right back
self.leftFrontMotor = wpilib.Talon(8) #Left front
self.leftBackMotor = wpilib.Talon(9) #Left back
#Ball Fly Wheel Setup
#self.rightFly = wpilib.Talon(0) #Right Fly Wheel
#self.leftFly = wpilib.Talon(16) #Left Fly Wheel
#Arm Elevator Setup
#self.bottomLiftRight = wpilib.Talon(0) #Arm Lifter Right
#self.bottomLiftLeft = wpilib.Talon(16) #Arm Lifter Left
self.topLift = wpilib.Talon(0) #Wrist Lifter
#Limit Switch Setup
#self.limit = wpilib.DigitalInput(1)
#Climber Setup
#self.climberRight = wpilib.Talon(x)
#self.climberLeft = wpilib.Talon(x)
#self.climberBack = wpilib.Talon(x)
#self.climberWheel = wpilib.Talon(x)
#Robot Drive Setup
self.robotDrive = wpilib.RobotDrive(self.leftFrontMotor,
self.leftBackMotor,
self.rightFrontMotor,
self.rightBackMotor)
#Misc Variables Setup
self.intakeSpeed = 0.1
self.spitSpeed = -0.1
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.br_motor = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.bl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.fl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(5)
self.fr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(7)
self.fr_motor.setInverted(True)
self.br_motor.setInverted(True)
self.robot_drive = wpilib.RobotDrive(self.fl_motor, self.bl_motor,
self.fr_motor, self.br_motor)
self.joystick = wpilib.Joystick(0)
self.elevator_motor = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.elevator_follower = ctre.wpi_talonsrx.WPI_TalonSRX(10)
self.elevator_follower.follow(self.elevator_motor)
self.chomp_relay = wpilib.Relay(3)
self.servo = wpilib.Servo(2)
self.timer = wpilib.Timer()
self.initial_time = 0
self.chomp_pressed = False
self.chomp_position = 0
self.last_button_value = False
def robotInit(self):#This is a function or method
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.robot_drive = wpilib.RobotDrive(0,1,2,3)
self.stick = wpilib.Joystick(0)
self.Motor1 = wpilib.VictorSP(4)
self.Motor2 = wpilib.VictorSP(5)
self.Switch1 = wpilib.DigitalInput(0)
self.Switch2 = wpilib.DigitalInput(1)
self.Servo1 = wpilib.Servo(6)
self.Servo2 = wpilib.Servo(7)
self.camera1 = wpilib.CameraServer.launch('vision.py:main')
NetworkTables.initialize(server='10.61.62.2')
sd = NetworkTables.getTable('SmartDashboard')
sd.putNumber('someNumber', 1234)
def robotInit(self):
# Construct the Network Tables Object
self.sd = NetworkTables.getTable('SmartDashboard')
self.sd.putNumber('RobotSpeed', .5)
#self.motor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
"""Robot initialization function"""
self.frontLeftMotor = rev.CANSparkMax(self.frontLeftChannel,
rev.MotorType.kBrushless)
self.frontLeftMotor.restoreFactoryDefaults()
self.rearLeftMotor = rev.CANSparkMax(self.rearLeftChannel,
rev.MotorType.kBrushless)
self.rearLeftMotor.restoreFactoryDefaults()
self.frontRightMotor = rev.CANSparkMax(self.frontRightChannel,
rev.MotorType.kBrushless)
self.frontRightMotor.restoreFactoryDefaults()
self.rearRightMotor = rev.CANSparkMax(self.rearRightChannel,
rev.MotorType.kBrushless)
self.rearRightMotor.restoreFactoryDefaults()
#Servo for the shooter angle
#Lift
self.leftLift = rev.CANSparkMax(self.leftLift,
rev.MotorType.kBrushless)
#lift 1 is the motor that moves the hook up.
self.rightLift = rev.CANSparkMax(self.rightLift,
rev.MotorType.kBrushless)
#self.rightLift.setInverted(True)
self.rightLift.follow(self.leftLift, False)
#lift 2 is the motor that moves the hook down.
self.tilt = wpilib.Servo(0)
self.shooter = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.intake = ctre.WPI_VictorSPX(7)
#intake & shooter
# invert the left side motors
self.frontLeftMotor.setInverted(True)
# you may need to change or remove this to match your robot
self.rearLeftMotor.setInverted(True)
self.rearRightMotor.setInverted(True)
self.frontRightMotor.setInverted(True)
self.drive = MecanumDrive(
self.frontLeftMotor,
self.rearLeftMotor,
self.frontRightMotor,
self.rearRightMotor,
)
self.stick = wpilib.XboxController(self.joystickChannel)
self.stick2 = wpilib.XboxController(self.joystickChannel2)
self.robotSpeed = self.sd.getNumber('RobotSpeed', .5)
def robotInit(self):
"""Init is called once when the robot is turned on."""
self.efacing = 1
"""efacing is used to invert our controls."""
self.CarEncoder = wpilib.Encoder(0, 1)
#self.HatchEncoder = wpilib.Encoder(3, 4)
self.chooser = wpilib.SendableChooser()
self.chooser.addObject('cargo', '1')
self.chooser.addObject('hatch panel', '2')
self.left = wpilib.VictorSP(0)
self.right = wpilib.VictorSP(1)
"""Motors used for driving"""
self.myRobot = DifferentialDrive(self.left, self.right)
"""DifferentialDrive is the main object that deals with driving"""
self.RotServo = wpilib.Servo(2)
self.TiltServo = wpilib.Servo(3)
"""Our two servos will rotate (RotServo) and tilt (TiltServo) our
vision cameras."""
self.motor1 = wpilib.Talon(5)
self.motor2 = wpilib.Talon(6)
"""I mostly just added these motor controllers anticipating some sort
of intake system that uses motors."""
self.Punch = wpilib.DoubleSolenoid(0, 1)
self.DPunch = wpilib.DoubleSolenoid(3, 2)
"""The punching mechanism for removal of the hatch panels can use a
DoubleSolenoid or regular Solenoid. The Solenoid only needs the channel
it's plugged into (4) whereas the Double Solenoid needs the module
number, forward channel number, and reverse channel order in that
order."""
self.XBox0 = wpilib.XboxController(0)
self.XBox1 = wpilib.XboxController(1)
"""Xbox controllers 1 and 2 on the driver station."""
self.myRobot.setExpiration(0.1)
"""If communication is cut off between the driver station and the robot
def robotInit(self):
self.BRmotor = ctre.wpi_talonsrx.WPI_TalonSRX(40)
self.BLmotor = ctre.wpi_talonsrx.WPI_TalonSRX(50)
self.FRmotor = ctre.wpi_talonsrx.WPI_TalonSRX(10)
self.FLmotor = ctre.wpi_talonsrx.WPI_TalonSRX(30)
self.servo = wpilib.Servo(8)
self.timer = wpilib.Timer()
self.robot_drive = wpilib.RobotDrive(self.FLmotor, self.BLmotor, self.FRmotor, self.BRmotor)
self.joystick = wpilib.Joystick(0)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#Set up motors to drive robot
self.M2 = wpilib.VictorSP(2)
self.M3 = wpilib.VictorSP(3)
#self.M2.setInverted(True)
#self.M3.setInverted(True)
self.left = wpilib.SpeedControllerGroup(self.M2,self.M3)
self.M0 = wpilib.VictorSP(0)
self.M1 = wpilib.VictorSP(1)
self.SV1 = wpilib.Servo(8)
self.SV2 = wpilib.Servo(9)
self.right = wpilib.SpeedControllerGroup(self.M0,self.M1)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.stick = wpilib.Joystick(0)
self.timer = wpilib.Timer()
def robotInit(self):
self.servo = wpilib.Servo(6)
##INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
##INITIALIZE MOTORS##
self.lf_motor = wpilib.CANTalon(6)
self.lr_motor = wpilib.CANTalon(2)
self.rf_motor = wpilib.CANTalon(8)
self.rr_motor = wpilib.CANTalon(4)
##ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kRearRight, True)
def __init__(self):
super().__init__()
self.drive_stick = wpilib.Joystick(1)
self.arm_stick = wpilib.Joystick(2)
self.front_right_motor = wpilib.Jaguar(2)
self.front_left_motor = wpilib.Jaguar(1)
self.back_left_motor = wpilib.Jaguar(3)
self.back_right_motor = wpilib.Jaguar(4)
self.intake_wheels_motor = wpilib.Jaguar(10)
self.intake_arm_motor = wpilib.Jaguar(6)
self.shooter_servo = wpilib.Servo(7)
self.shooter_motor = wpilib.Jaguar(5)
self.encoder = wpilib.Encoder(1, 2, True)
self.mecanum_drive = MecanumDrive(self.front_right_motor,
self.front_left_motor,
self.back_right_motor,
self.back_left_motor,
self.drive_stick)
self.intake = Intake(self.intake_wheels_motor, self.intake_arm_motor,
self.arm_stick)
self.shooter_service = ShooterService(self.shooter_motor,
self.shooter_servo,
self.arm_stick)
self.shooter = Shooter(self.shooter_motor, self.encoder,
self.shooter_servo, self.arm_stick,
self.shooter_service)
self.autonomous = Autonomous(self.shooter_service,
self.intake_arm_motor,
self.front_left_motor,
self.front_right_motor,
self.back_left_motor,
self.back_right_motor)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.left_motor = ctre.WPI_TalonSRX(5)
self.right_motor = ctre.WPI_TalonSRX(6)
self.drive = wpilib.drive.DifferentialDrive(self.left_motor,
self.right_motor)
self.drive_stick = wpilib.Joystick(0)
#self.forklift_stick = wpilib.Joystick(1)
# Other motors
self.winch_motor = ctre.WPI_TalonSRX(7)
self.other_motor2 = ctre.WPI_TalonSRX(8)
self.forklift_fork = wpilib.Servo(0)
wpilib.SmartDashboard.putNumber('servo', 0)
def robotInit(self):
'''Robot Initiation'''
self.controller = wpilib.XboxController(0)
wpilib.CameraServer.launch("vision.py:main")
self.colorSensor = funct.ColorSensor(10, 11, 12, 13, 14,
15) # Color sensor for sorting
self.colorSensor.setFrequency(20)
self.correctColor = 'r' # Color for the color sensor to look for
self.sortMotor = wpilib.NidecBrushless(3, 3) # Nidec motor for sorting
self.sortSwitch = wpilib.DigitalInput(
0) # Switch to stop sorting motor
self.camServo = wpilib.Servo(2) # Camera servo motor
self.intake = wpilib.Talon(1) # Intake motor
self.door = wpilib.Talon(0) # Door motor
self.topSwitch = wpilib.DigitalInput(2) # Top switch for door motor
self.bottomSwitch = wpilib.DigitalInput(
1) # Bottom switch for door motor
# self.rf = rangefinder.MaxUltrasonic(0)
# Talon SRX #
# Right drivetrain
self.fr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(2) # 2
self.rr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(3) # 3
self.right = wpilib.SpeedControllerGroup(self.fr_motor, self.rr_motor)
# # Left drivetrain
self.fl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(0) # 0
self.rl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(1) # 1
self.left = wpilib.SpeedControllerGroup(self.fl_motor, self.rl_motor)
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
def createObjects(self):
# CARGO MECHANISM
self.cargo_mechanism_motor_rotator = ctre.WPI_TalonSRX(
robotmap.Ports.Cargo.rotator)
self.cargo_mechanism_servo_lock = wpilib.Servo(
robotmap.Ports.Cargo.locking_servo)
utils.configure_motor(
self.cargo_mechanism_motor_rotator, ctre.NeutralMode.Brake,
robotmap.MotorConfiguration.Cargo.peak_current,
robotmap.MotorConfiguration.Cargo.peak_current_duration)
self.cargo_mechanism_motor_rotator.setInverted(
robotmap.MotorConfiguration.Cargo.inverted)
# HATCH MECHANISM
self.hatch_grab_actuator = wpilib.DoubleSolenoid(
robotmap.Ports.Hatch.Grabber.pcm,
robotmap.Ports.Hatch.Grabber.front,
robotmap.Ports.Hatch.Grabber.back)
self.hatch_rack_actuator_left = wpilib.DoubleSolenoid(
robotmap.Ports.Hatch.Extension.pcm,
robotmap.Ports.Hatch.Extension.left_front,
robotmap.Ports.Hatch.Extension.left_back)
self.hatch_rack_actuator_right = wpilib.DoubleSolenoid(
robotmap.Ports.Hatch.Extension.pcm,
robotmap.Ports.Hatch.Extension.right_front,
robotmap.Ports.Hatch.Extension.right_back)
# DRIVETRAIN SYSTEM
# TODO fix motor port naming.
self.drivetrain_right_front = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.right_back)
self.drivetrain_right_back = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.right_bottom)
self.drivetrain_right_top = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.right_top)
self.drivetrain_left_front = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.left_front)
self.drivetrain_left_back = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.left_bottom)
self.drivetrain_left_top = ctre.WPI_TalonSRX(
robotmap.Ports.Drivetrain.Motors.left_top)
utils.configure_drivetrain_motors(self.drivetrain_left_back,
self.drivetrain_left_front,
self.drivetrain_left_top,
self.drivetrain_right_back,
self.drivetrain_right_front,
self.drivetrain_right_top)
self.drivetrain_right_motors = wpilib.SpeedControllerGroup(
self.drivetrain_right_back, self.drivetrain_right_front,
self.drivetrain_right_top)
self.drivetrain_left_motors = wpilib.SpeedControllerGroup(
self.drivetrain_left_back, self.drivetrain_left_front,
self.drivetrain_left_top)
self.differential_drive = wpilib.drive.DifferentialDrive(
self.drivetrain_left_motors, self.drivetrain_right_motors)
self.left_shifter_actuator = wpilib.DoubleSolenoid(
robotmap.Ports.Drivetrain.Shifters.pcm,
robotmap.Ports.Drivetrain.Shifters.left_front,
robotmap.Ports.Drivetrain.Shifters.left_back)
self.right_shifter_actuator = wpilib.DoubleSolenoid(
robotmap.Ports.Drivetrain.Shifters.pcm,
robotmap.Ports.Drivetrain.Shifters.right_front,
robotmap.Ports.Drivetrain.Shifters.right_back)
self.pressure_sensor_input = wpilib.AnalogInput(
robotmap.Ports.PressureSensor.port)
# MISC
self.oi = OI()
# run camera streaming program
wpilib.CameraServer.launch("vision.py:main")
self.current_camera = 0
self.camera_table = networktables.NetworkTables.getTable(
"/CameraPublisher")
# this is important for this year...
# self.use_teleop_in_autonomous = True
self.navx = navx.AHRS.create_spi()
def robotInit(self):
self.BUTTON_RBUMPER = 6
self.BUTTON_LBUMPER = 5
self.LY_AXIS = 1
self.LX_AXIS = 0
self.RX_AXIS = 4
self.RY_AXIS = 5
self.rev_per_ft = 12 / (math.pi * self.wheel_diameter)
self.br_motor = ctre.wpi_talonsrx.WPI_TalonSRX(40)
self.bl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(50)
self.fr_motor = ctre.wpi_talonsrx.WPI_TalonSRX(10)
self.fl_motor = ctre.wpi_talonsrx.WPI_TalonSRX(30)
self.br_motor.setInverted(True)
self.fr_motor.setInverted(True)
self.fl_motor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.bl_motor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.fr_motor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.br_motor.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0, MyRobot.TIMEOUT_MS)
self.fr_motor.setSensorPhase(False) # Reverse negative encoder values
self.br_motor.setSensorPhase(True)
self.deadzone_amount = 0.15
self.control_state = "speed"
self.start_navx = 0
self.previous_hyp = 0
self.js_startAngle = 0
self.rot_startNavx = 0
self.spinman = ctre.wpi_talonsrx.WPI_TalonSRX(5)
self.littlearms1 = wpilib.Servo(7)
self.littlearms2 = wpilib.Servo(8)
self.joystick = wpilib.Joystick(0)
NetworkTables.addEntryListener(self.entry_listener)
self.use_pid = False
self.prev_pid_toggle_btn_value = False
self.navx = navx.AHRS.create_spi()
self.relativeGyro = RelativeGyro(self.navx)
self.timer = wpilib.Timer()
self.init_time = 0
self.desired_rate = 0
self.pid_turn_rate = 0
def normalized_navx():
return self.get_normalized_angle(self.navx.getAngle())
self.angle_pid = wpilib.PIDController(self.turn_rate_p, self.turn_rate_i, self.turn_rate_d, self.relativeGyro.getAngle, self.set_pid_turn_rate)
#self.turn_rate_pid.
#self.turn_rate_pid.
self.angle_pid.setInputRange(-self.turn_rate_pid_input_range, self.turn_rate_pid_input_range)
self.angle_pid.setOutputRange(-self.turn_rate_pid_output_range, self.turn_rate_pid_output_range)
self.angle_pid.setContinuous(True)
self.turn_rate_values = [0] * 10
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):
"""`Gear` constructor. Constructs a `Gear` object.
"""
super().__init__('Gear Outake')
self.servo = wpilib.Servo(robotmap.portsList.gearDoorID)
def __init__(self, robot):
super().__init__()
self.pdp = robot.pdp
self.datalogger = robot.datalogger
self.currentAverageValues = 20
self.armCurrentValues = [0]*self.currentAverageValues
self.angleOffset = config.articulateAngleLow
self.tShooterL = wpilib.CANTalon(4)
self.tShooterR = wpilib.CANTalon(5)
self.tShooterL.setInverted(not config.isPracticeBot) # True for comp bot, false for practice
self.tShooterR.setInverted(config.isPracticeBot) # True for comp bot, false for practice
self.shooterEncoderType = wpilib.CANTalon.FeedbackDevice.CtreMagEncoder_Relative
self.tShooterL.setFeedbackDevice(self.shooterEncoderType)
self.tShooterR.setFeedbackDevice(self.shooterEncoderType)
self.shooterLPID = wpilib.PIDController(Kp=config.shooterLKp, Ki=config.shooterLKi, Kd=0,
kf=config.shooterLKf,
source=lambda: self.tShooterL.getSpeed() *
(-1 if config.isPracticeBot else 1),
output=self.tShooterL.set, period=20/1000) # Fast PID Loop
self.shooterLPID.setOutputRange(-1, 1)
self.shooterLPID.setInputRange(-18700/3, 18700/3)
self.shooterRPID = wpilib.PIDController(Kp=config.shooterRKp, Ki=config.shooterRKi, Kd=0.000,
kf=config.shooterRKf,
source=lambda: self.tShooterR.getSpeed() *
(-1 if not config.isPracticeBot else 1),
output=self.tShooterR.set, period=20/1000) # Fast PID Loop
self.shooterRPID.setOutputRange(-1, 1)
self.shooterRPID.setInputRange(-18700/3, 18700/3)
self.shooterLPID.setSetpoint(0)
self.shooterRPID.setSetpoint(0)
self.shooterLPID.enable()
self.shooterRPID.enable()
self.articulateEncoder = wpilib.Encoder(4, 5)
self.articulateEncoder.reset()
self.vArticulate = wpilib.VictorSP(1)
self.articulateEncoder.setDistancePerPulse(
(1440/(360*4)) * 1.5) # Clicks per degree / Magic numbers
self.articulatePID = wpilib.PIDController(Kp=config.articulateKp, Ki=config.articulateKi, Kd=config.articulateKd,
source=self.getAngle,
output=self.updateArticulate)
self.articulatePID.setOutputRange(-1, +1)
self.articulatePID.setInputRange(config.articulateAngleLow, config.articulateAngleHigh)
self.articulatePID.setSetpoint(35)
self.articulatePID.setPercentTolerance(100/130)
self.articulatePID.enable()
self.vIntake = wpilib.VictorSP(0)
self.sKicker = wpilib.Servo(2)
self.limLow = wpilib.DigitalInput(6)
self.limHigh = wpilib.DigitalInput(7)
self.lastKicker = False
# Data logger
self.datalogger.add_data_source("Left Shooter Motor", self.tShooterL.getSpeed)
self.datalogger.add_data_source("Right Shooter Motor", self.tShooterR.getSpeed)
self.datalogger.add_data_source("Articulate Angle", self.getAngle)
def robotInit(self):
self.drive1 = wpilib.Talon(1)
self.drive2 = wpilib.Talon(2)
self.drive1.setInverted(True)
self.shooter = wpilib.Talon(3)
self.cam = wpilib.Talon(4)
self.climber = wpilib.Talon(5)
#navx
self.navx = navx.AHRS.create_spi()
#Robot Driving Arcade
self.arcade_drive = wpilib.RobotDrive(self.drive2,
self.drive1) #Major change
#Solenoid me
self.arm1 = wpilib.DoubleSolenoid(0, 1, 2)
self.arm2 = wpilib.DoubleSolenoid(0, 3, 4)
self.bottoms_up = wpilib.Solenoid(0, 5)
#Camera servo in the front to be able to navigate around
self.servo = wpilib.Servo(0)
#TWO CONTROLLERS
self.controller = wpilib.Joystick(0)
self.second_controller = wpilib.Joystick(1)
#A button Second Controller
self.intake_button = wpilib.buttons.JoystickButton(
self.second_controller, 1)
#A button on Main
self.turn_button = wpilib.buttons.JoystickButton(self.controller, 1)
#X button for cancelling a rogue navx
self.cancel = wpilib.buttons.JoystickButton(self.controller, 2)
#Y Button on Second Controller
self.shooter_button = wpilib.buttons.JoystickButton(
self.second_controller, 4)
self.back_up = wpilib.buttons.JoystickButton(self.controller, 5)
#You can press X to line up your shot if need be
self.auto_alineX = wpilib.buttons.JoystickButton(self.controller, 3)
self.auto_alineY = wpilib.buttons.JoystickButton(self.controller, 4)
#Right bumper
self.right_bumper = wpilib.buttons.JoystickButton(
self.second_controller, 6)
self.left_bumper = wpilib.buttons.JoystickButton(
self.second_controller, 5)
#For controlling the servos, A and B button
self.low_goal = wpilib.buttons.JoystickButton(self.second_controller,
3)
#So you can always know where is front
self.ball_servo = wpilib.buttons.JoystickButton(
self.second_controller, 9)
self.forward_servo = wpilib.buttons.JoystickButton(
self.second_controller, 10)
#In case one of your slow drivers can't line up in time, you can hold off the sequence
self.hold_button = wpilib.buttons.JoystickButton(
self.second_controller, 2)
#Allows you to fine tine the shooter speed on the go if conditions change
self.higher_speed = wpilib.buttons.JoystickButton(
self.second_controller, 8)
self.lower_speed = wpilib.buttons.JoystickButton(
self.second_controller, 7)
#Make all the variables needed
self.shooter_piston = 1
self.speedShooter = 0
self.speedCam = 0
self.piston_update = "I DON'T KNOW"
#Timer stuff
self.timer = wpilib.Timer()
self.timer.start()
self.auto_timer = wpilib.Timer()
self.auto_timer.start()
self.vision_table = networktables.NetworkTable.getTable(
'/GRIP/myContoursReport')
self.vision_x = networktables.NumberArray()
self.vision_y = networktables.NumberArray()
self.vision_numberX = 0
self.vision_numberY = 0
self.automatedShooter = 0
#PIDs!!
kP = 0.3
kI = 0.00
kD = 0.00
kF = 0.00
turnController = wpilib.PIDController(kP,
kI,
kD,
kF,
self.navx,
output=self)
turnController.setInputRange(-180.0, 180.0)
turnController.setOutputRange(-.5, .5)
turnController.setAbsoluteTolerance(2.0)
turnController.setContinuous(True)
self.turnController = turnController
#encoder
self.encoder = wpilib.Encoder(9, 8)
self.encoder.setDistancePerPulse(4)
#Gets sends the options to the SmartDashboard
self.auto_chooser = wpilib.SendableChooser()
self.auto_chooser.addObject("High Goal, Low Bar", "1")
self.auto_chooser.addDefault("Crosser Low Bar", "2")
self.auto_chooser.addObject("Reacher", "3")
self.auto_chooser.addObject("Crosser RT", "4")
wpilib.SmartDashboard.putData('Choice', self.auto_chooser)
self.auto_aline_autoNow = False
self.shooter_counter = 0
#Shooter speed
self.shooter_high = .47
self.auto_aline_autoY = False
self.updater()
self.multiplier = .0000009
self.fire_counter = False
self.ready = False
self.auto_aline_auto = False
self.ready_aline = False
self.ready_alineX = False
self.turner = False
self.vision_state = 3
self.rotateReady = False
self.rotateToAngleRate = 0
self.totalSpeed = 0
self.auto_state_fire = 4
self.move = 0
self.rotationX = 0
self.focal_length = 160 / math.tan(math.radians(30.0))
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):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#Camera:
wpilib.CameraServer.launch()
#Counters
self.getCubeCounter = 0
self.dropCubeCounter = 0
self.elevatorDownCounter = 0
self.elevatorUpCounter = 0
self.cubeTravelUp = 50
self.cubeTravelStop = 1
#Drive Factor - adjust controller reponsiveness
self.driveFactor = 0.5
#Encoders - left and right, attached to gearbox
self.leftEncoder = wpilib.Encoder(4, 5)
self.rightEncoder = wpilib.Encoder(6, 7)
# Pneumatics:
self.leftGearShift = wpilib.Solenoid(5, 0)
self.rightGearShift = wpilib.Solenoid(5, 1)
self.goldenArrowhead = wpilib.Solenoid(5,
2) # Reference to Guyanese flag
# Include limit switches for the elevator and shoulder mechanisms
# 2018-2-16 Warning! The Switch's channel should be modified according to the robot! - Fixed
self.SW0 = wpilib.DigitalInput(0) #Lower Elevator Switch
self.SW1 = wpilib.DigitalInput(1) #Upper Elevator Switch
self.SW2 = wpilib.DigitalInput(2) #Lower shoulder switch
self.SW3 = wpilib.DigitalInput(3) #Upper shoulder switch
# Left Motor Group Setup
self.M0 = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.M1 = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.M0.setInverted(True)
self.M1.setInverted(True)
self.left = wpilib.SpeedControllerGroup(self.M0, self.M1)
# Right Motor Group Setup
self.M2 = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.M3 = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.right = wpilib.SpeedControllerGroup(self.M2, self.M3)
# Drive setup
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.drive.setMaxOutput(self.driveFactor)
# Misc Setting
self.stick = wpilib.Joystick(0)
self.timer = wpilib.Timer()
# E = Elevator
self.E1 = wpilib.VictorSP(0)
self.E2 = wpilib.VictorSP(1)
# Shoulder
self.S1 = wpilib.VictorSP(2)
self.S2 = wpilib.VictorSP(3)
#Servo
#channel number!
self.SV1 = wpilib.Servo(4)
def __init__(self, robot, pwm_channel, default):
super().__init__()
self.servo = wpilib.Servo(pwm_channel)
self.servo.set(default)
def robotInit(self):
self.sd = NetworkTable.getTable('SmartDashboard')
# #INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.ui_joystick = wpilib.Joystick(2)
self.pinServo = wpilib.Servo(6)
# hello
# #INITIALIZE MOTORS##
self.lf_motor = wpilib.Talon(0)
self.lr_motor = wpilib.Talon(1)
self.rf_motor = wpilib.Talon(2)
self.rr_motor = wpilib.Talon(3)
# #ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kRearRight, True)
# #INITIALIZE SENSORS#
self.sweeper_relay = wpilib.Relay(0)
self.gyro = wpilib.Gyro(0)
self.tote_motor = wpilib.CANTalon(5)
self.can_motor = wpilib.CANTalon(15)
self.sensor = Sensor(self.tote_motor, self.can_motor)
self.tote_forklift = ToteForklift(self.tote_motor, self.sensor, 2)
self.can_forklift = CanForklift(self.can_motor, self.sensor, 3)
self.calibrator = Calibrator(self.tote_forklift, self.sensor)
self.next_pos = 1
self.backSensor = SharpIRGP2Y0A41SK0F(6)
self.drive = drive.Drive(self.robot_drive, self.gyro, self.backSensor)
self.align = alignment.Alignment(self.sensor, self.tote_forklift,
self.drive)
self.components = {
'tote_forklift': self.tote_forklift,
'can_forklift': self.can_forklift,
'drive': self.drive,
'align': self.align,
'sensors': self.sensor
}
self.control_loop_wait_time = 0.025
self.automodes = AutonomousModeSelector('autonomous', self.components)
# #Defining Buttons##
self.canUp = Button(self.joystick1, 3)
self.canDown = Button(self.joystick1, 2)
self.canTop = Button(self.joystick1, 6)
self.canBottom = Button(self.joystick1, 7)
self.toteUp = Button(self.joystick2, 3)
self.toteDown = Button(self.joystick2, 2)
self.toteTop = Button(self.joystick2, 6)
self.toteBottom = Button(self.joystick2, 7)
self.ui_joystick_tote_down = Button(self.ui_joystick, 4)
self.ui_joystick_tote_up = Button(self.ui_joystick, 6)
self.ui_joystick_can_up = Button(self.ui_joystick, 5)
self.ui_joystick_can_down = Button(self.ui_joystick, 3)
self.reverseDirection = Button(self.joystick1, 1)
#self.alignTrigger = Button(self.joystick2, 1)
self.toteTo = 0
self.oldTote = 0
self.canTo = 0
self.oldCan = 0
self.reverseRobot = False
self.oldReverseRobot = False
self.autoLift = False
self.sd.putNumber('liftTo', 0)
self.sd.putNumber('binTo', 0)
self.sd.putBoolean('autoLift', False)
self.sd.putBoolean('reverseRobot', False)
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
#This initializes Networktables which will be used for communication between our robot and the GUI.
self.sd = NetworkTables.getTable('SmartDashboard')
#This initializes the camera.
wpilib.CameraServer.launch()
#This sets some counters for the state machines. (FOR Previous version without the encoders)
self.getCubeCounter = 0
self.dropCubeCounter = 0
self.elevatorDownCounter = 0
self.elevatorUpCounter = 0
self.cubeTravelUp = 50
self.cubeTravelStop = 1
#This sets some flags for the state machines.
self.prepareCubeFlag = 0 #This is the flag for the state machine which makes the robot prepared to grab the cube.
self.grabCubeFlag = 0 #This is the flag for the state machine which makes the robot grab the cube.
self.deliverCubeFlag = 0 #This is the flag for the state machine which makes the robot deliver the cube.
self.state = 0
#This sets the Drive Factor, which adjusts controller responsiveness.
self.driveFactor = 0.5
#This initializes the Encoders - left and right, attached to gearbox
self.EC1 = wpilib.Encoder(4, 5)
self.EC2 = wpilib.Encoder(6, 7)
#Encoder for the elevator and shoulder
self.EC3 = wpilib.Encoder(6,
7) #This sets the encoder for the elevator.
self.EC4 = wpilib.Encoder(8, 9) #This sets the encoder for the arm.
#This sets the Pneumatics.
self.leftGearShift = wpilib.Solenoid(5, 0)
self.rightGearShift = wpilib.Solenoid(5, 1)
self.goldenArrowhead = wpilib.Solenoid(5,
2) # Reference to Guyanese flag
#This controls the function of the arm.
# Include limit switches for the elevator and shoulder mechanisms
# 2018-2-16 Warning! The Switch's channel should be modified according to the robot! - Fixed
self.SW0 = wpilib.DigitalInput(0) #Lower Elevator Switch
self.SW1 = wpilib.DigitalInput(1) #Upper Elevator Switch
#self.SW2 = wpilib.DigitalInput(2) #Lower shoulder switch
#self.SW3 = wpilib.DigitalInput(3) #Upper shoulder switch
# Left Motor Group Setup
self.M0 = ctre.wpi_talonsrx.WPI_TalonSRX(4)
self.M1 = ctre.wpi_talonsrx.WPI_TalonSRX(3)
self.M0.setInverted(True) #This inverts the motors.
self.M1.setInverted(True)
self.left = wpilib.SpeedControllerGroup(self.M0, self.M1)
# Right Motor Group Setup
self.M2 = ctre.wpi_talonsrx.WPI_TalonSRX(2)
self.M3 = ctre.wpi_talonsrx.WPI_TalonSRX(1)
self.right = wpilib.SpeedControllerGroup(self.M2, self.M3)
# Drive setup
self.drive = wpilib.drive.DifferentialDrive(self.left, self.right)
self.drive.setMaxOutput(self.driveFactor)
# Misc Setting
self.stick = wpilib.Joystick(0)
self.timer = wpilib.Timer()
# E = Elevator
self.E1 = wpilib.VictorSP(0) #This initializes the left elevator.
self.E2 = wpilib.VictorSP(1) #This initializes the right elevator.
# Shoulder
self.S1 = wpilib.VictorSP(2) #This initializes the left shoulder.
self.S2 = wpilib.VictorSP(3) #This initializes the right shoulder.
#Servo
self.SV1 = wpilib.Servo(4) #This initializes a servo.
#self.SV2 = wpilib.Servo(5)
#self.SV1.set(0.0)
#self.SV2.set(0.0)
#Gyro
self.gyro = wpilib.ADXRS450_Gyro(
0) #This initializes a gyro to detect the direction of the robot.
self.gyro.reset() #This resets the gyro.
