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(magicbot.MagicRobot): location: PosApprox powertrain: Powertrain encoders: Encoders navx: NavX drivetrain: Drivetrain limelight: Limelight shooter: Shooter serializer: Serializer color_sensor: WheelOfFortuneSensor fortune_controller: ColorWheelController intake_lift: IntakeLift intake_roller: IntakeRoller intake: Intake climber: Climber serializer: Serializer def createObjects(self): # initialize physical objects #limelight self.limelight_table = NetworkTables.getTable("limelight") # drivetrain self.limelight_table = NetworkTables.getTable('limelight') motor_objects_left = [ CANSparkMax(port, MotorType.kBrushless) for port in RobotMap.Drivetrain.motors_left ] self.left_motors = wpilib.SpeedControllerGroup(motor_objects_left[0], *motor_objects_left[1:]) motor_objects_right = [ CANSparkMax(port, MotorType.kBrushless) for port in RobotMap.Drivetrain.motors_right ] self.right_motors = wpilib.SpeedControllerGroup( motor_objects_right[0], *motor_objects_right[1:]) for motor in motor_objects_left + motor_objects_right: config_spark(motor, RobotMap.Drivetrain.motor_config) self.differential_drive = wpilib.drive.DifferentialDrive( self.left_motors, self.right_motors) self.differential_drive.setMaxOutput( RobotMap.Drivetrain.max_motor_power) self.left_encoder = wpilib.Encoder(RobotMap.Encoders.left_encoder_b, RobotMap.Encoders.left_encoder_a) self.right_encoder = wpilib.Encoder(RobotMap.Encoders.right_encoder_b, RobotMap.Encoders.right_encoder_a) self.right_encoder.setReverseDirection(False) self.left_encoder.setDistancePerPulse( RobotMap.Encoders.distance_per_pulse) self.right_encoder.setDistancePerPulse( RobotMap.Encoders.distance_per_pulse) self.navx_ahrs = navx.AHRS.create_spi() self.driver = Driver(wpilib.XboxController(0)) self.sysop = Sysop(wpilib.XboxController(1)) # intake self.intake_lift_motor = WPI_TalonSRX(RobotMap.IntakeLift.motor_port) self.intake_lift_motor.configPeakOutputForward( RobotMap.IntakeLift.max_power) self.intake_lift_motor.configPeakOutputReverse( -RobotMap.IntakeLift.max_power) config_talon(self.intake_lift_motor, RobotMap.IntakeLift.motor_config) self.intake_lift_motor.setSelectedSensorPosition(0) self.intake_roller_motor = WPI_TalonSRX( RobotMap.IntakeRoller.motor_port) self.intake_roller_motor.configPeakOutputForward( RobotMap.IntakeRoller.max_power) self.intake_roller_motor.configPeakOutputReverse( -RobotMap.IntakeRoller.max_power) config_talon(self.intake_roller_motor, RobotMap.IntakeRoller.motor_config) #self.intake_roller_motor.setSelectedSensorPosition(0) # climber self.climber_motor = WPI_TalonSRX(RobotMap.Climber.motor_port) config_talon(self.climber_motor, RobotMap.Climber.motor_config) # color wheel self.color_wheel_motor = WPI_TalonSRX(RobotMap.ColorWheel.motor_port) config_talon(self.color_wheel_motor, RobotMap.ColorWheel.motor_config) # shooter self.shooter_motor = WPI_TalonSRX(RobotMap.Shooter.motor_port) config_talon(self.shooter_motor, RobotMap.Shooter.motor_config) # serializer self.serializer_motor = WPI_TalonSRX(RobotMap.Serializer.motor_port) config_talon(self.serializer_motor, RobotMap.Serializer.motor_config) self.i2c_color_sensor = ColorSensorV3(wpilib.I2C.Port.kOnboard) # controllers and electrical stuff self.driver = Driver(wpilib.XboxController(0)) self.sysop = Sysop(wpilib.XboxController(1)) NetworkTables.initialize(server="roborio") # camera server wpilib.CameraServer.launch() def reset_subsystems(self): self.drivetrain.reset() self.climber.reset() self.limelight.reset() def teleopInit(self): self.reset_subsystems() self.serializer.turn_on() def teleopPeriodic(self): #print(self.limelight.get) #print(self.color_wheel_motor.__dir__()); #print("execute method") try: # drive the drivetrain as needed driver_x, driver_y = self.driver.get_curvature_output() # manually handled driving if self.drivetrain.state == DrivetrainState.MANUAL_DRIVE: self.drivetrain.curvature_drive(driver_y, driver_x) # set turbo mode self.drivetrain.set_power_scaling(self.driver.process_turbo_mode()) # # check and see if we need to activate driver assists # if self.drivetrain.ready_straight_assist() and self.driver.ready_straight_assist(): # self.drivetrain.drive_straight(driver_y) # elif self.drivetrain.state == DrivetrainState.AIDED_DRIVE_STRAIGHT: # self.drivetrain.state = DrivetrainState.MANUAL_DRIVE # revert to manual control if enabled if self.driver.get_manual_control_override(): self.drivetrain.state = DrivetrainState.MANUAL_DRIVE except: print("DRIVETRAIN ERROR") try: # move intake lift if self.sysop.get_intake_lower(): self.intake_lift.send_down() elif self.sysop.get_intake_raise(): self.intake_lift.send_up() except: print("INTAKE LIFT ERROR") try: #print(self.intake_roller_motor.getSelectedSensorPosition()); # intake rollers if self.sysop.get_intake_intake(): self.intake_roller.intake() elif self.sysop.get_intake_outtake(): self.intake_roller.outtake() else: self.intake_roller.stop() except: print("INTAKE ROLLER ERROR") try: if self.sysop.get_target_aim(): self.shooter.aim_and_fire() elif self.sysop.get_shooter_stop(): self.shooter.stop_fire() self.drivetrain.reset_state() elif self.sysop.get_change_shooter_power() != 0: self.shooter.adjust_power( self.sysop.get_change_shooter_power()) except: traceback.print_exc() print("AUTO AIM ERROR") try: self.climber.set_power(self.sysop.get_climb_axis()) except: print("CLIMBER ERROR") try: manual_fortune_input = self.sysop.get_manual_fortune_axis() fms_color_position = self.ds.getGameSpecificMessage() dash.putString("Color sensor color", self.color_sensor.get_color()) dash.putString("FMS Color", fms_color_position) if manual_fortune_input != 0: self.fortune_controller.manual_power(manual_fortune_input) # elif self.sysop.get_position_control() and fms_color_position != "": # self.fortune_controller.position_control({ # "B": WheelOfFortuneColor.BLUE, # "R": WheelOfFortuneColor.GREEN, # "G": WheelOfFortuneColor.RED, # "Y": WheelOfFortuneColor.YELLOW, # }[fms_color_position]) # TODO this is commented out because it'll be more effective to use manual control # elif self.sysop.get_rotation_control(): # self.fortune_controller.rotation_control() elif self.fortune_controller.state == ColorWheelState.MANUAL: self.fortune_controller.manual_power(0) except: print("COLOR WHEEL ERROR")
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 Climber(Subsystem): 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 climberFunction(self): self.climbLatch() if self.driverOne.getPOV() == 0: if self.getCheckSwitch(): self.lightsHandler(2) else: self.lightsHandler(1) self.climbMotor.set(0.75) elif self.driverOne.getPOV() == 180: if self.getCheckSwitch(): self.lightsHandler(2) else: self.lightsHandler(1) self.climbMotor.set(-0.75) else: if self.getCheckSwitch(): self.lightsHandler(2) self.climbMotor.set(0) wpilib.SmartDashboard.putBoolean('Latch Switch', self.checkSwitch.get() == 1) def climbLatch(self): if self.driverOne.getRawButtonReleased(1): self.latchToggleCount += 1 if self.latchToggleCount % 2 == 1: self.solenoid.set(self.solenoid.Value.kReverse) else: self.solenoid.set(self.solenoid.Value.kForward) def getCheckSwitch(self): return not self.checkSwitch.get() if not self.checkSwitch.get(): self.lightsHandler(1) def lightsHandler(self, tempHandlerState): if tempHandlerState != self.handlerState: if tempHandlerState == 1: self.lights.sendDontClimb() elif tempHandlerState == 2: self.lights.sendOKToClimb() self.handlerState = tempHandlerState
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)
class DriveTrain(Subsystem): ''' 'Tank Drive' system set up with 2 motors per side, one a "master" with a mag encoder attached and the other "slave" controller set to follow the "master". ''' def __init__(self, robot): self.robot = robot self.ahrs = AHRS.create_spi() self.ahrs.reset() # self.angleAdjustment = self.ahrs.getAngle() # self.ahrs.setAngleAdjustment(self.angleAdjustment) # 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) self.driveLeftMaster.configNominalOutputForward(0, 0) self.driveLeftMaster.configNominalOutputReverse(0, 0) self.driveRightMaster.configNominalOutputForward(0, 0) self.driveRightMaster.configNominalOutputReverse(0, 0) self.speed = .4 self.driveLeftMaster.configPeakOutputForward(self.speed, 0) self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0) self.driveRightMaster.configPeakOutputForward(self.speed, 0) self.driveRightMaster.configPeakOutputReverse(-self.speed, 0) self.driveLeftMaster.configClosedLoopRamp(.2, 0) self.driveRightMaster.configClosedLoopRamp(.2, 0) self.driveLeftMaster.setSafetyEnabled(False) self.driveRightMaster.setSafetyEnabled(False) # 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) self.PID() """ 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) self.driveLeftMaster.setSelectedSensorPosition(0, 0, 0) self.driveRightMaster.setSelectedSensorPosition(0, 0, 0) # 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.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) self.drive.setSafetyEnabled(False) self.previousError = 0 super().__init__() def autoInit(self): self.speed = .5 self.driveLeftMaster.configPeakOutputForward(self.speed, 0) self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0) self.driveRightMaster.configPeakOutputForward(self.speed, 0) self.driveRightMaster.configPeakOutputReverse(-self.speed, 0) self.driveLeftMaster.config_kP(0, .115, 0) self.driveRightMaster.config_kP(0, .115, 0) # self.driveLeftMaster.config_kP(0, .185, 0) # self.driveRightMaster.config_kP(0, .185, 0) # self.driveLeftMaster.config_kP(0, 20, 0) # self.driveRightMaster.config_kP(0, 20, 0) self.driveLeftMaster.config_kF(0, 0.0, 0) self.driveRightMaster.config_kF(0, 0.0, 0) def teleInit(self): self.speed = .55 self.driveLeftMaster.configPeakOutputForward(self.speed, 0) self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0) self.driveRightMaster.configPeakOutputForward(self.speed, 0) self.driveRightMaster.configPeakOutputReverse(-self.speed, 0) self.driveLeftMaster.config_kP(0, 0.0, 0) self.driveRightMaster.config_kP(0, 0.0, 0) self.driveLeftMaster.config_kF(0, 0.313, 0) self.driveRightMaster.config_kF(0, 0.313, 0) def moveToPosition(self, position): self.driveLeftMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Position, position) self.driveRightMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Position, position) def stop(self): self.drive.stopMotor() def arcade(self, speed, rotation): # self.updateSD() if self.robot.dStick.getRawButtonReleased(3): self.robotFrontToggleCount += 1 """ This if statement acts as a toggle to change which motors are inverted, completely changing the "front" of the robot. This is useful for when we are about to climb. """ if self.robotFrontToggleCount % 2 == 0: self.drive.arcadeDrive(speed, rotation, True) else: self.drive.arcadeDrive(-speed, rotation, True) def arcadeWithRPM(self, speed, rotation, maxRPM): # self.updateSD() self.driveLeftMaster.setSafetyEnabled(False) if self.robot.dStick.getRawButtonReleased(3): self.robotFrontToggleCount += 1 if self.robotFrontToggleCount % 2 == 0: XSpeed = wpilib.RobotDrive.limit(speed) else: XSpeed = wpilib.RobotDrive.limit(-speed) XSpeed = self.applyDeadband(XSpeed, .02) ZRotation = wpilib.RobotDrive.limit(rotation) ZRotation = self.applyDeadband(ZRotation, .02) XSpeed = math.copysign(XSpeed * XSpeed, XSpeed) ZRotation = math.copysign(ZRotation * ZRotation, ZRotation) maxInput = math.copysign(max(abs(XSpeed), abs(ZRotation)), XSpeed) if XSpeed >= 0.0: if ZRotation >= 0.0: leftMotorSpeed = maxInput rightMotorSpeed = XSpeed - ZRotation else: leftMotorSpeed = XSpeed + ZRotation rightMotorSpeed = maxInput else: if ZRotation >= 0.0: leftMotorSpeed = XSpeed + ZRotation rightMotorSpeed = maxInput else: leftMotorSpeed = maxInput rightMotorSpeed = XSpeed - ZRotation leftMotorSpeed = wpilib.RobotDrive.limit(leftMotorSpeed) rightMotorSpeed = wpilib.RobotDrive.limit(rightMotorSpeed) leftMotorRPM = leftMotorSpeed * maxRPM rightMotorRPM = rightMotorSpeed * maxRPM self.driveLeftMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Velocity, leftMotorRPM) self.driveRightMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Velocity, rightMotorRPM) def updateSD(self): leftVel = self.driveLeftMaster.getSelectedSensorVelocity(0) leftPos = self.driveLeftMaster.getSelectedSensorPosition(0) rightVel = self.driveRightMaster.getSelectedSensorVelocity(0) rightPos = self.driveRightMaster.getSelectedSensorPosition(0) # calculate side deltas if self.leftVel: leftVelDelta = leftVel - self.leftVel else: leftVelDelta = 0 if self.leftPos: leftPosDelta = leftPos - self.leftPos else: leftPosDelta = 0 if self.rightVel: rightVelDelta = rightVel - self.rightVel else: rightVelDelta = 0 if self.rightPos: rightPosDelta = rightPos - self.rightPos else: rightPosDelta = 0 # calculate delta of delta differenceVel = leftVelDelta - rightVelDelta differencePos = leftPosDelta - rightPosDelta SD.putNumber("LeftSensorVel", leftVel) SD.putNumber("LeftSensorPos", leftPos) SD.putNumber("RightSensorVel", rightVel) SD.putNumber("RightSensorPos", rightPos) SD.putNumber('LeftVelDelta', leftVelDelta) SD.putNumber('LeftPosDelta', leftPosDelta) SD.putNumber('RightVelDelta', rightVelDelta) SD.putNumber('RightPosDelta', rightPosDelta) SD.putNumber('DifferenceVel', differenceVel) SD.putNumber('DifferencePos', differencePos) SD.putNumber('Angle', self.ahrs.getAngle()) SD.putNumber('Angle Adjustment', self.ahrs.getAngleAdjustment()) self.leftVel = leftVel self.leftPos = leftPos self.rightVel = rightVel self.rightPos = rightPos # kP = self.driveLeftMaster.configGetParameter( # self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10) # SmartDashboard.putNumber('Left Proportional', kP) # 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)) def applyDeadband(self, value, deadband): """Returns 0.0 if the given value is within the specified range around zero. The remaining range between the deadband and 1.0 is scaled from 0.0 to 1.0. :param value: value to clip :param deadband: range around zero """ if abs(value) > deadband: if value < 0.0: return (value - deadband) / (1.0 - deadband) else: return (value + deadband) / (1.0 - deadband) return 0.0 def setAngle(self, angle, tolerance): #self.tolerance = tolerance #self.calculateAdjustedSetpoint(angle) self.turnController.setSetpoint(angle) if ((self.ahrs.getYaw() <= (angle + tolerance)) and (self.ahrs.getYaw() >= (angle - tolerance))): self.turnController.disable() self.driveLeftMaster.set(0) self.driveRightMaster.set(0) else: self.turnController.enable() self.drive.arcadeDrive(0, self.output) #self.leftTurnController.setSetpoint(angle) def isInGyroPosition(self): SD.putNumber('Is in gyro position', ((self.ahrs.getYaw() <= (self.turnController.getSetpoint() + self.robot.autonomous.ANGLE_TOLERANCE)) and (self.ahrs.getYaw() >= (self.turnController.getSetpoint() - self.robot.autonomous.ANGLE_TOLERANCE)))) return ((self.ahrs.getYaw() <= (self.turnController.getSetpoint() + self.robot.autonomous.ANGLE_TOLERANCE)) and (self.ahrs.getYaw() >= (self.turnController.getSetpoint() - self.robot.autonomous.ANGLE_TOLERANCE))) def calculateAdjustedSetpoint(self, angle): self.startingYaw = self.robot.autonomous.startingYaw adjustedAngle = angle + self.startingYaw if adjustedAngle < -180: undershot = adjustedAngle + 180 adjustedAngle = 180 + undershot elif adjustedAngle > 180: overshot = adjustedAngle - 180 adjustedAngle = -180 + overshot self.adjustedSetpoint = adjustedAngle def PID(self): self.kP = .045 self.kI = 0.00 self.kD = 0.00 self.kF = 0.00 self.turnController = wpilib.PIDController(self.kP, self.kI, self.kD, self.kF, self.ahrs, output=self) self.turnController.setInputRange(-180, 180) self.turnController.setOutputRange(-0.55, 0.55) self.turnController.disable() def pidWrite(self, output): self.output = output
class cubeGrabber(Subsystem): def __init__(self, robot): self.robot = robot self.armMotor = Talon(self.robot.kCubeGrabber['right_arm']) self.armMotor.configPeakOutputForward(1, 0) self.armMotor.configPeakOutputReverse(-1, 0) 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 super().__init__() def grabberFunction(self): """ Updates the arm ultra sonic sensor divided by a predetermined ratio to get the output to inches. """ self.cubeDistanceIn = self.armUS.getValue() / 8.417 """ If statement adding to the toggle count as the driver changes from automatic grabbing to manual grabbing. """ if self.robot.cStick.getRawButtonReleased(3): self.armModeToggleCount = self.armModeToggleCount + 1 #self.armGrabReset() """ If statement controlling whether the arm functionality is automatic or manual, determined by the Arm Mode Toggle remainder when divided by 2. """ if self.armModeToggleCount % 2 == 0: self.armGrabAuto() """Does not rumble the controller when in Automatic Grabbing""" self.robot.cStick.setRumble(0, 0) else: self.armGrabManual() """Does rumble the controller when in Manual Grabbing""" self.robot.cStick.setRumble(0, 0.5) """ These send information over to the SmartDashboard """ wpilib.SmartDashboard.putNumber("cubeGrabber Ultra Sonic", self.cubeDistanceIn) wpilib.SmartDashboard.putNumber("cubeGrabber Limit Switch", self.armSwitch.get()) """ Code for resetting the Cube cubeGrabber """ def armGrabReset(self): """ Stops the motors and returns arms to default open position. """ self.armMotor.set(0) self.armSolenoid.set(self.armOpenPosition) """ Code for automatic cube grabbing """ def armGrabAuto(self): """ 1) Checks if the left trigger on Driver Two's controller is pressed in more than a quarter of the way down, then closes the Arms and spins the motors outwards. This spits out the cube and has first priority. 2) Checks if the limit switch on the arm is pressed. Will stop the motors and keep the arms closed and has second priority. 3) Checks if the cube is close enough to start spinning the intake wheels inward and keeps the arms open. 4) Checks if the cube is in range to close the arms. Spins motors inward and keeps arms closed. 5)If nothing is sensed the arms will be in a default position with the motors stopped and the arms open. """ if self.driverTwo.getRawAxis(2) > .25: self.armMotor.set(1) self.armSolenoid.set(self.armClosePosition) """This will prevent the arms from changing position when changing to Manual Mode.""" self.openToggleCount = 2 elif self.armSwitch.get() == 1: self.armMotor.set(0) self.armSolenoid.set(self.armClosePosition) """This will prevent the arms from changing position when changing to Manual Mode.""" self.openToggleCount = 2 elif (self.cubeDistanceIn <= self.spinDistance) and ( self.cubeDistanceIn > self.closeDistance): self.armMotor.set(-1) self.armSolenoid.set(self.armOpenPosition) """This will prevent the arms from changing position when changing to Manual Mode.""" self.openToggleCount = 3 elif (self.cubeDistanceIn <= self.closeDistance): self.armMotor.set(-1) self.armSolenoid.set(self.armClosePosition) """This will prevent the arms from changing position when changing to Manual Mode.""" self.openToggleCount = 2 else: self.armMotor.set(0) self.armSolenoid.set(self.armOpenPosition) """This will prevent the arms from changing position when changing to Manual Mode.""" self.openToggleCount = 3 """ Code for manual cube grabbing """ def armGrabManual(self): """ Combines the trigger axes on Driver Two's controller to create one "axis". """ self.manualArmSpeed = -self.driverTwo.getRawAxis( 2) + self.driverTwo.getRawAxis(3) """ If statement adding to the toggle count that will determine whether the arms are closed or open. """ if self.driverTwo.getRawButtonReleased(1): self.openToggleCount = self.openToggleCount + 1 """ If statement determining whether the arms are closed or opened determined by the open toggle count remainder when divided by two. """ if self.openToggleCount % 2 == 0: self.armSolenoid.set(self.armClosePosition) else: self.armSolenoid.set(self.armOpenPosition) """ Sets the speed of the intake wheels, using the makeshift axis made above as diret input. """ self.armMotor.set(-self.manualArmSpeed) def getSwitch(self): return self.armSwitch.get() == 1
class Elevator(Subsystem): kSwitch = -20000 kScale = -50000 kStart = -5000 kBottom = 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 elevatorFunction(self): # if self.elevator.isRevLimitSwitchClosed(): # self.elevator.setSelectedSensorPosition(0, 0, 0) #self.elevator.set(self.driverTwo.getRawAxis(1)) self.smartPositioning() #wpilib.SmartDashboard.putNumber('elevator amperage', self.elevator.getOutputCurrent()) # def updateSD(self): def setElevatorPosition(self, position): # if self.elevator.isRevLimitSwitchClosed(): # self.elevator.setSelectedSensorPosition(0, 0, 0) # self.elevator.set(ctre.talonsrx.TalonSRX.ControlMode.Position, position) def smartPositioning(self): if self.driverTwo.getRawButtonReleased(5): self.smartPosition -= 1 self.smartToggle = True if self.smartPosition < 0: self.smartPosition = 0 elif self.driverTwo.getRawButtonReleased(6): self.smartPosition += 1 self.smartToggle = True if self.smartPosition > 2: self.smartPosition = 2 elif (self.driverTwo.getRawAxis(1) > 0.2) or (self.driverTwo.getRawAxis(1) < -0.2): self.smartToggle = False if self.smartToggle: if self.smartPosition == 2: self.setElevatorPosition(self.kScale) elif self.smartPosition == 1: self.setElevatorPosition(self.kSwitch) elif self.smartPosition == 0: self.elevator.set(0) else: self.elevator.set(self.driverTwo.getRawAxis(1)) if self.elevator.getSelectedSensorPosition(0) < (self.kSwitch + self.kScale)/2: self.smartPosition = 2 elif self.elevator.getSelectedSensorPosition(0) < self.kSwitch/2: self.smartPosition = 1 else: self.smartPosition = 0 def calibrateBottomAutonomous(self): if self.elevator.isRevLimitSwitchClosed(): self.elevator.setSelectedSensorPosition(0, 0, 0) self.elevator.set(0) # self.calibrateStep = 1 else: self.elevator.set(-.5) def getBottomLimit(self): return self.elevator.isFwdLimitSwitchClosed() def telemetry(self): wpilib.SmartDashboard.putNumber('Lift Position', self.elevator.getSelectedSensorPosition(0)) wpilib.SmartDashboard.putBoolean('Forward Limit', self.elevator.isFwdLimitSwitchClosed()) wpilib.SmartDashboard.putBoolean('Reverse Limit', self.elevator.isRevLimitSwitchClosed())