def __init__(self, distance_per_pulse, robot): super().__init__() left_motors_instance = Left_Motors() right_motors_instance = Right_Motors() self.left_motors = left_motors_instance.left_motor_group self.right_motors = right_motors_instance.right_motor_group self.left_shifter = wpilib.Solenoid(0) self.right_shifter = wpilib.Solenoid(1) self.left_encoder = wpilib.Encoder(2,3) self.right_encoder = wpilib.Encoder(4,5) self.left_encoder.setDistancePerPulse(distance_per_pulse) self.right_encoder.setDistancePerPulse(distance_per_pulse) self.gyro = wpilib.ADXRS450_Gyro() self.robot_instance = robot self.drive = DifferentialDrive(self.left_motors, self.right_motors) self.gyro.reset() self.x = 0 self.y = 0 self.heading = math.pi/2 self.last_left_encoder_distance = 0 self.last_right_encoder_distance = 0
def robotInit(self): """Robot initialization function""" self.gyro = wpilib.ADXRS450_Gyro() self.gyro.reset() self.gyro.calibrate() self.modules = [ SwerveModule( 0.8 / 2 - 0.125, 0.75 / 2 - 0.1, CANSparkMax(9, MotorType.kBrushless), ctre.WPI_TalonFX(3), steer_reversed=False, drive_reversed=True, ), SwerveModule( -0.8 / 2 + 0.125, -0.75 / 2 + 0.1, CANSparkMax(7, MotorType.kBrushless), ctre.WPI_TalonFX(5), steer_reversed=False, drive_reversed=True, ), ] self.kinematics = SwerveDrive2Kinematics(self.modules[0].translation, self.modules[1].translation) self.joystick = wpilib.Joystick(0) self.spin_rate = 1.5
def robotInit(self): """Robot-wide initialization code should go here""" # Basic robot chassis setup self.stick = wpilib.Joystick(0) self.robot_drive = wpilib.RobotDrive(0, 1) # Position gets automatically updated as robot moves self.gyro = wpilib.ADXRS450_Gyro() # Use PIDController to control angle turnController = wpilib.PIDController(self.kP, self.kI, self.kD, self.kF, self.pidGet, output=self.pidWrite) turnController.setInputRange(-180.0, 180.0) turnController.setOutputRange(-1.0, 1.0) turnController.setAbsoluteTolerance(self.kToleranceDegrees) turnController.setContinuous(True) self.turnController = turnController self.rotateToAngleRate = 0 self.i = 0 self.sd = NetworkTables.getTable("SmartDashboard") self.hephestus = NetworkTables.getTable("hephestus")
def __init__(self, robot): """Save the robot object, and assign and save hardware ports connected to the drive motors.""" super().__init__(name = "drivetrain") self.robot = robot # The digital gyro plugged into the SPI port on RoboRIO self.gyro = wpilib.ADXRS450_Gyro() # Motors used for driving self.left = rev.CANSparkMax(1, rev.CANSparkMax.MotorType.kBrushless) self.leftB = rev.CANSparkMax(3, rev.CANSparkMax.MotorType.kBrushless) self.right = rev.CANSparkMax(2, rev.CANSparkMax.MotorType.kBrushless) self.rightB = rev.CANSparkMax(4, rev.CANSparkMax.MotorType.kBrushless) # TODO: switch to DifferentialDrive is the main object that deals with driving self.drive = DifferentialDrive(self.left, self.right) #TODO: These probably will not be the actual ports used self.left_encoder = wpilib.Encoder(2, 3) self.right_encoder = wpilib.Encoder(4, 5) # Encoders may measure differently in the real world and in # simulation. In this example the robot moves 0.042 barleycorns # per tick in the real world, but the simulated encoders # simulate 360 tick encoders. This if statement allows for the # real robot to handle this difference in devices. # TODO: Measure our encoder's distance per pulse if robot.isReal(): self.left_encoder.setDistancePerPulse(0.042) self.right_encoder.setDistancePerPulse(0.042) else: # Circumference in ft = 4in/12(in/ft)*PI self.left_encoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0) self.right_encoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0)
def robotInit(self): '''Robot initialization function''' # object that handles basic drive operations self.frontLeftMotor = wpilib.Spark(1) # self.middleLeftMotor = wpilib.Spark(4) self.rearLeftMotor = wpilib.Spark(2) self.frontRightMotor = wpilib.Spark(3) # self.middleRightMotor = wpilib.Spark(1) self.rearRightMotor = wpilib.Spark(4) self.ihigh_motor = wpilib.Spark(6) self.ilow_motor = wpilib.Spark(9) self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) self.myRobot = DifferentialDrive(self.left, self.right) self.myRobot.setExpiration(0.1) self.high = 0 self.low = 0 self.gameData = '' # joysticks 1 & 2 on the driver station self.Stick1 = wpilib.XboxController(0) self.Stick2 = wpilib.Joystick(1) self.aSolenoidLow = wpilib.DoubleSolenoid(2,3) self.aSolenoidHigh = wpilib.DoubleSolenoid(0,1) self.iSolenoid = wpilib.DoubleSolenoid(4,5) self.gyro = wpilib.ADXRS450_Gyro()
def __init__(self, robot): self.robot = robot # get drive motors self.frontLeftCim = ctre.WPI_TalonSRX(robotmap.frontLeftDrive) self.frontRightCim = ctre.WPI_TalonSRX(robotmap.frontRightDrive) self.backLeftCim = ctre.WPI_TalonSRX(robotmap.backLeftDrive) self.backRightCim = ctre.WPI_TalonSRX(robotmap.backRightDrive) # configure motors self.configureMotorCurrent(self.frontLeftCim) self.configureMotorCurrent(self.frontRightCim) self.configureMotorCurrent(self.backLeftCim) self.configureMotorCurrent(self.backRightCim) # reverse left side motors self.frontLeftCim.setInverted(True) self.backLeftCim.setInverted(True) # make drivetrain self.drivetrain = mecanumdrive.MecanumDrive(self.frontLeftCim, self.backLeftCim, self.frontRightCim, self.backRightCim) # setup encoders self.encoderLeft = wpilib.Encoder(aChannel=robotmap.leftEncoderChannelA, bChannel=robotmap.leftEncoderChannelB, reverseDirection=False, encodingType=wpilib.Encoder.EncodingType.k4X) self.encoderLeft.setPIDSourceType(wpilib.Encoder.PIDSourceType.kDisplacement) self.encoderLeft.setDistancePerPulse((6*math.pi)/(256)) self.encoderRight = wpilib.Encoder(aChannel=robotmap.rightEncoderChannelA, bChannel=robotmap.rightEncoderChannelB, reverseDirection=False, encodingType=wpilib.Encoder.EncodingType.k4X) self.encoderRight.setPIDSourceType(wpilib.Encoder.PIDSourceType.kDisplacement) self.encoderRight.setDistancePerPulse((6*math.pi)/(256)) # setup gyro self.gyro = wpilib.ADXRS450_Gyro(0) super().__init__()
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 createObjects(self): """ Create motors and stuff here """ # drivetrain motors self.right_front_motor = ctre.WPI_TalonSRX(robotmap.right_front_drive) self.right_back_motor = ctre.WPI_TalonSRX(robotmap.right_back_drive) self.left_front_motor = ctre.WPI_TalonSRX(robotmap.left_front_drive) self.left_back_motor = ctre.WPI_TalonSRX(robotmap.left_back_drive) # configure motors - current limit, ramp rate, etc. MotorConfigurator.bulk_config_drivetrain(self.right_front_motor, self.right_back_motor, self.left_front_motor, self.left_back_motor) # create motor groups based on side self.left_drive_motors = wpilib.SpeedControllerGroup( self.left_back_motor, self.left_front_motor) self.right_drive_motors = wpilib.SpeedControllerGroup( self.right_front_motor, self.right_back_motor) # encoders self.left_encoder = wpilib.Encoder( aChannel=robotmap.left_encoder_a, bChannel=robotmap.left_encoder_b, reverseDirection=False, encodingType=wpilib.Encoder.EncodingType.k4X) self.left_encoder.setPIDSourceType( wpilib.Encoder.PIDSourceType.kDisplacement) self.right_encoder = wpilib.Encoder( aChannel=robotmap.right_encoder_a, bChannel=robotmap.right_encoder_b, reverseDirection=False, encodingType=wpilib.Encoder.EncodingType.k4X) self.right_encoder.setPIDSourceType( wpilib.Encoder.PIDSourceType.kDisplacement) # create drivetrain based on groupings self.drive = wpilib.drive.DifferentialDrive(self.left_drive_motors, self.right_drive_motors) # ahrs gyro self.gyro = wpilib.ADXRS450_Gyro(0) self.gyro.calibrate() # oi class self.oi = OI.OI() # launch automatic camera capturing for main drive cam # TODO Mount camera wpilib.CameraServer.launch() # launch arduino code and start data server self.arduino_server = ArduinoServer() self.arduino_server.startServer()
def robotInit(self): '''Robot initialization function''' self.motorFrontRight = wp.VictorSP(2) self.motorBackRight = wp.VictorSP(4) self.motorMiddleRight = wp.VictorSP(6) self.motorFrontLeft = wp.VictorSP(1) self.motorBackLeft = wp.VictorSP(3) self.motorMiddleLeft = wp.VictorSP(5) self.intakeMotor = wp.VictorSP(8) self.shootMotor1 = wp.VictorSP(7) self.shootMotor2 = wp.VictorSP(9) self.liftMotor = wp.VictorSP(0) self.gyro = wp.ADXRS450_Gyro(0) self.accel = wp.BuiltInAccelerometer() self.gearSensor = wp.DigitalInput(6) self.LED = wp.DigitalOutput(9) self.rightEncd = wp.Encoder(0, 1) self.leftEncd = wp.Encoder(2, 3) self.shootEncd = wp.Counter(4) self.compressor = wp.Compressor() self.shifter = wp.DoubleSolenoid(0, 1) self.ptoSol = wp.DoubleSolenoid(2, 3) self.kicker = wp.DoubleSolenoid(4, 5) self.gearFlap = wp.DoubleSolenoid(6, 7) self.stick = wp.Joystick(0) self.stick2 = wp.Joystick(1) self.stick3 = wp.Joystick(2) #Initial Dashboard Code wp.SmartDashboard.putNumber("Turn Left pos 1:", 11500) wp.SmartDashboard.putNumber("Lpos 2:", -57) wp.SmartDashboard.putNumber("Lpos 3:", 5000) wp.SmartDashboard.putNumber("stdist:", 6000) wp.SmartDashboard.putNumber("Turn Right pos 1:", 11500) wp.SmartDashboard.putNumber("Rpos 2:", 57) wp.SmartDashboard.putNumber("Rpos 3:", 5000) wp.SmartDashboard.putNumber("pos 4:", 39) wp.SmartDashboard.putNumber("-pos 4:", -39) wp.SmartDashboard.putNumber("Time", 5) wp.SmartDashboard.putBoolean("Shooting Auto", False) wp.SmartDashboard.putNumber("P", .08) wp.SmartDashboard.putNumber("I", 0.005) wp.SmartDashboard.putNumber("D", 0) wp.SmartDashboard.putNumber("FF", 0.85) self.chooser = wp.SendableChooser() self.chooser.addDefault("None", 4) self.chooser.addObject("Left Turn Auto", 1) self.chooser.addObject("Straight/Enc", 2) self.chooser.addObject("Right Turn Auto", 3) self.chooser.addObject("Straight/Timer", 5) self.chooser.addObject("Shoot ONLY", 6) wp.SmartDashboard.putData("Choice", self.chooser) wp.CameraServer.launch("vision.py:main")
def __init__(self): self.frontLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX(frontLeftTalonSRX) self.rearLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX(rearLeftTalonSRX) self.frontRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX( frontRightTalonSRX) self.rearRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX(rearRightTalonSRX) self.gyro = wpilib.ADXRS450_Gyro() self.mecanumDrive = wpilib.drive.MecanumDrive(self.frontLeftMotor, self.rearLeftMotor, self.frontRightMotor, self.rearRightMotor)
def createObjects(self): self.fl_motor = CANTalon(0) self.fr_motor = CANTalon(1) self.bl_motor = CANTalon(2) self.br_motor = CANTalon(3) self.robot_drive = wpilib.RobotDrive(self.fl_motor, self.fr_motor, self.bl_motor, self.br_motor) self.drivetrain_gyro = wpilib.ADXRS450_Gyro() self.bunny_motor = wpilib.Talon(0) self.flipper_motor = wpilib.Talon(1) self.drive_joystick = wpilib.Joystick(0) self.operator_joystick = wpilib.Joystick(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.JOYSTICK_DEADZONE_THRESHOLD = .5 #self.gyro = navx.AHRS.create_spi() self.gyro = wpilib.ADXRS450_Gyro()
def robotInit(self): '''Robot initialization function''' self.leftMotor1 = ctre.WPI_VictorSPX(1) #motor initialization self.leftMotor2 = ctre.WPI_VictorSPX(3) self.leftMotor3 = ctre.WPI_VictorSPX(5) self.rightMotor1 = ctre.WPI_VictorSPX(0) self.rightMotor2 = ctre.WPI_VictorSPX(2) self.rightMotor3 = ctre.WPI_VictorSPX(4) self.armMotor = ctre.WPI_VictorSPX(6) self.leftIntakeMotor = ctre.WPI_VictorSPX(7) self.rightIntakeMotor = ctre.WPI_VictorSPX(8) self.leftSide = wp.SpeedControllerGroup( self.leftMotor1, self.leftMotor2, self.leftMotor3) #speed controller groups self.rightSide = wp.SpeedControllerGroup(self.rightMotor1, self.rightMotor2, self.rightMotor3) self.intakeMotors = wp.SpeedControllerGroup(self.leftIntakeMotor, self.rightIntakeMotor) self.myRobot = DifferentialDrive(self.leftSide, self.rightSide) self.myRobot.setExpiration(0.1) self.leftStick = wp.Joystick(0) self.rightStick = wp.Joystick(1) self.armStick = wp.Joystick(2) self.gyro = wp.ADXRS450_Gyro(0) self.rightEncd = wp.Encoder(0, 1) self.leftEncd = wp.Encoder(2, 3) self.compressor = wp.Compressor() wp.SmartDashboard.putNumber("Straight Position", 4000) self.chooser = wp.SendableChooser() self.chooser.addDefault("None", 4) self.chooser.addObject("Straight/Enc", 1) self.chooser.addObject("Left Turn Auto", 2) self.chooser.addObject("Right Turn Auto", 3) self.chooser.addObject("Straight/Timer", 5) wp.SmartDashboard.putData("Choice", self.chooser) wp.CameraServer.launch("vision.py:main")
def robotInit(self): '''Robot-wide initialization code should go here''' self.lstick = wpilib.Joystick(0) self.rstick = wpilib.Joystick(1) self.l_motor = wpilib.Jaguar(1) self.r_motor = wpilib.Jaguar(2) # Position gets automatically updated as robot moves self.gyro = wpilib.ADXRS450_Gyro() self.robot_drive = wpilib.RobotDrive(self.l_motor, self.r_motor) self.motor = wpilib.Jaguar(4) self.limit1 = wpilib.DigitalInput(1) self.limit2 = wpilib.DigitalInput(2) self.position = wpilib.AnalogInput(2)
def __init__(self): '''Instantiates the drivetrain object.''' super().__init__('Drivetrain') self.frontLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX( RobotMap.drivetrain.frontLeftMotor) self.frontRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX( RobotMap.drivetrain.frontRightMotor) self.rearLeftMotor = ctre.wpi_talonsrx.WPI_TalonSRX( RobotMap.drivetrain.rearLeftMotor) self.rearRightMotor = ctre.wpi_talonsrx.WPI_TalonSRX( RobotMap.drivetrain.rearRightMotor) self.frontLeftMotor.setInverted(True) self.rearLeftMotor.setInverted(True) self.frontRightMotor.setInverted(True) self.rearRightMotor.setInverted(True) self.leftMotors = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor) self.rightMotors = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor) self.drivetrain = DifferentialDrive(self.leftMotors, self.rightMotors) self.lastMoveValue = 0 self.lastRotateValue = 0 self.gyro = wpilib.ADXRS450_Gyro() self.setpoint = 0 self.angleAcc = 0 self.anglePreviousTime = time.time() self.rangeFinder = wpilib.AnalogInput(0) self.didResetRevos = True self.startTime = 0 self.revoVelos = [] self.gyro.calibrate()
def __init__(self): super().__init__() # Initiate up robot drive self.left_motor = wpilib.Spark(LEFT_PORT) self.right_motor = wpilib.Spark(RIGHT_PORT) self.drive = DifferentialDrive(self.left_motor, self.right_motor) self.drive.setExpiration(.1) # Initiate arm and lift self.robot_lift = wpilib.PWMTalonSRX(LIFT_PORT) self.robot_lift_2 = wpilib.PWMTalonSRX(LIFT_PORT_2) # Initiate button stuff self.buttons = set() self.button_toggles = None self.pressed_buttons = {} self.rumble_time = None self.rumble_length = RUMBLE_LENGTH self.reset_buttons() # Initiate gamepad self.game_pad = wpilib.Joystick(GAMEPAD_NUM) self.max_enhancer = 0 self.smart_dashboard = wpilib.SmartDashboard # auto_chooser = wpilib.SendableChooser() # auto_chooser.addDefault("Enable Auto", "Enable Auto") # auto_chooser.addObject("Disable Auto", "Disable Auto") self.smart_dashboard.putBoolean("Enable Auto", True) self.auto_start_time = None self.current_speed = [0, 0] self.last_tank = 0 self.gyro = wpilib.ADXRS450_Gyro(0) # TODO: Figure out channel self.tank_dir = None
def createObjects(self): """Initialize all wpilib motors & sensors""" # camera # utrasoni sensors # motors self.robot = self self.left_motor = wpilib.Spark(0) self.right_motor = wpilib.Spark(1) self.left_motor.setInverted(True) self.right_motor.setInverted(True) self.lifter_motor = wpilib.Talon(2) self.ultrasonic = wpilib.AnalogInput(0) self.light_relay = wpilib.Relay(0) self.light_relay.set(wpilib.Relay.Value.kOn) self.myRobot = wpilib.RobotDrive(self.left_motor, self.right_motor) self.myRobot.setSafetyEnabled(False) #2Joysticks self.leftStick = wpilib.Joystick(0) #self.rightStick = wpilib.Joystick(1) # 5 motor controlors: 1colocter, 2 for weels, 1 for shooter #light #lifter: 1 motor # self.gyro = wpilib.ADXRS450_Gyro() wpilib.CameraServer.launch('vision.py:main')
def createObjects(self): # Drivetrain self.motor_fl0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fl"][0], rev.MotorType.kBrushless) # 11 self.motor_fr0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fr"][0], rev.MotorType.kBrushless) # 15 self.motor_rl0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rl"][0], rev.MotorType.kBrushless) # 13 self.motor_rr0 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rr"][0], rev.MotorType.kBrushless) # 17 self.motor_rr0.setInverted(True) self.motor_fr0.setInverted(True) # Set Motors to follow each other # Ignore secondary motors for simulation, not fully supported yet if not self.isSimulation(): self.motor_fr1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fr"][1], rev.MotorType.kBrushless) self.motor_fl1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["fl"][1], rev.MotorType.kBrushless) self.motor_rl1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rl"][1], rev.MotorType.kBrushless) self.motor_rr1 = rev.CANSparkMax(robotmap.DRIVE_MOTORS["rr"][1], rev.MotorType.kBrushless) self.motor_fl1.follow(self.motor_fl0) self.motor_fr1.follow(self.motor_fr0) self.motor_rl1.follow(self.motor_rl0) self.motor_rr1.follow(self.motor_rr0) self.drivetrain_train = wpilib.drive.MecanumDrive( self.motor_fl0, self.motor_rl0, self.motor_fr0, self.motor_rr0) self.drivetrain_gyro = wpilib.ADXRS450_Gyro() # Elevator self.elevator_motor1 = ctre.TalonSRX(robotmap.ELEVATOR["motors"][0]) self.elevator_motor2 = ctre.TalonSRX(robotmap.ELEVATOR["motors"][1]) self.elevator_motor2.follow(self.elevator_motor1) self.elevator_limit_switch_bottom = wpilib.DigitalInput( robotmap.ELEVATOR["lower"]) self.elevator_encoder = TalonEncoder(self.elevator_motor1) # Hatch self.hatch_hold_piston = wpilib.Solenoid( robotmap.INTAKE["hatch"]["actuator"]) self.hatch_lift_piston = wpilib.Solenoid( robotmap.INTAKE["hatch"]["lift"]) # Cargo self.cargo_motor = ctre.TalonSRX(robotmap.INTAKE["cargo"]["actuator"]) self.cargo_lift_piston = wpilib.Solenoid( robotmap.INTAKE["cargo"]["lift"]) # Climber self.climber_front_left_motor = ctre.VictorSPX( robotmap.CLIMBER_FL["motor"]) self.climber_front_right_motor = ctre.VictorSPX( robotmap.CLIMBER_FR["motor"]) self.climber_back_motor = ctre.VictorSPX( robotmap.CLIMBER_BACK["motor"]) self.climber_wheel_motor = ctre.VictorSPX(robotmap.CLIMBER_WHEELS) # Climber Limit Switches self.climber_front_left_upper_switch = wpilib.DigitalInput( robotmap.CLIMBER_FL["switch"]["top"]) self.climber_front_right_upper_switch = wpilib.DigitalInput( robotmap.CLIMBER_FR["switch"]["top"]) self.climber_front_left_lower_switch = wpilib.DigitalInput( robotmap.CLIMBER_FL["switch"]["bottom"]) self.climber_front_right_lower_switch = wpilib.DigitalInput( robotmap.CLIMBER_FR["switch"]["bottom"]) self.climber_back_upper_switch = wpilib.DigitalInput( robotmap.CLIMBER_BACK["switch"]["top"]) self.climber_back_lower_switch = wpilib.DigitalInput( robotmap.CLIMBER_BACK["switch"]["bottom"]) # Joystick 1 self.drive_joystick = wpilib.Joystick(0) self.slow_button = JoystickButton(self.drive_joystick, 1) self.bottom_tower_front_button = JoystickButton(self.drive_joystick, 9) self.bottom_tower_back_button = JoystickButton(self.drive_joystick, 10) self.perp_button = JoystickButton(self.drive_joystick, 6) self.top_tower_front_button = JoystickButton(self.drive_joystick, 7) self.top_tower_back_button = JoystickButton(self.drive_joystick, 8) self.climb_button = JoystickButton(self.drive_joystick, 3) self.climb_cancel_button = JoystickButton(self.drive_joystick, 4) # Joystick 2 self.op_joystick = wpilib.Joystick(1) self.tower_l1_button = JoystickButton(self.op_joystick, 1) self.tower_l2_button = JoystickButton(self.op_joystick, 2) self.tower_l3_button = JoystickButton(self.op_joystick, 3) self.elevator_load_button = JoystickButton(self.op_joystick, 4) self.hatch_panel_button = JoystickButton(self.op_joystick, 9) self.cargo_ball_button = JoystickButton(self.op_joystick, 10) self.defense_mode_button = JoystickButton(self.op_joystick, 5) self.intake_button = JoystickButton(self.op_joystick, 5) self.release_button = JoystickButton(self.op_joystick, 6) # Climb Joystick self.climb_joystick = wpilib.Joystick(2) self.climber_front_lift_button = JoystickButton(self.climb_joystick, 1) self.climber_back_lift_button = JoystickButton(self.climb_joystick, 2) self.climber_front_lower_button = JoystickButton( self.climb_joystick, 3) self.climber_back_lower_button = JoystickButton(self.climb_joystick, 4) self.climber_drive_button = JoystickButton(self.climb_joystick, 5) self.climber_reverse_button = JoystickButton(self.climb_joystick, 6)
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.
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ """ Button Map for Dual Joysticks Right Joystick (Intake): 1. Chute + Loader (100%) 2. Climb Up 3. Chute + Loader (50%) 4. Loader (50%) 5. Chute (50%) Left Joystick (Outtake): 1. Chute + Loader (-100%) 2. Climb Down 3. Chute + Loader (-50%) 4. Loader (-50%) 5. Chute (-50%) """ #Here is the encoder setup for the 4 motor drivetrain self.l_motorFront = ctre.wpi_talonsrx.WPI_TalonSRX(0) self.l_motorFront.setInverted(False) self.l_motorBack = ctre.wpi_talonsrx.WPI_TalonSRX(1) self.l_motorBack.setInverted(False) self.r_motorFront = ctre.wpi_talonsrx.WPI_TalonSRX(2) self.r_motorFront.setInverted(False) self.r_motorBack = ctre.wpi_talonsrx.WPI_TalonSRX(3) self.r_motorBack.setInverted(False) self.l_motorFront.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast) self.l_motorBack.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast) self.r_motorFront.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast) self.r_motorBack.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast) ## self.l_motorFront.configSelectedFeedbackSensor(ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0) ## self.r_motorFront.configSelectedFeedbackSensor(ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0) self.l_motorFront.setQuadraturePosition(0, 0) self.r_motorFront.setQuadraturePosition(0, 0) #Here is the encoder setup for the left and right chute motors self.l_chute = ctre.wpi_talonsrx.WPI_TalonSRX(4) self.l_chute.setInverted(False) self.r_chute = ctre.wpi_talonsrx.WPI_TalonSRX(5) self.r_chute.setInverted(False) self.l_chute.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast) self.r_chute.setNeutralMode(ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast) ## self.l_chute.configSelectedFeedbackSensor(ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0) ## self.r_chute.configSelectedFeedbackSensor(ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0) ## self.l_chute.setQuadraturePosition(0, 0) ## self.r_chute.setQuadraturePosition(0, 0) #This is the setup for the drive groups and loaders self.l_joy = wpilib.Joystick(0) self.r_joy = wpilib.Joystick(1) self.l_loader = wpilib.Spark(0) self.l_loader.setInverted(False) self.r_loader = wpilib.Spark(1) self.r_loader.setInverted(True) self.climb = wpilib.Spark(2) self.left = wpilib.SpeedControllerGroup(self.l_motorFront, self.l_motorBack) self.right = wpilib.SpeedControllerGroup(self.r_motorFront, self.r_motorBack) self.drive = wpilib.drive.DifferentialDrive(self.left, self.right) self.auto_switch0 = wpilib.DigitalInput(0) self.auto_switch1 = wpilib.DigitalInput(1) self.auto_switch2 = wpilib.DigitalInput(2) self.auto_switch3 = wpilib.DigitalInput(3) self.optical = wpilib.DigitalInput(4) self.gyro = wpilib.ADXRS450_Gyro(0) self.gyro.calibrate() self.gyro.reset() self.counter = 0
def createObjects(self): # GOTTA DO STUFF self.fork_switch = wpilib.DigitalInput(8) self.gyro = wpilib.ADXRS450_Gyro() 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.arm_motor = ctre.WPI_TalonSRX(8) # choose the sensor and sensor direction self.winch_motor.configSelectedFeedbackSensor( WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, self.kPIDLoopIdx, self.kTimeoutMs) # choose to ensure sensor is positive when output is positive self.winch_motor.setSensorPhase(True) # choose based on what direction you want forward/positive to be. # This does not affect sensor phase. self.winch_motor.setInverted(False) # set the peak and nominal outputs, 12V means full self.winch_motor.configNominalOutputForward(0, self.kTimeoutMs) self.winch_motor.configNominalOutputReverse(0, self.kTimeoutMs) self.winch_motor.configPeakOutputForward(1, self.kTimeoutMs) self.winch_motor.configPeakOutputReverse(-1, self.kTimeoutMs) # Set the allowable closed-loop error, Closed-Loop output will be # neutral within this range. See Table in Section 17.2.1 for native # units per rotation. self.winch_motor.configAllowableClosedloopError( 0, self.kPIDLoopIdx, self.kTimeoutMs) # set closed loop gains in slot0, typically kF stays zero - see documentation */ self.winch_motor.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx) self.winch_motor.config_kF(0, 0, self.kTimeoutMs) self.winch_motor.config_kP(0, 0.9, self.kTimeoutMs) self.winch_motor.config_kI(0, 0, self.kTimeoutMs) self.winch_motor.config_kD(0, 0, self.kTimeoutMs) # zero the sensor self.winch_motor.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs) # Elevator motor/sensors #self.elevator_top = wpilib.DigitalInput(1) #self.elevator_mid = wpilib.DigitalInput(2) #self.elevator_bottom = wpilib.DigitalInput(3) #Drive motors self.joy = wpilib.Joystick(0) wpilib.CameraServer.launch()
def create_adxrs450_gyro(port=None): return wpilib.ADXRS450_Gyro(port)
def robotInit(self): # Gear plate limit switch self.gear_switch = wpilib.DigitalInput(const.ID_GEAR_SWITCH) ### Subsystems ### # Agitator self.agitator = subsystems.agitator.Agitator(self) # Climber self.climber = subsystems.climber.Climber(self) # Drive for Xbox Controller self.drive = subsystems.drivetrain.Drivetrain(self) # Gear self.gear_funnel = subsystems.gear_funnel.Gear_Funnel(self) self.gear_lexan = subsystems.gear_lexan.Gear_Lexan(self) self.gear_claw = subsystems.gear_claw.Gear_Claw(self) # Ground Feeder self.feeder = subsystems.feeder.Feeder(self) # Shooter self.shooter = subsystems.shooter.Shooter(self) # Shooter Camera self.shooter_camera = subsystems.shooter_camera.Shooter_Camera(self) # Gear Camera self.gear_camera = subsystems.gear_camera.Gear_Camera(self) # Encoders self.drive_encoder_left = wpilib.Encoder(2, 3, reverseDirection=True) self.drive_encoder_left.setDistancePerPulse( const.DRIVE_DISTANCE_PER_ENCODER_TICK_OMNI) self.drive_encoder_left.reset() self.drive_encoder_right = wpilib.Encoder(0, 1, reverseDirection=False) self.drive_encoder_right.setDistancePerPulse( const.DRIVE_DISTANCE_PER_ENCODER_TICK_OMNI) self.drive_encoder_right.reset() # Pressure sensor (200 psi) self.pressure_sensor = wpilib.AnalogInput(const.ID_PRESSURE_SENSOR) self._pressure_samples = [] self._last_pressure_value = 0.0 # Gyro self.gyro = wpilib.ADXRS450_Gyro() #self.gyro = wpilib.AnalogGyro( 0 ) #self.gyro.setSensitivity( 0.08 ) ### Misc ### # Operator Input self.oi = oi.OI(self) # Log to file self.log_file = open( os.path.join(os.path.dirname(__file__), 'log.csv'), 'w') # Time robot object was created self.start_time = time.time() ## Autonomous ## self.subsystems = { 'agitator': self.agitator, 'climber': self.climber, 'drive': self.drive, 'gear_funnel': self.gear_funnel, 'gear_lexan': self.gear_lexan, 'gear_claw': self.gear_claw, 'feeder': self.feeder, 'shooter_camera': self.shooter_camera, 'gear_camera': self.gear_camera, 'shooter': self.shooter, } ## Scheduler ## self.scheduler = wpilib.command.Scheduler.getInstance() ## MISC ## self.gear_is_ejecting = False self.gear_ejecting_start_time = 0 ### Logging ### # NetworkTables self.nt_smartdash = networktables.NetworkTable.getTable( 'SmartDashboard') self.nt_grip_peg = networktables.NetworkTable.getTable( 'vision/peg_targets') self.nt_grip_boiler = networktables.NetworkTable.getTable( 'vision/boiler_targets') self.nt_vision = networktables.NetworkTable.getTable('vision') # Timers for NetworkTables update so we don't use too much bandwidth self.log_timer = wpilib.Timer() self.log_timer.start() self.log_timer_delay = 0.1 # 10 times/second # Timer for pressure sensor's running average self.pressure_timer = wpilib.Timer() self.pressure_timer.start() self.pressure_timer_delay = 1.0 # once per second self.log()
def robotInit(self): '''Robot initialization function''' self.leftMotor1 = wp.VictorSP(1) #motor initialization self.leftMotor2 = wp.VictorSP(3) self.leftMotor3 = wp.VictorSP(5) self.rightMotor1 = wp.VictorSP(2) self.rightMotor2 = wp.VictorSP(4) self.rightMotor3 = wp.VictorSP(6) self.armMotor = wp.VictorSP(7) self.leftIntakeMotor = wp.VictorSP(8) self.rightIntakeMotor = wp.VictorSP(9) self.leftSide = wp.SpeedControllerGroup( self.leftMotor1, self.leftMotor2, self.leftMotor3) #speed controller groups self.rightSide = wp.SpeedControllerGroup(self.rightMotor1, self.rightMotor2, self.rightMotor3) self.intakeMotors = wp.SpeedControllerGroup(self.leftIntakeMotor, self.rightIntakeMotor) self.myRobot = DifferentialDrive(self.leftSide, self.rightSide) self.myRobot.setExpiration(0.1) self.leftStick = wp.Joystick(0) self.rightStick = wp.Joystick(1) self.armStick = wp.XboxController(2) self.gyro = wp.ADXRS450_Gyro(0) self.rightEncd = wp.Encoder(2, 3) self.leftEncd = wp.Encoder(0, 1) self.armPot = wp.AnalogPotentiometer(0, 270, -135) self.compressor = wp.Compressor() self.shifter = wp.DoubleSolenoid(0, 1) wp.SmartDashboard.putNumber("Straight Position", 15000) wp.SmartDashboard.putNumber("leftMiddleAutoStraight", 13000) wp.SmartDashboard.putNumber("leftMiddleAutoLateral", 14000) wp.SmartDashboard.putNumber("Left Switch Pos 1", 18000) wp.SmartDashboard.putNumber("Left Switch Angle", 90) wp.SmartDashboard.putNumber("Left Switch Pos 2", 5000) wp.SmartDashboard.putNumber("Switch Score Arm Position", 70) wp.SmartDashboard.putNumber("Front Position 1", 74) wp.SmartDashboard.putNumber("Front Position 2", 16) wp.SmartDashboard.putNumber("Front Position 3", -63) wp.SmartDashboard.putNumber("lvl2 Position 1", 60) wp.SmartDashboard.putNumber("lvl2 Position 3", -50) wp.SmartDashboard.putNumber("lvl3 Position 3", -38) wp.SmartDashboard.putNumber("lvl3 Position 1", 45) wp.SmartDashboard.putBoolean("switchScore?", False) self.chooser = wp.SendableChooser() self.chooser.addDefault("None", 4) self.chooser.addObject("Straight/Enc", 1) self.chooser.addObject("Left Side Switch Auto", 2) self.chooser.addObject("Right Side Switch Auto (switch)", 3) self.chooser.addObject("Center Auto", 5) wp.SmartDashboard.putData("Choice", self.chooser) wp.CameraServer.launch("vision.py:main")
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ """ 1. Chute 2. Loader 3. Climber """ # Configure shooter motor controller. self.Gyro = wpilib.ADXRS450_Gyro() self.Chute = ctre.wpi_talonsrx.WPI_TalonSRX(7) self.Chute = ctre.wpi_talonsrx.WPI_TalonSRX( 8) # Create a CANTalon object. self.Chute.configSelectedFeedbackSensor( ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0 ) # Choose an encoder as a feedback device. The default should be QuadEncoder already, but might as well make sure. # I thought the encoder was 20 pulses per revolution per phase, which would require "80" as an argument below, but after trying it, it looks like there are 12. # Setting this parameter changes things so getPosition() returns decimal revolutions, and getSpeed() returns RPM. #self.shooter.configEncoderCodesPerRev(48) # resets shooter position on startup #self.unloader.setQuadraturePosition(0, 0) #self.unloader.setNeutralMode(ctre.wpilib.Spark.NeutralMode.Coast)# This should change between brake and coast modes. self.l_motor1 = ctre.wpi_talonsrx.WPI_TalonSRX(0) self.l_motor2 = ctre.wpi_talonsrx.WPI_TalonSRX(1) self.l_motor1.setInverted(False) self.l_motor2.setInverted(False) self.r_motor1 = ctre.wpi_talonsrx.WPI_TalonSRX(2) self.r_motor2 = ctre.wpi_talonsrx.WPI_TalonSRX(3) self.r_motor1.setInverted(False) self.r_motor2.setInverted(False) # Configure shooter motor controller. # Create a CANTalon object. self.l_motor1.setNeutralMode( ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast) self.l_motor1.configSelectedFeedbackSensor( ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0) self.l_motor2.setNeutralMode( ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast) self.l_motor2.configSelectedFeedbackSensor( ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0) self.l_motor1.setNeutralMode( ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast) self.r_motor1.configSelectedFeedbackSensor( ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0) self.l_motor2.setNeutralMode( ctre.wpi_talonsrx.WPI_TalonSRX.NeutralMode.Coast) self.r_motor2.configSelectedFeedbackSensor( ctre.wpi_talonsrx.WPI_TalonSRX.FeedbackDevice.QuadEncoder, 0, 0 ) # Choose an encoder as a feedback device. The default should be QuadEncoder already, but might as well make sure. # I thought the encoder was 20 pulses per revolution per phase, which would require "80" as an argument below, but after trying it, it looks like there are 12. # Setting this parameter changes things so getPosition() returns decimal revolutions, and getSpeed() returns RPM. #self.l_motor1.configEncoderCodesPerRev(48) #self.l_motor2.configEncoderCodesPerRev(48) #self.r_motor1.configEncoderCodesPerRev(48) #self.r_motor2.configEncoderCodesPerRev(48) # resets shooter position on startup self.l_motor1.setQuadraturePosition(0, 0) self.l_motor2.setQuadraturePosition(0, 0) self.r_motor1.setQuadraturePosition(0, 0) self.r_motor2.setQuadraturePosition(0, 0) #self.stick = wpilib.Joystick(0) self.l_joy = wpilib.Joystick(0) self.r_joy = wpilib.Joystick(1) self.loader = ctre.wpi_talonsrx.WPI_TalonSRX(4) self.climber = wpilib.Spark(6) self.loader = ctre.wpi_talonsrx.WPI_TalonSRX(5) self.drive = wpilib.RobotDrive(self.l_motor1, self.l_motor2, self.r_motor1, self.r_motor2) self.counter = 0 self.mode = 0