class Robot(wpilib.IterativeRobot): #: Which PID slot to pull gains from. Starting 2018, you can choose from #: 0,1,2 or 3. Only the first two (0,1) are visible in web-based #: configuration. kSlotIdx = 0 #: Talon SRX/ Victor SPX will supported multiple (cascaded) PID loops. For #: now we just want the primary one. kPIDLoopIdx = 0 #: set to zero to skip waiting for confirmation, set to nonzero to wait and #: report to DS if action fails. kTimeoutMs = 10 def robotInit(self): self.talon = WPI_TalonSRX(3) self.joy = wpilib.Joystick(0) self.loops = 0 self.timesInMotionMagic = 0 # first choose the sensor self.talon.configSelectedFeedbackSensor( WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, self.kPIDLoopIdx, self.kTimeoutMs, ) self.talon.setSensorPhase(True) self.talon.setInverted(False) # Set relevant frame periods to be at least as fast as periodic rate self.talon.setStatusFramePeriod( WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, self.kTimeoutMs ) self.talon.setStatusFramePeriod( WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10, self.kTimeoutMs ) # set the peak and nominal outputs self.talon.configNominalOutputForward(0, self.kTimeoutMs) self.talon.configNominalOutputReverse(0, self.kTimeoutMs) self.talon.configPeakOutputForward(1, self.kTimeoutMs) self.talon.configPeakOutputReverse(-1, self.kTimeoutMs) # set closed loop gains in slot0 - see documentation */ self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx) self.talon.config_kF(0, 0.2, self.kTimeoutMs) self.talon.config_kP(0, 0.2, self.kTimeoutMs) self.talon.config_kI(0, 0, self.kTimeoutMs) self.talon.config_kD(0, 0, self.kTimeoutMs) # set acceleration and vcruise velocity - see documentation self.talon.configMotionCruiseVelocity(15000, self.kTimeoutMs) self.talon.configMotionAcceleration(6000, self.kTimeoutMs) # zero the sensor self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs) def teleopPeriodic(self): """ This function is called periodically during operator control """ # get gamepad axis - forward stick is positive leftYstick = -1.0 * self.joy.getY() # calculate the percent motor output motorOutput = self.talon.getMotorOutputPercent() # prepare line to print sb = [] sb.append("\tOut%%: %.3f" % motorOutput) sb.append( "\tVel: %.3f" % self.talon.getSelectedSensorVelocity(self.kPIDLoopIdx) ) if self.joy.getRawButton(1): # Motion Magic - 4096 ticks/rev * 10 Rotations in either direction targetPos = leftYstick * 4096 * 10.0 self.talon.set(WPI_TalonSRX.ControlMode.MotionMagic, targetPos) # append more signals to print when in speed mode. sb.append("\terr: %s" % self.talon.getClosedLoopError(self.kPIDLoopIdx)) sb.append("\ttrg: %.3f" % targetPos) else: # Percent voltage mode self.talon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftYstick) # instrumentation self.processInstrumentation(self.talon, sb) def processInstrumentation(self, tal, sb): # smart dash plots wpilib.SmartDashboard.putNumber( "SensorVel", tal.getSelectedSensorVelocity(self.kPIDLoopIdx) ) wpilib.SmartDashboard.putNumber( "SensorPos", tal.getSelectedSensorPosition(self.kPIDLoopIdx) ) wpilib.SmartDashboard.putNumber( "MotorOutputPercent", tal.getMotorOutputPercent() ) wpilib.SmartDashboard.putNumber( "ClosedLoopError", tal.getClosedLoopError(self.kPIDLoopIdx) ) # check if we are motion-magic-ing if tal.getControlMode() == WPI_TalonSRX.ControlMode.MotionMagic: self.timesInMotionMagic += 1 else: self.timesInMotionMagic = 0 if self.timesInMotionMagic > 10: # print the Active Trajectory Point Motion Magic is servoing towards wpilib.SmartDashboard.putNumber( "ClosedLoopTarget", tal.getClosedLoopTarget(self.kPIDLoopIdx) ) if not self.isSimulation(): wpilib.SmartDashboard.putNumber( "ActTrajVelocity", tal.getActiveTrajectoryVelocity() ) wpilib.SmartDashboard.putNumber( "ActTrajPosition", tal.getActiveTrajectoryPosition() ) wpilib.SmartDashboard.putNumber( "ActTrajHeading", tal.getActiveTrajectoryHeading() ) # periodically print to console self.loops += 1 if self.loops >= 10: self.loops = 0 print(" ".join(sb)) # clear line cache sb.clear()
class LifterComponent(Events): class EVENTS: on_control_move = "on_control_move" on_manual_move = "on_manual move" # RPM Multipliers # ELEVATOR_MULTIPLIER = 1635.15 / 6317.67 ELEVATOR_MULTIPLIER = 2102.35 / 3631.33 CARRIAGE_MULTIPLIER = 1.0 - ELEVATOR_MULTIPLIER # Max heights of each stage. ELEVATOR_MAX_HEIGHT = 40 CARRIAGE_MAX_HEIGHT = 40 # Conversion factors (counts to inches) # CHANGE THESE VALUES ELEVATOR_CONV_FACTOR = 0.00134 CARRIAGE_CONV_FACTOR = 0.000977 el_down = -1 el_up = 1 carriage_down = -1 carriage_up = 1 MAX_SPEED = 0.5 ELEVATOR_ZERO = 0.0 CARRIAGE_ZERO = 0.0 TIMEOUT_MS = 0 ELEVATOR_kF = 0.0 ELEVATOR_kP = 0.1 ELEVATOR_kI = 0.0 ELEVATOR_kD = 0.0 # ALLOWABLE_ERROR = 2 CARRIAGE_ALLOWABLE_ERROR = int(2 / CARRIAGE_CONV_FACTOR) ELEVATOR_ALLOWABLE_ERROR = int(2 / ELEVATOR_CONV_FACTOR) # positions = { # "floor": 2.0, # "portal": 34.0, # "scale_low": 48.0, # "scale_mid": 60.0, # "scale_high": 72.0, # "max_height": 84.0 # } positions = { "floor": 0.5, "portal": 34.0, "scale_low": 52.0, "scale_mid": 68.0, "scale_high": 78.0 } def __init__(self): Events.__init__(self) self.elevator_motor = WPI_TalonSRX(5) self.elevator_bottom_switch = DigitalInput(9) self.carriage_motor = WPI_TalonSRX(3) self.carriage_bottom_switch = DigitalInput(1) self.carriage_top_switch = DigitalInput(2) self._create_events([ LifterComponent.EVENTS.on_control_move, LifterComponent.EVENTS.on_manual_move ]) self._is_reset = False # configure elevator motor and encoder self.elevator_motor.setNeutralMode(NeutralMode.Brake) self.elevator_motor.configSelectedFeedbackSensor( FeedbackDevice.CTRE_MagEncoder_Relative, 0, LifterComponent.TIMEOUT_MS) self.elevator_motor.setSensorPhase(True) self.elevator_motor.setInverted(True) self.elevator_motor.configNominalOutputForward( LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS) self.elevator_motor.configNominalOutputReverse( LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS) self.elevator_motor.configPeakOutputForward(LifterComponent.el_up, LifterComponent.TIMEOUT_MS) self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down, LifterComponent.TIMEOUT_MS) self.elevator_motor.configNominalOutputForward( LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS) self.elevator_motor.configNominalOutputReverse( LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS) self.elevator_motor.configPeakOutputForward(LifterComponent.el_up, LifterComponent.TIMEOUT_MS) self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down, LifterComponent.TIMEOUT_MS) self.elevator_motor.configAllowableClosedloopError( 0, LifterComponent.ELEVATOR_ALLOWABLE_ERROR, LifterComponent.TIMEOUT_MS) self.elevator_motor.configForwardSoftLimitThreshold( int(LifterComponent.ELEVATOR_MAX_HEIGHT / LifterComponent.ELEVATOR_CONV_FACTOR), 0) self.elevator_motor.config_kF(0, LifterComponent.ELEVATOR_kF, LifterComponent.TIMEOUT_MS) self.elevator_motor.config_kP(0, LifterComponent.ELEVATOR_kP, LifterComponent.TIMEOUT_MS) self.elevator_motor.config_kI(0, LifterComponent.ELEVATOR_kI, LifterComponent.TIMEOUT_MS) self.elevator_motor.config_kD(0, LifterComponent.ELEVATOR_kD, LifterComponent.TIMEOUT_MS) # self.elevator_motor.setCurrentLimit() # self.elevator_motor.EnableCurrentLimit() # self.elevator_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS) # configure carriage motor and encoder self.carriage_motor.setNeutralMode(NeutralMode.Brake) self.carriage_motor.configSelectedFeedbackSensor( FeedbackDevice.CTRE_MagEncoder_Relative, 0, LifterComponent.TIMEOUT_MS) self.carriage_motor.setSensorPhase(True) self.carriage_motor.setInverted(True) self.carriage_motor.configNominalOutputForward( LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS) self.carriage_motor.configNominalOutputReverse( LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS) self.carriage_motor.configPeakOutputForward(LifterComponent.el_up, LifterComponent.TIMEOUT_MS) self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down, LifterComponent.TIMEOUT_MS) self.carriage_motor.configNominalOutputForward( LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS) self.carriage_motor.configNominalOutputReverse( LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS) self.carriage_motor.configPeakOutputForward(LifterComponent.el_up, LifterComponent.TIMEOUT_MS) self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down, LifterComponent.TIMEOUT_MS) self.carriage_motor.configAllowableClosedloopError( 0, LifterComponent.CARRIAGE_ALLOWABLE_ERROR, LifterComponent.TIMEOUT_MS) self.carriage_motor.configForwardSoftLimitThreshold( int(LifterComponent.CARRIAGE_MAX_HEIGHT / LifterComponent.CARRIAGE_CONV_FACTOR), 0) self.carriage_motor.config_kF(0, LifterComponent.ELEVATOR_kF, LifterComponent.TIMEOUT_MS) self.carriage_motor.config_kP(0, LifterComponent.ELEVATOR_kP, LifterComponent.TIMEOUT_MS) self.carriage_motor.config_kI(0, LifterComponent.ELEVATOR_kI, LifterComponent.TIMEOUT_MS) self.carriage_motor.config_kD(0, LifterComponent.ELEVATOR_kD, LifterComponent.TIMEOUT_MS) # self.carriage_motor.setCurrentLimit() # self.carriage_motor.EnableCurrentLimit() # self.carriage_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS) def set_elevator_speed(self, speed): if (speed > 0 and self.current_elevator_position >= LifterComponent.ELEVATOR_MAX_HEIGHT - 2) \ or (speed < 0 and self.elevator_bottom_switch.get()): self.elevator_motor.set(0) else: self.elevator_motor.set(speed) self.trigger_event(LifterComponent.EVENTS.on_manual_move) def set_carriage_speed(self, speed): if (speed > 0 and self.carriage_top_switch.get()) \ or (speed < 0 and self.carriage_bottom_switch.get()): self.carriage_motor.set(0) else: self.carriage_motor.set(speed) self.trigger_event(LifterComponent.EVENTS.on_manual_move) def reset_sensors(self): self.carriage_motor.setSelectedSensorPosition( 0, 0, LifterComponent.TIMEOUT_MS) self.elevator_motor.setSelectedSensorPosition( 0, 0, LifterComponent.TIMEOUT_MS) self._is_reset = True @property def current_elevator_position(self) -> float: return self.elevator_motor.getSelectedSensorPosition( 0) * LifterComponent.ELEVATOR_CONV_FACTOR @property def current_carriage_position(self) -> float: return self.carriage_motor.getSelectedSensorPosition( 0) * LifterComponent.CARRIAGE_CONV_FACTOR @property def current_position(self) -> float: return self.current_elevator_position + self.current_carriage_position def stop_lift(self): self.elevator_motor.stopMotor() self.carriage_motor.stopMotor() def elevator_to_target_position(self, inches: float): if self._is_reset: self.elevator_motor.set( WPI_TalonSRX.ControlMode.Position, inches / LifterComponent.ELEVATOR_CONV_FACTOR) self.trigger_event(LifterComponent.EVENTS.on_control_move) def carriage_to_target_position(self, inches: float): if self._is_reset: self.carriage_motor.set( WPI_TalonSRX.ControlMode.Position, inches / LifterComponent.CARRIAGE_CONV_FACTOR) self.trigger_event(LifterComponent.EVENTS.on_control_move) def lift_to_distance(self, inches): i = inches + 6 elevator = min(i * LifterComponent.ELEVATOR_MULTIPLIER, LifterComponent.ELEVATOR_MAX_HEIGHT) carriage = i - elevator print("lift_to_distance carriage" + str(carriage)) print("lift_to_distance elevate" + str(elevator)) print("lift_to_distance lifter" + str(carriage + elevator)) self.elevator_to_target_position(elevator) self.carriage_to_target_position(carriage) self.trigger_event(LifterComponent.EVENTS.on_control_move) def is_at_position(self, position: str) -> bool: return self.is_at_distance(LifterComponent.positions[position]) def is_at_distance(self, inches: float) -> bool: return inches - 5 < self.current_position < inches + 5 def closest_position(self) -> tuple: # returns the key for the position closest to the current position positions = [(key, position) for key, position in LifterComponent.positions.items()] return min( positions, key=lambda position: abs(self.current_position - position[1])) def next_position(self) -> str: position = self.closest_position() positions = [(key, position) for key, position in LifterComponent.positions.items()] index = positions.index(position) if index == len(positions) - 1: return position[0] return positions[index + 1][0] def prev_position(self) -> str: position = self.closest_position() positions = [(key, position) for key, position in LifterComponent.positions.items()] index = positions.index(position) if index == 0: return position[0] return positions[index - 1][0]
class Robot(wpilib.IterativeRobot): #: Which PID slot to pull gains from. Starting 2018, you can choose from #: 0,1,2 or 3. Only the first two (0,1) are visible in web-based #: configuration. kSlotIdx = 0 #: Talon SRX/ Victor SPX will supported multiple (cascaded) PID loops. For #: now we just want the primary one. kPIDLoopIdx = 0 #: set to zero to skip waiting for confirmation, set to nonzero to wait and #: report to DS if action fails. kTimeoutMs = 10 def robotInit(self): self.talon = WPI_TalonSRX(3) self.joy = wpilib.Joystick(0) self.loops = 0 self.lastButton1 = False self.targetPos = 0 # choose the sensor and sensor direction self.talon.configSelectedFeedbackSensor( WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, self.kPIDLoopIdx, self.kTimeoutMs, ) # choose to ensure sensor is positive when output is positive self.talon.setSensorPhase(True) # choose based on what direction you want forward/positive to be. # This does not affect sensor phase. self.talon.setInverted(False) # set the peak and nominal outputs, 12V means full self.talon.configNominalOutputForward(0, self.kTimeoutMs) self.talon.configNominalOutputReverse(0, self.kTimeoutMs) self.talon.configPeakOutputForward(1, self.kTimeoutMs) self.talon.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.talon.configAllowableClosedloopError(0, self.kPIDLoopIdx, self.kTimeoutMs) # set closed loop gains in slot0, typically kF stays zero - see documentation */ self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx) self.talon.config_kF(0, 0, self.kTimeoutMs) self.talon.config_kP(0, 0.1, self.kTimeoutMs) self.talon.config_kI(0, 0, self.kTimeoutMs) self.talon.config_kD(0, 0, self.kTimeoutMs) # zero the sensor self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs) def teleopPeriodic(self): """ This function is called periodically during operator control """ # get gamepad axis - forward stick is positive leftYstick = self.joy.getY() # calculate the percent motor output motorOutput = self.talon.getMotorOutputPercent() button1 = self.joy.getRawButton(1) button2 = self.joy.getRawButton(2) # deadband gamepad if abs(leftYstick) < 0.1: leftYstick = 0 # prepare line to print sb = [] sb.append("\tOut%%: %.3f" % motorOutput) sb.append("\tPos: %.3fu" % self.talon.getSelectedSensorPosition(self.kPIDLoopIdx)) if self.lastButton1 and button1: # Position mode - button just pressed # 10 Rotations * 4096 u/rev in either direction self.targetPos = leftYstick * 4096 * 10.0 self.talon.set(WPI_TalonSRX.ControlMode.Position, self.targetPos) # on button2 just straight drive if button2: # Percent voltage mode self.talon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftYstick) if self.talon.getControlMode() == WPI_TalonSRX.ControlMode.Position: # append more signals to print when in speed mode. sb.append("\terr: %s" % self.talon.getClosedLoopError(self.kPIDLoopIdx)) sb.append("\ttrg: %.3f" % self.targetPos) # periodically print to console self.loops += 1 if self.loops >= 10: self.loops = 0 print(" ".join(sb)) # save button state for on press detect self.lastButton1 = button1
class CargoMech(): kSlotIdx = 0 kPIDLoopIdx = 0 kTimeoutMs = 10 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 intake(self, mode): ''' Intake/Outtake/Stop Intake the cargo (turn wheels inward)''' if mode == "intake": self.motor.set(self.intakeSpeed) elif mode == "outtake": self.motor.set(-1 * self.intakeSpeed) elif mode == "stop": self.motor.set(0) def moveWrist(self,mode): '''move wrist in and out of robot''' if mode == "up": self.wrist.set(self.getPowerSimple("up")) elif mode == "down": self.wrist.set(-1 * self.getPowerSimple("down")) elif mode == "upVolts": self.wrist.set(self.wristUpVolts/self.maxVolts) elif mode == "downVolts": self.wrist.set(-1 * self.wristDownVolts/ self.maxVolts) elif mode == "upMagic": if self.lastMode != mode: 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) # Motion Magic - 4096 ticks/rev * 10 Rotations in either direction self.wrist.set(Talon.ControlMode.MotionMagic, self.targetPosUp) # append more signals to print when in speed mode. self.sb.append("\terr: %s" % self.wrist.getClosedLoopError(self.kPIDLoopIdx)) self.sb.append("\ttrg: %.3f" % self.targetPosUp) elif mode == "downMagic": if self.lastMode != mode: 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) # Motion Magic - 4096 ticks/rev * 10 Rotations in either direction self.wrist.set(Talon.ControlMode.MotionMagic, self.targetPosDown) # append more signals to print when in speed mode. self.sb.append("\terr: %s" % self.wrist.getClosedLoopError(self.kPIDLoopIdx)) self.sb.append("\ttrg: %.3f" % self.targetPosDown) elif mode == "stop": self.wrist.set(0) else: self.wrist.set(self.getGravitySimple()) self.lastMode = mode def periodic(self): deadband = 0.4 if self.xbox.getRawAxis(map.intakeCargo)>deadband: self.intake("intake") elif self.xbox.getRawAxis(map.outtakeCargo)>deadband: self.intake("outtake") else: self.intake("stop") if self.xbox.getRawButton(map.wristUp): self.moveWrist("up") elif self.xbox.getRawButton(map.wristDown): self.moveWrist("down") elif self.joystick0.getRawButton(map.wristUpVolts): self.moveWrist("upVolts") elif self.joystick0.getRawButton(map.wristDownVolts): self.moveWrist("downVolts") elif self.joystick0.getRawButton(map.wristUpMagic): self.moveWrist("upMagic") elif self.joystick0.getRawButton(map.wristDownMagic): self.moveWrist("downMagic") else: self.moveWrist("gravity") # calculate the percent motor output motorOutput = self.wrist.getMotorOutputPercent() self.sb = [] # prepare line to print self.sb.append("\tOut%%: %.3f" % motorOutput) self.sb.append("\tVel: %.3f" % self.wrist.getSelectedSensorVelocity(self.kPIDLoopIdx)) # instrumentation self.processInstrumentation(self.wrist, self.sb) def disable(self): self.intake("stop") def processInstrumentation(self, tal, sb): # smart dash plots wpilib.SmartDashboard.putNumber("SensorVel", tal.getSelectedSensorVelocity(self.kPIDLoopIdx)) wpilib.SmartDashboard.putNumber("SensorPos", tal.getSelectedSensorPosition(self.kPIDLoopIdx)) wpilib.SmartDashboard.putNumber("MotorOutputPercent", tal.getMotorOutputPercent()) wpilib.SmartDashboard.putNumber("ClosedLoopError", tal.getClosedLoopError(self.kPIDLoopIdx)) # check if we are motion-magic-ing if tal.getControlMode() == Talon.ControlMode.MotionMagic: self.timesInMotionMagic += 1 else: self.timesInMotionMagic = 0 if self.timesInMotionMagic > 10: # print the Active Trajectory Point Motion Magic is servoing towards wpilib.SmartDashboard.putNumber( "ClosedLoopTarget", tal.getClosedLoopTarget(self.kPIDLoopIdx) ) if not wpilib.RobotBase.isSimulation(): wpilib.SmartDashboard.putNumber( "ActTrajVelocity", tal.getActiveTrajectoryVelocity() ) wpilib.SmartDashboard.putNumber( "ActTrajPosition", tal.getActiveTrajectoryPosition() ) wpilib.SmartDashboard.putNumber( "ActTrajHeading", tal.getActiveTrajectoryHeading() ) # periodically print to console self.loops += 1 if self.loops >= 10: self.loops = 0 print(" ".join(self.sb)) # clear line cache self.sb.clear() def getAngle(self): pos = self.getPosition() angle = abs(pos * 115/self.targetPosDown) return angle - 25 def getPosition(self): return self.wrist.getQuadraturePosition() def getFeedForward(self, gain): angle = self.getAngle() return angle*gain def getPowerSimple(self, direction): '''true direction is up into robot false direction is down out of robot''' angle = self.getAngle() power = abs(self.simpleGainGravity * math.sin(math.radians(angle)) + self.simpleGain) if angle > 80: if direction == "down": power = 0 if angle < -20: if direction == "up": power = 0 return power def getGravitySimple(self): angle = self.getAngle() power = self.simpleGainGravity * math.sin(math.radians(angle)) return power def getNumber(self, key, defVal): val = SmartDashboard.getNumber(key, None) if val == None: val = defVal SmartDashboard.putNumber(key, val) return val def dashboardInit(self): pass def dashboardPeriodic(self): self.wristUp = self.getNumber("WristUpSpeed" , 0.5) self.wristDown = self.getNumber("WristDownSpeed" , 0.2) self.wristUpVolts = self.getNumber("WristUpVoltage" , 5) self.wristDownVolts = self.getNumber("WristDownVoltage" , 2) 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) self.simpleGain = self.getNumber("Wrist Simple Gain", 0.5) self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", 0.07) SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition()) SmartDashboard.putNumber("Wrist Angle" , self.getAngle()) SmartDashboard.putNumber("Wrist Power Up" , self.getPowerSimple("up")) SmartDashboard.putNumber("Wrist Power Down" , self.getPowerSimple("down"))
class DriveSubsystem(Subsystem): def __init__(self, robot): super().__init__("Drive") self.robot = robot self.left_master = WPI_TalonSRX(robotmap.DRIVELEFTMASTER) self.right_master = WPI_TalonSRX(robotmap.DRIVERIGHTMASTER) self.left_slave = WPI_TalonSRX(robotmap.DRIVELEFTSLAVE) self.right_slave = WPI_TalonSRX(robotmap.DRIVERIGHTSLAVE) self.shifter_high = Solenoid(robotmap.PCM1_CANID, robotmap.SHIFTER_HIGH) self.shifter_low = Solenoid(robotmap.PCM1_CANID, robotmap.SHIFTER_LOW) self.differential_drive = DifferentialDrive(self.left_master, self.right_master) self.TalonConfig() self.shiftLow() def teleDrive(self, xSpeed, zRotation): if self.robot.oi.getController1().B().get(): scale = SmartDashboard.getNumber("creep_mult", 0.3) xSpeed = xSpeed * scale zRotation = zRotation * scale self.differential_drive.arcadeDrive(xSpeed, zRotation, False) def getLeftPosition(self): return self.left_master.getSelectedSensorPosition(0) def getRightPosition(self): return self.right_master.getSelectedSensorPosition(0) def getRightSpeed(self): return self.right_master.getSelectedSensorVelocity(0) def getLeftSpeed(self): return self.unitsToInches( self.left_master.getSelectedSensorVelocity(0)) def getErrorLeft(self): return self.left_master.getClosedLoopError(0) def getErrorRight(self): return self.right_master.getClosedLoopError(0) def isLeftSensorConnected(self): return self.left_master.getSensorCollection( ).getPulseWidthRiseToRiseUs() != 0 def isRightSensorConnected(self): return self.right_master.getSensorCollection( ).getPulseWidthRiseToRiseUs() != 0 def resetEncoders(self): self.left_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS) self.right_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS) self.left_master.getSensorCollection().setQuadraturePosition( 0, robotmap.TIMEOUT_MS) self.right_master.getSensorCollection().setQuadraturePosition( 0, robotmap.TIMEOUT_MS) def setMotionMagicLeft(self, setpoint): SmartDashboard.putNumber("setpoint_left_units", self.inchesToUnits(setpoint)) self.left_master.set(ControlMode.MotionMagic, self.inchesToUnits(setpoint)) def setMotionMagicRight(self, setpoint): self.right_master.set(ControlMode.MotionMagic, self.inchesToUnits(-setpoint)) def isMotionMagicNearTarget(self): retval = False if self.left_master.getActiveTrajectoryPosition( ) == self.left_master.getClosedLoopTarget(0): if self.right_master.getActiveTrajectoryPosition( ) == self.right_master.getClosedLoopTarget(0): if abs(self.left_master.getClosedLoopError(0)) < 300: if abs(self.right_master.getClosedLoopError(0)) < 300: retval = True return retval def shiftHigh(self): self.shifter_high.set(True) self.shifter_low.set(False) def shiftLow(self): self.shifter_high.set(False) self.shifter_low.set(True) def stop(self): self.left_master.set(ControlMode.PercentOutput, 0.0) self.right_master.set(ControlMode.PercentOutput, 0.0) def unitsToInches(self, units): return units * robotmap.DRIVE_WHEEL_CIRCUMFERENCE / robotmap.DRIVE_ENCODER_PPR def inchesToUnits(self, inches): return inches * robotmap.DRIVE_ENCODER_PPR / robotmap.DRIVE_WHEEL_CIRCUMFERENCE def autoInit(self): self.resetEncoders() self.left_master.setNeutralMode(NeutralMode.Brake) self.left_slave.setNeutralMode(NeutralMode.Brake) self.right_master.setNeutralMode(NeutralMode.Brake) self.right_slave.setNeutralMode(NeutralMode.Brake) self.differential_drive.setSafetyEnabled(False) self.shiftLow() def teleInit(self): self.left_master.setNeutralMode(NeutralMode.Coast) self.left_slave.setNeutralMode(NeutralMode.Coast) self.right_master.setNeutralMode(NeutralMode.Coast) self.right_slave.setNeutralMode(NeutralMode.Coast) self.differential_drive.setSafetyEnabled(True) self.shiftLow() def TalonConfig(self): self.left_master.configSelectedFeedbackSensor( FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotmap.TIMEOUT_MS) self.right_master.configSelectedFeedbackSensor( FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotmap.TIMEOUT_MS) self.left_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS) self.right_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS) self.left_master.setStatusFramePeriod( WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, robotmap.TIMEOUT_MS) self.left_master.setStatusFramePeriod( WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10, robotmap.TIMEOUT_MS) self.right_master.setStatusFramePeriod( WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, robotmap.TIMEOUT_MS) self.right_master.setStatusFramePeriod( WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10, robotmap.TIMEOUT_MS) self.left_master.setSensorPhase(False) self.right_master.setSensorPhase(False) self.left_master.setInverted(True) self.left_slave.setInverted(True) self.right_master.setInverted(True) self.right_slave.setInverted(True) #Makes talons force zero voltage across when zero output to resist motion self.left_master.setNeutralMode(NeutralMode.Brake) self.left_slave.setNeutralMode(NeutralMode.Brake) self.right_master.setNeutralMode(NeutralMode.Brake) self.right_slave.setNeutralMode(NeutralMode.Brake) self.left_slave.set(ControlMode.Follower, robotmap.DRIVELEFTMASTER) self.right_slave.set(ControlMode.Follower, robotmap.DRIVERIGHTMASTER) #Closed Loop Voltage Limits self.left_master.configNominalOutputForward(0.0, robotmap.TIMEOUT_MS) self.right_master.configNominalOutputForward(0.0, robotmap.TIMEOUT_MS) self.left_master.configNominalOutputReverse(-0.0, robotmap.TIMEOUT_MS) self.right_master.configNominalOutputReverse(-0.0, robotmap.TIMEOUT_MS) self.left_master.configPeakOutputForward(1.0, robotmap.TIMEOUT_MS) self.right_master.configPeakOutputForward(1.0, robotmap.TIMEOUT_MS) self.left_master.configPeakOutputReverse(-1.0, robotmap.TIMEOUT_MS) self.right_master.configPeakOutputReverse(-1.0, robotmap.TIMEOUT_MS) def configGains(self, f, p, i, d, izone, cruise, accel): self.left_master.selectProfileSlot(0, 0) self.left_master.config_kF(0, f, robotmap.TIMEOUT_MS) self.left_master.config_kP(0, p, robotmap.TIMEOUT_MS) self.left_master.config_kI(0, i, robotmap.TIMEOUT_MS) self.left_master.config_kD(0, d, robotmap.TIMEOUT_MS) self.left_master.config_IntegralZone(0, izone, robotmap.TIMEOUT_MS) self.right_master.selectProfileSlot(0, 0) self.right_master.config_kF(0, f, robotmap.TIMEOUT_MS) self.right_master.config_kP(0, p, robotmap.TIMEOUT_MS) self.right_master.config_kI(0, i, robotmap.TIMEOUT_MS) self.right_master.config_kD(0, d, robotmap.TIMEOUT_MS) self.right_master.config_IntegralZone(0, izone, robotmap.TIMEOUT_MS) self.left_master.configMotionCruiseVelocity(cruise, robotmap.TIMEOUT_MS) self.left_master.configMotionAcceleration(accel, robotmap.TIMEOUT_MS) self.right_master.configMotionCruiseVelocity(cruise, robotmap.TIMEOUT_MS) self.right_master.configMotionAcceleration(accel, robotmap.TIMEOUT_MS)