def robotInit(self): # SPARK MAX controllers are intialized over CAN by constructing a # CANSparkMax object # # The CAN ID, which can be configured using the SPARK MAX Client, is passed # as the first parameter # # The motor type is passed as the second parameter. # Motor type can either be: # rev.MotorType.kBrushless # rev.MotorType.kBrushed # # The example below initializes four brushless motors with CAN IDs # 1, 2, 3, 4. Change these parameters to match your setup self.leftLeadMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.rightLeadMotor = rev.CANSparkMax(3, rev.MotorType.kBrushless) self.leftFollowMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.rightFollowMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) # Passing in the lead motors into DifferentialDrive allows any # commmands sent to the lead motors to be sent to the follower motors. self.driveTrain = DifferentialDrive(self.leftLeadMotor, self.rightLeadMotor) self.joystick = wpilib.Joystick(0) # The RestoreFactoryDefaults method can be used to reset the # configuration parameters in the SPARK MAX to their factory default # state. If no argument is passed, these parameters will not persist # between power cycles self.leftLeadMotor.restoreFactoryDefaults() self.rightLeadMotor.restoreFactoryDefaults() self.leftFollowMotor.restoreFactoryDefaults() self.rightFollowMotor.restoreFactoryDefaults() # In CAN mode, one SPARK MAX can be configured to follow another. This # is done by calling the follow() method on the SPARK MAX you want to # configure as a follower, and by passing as a parameter the SPARK MAX # you want to configure as a leader. # # This is shown in the example below, where one motor on each side of # our drive train is configured to follow a lead motor. self.leftFollowMotor.follow(self.leftLeadMotor) self.rightFollowMotor.follow(self.rightLeadMotor)
def __init__(self, driveID, turnID, encoderID, encoderOffset, name): if name == "Front Left": kPTurn = .006 kITurn = .002 elif name == "Front Right": kPTurn = .008 kITurn = .002 elif name == "Rear Left": kPTurn = .007 kITurn = .002 elif name == "Rear Right": kPTurn = .0066 kITurn = 0 else: kPTurn = .0066 kITurn = .0018 kDTurn = 0 self.driveMotor = rev.CANSparkMax(driveID, rev.MotorType.kBrushless) self.driveMotor.restoreFactoryDefaults() self.turnMotor = rev.CANSparkMax(turnID, rev.MotorType.kBrushless) self.turnEncoder = self.turnMotor.getEncoder() self.turnEncoder.setPositionConversionFactor( self.turnMotorEncoderConversion) #now is 0-360 self.driveEncoder = self.driveMotor.getEncoder() self.absoluteEncoder = wpilib.AnalogInput(encoderID) self.encoderOffset = encoderOffset self.turnController = wpilib.controller.PIDController( kPTurn, kITurn, kDTurn) self.turnController.enableContinuousInput( -180, 180) #the angle range we decided to make standard self.turnDeadband = .035 #this needs to be updated self.moduleName = name self.x = 0 self.y = 0 self.pastPosition = 0
def initDrivetrain(self): drivetrain = sea.SuperHolonomicDrive() leftSpark = rev.CANSparkMax(1, rev.MotorType.kBrushless) rightSpark = rev.CANSparkMax(2, rev.MotorType.kBrushless) for spark in [leftSpark, rightSpark]: spark.restoreFactoryDefaults() spark.setIdleMode(rev.IdleMode.kBrake) leftWheel = sea.AngledWheel(leftSpark, -1, 0, math.pi / 2, 1, 16) rightWheel = sea.AngledWheel(rightSpark, 1, 0, math.pi / 2, 1, 16) drivetrain.addWheel(leftWheel) drivetrain.addWheel(rightWheel) for wheel in drivetrain.wheels: wheel.driveMode = rev.ControlType.kVelocity return drivetrain
def __init__(self, driveID, turnID, encoderID, encoderOffset, name): self.driveMotor = rev.CANSparkMax(driveID, rev.MotorType.kBrushless) self.turnMotor = rev.CANSparkMax(turnID, rev.MotorType.kBrushless) self.turnEncoder = self.turnMotor.getEncoder() self.turnEncoder.setPositionConversionFactor( self.turnMotorEncoderConversion) #now is 0-360 self.absoluteEncoder = wpilib.AnalogInput(encoderID) self.encoderOffset = encoderOffset #the above line centers the absolute encoder and then changes its direction, as the absolute encoders spin #the opposite direction of the NEO encoders self.turnController = wpilib.controller.PIDController( self.kP, self.kI, self.kD) self.turnController.enableContinuousInput( -180, 180) #the angle range we decided to make standard self.turnDeadband = .035 self.moduleName = name
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ # Joystick self.stick = wpilib.Joystick(0) # Rev through CAN(lift) self.left_lift = rev.CANSparkMax(4, rev.MotorType.kBrushless) #Timer self.timer = wpilib.Timer()
def robotInit(self): #Controllers self.driver = wpilib.XboxController(0) self.operator = wpilib.XboxController(1) #Motors self.left_motor_1 = rev.CANSparkMax(robotmap.LEFT_LEADER_ID, rev.MotorType.kBrushed) self.left_motor_2 = rev.CANSparkMax(robotmap.LEFT_FOLLOWER_ID, rev.MotorType.kBrushed) self.right_motor_1 = rev.CANSparkMax(robotmap.RIGHT_LEADER_ID, rev.MotorType.kBrushed) self.right_motor_2 = rev.CANSparkMax(robotmap.RIGHT_FOLLOWER_ID, rev.MotorType.kBrushed) self.left_motor_1.setClosedLoopRampRate(1.0) self.left_motor_2.setClosedLoopRampRate(1.0) self.right_motor_1.setClosedLoopRampRate(1.0) self.right_motor_2.setClosedLoopRampRate(1.0) self.left_side = wpilib.SpeedControllerGroup(self.left_motor_1, self.left_motor_2) self.right_side = wpilib.SpeedControllerGroup(self.right_motor_1, self.right_motor_2) #Drivetrain self.drivetrain = wpilib.drive.DifferentialDrive(self.left_side, self.right_side) #TODO: Add subsystems and sensors as the code is written #TODO: SmartDashboard #Pneumatics self.colorPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.COLOR_SENSOR_EXTEND, robotmap.COLOR_SENSOR_RETRACT)) self.climberPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.CLIMBER_EXTEND, robotmap.CLIMBER_RETRACT)) self.gearshiftPiston = pneumatic_system(wpilib.DoubleSolenoid(0, robotmap.DRIVE_SHIFT_EXTEND, robotmap.DRIVE_SHIFT_RETRACT)) self.climberWinchMotor1 = rev.CANSparkMax(robotmap.CLIMBER_WINCH_MOTOR1, rev.MotorType.kBrushed) self.climberWinchMotor2 = rev.CANSparkMax(robotmap.CLIMBER_WINCH_MOTOR2, rev.MotorType.kBrushed) # Color Sensor self.colorSensor = color_sensor() self.colorSensorMotor = rev.CANSparkMax(robotmap.COLOR_SENSOR_MOTOR, rev.MotorType.kBrushed) self.stopColorMap = {"R":"B", "Y":"G", "B":"R", "G":"Y"} self.gameData = "" self.setupColorSensor() #Shooter self.shooter = shooter(robotmap.LOADER, robotmap.SHOOTER) # Gyro self.ahrs = AHRS.create_spi() self.Aimer = Aimer(self.ahrs) #network tables self.sd = NetworkTables.getTable('VISION')
def __init__(self, robot, can_id): super().__init__() self.robot = robot self.wristMotor = rev.CANSparkMax(can_id, rev.MotorType.kBrushless) self.encoder = SwiftCanEncoder(self.wristMotor.getEncoder()) self.encoder.setDistancePerPulse(constants.WRIST_DEGREES_FACTOR) self.encoder.setPosition(constants.WRIST_START_POSITION) self.pid = wpilib.PIDController(.07, 0, 0, self.encoder, self.wristMotor) self.pid.setAbsoluteTolerance(3) self.pid.setEnabled(False)
def robotInit(self): self.frontLeft: CANSparkMax = rev.CANSparkMax(3, rev.MotorType.kBrushless) self.rearLeft: CANSparkMax = rev.CANSparkMax(11, rev.MotorType.kBrushless) self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft) self.frontRight: CANSparkMax = rev.CANSparkMax(22, rev.MotorType.kBrushless) self.rearRight: CANSparkMax = rev.CANSparkMax(10, rev.MotorType.kBrushless) self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight) for motor in [self.frontLeft, self.rearLeft, self.frontRight, self.rearRight]: motor.clearFaults() motor.setOpenLoopRampRate(0.5) motor.setSmartCurrentLimit(60, 60, 6400) # >= 15 sec. stall tested motor.setSecondaryCurrentLimit(100) motor.getEncoder().setPositionConversionFactor(42) #motor.setIdleMode(0) SmartDashboard.putNumber("Left Power", 0) SmartDashboard.putNumber("Right Power", 0) self.leftStick = wpilib.Joystick(0)
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ self.lt_motor = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.lf_motor = rev.CANSparkMax(3, rev.MotorType.kBrushless) self.lb_motor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.rt_motor = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.rf_motor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.rb_motor = rev.CANSparkMax(6, rev.MotorType.kBrushless) self.left = wpilib.SpeedControllerGroup(self.lt_motor, self.lf_motor, self.lb_motor) self.right = wpilib.SpeedControllerGroup(self.rt_motor, self.rf_motor, self.rb_motor) self.drive = DifferentialDrive(self.left, self.right) self.lt_motor.setInverted(True) self.rt_motor.setInverted(True) self.joystick = wpilib.Joystick(0) self.previous_button = False self.gear_switcher = wpilib.DoubleSolenoid(0, 1)
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ self.leftSpark1 = rev.CANSparkMax(15, rev.MotorType.kBrushless) self.leftSpark2 = rev.CANSparkMax(14, rev.MotorType.kBrushless) self.leftSpark3 = rev.CANSparkMax(13, rev.MotorType.kBrushless) self.rightSpark1 = rev.CANSparkMax(20, rev.MotorType.kBrushless) self.rightSpark2 = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.rightSpark3 = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.joystick1 = wpilib.Joystick(0) self.joystick2 = wpilib.Joystick(1) self.leftSparks = wpilib.SpeedControllerGroup(self.leftSpark1, self.leftSpark2, self.leftSpark3) self.rightSparks = wpilib.SpeedControllerGroup(self.rightSpark1, self.rightSpark2, self.rightSpark3) self.leftSpark1.setIdleMode(rev.IdleMode.kBrake) self.leftSpark2.setIdleMode(rev.IdleMode.kBrake) self.leftSpark3.setIdleMode(rev.IdleMode.kBrake) self.rightSpark1.setIdleMode(rev.IdleMode.kBrake) self.rightSpark2.setIdleMode(rev.IdleMode.kBrake) self.rightSpark3.setIdleMode(rev.IdleMode.kBrake) self.openCloseSolenoid = wpilib.DoubleSolenoid(2, 5) self.inOutSolenoid = wpilib.DoubleSolenoid(3, 4)
def __init__(self, robot): super().__init__("drivetrainsim") self.robot = robot self.counter = 0 # used for updating the log self.x, self.y = 0, 0 # initialize sensors self.navx = navx.AHRS.create_spi() # initialize motors self.spark_neo_left_front = wpilib.Jaguar(1) self.spark_neo_left_rear = wpilib.Jaguar(2) self.spark_neo_right_front = wpilib.Jaguar(3) self.spark_neo_right_rear = wpilib.Jaguar(4) motor_type = rev.MotorType.kBrushless self.spark_neo_left_front1 = rev.CANSparkMax(1, motor_type) self.spark_neo_left_rear1 = rev.CANSparkMax(2, motor_type) self.spark_neo_right_front1 = rev.CANSparkMax(3, motor_type) self.spark_neo_right_rear1 = rev.CANSparkMax(4, motor_type) self.sparkneo_encoder_1 = rev.CANSparkMax.getEncoder(self.spark_neo_left_front1) self.sparkneo_encoder_2 = rev.CANSparkMax.getEncoder(self.spark_neo_left_rear1) self.sparkneo_encoder_3 = rev.CANSparkMax.getEncoder(self.spark_neo_right_front1) self.sparkneo_encoder_4 = rev.CANSparkMax.getEncoder(self.spark_neo_right_rear1) # create drivetrain from motors self.speedgroup_left = SpeedControllerGroup(self.spark_neo_left_front, self.spark_neo_left_rear) self.speedgroup_right = SpeedControllerGroup(self.spark_neo_right_front, self.spark_neo_right_rear) self.drive = wpilib.drive.DifferentialDrive(self.speedgroup_left, self.speedgroup_right) # self.drive = wpilib.drive.DifferentialDrive(self.spark_neo_left_front, self.spark_neo_right_front) self.drive.setMaxOutput(1.0) # initialize encoders - doing this after motors because they may be part of the motor controller self.l_encoder = wpilib.Encoder(0, 1, True) self.r_encoder = wpilib.Encoder(2, 3, True) self.l_encoder.setDistancePerPulse(drive_constants.k_encoder_distance_per_pulse_m) self.r_encoder.setDistancePerPulse(drive_constants.k_encoder_distance_per_pulse_m) # odometry for tracking the robot pose self.odometry = wpimath.kinematics.DifferentialDriveOdometry(geo.Rotation2d.fromDegrees( -self.navx.getAngle() ))
def createObjects(self): """Initialize all wpilib motors & sensors""" # setup camera # setup master and slave drive motors self.drive_slave_left = lazytalonfx.LazyTalonFX( self.DRIVE_SLAVE_LEFT_ID) self.drive_master_left = lazytalonfx.LazyTalonFX( self.DRIVE_MASTER_LEFT_ID) self.drive_master_left.follow(self.drive_slave_left) self.drive_slave_right = lazytalonfx.LazyTalonFX( self.DRIVE_SLAVE_RIGHT_ID) self.drive_master_right = lazytalonfx.LazyTalonFX( self.DRIVE_MASTER_RIGHT_ID) self.drive_master_right.follow(self.drive_slave_right) # setup actuator motors self.intake_motor = lazytalonsrx.LazyTalonSRX(self.INTAKE_ID) self.low_tower_motor = lazytalonsrx.LazyTalonSRX(self.LOW_TOWER_ID) self.high_tower_motor = lazytalonsrx.LazyTalonSRX(self.HIGH_TOWER_ID) self.turret_motor = lazytalonsrx.LazyTalonSRX(self.TURRET_ID) self.turret_motor.setEncoderConfig(lazytalonsrx.CTREMag, True) self.flywheel_motor = rev.CANSparkMax(self.FLYWHEEL_MOTOR_ID, rev.MotorType.kBrushless) self.climb_winch_slave = lazytalonsrx.LazyTalonSRX( self.CLIMB_WINCH_SLAVE_ID) self.climb_winch_master = lazytalonsrx.LazyTalonSRX( self.CLIMB_WINCH_MASTER_ID) self.climb_winch_slave.follow(self.climb_winch_master) self.slider_motor = lazytalonsrx.LazyTalonSRX(self.SLIDER_ID) # setup imu self.imu = lazypigeonimu.LazyPigeonIMU(self.intake_motor) # setup proximity sensors self.tower_sensors = [wpilib.DigitalInput(i) for i in range(0, 5)] self.intake_sensors = [wpilib.DigitalInput(8), wpilib.DigitalInput(9)] # setup joysticks self.driver = wpilib.Joystick(0) self.operator = wpilib.XboxController(1) self.nt = NetworkTables.getTable("robot") self.manual_indexer = True self.chassis_slow = False
def robotInit(self): try: wpilib.CameraServer.launch() except: pass self.stick = wpilib.Joystick(0) self.base = Base(rev.CANSparkMax(4, rev.MotorType.kBrushless), rev.CANSparkMax(2, rev.MotorType.kBrushless), rev.CANSparkMax(3, rev.MotorType.kBrushless), rev.CANSparkMax(1, rev.MotorType.kBrushless), AHRS.create_spi(wpilib.SPI.Port.kMXP), self.stick) self.lift = Lift(WPI_TalonSRX(3), wpilib.DigitalInput(0), wpilib.DigitalInput(1), self.stick) self.arm = Arm(wpilib.Solenoid(5, 2), wpilib.Solenoid(5, 4), wpilib.Solenoid(5, 1), wpilib.Solenoid(5, 5), self.stick) self.testvar = 0 self.ballintake = BallIntake(WPI_TalonSRX(4)) try: self.ledController = LEDController.getInstance() except: pass # Various one-time initializations happen here. self.lift.initSensor() self.base.navx.reset() try: NetworkTables.initialize() except: pass try: self.visiontable = NetworkTables.getTable("visiontable") except: pass self.lights = False self.op = False self.testButton = JoystickButton(self.stick, 16) self.roller = Roller(WPI_TalonSRX(20), WPI_TalonSRX(10), self.stick, "init")
def robotInit(self): """Robot initialization function. The Low level is to use the brushless function on the controllers.""" '''if wpilib.RobotBase.isSimulation(): self.frontLeftMotor = ctre.WPI_VictorSPX(self.frontLeftChannel) self.rearLeftMotor = ctre.WPI_VictorSPX(self.rearLeftChannel) self.frontRightMotor = ctre.WPI_VictorSPX(self.frontRightChannel) self.rearRightMotor = ctre.WPI_VictorSPX(self.rearRightChannel) else: ''' self.frontLeftMotor = rev.CANSparkMax( self.frontLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.rearLeftMotor = rev.CANSparkMax( self.rearLeftChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.frontRightMotor = rev.CANSparkMax( self.frontRightChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) self.rearRightMotor = rev.CANSparkMax( self.rearRightChannel, rev.CANSparkMaxLowLevel.MotorType.kBrushless) # invert the left side motors self.rearRightMotor.setInverted(False) # you may need to change or remove this to match your robot self.rearLeftMotor.setInverted(False) self.drive = MecanumDrive( self.frontLeftMotor, self.frontRightMotor, self.rearLeftMotor, self.rearRightMotor, ) self.drive.setExpiration(0.1) self.stick = wpilib.XboxController(self.joystickChannel) self.lift = ctre.WPI_VictorSPX(6)
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.rightB) #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): self.driveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.turnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.turnEncoder = self.turnMotor.getEncoder() self.turnEncoder.setPositionConversionFactor(20) # PID coefficients self.kP = .0039 self.kI = 2e-6 self.kD = 0 #PID controllers for the turn motors self.turnController = PIDController(self.kP, self.kI, self.kD) self.PIDTolerance = 1.0 self.turnController.setTolerance(self.PIDTolerance) self.turnController.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .1 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well self.robotLength = 10.0 self.robotWidth = 10.0 wpilib.CameraServer.launch()
def robotInit(self): # Create motors self.motor = rev.CANSparkMax(1, rev.MotorType.kBrushless) # You must call getPIDController() on an existing CANSparkMax or # SparkMax object to fully use PID functionality self.pidController = self.motor.getPIDController() # Instantiate built-in encoder to display position self.encoder = self.motor.getEncoder() self.joystick = wpilib.Joystick(0) # PID Coefficents and Controller Output Range self.kP = 0.1 self.kI = 1e-4 self.kD = 0 self.kIz = 0 self.kFF = 0 self.kMinOutput = -1 self.kMaxOutput = 1 # Motor max RPM self.maxRPM = 5700 # The restoreFactoryDefaults() method can be used to reset the # configuration parameters in the SPARK MAX to their factory default # state. If no argument is passed, these parameters will not persist # between power cycles self.motor.restoreFactoryDefaults() # Set PID Constants self.pidController.setP(self.kP) self.pidController.setI(self.kI) self.pidController.setD(self.kD) self.pidController.setIZone(self.kIz) self.pidController.setFF(self.kFF) self.pidController.setOutputRange(self.kMinOutput, self.kMaxOutput) # Push PID Coefficients to SmartDashboard wpilib.SmartDashboard.putNumber("P Gain", self.kP) wpilib.SmartDashboard.putNumber("I Gain", self.kI) wpilib.SmartDashboard.putNumber("D Gain", self.kD) wpilib.SmartDashboard.putNumber("I Zone", self.kIz) wpilib.SmartDashboard.putNumber("Feed Forward", self.kFF) wpilib.SmartDashboard.putNumber("Min Output", self.kMinOutput) wpilib.SmartDashboard.putNumber("Max Output", self.kMaxOutput)
def robotInit(self): self.kP = 0.03 self.kI = 0.00 self.kD = 0.00 self.kF = 0.00 self.exampleButton = 7 #Sets up your PID controller with set constants self.controller = PIDController(self.kP, self.kI, self.kD) #set the range of values the Input will give #spark max are 42counts per revolution self.controller.enableContinuousInput(0, 42) #you can find information about the motor stuff in the documentation self.speed = self.controller.calculate(rev.CANSparkMax(4,rev.MotorType.kBrushless).getEncoder())
def robotInit(self): self.green = 1 self.red = 1 self.blue = 1 self.yellow = 1 # these buttons are for the different stages, where the wheel will spin until it has the right color or the right number of rotations self.rotationsSpinButton = wpilib.DriverStation.getStickButton(0,0) #joystick channel and button number self.colorSpinButton = wpilib.DriverStation.getStickButton(0,1) self.ethansMotor = rev.CANSparkMax(4,rev.MotorType.kBrushless) self.ethansServo = wpilib.PWM(1) #this is the servo that flips up the wheel, it is connected to pwm channel 1 # .setPosition(0.0-1.0) means -90 degrees to 90 degrees self.spinning = False self.targetColor= green self.colorTime = False self.colorSensor = ??
def createObjects(self): self.intake_motor = rev.CANSparkMax(7, rev.MotorType.kBrushless) self.flashdrive_left_master = ctre.WPI_TalonSRX(1) self.flashdrive_left_slave1 = ctre.WPI_VictorSPX(2) self.flashdrive_left_slave2 = ctre.WPI_VictorSPX(3) self.flashdrive_right_master = ctre.WPI_TalonSRX(4) self.flashdrive_right_slave1 = ctre.WPI_VictorSPX(5) self.flashdrive_right_slave2 = ctre.WPI_VictorSPX(6) self.flashdrive_left_drive = wpilib.SpeedControllerGroup( self.flashdrive_left_master, self.flashdrive_left_slave1, self.flashdrive_left_slave2) self.flashdrive_right_drive = wpilib.SpeedControllerGroup( self.flashdrive_right_master, self.flashdrive_right_slave1, self.flashdrive_right_slave2) self.flashdrive_drivetrain = wpilib.drive.DifferentialDrive( self.flashdrive_left_drive, self.flashdrive_right_drive) self.joystick = wpilib.Joystick(0) self.elevator_motor = ctre.WPI_TalonSRX(7)
def robotInit(self): self.arm_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless) self.stick = wpilib.Joystick(0) # self.thing = SharpIR2Y0A21(0) self.arm_motor.restoreFactoryDefaults() self.arm_pidController = self.arm_motor.getPIDController() self.arm_encoder = self.arm_motor.getEncoder() self.kP = 0.1 self.kI = 0 self.kD = 0 self.kMaxOutput = 0.3 self.kMinOutput = -0.2 self.arm_pidController.setP(self.kP) self.arm_pidController.setI(self.kI) self.arm_pidController.setD(self.kD) self.arm_pidController.setOutputRange(self.kMinOutput, self.kMaxOutput) self.arm_encoder.setPosition(0.0)
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ self.leftSpark1 = rev.CANSparkMax(15, rev.MotorType.kBrushless) self.leftSpark2 = rev.CANSparkMax(14, rev.MotorType.kBrushless) self.leftSpark3 = rev.CANSparkMax(13, rev.MotorType.kBrushless) self.rightSpark1 = rev.CANSparkMax(20, rev.MotorType.kBrushless) self.rightSpark2 = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.rightSpark3 = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.joystick1 = wpilib.Joystick(0) self.joystick2 = wpilib.Joystick(1) self.leftSparks = wpilib.SpeedControllerGroup(self.leftSpark1, self.leftSpark2, self.leftSpark3) self.rightSparks = wpilib.SpeedControllerGroup(self.rightSpark1, self.rightSpark2, self.rightSpark3)
def createObjects(self): """Robot initialization function""" self.logger.info("pyinfiniterecharge %s", GIT_COMMIT) self.chassis_left_front = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.chassis_left_rear = rev.CANSparkMax(6, rev.MotorType.kBrushless) self.chassis_right_front = rev.CANSparkMax(7, rev.MotorType.kBrushless) self.chassis_right_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.imu = navx.AHRS.create_spi(update_rate_hz=50) self.hang_winch_motor = ctre.WPI_TalonFX(30) self.hang_kracken_hook_latch = wpilib.Solenoid(0) self.intake_main_motor = ctre.WPI_TalonSRX(18) self.indexer_motors = [ ctre.WPI_TalonSRX(11), ctre.WPI_TalonSRX(12), ctre.WPI_TalonSRX(13), ] self.injector_motor = ctre.WPI_TalonSRX(14) self.piston_switch = wpilib.DigitalInput( 4) # checks if injector retracted self.intake_arm_piston = wpilib.Solenoid(1) self.intake_left_motor = rev.CANSparkMax(8, rev.MotorType.kBrushless) self.intake_right_motor = rev.CANSparkMax(9, rev.MotorType.kBrushless) self.led_screen_led = wpilib.AddressableLED(0) self.loading_piston = wpilib.DoubleSolenoid(2, 3) self.shooter_centre_motor = ctre.WPI_TalonFX(2) self.shooter_outer_motor = ctre.WPI_TalonFX(3) self.range_counter = wpilib.Counter(6) self.turret_centre_index = wpilib.DigitalInput(0) self.turret_right_index = wpilib.DigitalInput(1) self.turret_left_index = wpilib.DigitalInput(2) self.turret_motor = ctre.WPI_TalonSRX(10) # operator interface self.driver_joystick = wpilib.Joystick(0) self.codriver_gamepad = wpilib.XboxController(1) self.MEMORY_CONSTANT = int(0.1 / self.control_loop_wait_time) # how long before data times out self.has_zeroed = False
def __init__(self): self.revError = 30 self.turretTolerance = .5 self.turretMotorID = 15 self.turretMotor = rev.CANSparkMax(self.turretMotorID, rev.MotorType.kBrushless) self.turretEncoder = self.turretMotor.getEncoder() self.turretEncoder.setPositionConversionFactor(12.414) self.spinEncoder = self.spinMotor.getEncoder() self.kP = 0.0085 self.kI = 0.0 self.kD = 0.0 self.turretTurnController = wpilib.controller.PIDController( self.kP, self.kI, self.kD) self.turretTurnController.setSetpoint(0) self.turretTurnController.setTolerance(0.1) self.zeroTurret()
def robotInit(self): # Create motor self.motor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.joystick = wpilib.Joystick(0) # The restoreFactoryDefaults method can be used to reset the # configuration parameters in the SPARK MAX to their factory default # state. If no argument is passed, these parameters will not persist # between power cycles self.motor.restoreFactoryDefaults() # Parameters can be set by calling the appropriate set() method on the # CANSparkMax object whose properties you want to change # # Set methods will return one of three CANError values which will let # you know if the parameter was successfully set: # CANError.kOk # CANError.kError # CANError.kTimeout if self.motor.setIdleMode(rev.IdleMode.kCoast) is not rev.CANError.kOk: wpilib.SmartDashboard.putString("Idle Mode", "Error") # Similarly, parameters will have a get() method which allows you to # retrieve their values from the controller if self.motor.getIdleMode() is rev.IdleMode.kCoast: wpilib.SmartDashboard.putString("Idle Mode", "Coast") else: wpilib.SmartDashboard.putString("Idle Mode", "Brake") # Set ramp rate to 0 if self.motor.setOpenLoopRampRate(0) is not rev.CANError.kOk: wpilib.SmartDashboard.putString("Ramp Rate", "Error") # Read back ramp value wpilib.SmartDashboard.putString("Ramp Rate", str(self.motor.getOpenLoopRampRate()))
def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ # Set up smart Dashboard self.sd = NetworkTables.getTable("SmartDashboard") # Set up motors self.motor = rev.CANSparkMax(11, rev.MotorType.kBrushless) self.encoder = rev._impl.CANEncoder(self.motor) self.motor_PID = rev._impl.CANPIDController(self.motor) # self.right_motor = rev.CANSparkMax(1, rev.MotorType.kBrushless) # self.drive = wpilib.drive.DifferentialDrive(self.left_motor, self.right_motor) # Set up the joystick self.stick = wpilib.Joystick(0) # set up the timer self.timer = wpilib.Timer() # Set up the PID Gains as constants. These Values are Reccommended by Rev self.kP = .1 self.kI = .0001 self.kD = 0 self.kIz = 0 self.kFF = 0 self.kMaxOutput = .5 self.kMinOutput = -.5 self.rotations = 0.0 self.maxRPM = 5700 # Put the PID gains into shuffleboard self.sd.putNumber("P Gain", self.kP) self.sd.putNumber("I Gain", self.kI) self.sd.putNumber("D Gain", self.kD) self.sd.putNumber("I Zone", self.kIz) self.sd.putNumber("Feed Forward", self.kFF) self.sd.putNumber("Max Output", self.kMaxOutput) self.sd.putNumber("Min Output", self.kMinOutput) self.sd.putNumber("Setpoint", self.rotations)
def __init__(self, intakeID, halfMoonID): self.intakeMotor = rev.CANSparkMax(intakeID, rev.MotorType.kBrushless) self.halfMoonMotor = rev.CANSparkMax(halfMoonID, rev.MotorType.kBrushless)
def __init__(self,port): self.motor = rev.CANSparkMax(port, rev.MotorType.kBrushed)
def __init__(self, flyWheelID): self.flyWheelMotor = rev.CANSparkMax(flyWheelID, MotorType.kBrushless) self.flyWheelEncoder = self.flyWheelMotor.getEncoder()
def robotInit(self): #I got fed up with how long 'frontLeft, frontRight, etc.' looked #Drive motors self.flDriveMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.frDriveMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.rlDriveMotor = rev.CANSparkMax(7, rev.MotorType.kBrushless) self.rrDriveMotor = rev.CANSparkMax(5, rev.MotorType.kBrushless) self.driveMotors = (self.flDriveMotor, self.frDriveMotor, self.rlDriveMotor, self.rrDriveMotor) #Turn motors self.flTurnMotor = rev.CANSparkMax(3, rev.MotorType.kBrushless) self.frTurnMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) self.rlTurnMotor = rev.CANSparkMax(8, rev.MotorType.kBrushless) self.rrTurnMotor = rev.CANSparkMax(6, rev.MotorType.kBrushless) self.turnMotors = (self.flTurnMotor, self.frTurnMotor, self.rlTurnMotor, self.rrTurnMotor) #Turn encoders self.flTurnEncoder = self.flTurnMotor.getEncoder() self.frTurnEncoder = self.frTurnMotor.getEncoder() self.rlTurnEncoder = self.rlTurnMotor.getEncoder() self.rrTurnEncoder = self.rrTurnMotor.getEncoder() self.turnEncoders = (self.flTurnEncoder, self.frTurnEncoder, self.rlTurnEncoder, self.rrTurnEncoder) for encoder in self.turnEncoders: encoder.setPositionConversionFactor(20) #makes the encoder output in degrees '''We should use whichever encoder (built-in or absolute) gives the greatest results. If we use the absolute, easy. If we use the build-in, the following will be an issue and a potential solution. Every time we turn off/on the robot, these encoders will read 0 and the absolute encoder will read the real value. I didn't do it here, but we will need a function that moves all the turn motors so that their positions are set to what the absolute encoder says it is. Something like for index, encoder in enumerate(self.turnEncoders): encoder.setPosition(self.absoluteEncoders[index]%360) because I've been using an unhealthy amount of for loops this reason. This would allow the auto to go from there, although the motors should have been zeroed beforehand. There would be a similar function that can be ran in teleop when a sufficiently out-of-the-way button is pressed so that we can automatically reset the motors to 0 when in the pits, something like for index, encoder in enumerate(self.turnEncoders): encoder.setPosition(self.absoluteEncoders[index]%360) self.turnControllers[index].setReference(0, rev.controlType.kPosition) that of course then removes itself from the queue once all four motor encoders are at 0 and all the absolute encoders are at 0. Despite a reset being available in the pits, we would still need to set the motor encoders to the absolute encoder positions to account for possible bumping, absent-minded turning, or in case we didn't have time to return to the pits and so were only able to reset the wheel positions by hand''' # PID coefficients self.kP = .0039 self.kI = 0 self.kD = 2.0e-6 self.PIDTolerance = 1.0 #PID controllers for the turn motors self.flTurnController = PIDController(self.kP, self.kI, self.kD) self.frTurnController = PIDController(self.kP, self.kI, self.kD) self.rlTurnController = PIDController(self.kP, self.kI, self.kD) self.rrTurnController = PIDController(self.kP, self.kI, self.kD) self.turnControllers = (self.flTurnController, self.frTurnController, self.rlTurnController, self.rrTurnController) for controller in self.turnControllers: controller.setTolerance(self.PIDTolerance) controller.enableContinuousInput(0, 360) self.joystick = wpilib.Joystick(0) self.joystickDeadband = .05 self.timer = wpilib.Timer() #used to use it while testing stuff, don't need it now, but oh well self.robotLength = 33.75 self.robotWidth = 29.75