def __init__(self, elv, pay): super().__init__("stow arm") self.prefs = Preferences.getInstance() self.requires(subsystems.PAYLOAD) self.requires(subsystems.ELEVATOR) self.pay_pos = pay self.elv_pos = elv
def loadPreferences(): global config config = Preferences.getInstance() global robotId robotId = getConfigFloat("RobotId", astroV2) print("map.py robotId", robotId) if robotId == astroV1: global driveLeft1 global driveLeft2 global driveLeft3 global driveRight1 global driveRight2 global driveRight3 driveLeft1 = 20 driveLeft2 = 21 driveLeft3 = 22 driveRight1 = 10 driveRight2 = 11 driveRight3 = 12
def __init__(self): super().__init__("Elevator") # Initializes the elevator Talon, put it as one for now self.elevatorleader = TalonSRX(CAN_ELEVATOR_LEADER) set_motor(self.elevatorleader, TalonSRX.NeutralMode.Brake, False) self.elevatorleader.configSelectedFeedbackSensor( FeedbackDevice.QuadEncoder, 0, 0) self.elevatorleader.setSelectedSensorPosition(0, 0, 0) self.elevatorleader.setSensorPhase(False) self.elevatorfollower = VictorSPX(CAN_ELEVATOR_FOLLOWER) set_motor(self.elevatorfollower, TalonSRX.NeutralMode.Brake, False) self.elevatorfollower.follow(self.elevatorleader) # The preference table is used to get values to the code while the # robot is running, useful self.prefs = Preferences.getInstance() # for tuning the PIDs and stuff self.elevator_zero = False self.set_values()
def __init__(self): super().__init__("Payload") # TODO Two limit switches on the leader self.prefs = Preferences.getInstance() self.elbowleader = TalonSRX(CAN_ELBOW_LEADER) set_motor(self.elbowleader, TalonSRX.NeutralMode.Brake, False) self.elbowleader.configSelectedFeedbackSensor( FeedbackDevice.QuadEncoder, 0, 0) self.elbowleader.setSelectedSensorPosition(0, 0, 0) self.elbowleader.selectProfileSlot(0, 0) self.elbowleader.setSensorPhase(True) self.elbowfollower = VictorSPX(CAN_ELBOW_FOLLOWER) set_motor(self.elbowfollower, VictorSPX.NeutralMode.Brake, False) self.elbowfollower.follow(self.elbowleader) self.baller = TalonSRX(CAN_BALL_INTAKE) set_motor(self.baller, TalonSRX.NeutralMode.Brake, False) self.elbow_zero = False self.DS = DoubleSolenoid(2, 3) self.set_values()
def __init__(self, robot): super().__init__('Drive') SmartDashboard.putNumber("DriveStraight_P", 0.075) SmartDashboard.putNumber("DriveStraight_I", 0.0) SmartDashboard.putNumber("DriveStraight_D", 0.42) # OLD GAINS 0.075, 0, 0.42 SmartDashboard.putNumber("DriveAngle_P", 0.009) SmartDashboard.putNumber("DriveAngle_I", 0.0) SmartDashboard.putNumber("DriveAngle_D", 0.025) SmartDashboard.putNumber("DriveStraightAngle_P", 0.025) SmartDashboard.putNumber("DriveStraightAngle_I", 0.0) SmartDashboard.putNumber("DriveStraightAngle_D", 0.01) self.driveStyle = "Tank" SmartDashboard.putString("DriveStyle", self.driveStyle) #SmartDashboard.putData("Mode", self.mode) self.robot = robot self.lime = self.robot.limelight self.nominalPID = 0.15 self.nominalPIDAngle = 0.22 # 0.11 - v2 self.preferences = Preferences.getInstance() timeout = 0 TalonLeft = Talon(map.driveLeft1) TalonRight = Talon(map.driveRight1) leftInverted = True rightInverted = False TalonLeft.setInverted(leftInverted) TalonRight.setInverted(rightInverted) VictorLeft1 = Victor(map.driveLeft2) VictorLeft2 = Victor(map.driveLeft3) VictorLeft1.follow(TalonLeft) VictorLeft2.follow(TalonLeft) VictorRight1 = Victor(map.driveRight2) VictorRight2 = Victor(map.driveRight3) VictorRight1.follow(TalonRight) VictorRight2.follow(TalonRight) for motor in [VictorLeft1, VictorLeft2]: motor.clearStickyFaults(timeout) motor.setSafetyEnabled(False) #motor.setExpiration(2 * self.robot.period) motor.setInverted(leftInverted) for motor in [VictorRight1, VictorRight2]: motor.clearStickyFaults(timeout) motor.setSafetyEnabled(False) #motor.setExpiration(2 * self.robot.period) motor.setInverted(rightInverted) for motor in [TalonLeft, TalonRight]: motor.setSafetyEnabled(False) #motor.setExpiration(2 * self.robot.period) motor.clearStickyFaults(timeout) #Clears sticky faults motor.configContinuousCurrentLimit(40, timeout) #15 Amps per motor motor.configPeakCurrentLimit( 70, timeout) #20 Amps during Peak Duration motor.configPeakCurrentDuration( 300, timeout) #Peak Current for max 100 ms motor.enableCurrentLimit(True) motor.configVoltageCompSaturation(12, timeout) #Sets saturation value motor.enableVoltageCompensation( True) #Compensates for lower voltages motor.configOpenLoopRamp(0.2, timeout) #number of seconds from 0 to 1 self.left = TalonLeft self.right = TalonRight self.navx = navx.AHRS.create_spi() self.leftEncoder = Encoder(map.leftEncoder[0], map.leftEncoder[1]) self.leftEncoder.setDistancePerPulse(self.leftConv) self.leftEncoder.setSamplesToAverage(10) self.rightEncoder = Encoder(map.rightEncoder[0], map.rightEncoder[1]) self.rightEncoder.setDistancePerPulse(self.rightConv) self.rightEncoder.setSamplesToAverage(10) self.zero() #PID for Drive self.TolDist = 0.1 #feet [kP, kI, kD, kF] = [0.027, 0.00, 0.20, 0.00] if wpilib.RobotBase.isSimulation(): [kP, kI, kD, kF] = [0.25, 0.00, 1.00, 0.00] distController = PIDController(kP, kI, kD, kF, source=self.__getDistance__, output=self.__setDistance__) distController.setInputRange(0, 50) #feet distController.setOutputRange(-0.6, 0.6) distController.setAbsoluteTolerance(self.TolDist) distController.setContinuous(False) self.distController = distController self.distController.disable() '''PID for Angle''' self.TolAngle = 2 #degrees [kP, kI, kD, kF] = [0.025, 0.00, 0.01, 0.00] if RobotBase.isSimulation(): [kP, kI, kD, kF] = [0.005, 0.0, 0.01, 0.00] angleController = PIDController(kP, kI, kD, kF, source=self.__getAngle__, output=self.__setAngle__) angleController.setInputRange(-180, 180) #degrees angleController.setOutputRange(-0.5, 0.5) angleController.setAbsoluteTolerance(self.TolAngle) angleController.setContinuous(True) self.angleController = angleController self.angleController.disable() self.k = 1 self.sensitivity = 1 SmartDashboard.putNumber("K Value", self.k) SmartDashboard.putNumber("sensitivity", self.sensitivity) self.prevLeft = 0 self.prevRight = 0
from wpilib import Preferences # Robot preferences file stored on roboRIO # values can be set differently for each roboRIO config: Preferences = Preferences.getInstance() def getConfigInt(key: str, defVal: int) -> int: """ Looks up an integer value from the robot configuration file or creates the value if not present. : param key : Key to use to look up/set value. : param defVal : Default value to set/return if not found. : return : Value from configuration file or default if not found. """ global config if config.containsKey(key): val: int = config.getInt(key, defVal) else: # Value not set in config, set to default value provided # so we will see it and be able to edit it in the system # preferences editor val: int = defVal config.putInt(key, val) return val def getConfigFloat(key: str, defVal: float) -> float: """ Looks a float value from the robot configuration file or creates the value if not present.