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.