def __init__(self): # Initialize all controllers self.driveLeftMaster = Talon(kDriveTrain.lmId) self.driveLeftSlave = Talon(kDriveTrain.lsId) self.driveRightMaster = Talon(kDriveTrain.rmId) self.driveRightSlave = Talon(kDriveTrain.rsId) # Connect the slaves to the masters on each side self.driveLeftSlave.follow(self.driveLeftMaster) self.driveRightSlave.follow(self.driveRightMaster) # Makes sure both sides' controllers show green and use positive # values to move the bot forward. # CURRENTLY DISABLED WHEN USING WITH DifferentialDrive # self.driveLeftSlave.setInverted(False) # self.driveLeftMaster.setInverted(False) self.driveRightSlave.setInverted(True) self.driveRightMaster.setInverted(True) # Configures each master to use the attached Mag Encoders self.driveLeftMaster.configSelectedFeedbackSensor( ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0) self.driveRightMaster.configSelectedFeedbackSensor( ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0) # Reverses the encoder direction so forward movement always # results in a positive increase in the encoder ticks. self.driveLeftMaster.setSensorPhase(True) self.driveRightMaster.setSensorPhase(True) # Throw data on the SmartDashboard so we can work with it. SD.putNumber('Left Quad Pos.', self.driveLeftMaster.getQuadraturePosition()) SD.putNumber('Right Quad Pos.', self.driveRightMaster.getQuadraturePosition()) # these may give the derivitive an integral of the PID once # they are set. For now, they just show 0 SD.putNumber('Left Derivative', self.driveLeftMaster.getErrorDerivative(0)) SD.putNumber('Left Integral', self.driveLeftMaster.getIntegralAccumulator(0)) self.leftVel = None self.leftPos = None self.rightVel = None self.rightPos = None # self.driveLeftMaster.config_kP(0, .3, 10) # kP = self.driveLeftMaster.configGetParameter( # self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10) # SmartDashboard.putNumber('Left Proportional', kP) self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster) self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster) self.driveControllerRight.setInverted(True) self.drive = DifferentialDrive(self.driveControllerLeft, self.driveControllerRight)
def __init__(self, robot): self.robot = robot self.leftArm = Talon(self.robot.kCubeGrabber['left_arm']) self.rightArm = Talon(self.robot.kCubeGrabber['right_arm']) self.armUS = wpilib.AnalogInput(self.robot.kCubeGrabber['ultra_sonic']) self.armSwitch = wpilib.DigitalInput(self.robot.kCubeGrabber['switch']) self.armClosePosition = self.robot.kCubeGrabber['close'] self.armOpenPosition = self.robot.kCubeGrabber['open'] self.driverTwo = self.robot.cStick self.armSolenoid = wpilib.Solenoid(self.robot.pneumaticControlModuleCANID, self.robot.kCubeGrabber['solenoid']) """ Initializes the predetermined distances for grabber/cube interaction. """ self.spinDistance = 12 self.closeDistance = 7 """ Initializes Toggle Counts for the arm functionality. """ self.armModeToggleCount = 2 self.openToggleCount = 3
def initialize(self): timeout = 15 SmartDashboard.putNumber("Wrist Power Set", 0) SmartDashboard.putNumber("Gravity Power", 0) self.F = 0.25 SmartDashboard.putNumber("F Gain", self.F) #self.angle = 0 #SmartDashboard.putNumber("angle", self.angle) self.range = -1200 self.povPressed = False self.maxVolts = 10 self.wristUpVolts = 4 self.wristDownVolts = -4 SmartDashboard.putNumber("Wrist Up Volts", self.wristUpVolts) SmartDashboard.putNumber("Wrist Down Volts", self.wristDownVolts) self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) #below is the talon on the intake self.intake = Talon(map.intake) self.intake.setNeutralMode(2) self.intake.configVoltageCompSaturation(self.maxVolts) self.intake.configContinuousCurrentLimit(20, timeout) #20 Amps per motor self.intake.configPeakCurrentLimit( 30, timeout) #30 Amps during Peak Duration self.intake.configPeakCurrentDuration( 100, timeout) #Peak Current for max 100 ms self.intake.enableCurrentLimit(True) #Talon motor object created self.wrist = Talon(map.wrist) if not wpilib.RobotBase.isSimulation(): self.wrist.configFactoryDefault() self.wrist.setInverted(True) self.wrist.configVoltageCompSaturation(self.maxVolts) self.wrist.setNeutralMode(2) self.wrist.configClearPositionOnLimitF(True) self.wrist.configContinuousCurrentLimit(20, timeout) #20 Amps per motor self.wrist.configPeakCurrentLimit( 30, timeout) #30 Amps during Peak Duration self.wrist.configPeakCurrentDuration( 100, timeout) #Peak Current for max 100 ms self.wrist.enableCurrentLimit(True)
def initialize(self): timeout = 15 SmartDashboard.putNumber("Wrist Power Set", 0) self.lastMode = "unknown" self.sb = [] self.targetPosUp = -300 #!!!!! self.targetPosDown = -1500 #!!!!! self.maxVolts = 10 self.simpleGain = 0.57 self.wristUpVolts = 5 self.wristDownVolts = 2 self.simpleGainGravity = 0.07 self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) #below is the talon on the intake self.motor = Talon(map.intake) self.gPower = 0 self.input = 0 self.motor.configContinuousCurrentLimit(20, timeout) #15 Amps per motor self.motor.configPeakCurrentLimit( 30, timeout) #20 Amps during Peak Duration self.motor.configPeakCurrentDuration( 100, timeout) #Peak Current for max 100 ms self.motor.enableCurrentLimit(True) #Talon motor object created self.wrist = Talon(map.wrist) if not wpilib.RobotBase.isSimulation(): self.wrist.configFactoryDefault() self.wrist.configVoltageCompSaturation(self.maxVolts) self.wrist.setInverted(True) self.wrist.setNeutralMode(2) self.motor.setNeutralMode(2) self.motor.configVoltageCompSaturation(self.maxVolts) self.wrist.configClearPositionOnLimitF(True) #MOTION MAGIC CONFIG self.loops = 0 self.timesInMotionMagic = 0 #self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs) [self.kP, self.kI, self.kD] = [0, 0, 0] cargoController = PIDController(self.kP, self.kI, self.kD, source=self.getAngle, output=self.__setGravity__) self.cargoController = cargoController self.cargoController.disable()
def initialize(self): timeout = 15 self.wristPowerSet = 0 SmartDashboard.putNumber("Wrist Power Set", self.wristPowerSet) self.maxVolts = 10 self.wristUpVolts = 4 self.wristDownVolts = -4 self.xbox = oi.getJoystick(2) self.out = 0 #below is the talon on the intake self.intake = Talon(map.intake) self.intake.setNeutralMode(2) self.intake.configVoltageCompSaturation(self.maxVolts) self.intake.configContinuousCurrentLimit(20, timeout) #20 Amps per motor self.intake.configPeakCurrentLimit( 30, timeout) #30 Amps during Peak Duration self.intake.configPeakCurrentDuration( 100, timeout) #Peak Current for max 100 ms self.intake.enableCurrentLimit(True) #Talon motor object created self.wrist = Talon(map.wrist) if not wpilib.RobotBase.isSimulation(): self.wrist.configFactoryDefault() self.wrist.setInverted(True) self.wrist.configVoltageCompSaturation(self.maxVolts) self.wrist.setNeutralMode(2) self.wrist.configClearPositionOnLimitF(True) self.wrist.configContinuousCurrentLimit(20, timeout) #20 Amps per motor self.wrist.configPeakCurrentLimit( 30, timeout) #30 Amps during Peak Duration self.wrist.configPeakCurrentDuration( 100, timeout) #Peak Current for max 100 ms self.wrist.enableCurrentLimit(True) self.bottomSwitch = DI(map.bottomSwitch) self.topSwitch = DI(map.topSwitch)
def initialize(self): timeout = 15 self.lastMode = "unknown" self.sb = [] self.targetPosUp = -300 #!!!!! self.targetPosDown = -1500 #!!!!! self.maxVolts = 10 self.simpleGain = 0.57 self.wristUpVolts = 5 self.wristDownVolts = 2 self.simpleGainGravity = 0.07 self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) self.motor = Talon(map.intake) self.motor.configContinuousCurrentLimit(20,timeout) #15 Amps per motor self.motor.configPeakCurrentLimit(30,timeout) #20 Amps during Peak Duration self.motor.configPeakCurrentDuration(100,timeout) #Peak Current for max 100 ms self.motor.enableCurrentLimit(True) self.wrist = Talon(map.wrist) if not wpilib.RobotBase.isSimulation(): self.wrist.configFactoryDefault() self.wrist.setInverted(True) self.wrist.setNeutralMode(2) self.motor.setNeutralMode(2) self.motor.configVoltageCompSaturation(self.maxVolts) self.intakeSpeed = 0.9 self.wrist.configClearPositionOnLimitF(True) #MOTION MAGIC CONFIG self.loops = 0 self.timesInMotionMagic = 0 self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def __init__(self, robot): self.robot = robot self.lights = FROGLights() self.climbMotor = Talon(self.robot.kClimber['climb_motor']) self.checkSwitch = wpilib.DigitalInput( self.robot.kClimber['check_switch']) self.solenoid = wpilib.DoubleSolenoid( self.robot.pneumaticControlModuleCANID, self.robot.kClimber['solenoid'], self.robot.kClimber['solenoid_2']) self.driverOne = self.robot.dStick self.latchToggleCount = 2 self.handlerState = 0 self.climbMotor.configPeakOutputForward(1, 0) self.climbMotor.configPeakOutputReverse(-1, 0)
def initialize(self): #makes control objects self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) self.joystick1 = oi.getJoystick(1) #makes solenoid objects to be used in kick and slide functions self.kicker = Solenoid(map.hatchKick) self.slider = Solenoid(map.hatchSlide) self.maxVolts = 10 timeout = 0 self.wheels = Talon(map.hatchWheels) self.wheels.setNeutralMode(2) self.wheels.configVoltageCompSaturation(self.maxVolts) self.wheels.configContinuousCurrentLimit(20, timeout) #20 Amps per motor self.wheels.configPeakCurrentLimit( 30, timeout) #30 Amps during Peak Duration self.wheels.configPeakCurrentDuration( 100, timeout) #Peak Current for max 100 ms self.wheels.enableCurrentLimit(True) self.wheels.setInverted(True) self.powerIn = 0.9 self.powerOut = -0.9 #sets kicker and slide solenoids to in self.kick("in") self.slide("in") #starts lastkick/slide booleans self.lastKick = False self.lastSlide = False self.hasHatch = False self.outPower = 0
def __init__(self, robot): self.robot = robot self.elevator = Talon(self.robot.kElevator['elevator_motor']) self.elevator.setInverted(True) self.driverTwo = self.robot.cStick self.elevator.configSelectedFeedbackSensor( ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0) self.elevator.setSensorPhase(True) self.elevator.setSelectedSensorPosition(0, 0, 0) self.elevator.configPeakOutputForward(.2, 0) self.elevator.configPeakOutputReverse(-.95, 0) self.elevator.configNominalOutputForward(0, 0) self.elevator.configNominalOutputReverse(0, 0) self.elevator.setSafetyEnabled(False) self.smartToggle = True self.smartPosition = 1 super().__init__()
def initialize(self, robot): self.robot = robot self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) if map.robotId == map.astroV1: '''IDS AND DIRECTIONS FOR V1''' self.backLift = Talon(map.backLift) self.frontLift = Talon(map.frontLift) self.frontLift.setInverted(True) self.backLift.setInverted(True) self.wheelLeft = Victor(map.wheelLeft) self.wheelRight = Victor(map.wheelRight) self.wheelRight.setInverted(True) self.wheelLeft.setInverted(True) else: '''IDS AND DIRECTIONS FOR V2''' self.backLift = Talon(map.frontLift) self.frontLift = Talon(map.backLift) self.frontLift.setInverted(False) self.backLift.setInverted(False) self.wheelLeft = Talon(map.wheelLeft) self.wheelRight = Talon(map.wheelRight) self.wheelRight.setInverted(True) self.wheelLeft.setInverted(False) for motor in [self.backLift, self.frontLift]: motor.clearStickyFaults(0) motor.setSafetyEnabled(False) motor.configContinuousCurrentLimit(30, 0) #Amps per motor motor.enableCurrentLimit(True) motor.configVoltageCompSaturation(10, 0) #Sets saturation value motor.enableVoltageCompensation( True) #Compensates for lower voltages for motor in [self.wheelLeft, self.wheelRight]: motor.clearStickyFaults(0) motor.setSafetyEnabled(False) motor.setNeutralMode(2) self.backSwitch = DigitalInput(map.backBottomSensor) self.frontSwitch = DigitalInput(map.frontBottomSensor) self.upSwitch = DigitalInput(map.upSensor) self.MAX_ANGLE = 2 #degrees self.TARGET_ANGLE = 0 #degrees self.climbSpeed = 0.5 self.wheelSpeed = 0.5 self.backHold = -0.15 #holds back stationary if extended ADJUST** self.frontHold = -0.1 #holds front stationary if extended self.kP = 0.4 #proportional gain for angle to power ''' NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD ''' self.started = False
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
def initialize(self, robot): self.state = -1 self.robot = robot self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) self.usingNeo = True self.frontRetractStart = 0 self.wheelsStart = 0 self.wheelsStart2 = 0 if self.usingNeo: # NOTE: If using Spark Max in PWM mode to control Neo brushless # motors you MUST configure the speed controllers manually # using a USB cable and the Spark Max client software! self.frontLift: Spark = Spark(map.frontLiftPwm) self.backLift: Spark = Spark(map.backLiftPwm) self.frontLift.setInverted(False) self.backLift.setInverted(False) if map.robotId == map.astroV1: if not self.usingNeo: '''IDS AND DIRECTIONS FOR V1''' self.backLift = Talon(map.backLift) self.frontLift = Talon(map.frontLift) self.frontLift.setInverted(True) self.backLift.setInverted(True) self.wheelLeft = Victor(map.wheelLeft) self.wheelRight = Victor(map.wheelRight) self.wheelRight.setInverted(True) self.wheelLeft.setInverted(True) else: if not self.usingNeo: '''IDS AND DIRECTIONS FOR V2''' self.backLift = Talon(map.frontLift) self.frontLift = Talon(map.backLift) self.frontLift.setInverted(False) self.backLift.setInverted(False) self.backLift.setNeutralMode(2) self.frontLift.setNeutralMode(2) self.wheelLeft = Talon(map.wheelLeft) self.wheelRight = Talon(map.wheelRight) self.wheelRight.setInverted(True) self.wheelLeft.setInverted(False) if not self.usingNeo: for motor in [self.backLift, self.frontLift]: motor.clearStickyFaults(0) motor.setSafetyEnabled(False) motor.configContinuousCurrentLimit(30, 0) #Amps per motor motor.enableCurrentLimit(True) motor.configVoltageCompSaturation(10, 0) #Sets saturation value motor.enableVoltageCompensation( True) #Compensates for lower voltages for motor in [self.wheelLeft, self.wheelRight]: motor.clearStickyFaults(0) motor.setSafetyEnabled(False) motor.setNeutralMode(2) self.backSwitch = DigitalInput(map.backFloor) self.frontSwitch = DigitalInput(map.frontFloor) self.switchTopBack = DigitalInput(map.backTopSensor) self.switchTopFront = DigitalInput(map.frontTopSensor) self.switchBottomBack = DigitalInput(map.backBottomSensor) self.switchBottomFront = DigitalInput(map.frontBottomSensor) self.MAX_ANGLE = 2 #degrees self.TARGET_ANGLE = -1 #degrees self.climbSpeed = 0.9 self.wheelSpeed = 0.9 self.backHold = -0.10 #holds back stationary if extended ADJUST** self.frontHold = -0.10 #holds front stationary if extended self.kP = 0.35 #proportional gain for angle to power self.autoClimbIndicator = False ''' NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD ''' self.started = False
def robotInit(self): self.maxVolts = 10 self.joystick = Joystick(0) self.motor: Talon = Talon(31) self.motor.configClearPositionOnLimitF(True) self.motor.configVoltageCompSaturation(self.maxVolts)
def __init__(self, robot): self.robot = robot # Initialize all controllers self.driveLeftMaster = Talon(self.robot.kDriveTrain['left_master']) self.driveLeftSlave = Talon(self.robot.kDriveTrain['left_slave']) self.driveRightMaster = Talon(self.robot.kDriveTrain['right_master']) self.driveRightSlave = Talon(self.robot.kDriveTrain['right_slave']) wpilib.LiveWindow.addActuator("DriveTrain", "LeftMaster", self.driveLeftMaster) wpilib.LiveWindow.addActuator("DriveTrain", "RightMaster", self.driveRightMaster) # Connect the slaves to the masters on each side self.driveLeftSlave.follow(self.driveLeftMaster) self.driveRightSlave.follow(self.driveRightMaster) # Makes sure both sides' controllers show green and use positive # values to move the bot forward. self.driveLeftSlave.setInverted(False) self.driveLeftMaster.setInverted(False) self.driveRightSlave.setInverted(True) self.driveRightMaster.setInverted(True) """ Initializes the count for toggling which side of the robot will be considered the front when driving. """ self.robotFrontToggleCount = 2 # Configures each master to use the attached Mag Encoders self.driveLeftMaster.configSelectedFeedbackSensor( ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0) self.driveRightMaster.configSelectedFeedbackSensor( ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0) # Reverses the encoder direction so forward movement always # results in a positive increase in the encoder ticks. self.driveLeftMaster.setSensorPhase(True) self.driveRightMaster.setSensorPhase(True) # these supposedly aren't part of the WPI_TalonSRX class # self.driveLeftMaster.setSelectedSensorPostion(0, 0, 10) # self.driveRightMaster.setSelectedSensorPosition(0, 0, 10) # Throw data on the SmartDashboard so we can work with it. # SD.putNumber( # 'Left Quad Pos.', # self.driveLeftMaster.getQuadraturePosition()) # SD.putNumber( # 'Right Quad Pos.', # self.driveRightMaster.getQuadraturePosition()) self.leftVel = None self.leftPos = None self.rightVel = None self.rightPos = None self.leftMaxVel = 0 self.rightMaxVel = 0 self.leftMinVel = 0 self.rightMinVel = 0 # self.driveLeftMaster.config_kP(0, .3, 10) self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster) self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster) self.driveControllerRight.setInverted(True) self.drive = DifferentialDrive(self.driveControllerLeft, self.driveControllerRight) super().__init__()
def initialize(self): timeout = 15 self.lastMode = "unknown" self.sb = [] self.targetPosUp = -300 #!!!!! self.targetPosDown = -1500 #!!!!! self.maxVolts = 10 self.simpleGain = 0.1 self.wristUpVolts = 5 self.wristDownVolts = 2 self.simpleGainGravity = 0.05 self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) self.motor = Talon(map.intake) self.motor.configContinuousCurrentLimit(20,timeout) #15 Amps per motor self.motor.configPeakCurrentLimit(30,timeout) #20 Amps during Peak Duration self.motor.configPeakCurrentDuration(100,timeout) #Peak Current for max 100 ms self.motor.enableCurrentLimit(True) self.wrist = Talon(map.wrist) if not wpilib.RobotBase.isSimulation(): self.wrist.configFactoryDefault() self.wrist.setInverted(True) self.wrist.setNeutralMode(2) self.motor.setNeutralMode(2) self.motor.configVoltageCompSaturation(self.maxVolts) self.intakeSpeed = 0.9 self.wrist.configClearPositionOnLimitF(True) #MOTION MAGIC CONFIG self.loops = 0 self.timesInMotionMagic = 0 #choose sensor self.wrist.configSelectedFeedbackSensor( Talon.FeedbackDevice.CTRE_MagEncoder_Relative, self.kPIDLoopIdx, self.kTimeoutMs,) self.wrist.setSensorPhase(False) #!!!!! # Set relevant frame periods to be at least as fast as periodic rate self.wrist.setStatusFramePeriod( Talon.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, self.kTimeoutMs) self.wrist.setStatusFramePeriod( Talon.StatusFrameEnhanced.Status_10_MotionMagic, 10, self.kTimeoutMs) # set the peak and nominal outputs self.wrist.configNominalOutputForward(0, self.kTimeoutMs) self.wrist.configNominalOutputReverse(0, self.kTimeoutMs) self.wrist.configPeakOutputForward(0.6, self.kTimeoutMs) self.wrist.configPeakOutputReverse(-0.25, self.kTimeoutMs) self.kF = self.getFeedForward(self.getNumber("Wrist F Gain" , 0)) self.kP = self.getNumber("Wrist kP" , 0) self.kI = self.getNumber("Wrist kI" , 0) self.kD = self.getNumber("Wrist kD" , 0) # set closed loop gains in slot0 - see documentation */ self.wrist.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx) self.wrist.config_kF(0, self.kF, self.kTimeoutMs) self.wrist.config_kP(0, self.kP, self.kTimeoutMs) self.wrist.config_kI(0, 0, self.kTimeoutMs) self.wrist.config_kD(0, 0, self.kTimeoutMs) # set acceleration and vcruise velocity - see documentation self.wrist.configMotionCruiseVelocity(400, self.kTimeoutMs) #!!!!! self.wrist.configMotionAcceleration(500, self.kTimeoutMs) #!!!!! # zero the sensor self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def robotInit(self): self.joystick = Joystick(0) self.motor = Talon(10) self.motor.configClearPositionOnLimitF(True)
def initialize(self): timeout = 15 SmartDashboard.putNumber("Wrist Power Set", 0) SmartDashboard.putNumber("Gravity Power", 0) self.sb = [] self.targetPosUp = -300 #!!!!! self.targetPosDown = -1500 #!!!!! self.maxVolts = 10 self.simpleGain = 0.57 self.wristUpVolts = 5 self.wristDownVolts = 2 self.simpleGainGravity = 0.07 self.xbox = oi.getJoystick(2) self.joystick0 = oi.getJoystick(0) #below is the talon on the intake self.motor = Talon(map.intake) self.input = 0 self.input2 = 0 self.lastCargoCommand = "unknown" self.motor.configContinuousCurrentLimit(20, timeout) #15 Amps per motor self.motor.configPeakCurrentLimit( 30, timeout) #20 Amps during Peak Duration self.motor.configPeakCurrentDuration( 100, timeout) #Peak Current for max 100 ms self.motor.enableCurrentLimit(True) #Talon motor object created self.wrist = Talon(map.wrist) if not wpilib.RobotBase.isSimulation(): self.wrist.configFactoryDefault() self.wrist.configVoltageCompSaturation(self.maxVolts) self.wrist.setInverted(True) self.wrist.setNeutralMode(2) self.motor.setNeutralMode(2) self.motor.configVoltageCompSaturation(self.maxVolts) self.wrist.configClearPositionOnLimitF(True) #MOTION MAGIC CONFIG self.loops = 0 self.timesInMotionMagic = 0 #self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs) self.F = 0 #should be 0.4 SmartDashboard.putNumber("F Gain", self.F) [self.kP, self.kI, self.kD, self.kF] = [0, 0, 0, 0] cargoController = PIDController(self.kP, self.kI, self.kD, self.kF, self, self) self.cargoController = cargoController self.cargoController.disable() self.pidValuesForMode = { "resting": [-50, self.kP, self.kI, self.kD, 0.15 / -50], "cargoship": [-28, self.kP, self.kI, self.kD, 0.0], "intake": [50, self.kP, self.kI, self.kD, 0.0], "rocket": [5, self.kP, self.kI, self.kD, 0.19 / 5], }