class AutoSetLiftPosition(Command): def __init__(self, robot, d): super().__init__() self.robot = robot self.requires(robot.lift) self.m_setpoint = d #inches self.total_timer = Timer() # Called just before this Command runs the first time def initialize(self): self.total_timer.reset() self.total_timer.start() self.robot.lift.setMotionMagicSetpoint(self.m_setpoint) # Make this return true when this Command no longer needs to run execute() def isFinished(self): returnval = False if self.total_timer.get() > 1.0: if self.robot.lift.isMotionMagicNearTarget(): returnval = True elif self.total_timer > 9.0: returnval = True return returnval # Called when another command which requires one or more of the same # subsystems is scheduled to run def interrupted(self): self.robot.lift.hold() self.robot.debug.Stop()
class AcquireCube(Command): def __init__(self, drive_speed, timeout, drive: Drivetrain, intake: Intake): super().__init__("AcquireCube", timeout) self.drive_speed = drive_speed self.drive = drive self.intake = intake self.requires(intake) self.requires(drive) self.state = 0 self.timer = Timer() def initialize(self): self.state = 0 self.intake.set_arm_state(ArmState.DOWN) self.intake.set_grab_state(GrabState.OUT) self.timer.start() def execute(self): if self.state == 0: self.drive.arcade_drive(0, 0) if self.timer.get() > 0.2: self.state = 1 elif self.state == 1: self.intake.intake() self.drive.arcade_drive(self.drive_speed, 0) def isFinished(self): return self.intake.has_acquired_cube() or self.isTimedOut() def end(self): self.intake.set_grab_state(GrabState.IN) self.intake.set_arm_state(ArmState.UP) self.intake.run_intake(0) self.drive.arcade_drive(0, 0)
class MoveElevatorToInitialPosition(Command): def __init__(self): super().__init__('MoveElevatorToInitialPosition') self._elevator = self.getRobot().elevator self.requires(self._elevator) self._logger = self.getRobot().logger self._speed = -robotmap.ElevatorSpeed #Negated self._smartDashboard = NetworkTables.getTable('SmartDashboard') self._timer = Timer() def initialize(self): self._elevator.stop() self._smartDashboard.putString("ElevatorPosition", str(self._elevator.currentPosition())) self._timer.reset() self._timer.start() def execute(self): self._elevator.move(self._speed) self._smartDashboard.putString("ElevatorPosition", str(self._elevator.currentPosition())) def isFinished(self): if self._timer.get() > 1.0: return True else: return False def interrupted(self): self.end() def end(self): self._elevator.stop()
class TrajectoryTracker: chassis: chassis.Chassis BETA = 2.0 ZETA = 0.7 def __init__(self): self.trajectories: trajectories.Trajectories = trajectories.Trajectories( ) self.ramsete = RamseteController(self.BETA, self.ZETA) self.desired_trajectory: Trajectory = None self.timer = Timer() self.is_tracking = False def on_disable(self): self.is_tracking = False def setTrajectory(self, trajectory): self.timer.start() self.timer.reset() self.desired_trajectory = trajectory def track(self): self.is_tracking = True def stop(self): self.is_tracking = False def isFinished(self): if self.desired_trajectory == None: return True return self.timer.get() >= self.desired_trajectory.totalTime() def execute(self): if self.isFinished(): self.is_tracking = False if self.is_tracking: cur_state = self.chassis.getPose() desired_state = self.desired_trajectory.sample(self.timer.get()) desired_chassis = self.ramsete.calculate(cur_state, desired_state) self.chassis.setChassisVelocity(desired_chassis.vx, desired_chassis.vy, desired_chassis.omega)
class MoveIntakeCommand(Command): def __init__(self, intake: Intake, new_state: ArmState): super().__init__("MoveIntakeCommand") self.intake = intake self.new_state = new_state self.old_state = None self.timer = Timer() self.requires(intake) def initialize(self): self.old_state = self.intake.get_arm_state() self.intake.set_arm_state(self.new_state) self.timer.reset() self.timer.start() def isFinished(self): return self.new_state == self.old_state or \ (self.new_state == ArmState.DOWN and self.timer.get() > 0.2) or \ (self.new_state == ArmState.UP and self.timer.get() > 0.5) def end(self): self.timer.stop()
class MoveLiftMinHeight(Command): def __init__(self, robot): super().__init__() self.robot = robot self.requires(robot.lift) self.total_timer = Timer() # Called just before this Command runs the first time def initialize(self): self.robot.lift.setMotionMagicSetpoint(0) self.total_timer.reset() self.total_timer.start() # Make this return true when this Command no longer needs to run execute() def isFinished(self): return self.robot.lift.isMotionMagicNearTarget( ) or self.total_timer.get() > 5.0
class ToggleIntakeCommand(Command): def __init__(self, intake: Intake): super().__init__() self.last_arm_state = intake.get_arm_state() self.new_arm_state = ArmState.DOWN self.requires(intake) self.intake = intake self.arm_timer = Timer() self.arm_timer_latch = False def initialize(self): self.last_arm_state = self.intake.get_arm_state() self.new_arm_state = ArmState.DOWN if self.last_arm_state == ArmState.UP else ArmState.UP if self.last_arm_state != ArmState.UP: self.arm_timer.start() self.arm_timer.reset() self.arm_timer_latch = True def execute(self): intake = self.intake intake.set_arm_state(self.new_arm_state) self.last_arm_state = self.intake.get_arm_state() if self.arm_timer.get() > 0.5: self.arm_timer_latch = False self.arm_timer.stop() if self.arm_timer_latch: intake.run_intake(0.1) else: intake.run_intake(0) def isFinished(self): return not self.arm_timer_latch def end(self): self.arm_timer.reset() self.arm_timer.stop() self.arm_timer_latch = False self.intake.run_intake(0)
class AutoShootBox(Command): def __init__(self, robot): super().__init__() self.robot = robot self.requires(robot.claw) self.total_timer = Timer() # Called just before this Command runs the first time def initialize(self): self.total_timer.reset() self.total_timer.start() # Called repeatedly when this Command is scheduled to run def execute(self): self.robot.claw.setSpeed(robotmap.BOX_OUT_SPEED) self.robot.claw.close() # Make this return true when this Command no longer needs to run execute() def isFinished(self): return self.total_timer.get() > 0.25
class Expel(Command): def __init__(self): super().__init__('Expel') self._scooper = self.getRobot().scooper self.requires(self._scooper) self._logger = self.getRobot().logger self._timer = Timer() def initialize(self): self._scooper.stop() self._timer.reset() self._timer.start() def execute(self): self._scooper.expel() def isFinished(self): return True if self._timer.get() > 0.5 else False def end(self): self._scooper.stop()
class TurnByGyro(Command): def __init__(self, robot, angle): super().__init__() self.requires(robot.drivetrain) self.robot = robot self.angle = angle self.timer = Timer() def initialize(self): print("[TurnByGyro] initializing") self.robot.drivetrain.resetGyro() self.robot.drivetrain.turnController.setSetpoint(self.angle) self.timer.reset() self.timer.start() def execute(self): self.robot.drivetrain.turn() def isFinished(self): if self.timer.get() > 5.0: print("[TurnByGyro] timed out") return True # if self.robot.drivetrain.reachedAngle(self.angle): # print("[TurnByGyro] reached target") # return True return False def end(self): print("[TurnByGyro] ending") self.robot.drivetrain.resetDrive() def interrupted(self): print("[ClimbByGyro] interrupted") self.end()
class MyRobot(IterativeRobot): def robotInit(self): print('Starting gyro init...') self.myGyro = ADIS16448_IMU(ADIS16448_IMU.IMUAxis.kZ, SPI.Port.kMXP, 4) print('Gyro init completed.') self.statusTimer = Timer() self.statusTimer.start() def robotPeriodic(self): if (self.statusTimer.get() >= 0.25): print('X:', str(round(self.myGyro.getGyroAngleX(), 5)).ljust(25), \ 'Y:', str(round(self.myGyro.getGyroAngleY(), 5)).ljust(25), \ 'Z:', str(round(self.myGyro.getGyroAngleZ(), 5)).ljust(25)) self.statusTimer.reset() def teleopInit(self): """Executed at the start of teleop mode""" pass def teleopPeriodic(self): """Runs the motors with tank steering""" pass
class ADXRS453Z(object): def __init__(self, port): spi = SPI(port) spi.setClockRate(4000000) # 4 MHz (rRIO max, gyro can go high) spi.setClockActiveHigh() spi.setChipSelectActiveLow() spi.setMSBFirst() self._spi = spi self._command = [0x00] * DATA_SIZE self._data = [0x00] * DATA_SIZE self._command[0] = READ_COMMAND self._accumulated_angle = 0.0 self._current_rate = 0.0 self._accumulated_offset = 0.0 self._rate_offset = 0.0 self._last_time = 0 self._current_time = 0 self._calibration_timer = Timer() self._calibration_timer.start() self._update_timer = Timer() self._update_timer.start() # # self._update_thread = Thread(self.update, daemon=True) # self._update_thread.start() self.update() def update(self): # Check parity num_bits = sum([bits(c) for c in self._command]) if num_bits % 2 == 0: self._command[3] |= PARITY_BIT self._data = self._spi.transaction(self._command) if self._calibration_timer.get() < WARM_UP_PERIOD: self._last_time = self._current_time = self._update_timer.get() return elif self._calibration_timer.get() < CALIBRATE_PERIOD: self.calibrate() else: self.update_data() def update_data(self): sensor_data = self._assemble_sensor_data(self._data) rate = sensor_data / 80.0 self._current_rate = rate self._current_rate -= self._rate_offset self._current_time = self._update_timer.get() dt = self._current_time - self._last_time self._accumulated_offset += rate * dt self._accumulated_angle += self._current_rate * dt self._last_time = self._current_time def calibrate(self): sensor_data = self._assemble_sensor_data(self._data) rate = sensor_data / 80.0 self._current_rate = rate self._current_rate -= self._rate_offset self._current_time = self._update_timer.get() dt = self._current_time - self._last_time self._accumulated_offset += rate * dt self._rate_offset = self._accumulated_offset / ( self._calibration_timer.get() - WARM_UP_PERIOD) self._last_time = self._current_time @staticmethod def _assemble_sensor_data(data): #cast to short to make space for shifts #the 16 bits from the gyro are a 2's complement short #so we just cast it too a C++ short #the data is split across the output like this (MSB first): (D = data bit, X = not data) # X X X X X X D D | D D D D D D D D | D D D D D D X X | X X X X X X X X X return (data[0] & FIRST_BYTE_DATA) << 14 | (data[1]) << 6 | ( data[2] & THIRD_BYTE_DATA) >> 2 def getRate(self): return self._current_rate def getAngle(self): return self._accumulated_angle def getOffset(self): return self._accumulated_offset def reset(self): self._data = [0x00] * 4 self._current_rate = 0 self._accumulated_angle = 0 self._rate_offset = 0 self._accumulated_offset = 0 self._calibration_timer.reset() self._update_timer.reset()
class Auto1(): def __init__(self, robot, logger): self.logger = logger self.robot = robot self.timer = Timer() if not self.robot.isSimulation(): with open("/home/lvuser/traj", "rb") as fp: self.trajectory = pickle.load(fp) else: with open("/home/ubuntu/traj", "rb") as fp: self.trajectory = pickle.load(fp) self.target_chooser = sendablechooser.SendableChooser() self.target_chooser.setDefaultOption(TargetHeight.LOW.name, TargetHeight.LOW) self.target_chooser.addOption(TargetHeight.MIDDLE.name, TargetHeight.MIDDLE) self.target_chooser.addOption(TargetHeight.HIGH.name, TargetHeight.HIGH) self.left_right_chooser = sendablechooser.SendableChooser() self.left_right_chooser.setDefaultOption(RightLeft.RIGHT.name, RightLeft.RIGHT) self.left_right_chooser.addOption(RightLeft.LEFT.name, RightLeft.LEFT) self.hab_level_chooser = sendablechooser.SendableChooser() self.hab_level_chooser.setDefaultOption(HabLevel.LEVEL1.name, HabLevel.LEVEL1) self.hab_level_chooser.addOption(HabLevel.LEVEL2.name, HabLevel.LEVEL2) SmartDashboard.putData("TargetChooser", self.target_chooser) SmartDashboard.putData("LeftRightChooser", self.left_right_chooser) SmartDashboard.putData("HabLevelChooser", self.hab_level_chooser) self.chosen_target = TargetHeight.LOW self.left_right = RightLeft.RIGHT self.hab_level = HabLevel.LEVEL1 def init(self): self.logger.info("Initializing auto 1") self.chosen_target = self.target_chooser.getSelected() self.left_right = self.left_right_chooser.getSelected() self.hab_level = self.hab_level_chooser.getSelected() self.logger.info("<Auto1> " + self.hab_level.name + ", " + self.left_right.name + ", " + self.chosen_target.name) self.state = 0 self.timer.start() def iterate(self): SmartDashboard.putNumber("Auto1 State", self.state) if self.state == 0: self.robot.harpoon.deploy_harpoon() self.timer.reset() if self.hab_level == HabLevel.LEVEL2: self.state = 1 else: self.state = 2 elif self.state == 1: self.robot.drive.drive_straight(0.4) if self.timer.get() > 1.0: self.state = 2 elif self.state == 2: self.robot.drive.drive_straight(0.0) self.robot.lift.deploy_lift() self.state = 3 elif self.state == 3: if self.robot.lift.current_state == LiftState.LIFT_DEPLOYED: self.state = 4 elif self.state == 4: self.robot.drive.follow_a_path(self.trajectory) self.state = 5 elif self.state == 5: if self.robot.drive.leftDone and self.robot.drive.rightDone: self.state = 6 elif self.state == 6: if self.chosen_target == TargetHeight.LOW: self.robot.lift.go_to_preset(LiftPreset.LIFT_PRESET_HATCH_LOW) elif self.chosen_target == TargetHeight.MIDDLE: self.robot.lift.go_to_preset( LiftPreset.LIFT_PRESET_HATCH_MIDDLE) if self.chosen_target == TargetHeight.HIGH: self.robot.lift.go_to_preset(LiftPreset.LIFT_PRESET_HATCH_HIGH) self.state = 7 elif self.state == 7: if self.robot.lift.on_target: self.state = 8 elif self.state == 8: self.robot.harpoon.stow_harpoon() self.state = 9 elif self.state == 9: if self.robot.harpoon.current_state == HarpoonState.HARPOON_STOWED: self.timer.reset() self.state = 10 elif self.state == 10: self.robot.drive.drive_straight(-0.4) if self.timer.get() > 1.0: self.state = 11 elif self.state == 11: self.robot.drive.drive_straight(0.0) self.state = 12 elif self.state == 12: self.robot.lift.go_to_preset(LiftPreset.LIFT_PRESET_STOW) self.state = 13 elif self.state == 13: if self.robot.lift.on_target: self.state = 14 elif self.state == 14: pass
class Intake(Component): def __init__(self): super().__init__() self._l_motor = Talon(hardware.intake_left) self._r_motor = Talon(hardware.intake_right) self._intake_piston = Solenoid(hardware.intake_solenoid) self._left_pwm = 0 self._right_pwm = 0 self._open = False self._left_intake_amp = CircularBuffer(25) self._right_intake_amp = CircularBuffer(25) self._pulse_timer = Timer() def update(self): self._l_motor.set(self._left_pwm) self._r_motor.set(-self._right_pwm) self._intake_piston.set(not self._open) self._left_intake_amp.append(hardware.left_intake_current()) self._right_intake_amp.append(hardware.right_intake_current()) def intake_tote(self): if hardware.game_piece_in_intake(): # anti-bounce & slowdown power = .8 else: power = .3 if not hardware.back_photosensor.get() else 1 self.set(power) if self.intake_jammed() and not self._pulse_timer.running: self._pulse_timer.start() if self._pulse_timer.running: self.set(-self._pulse_timer.get() / 2) if self._pulse_timer.get() > .05: self._pulse_timer.stop() self._pulse_timer.reset() def intake_bin(self): power = .3 if not hardware.back_photosensor.get() else .65 self.set(power, power) def pause(self): self.set(0) def open(self): self._open = True def close(self): self._open = False def stop(self): self._l_motor.set(0) self._r_motor.set(0) def set(self, l, r=None): """ Spins the intakes at a given power. Pass either a power or left and right power. """ self._left_pwm = l if r is None: self._right_pwm = l else: self._right_pwm = r def intake_jammed(self): if not hardware.back_photosensor.get(): return False return self._left_intake_amp.average > AMP_THRESHOLD or self._right_intake_amp.average > AMP_THRESHOLD def update_nt(self): log.info("left current: %s" % self._left_intake_amp.average) log.info("right current: %s" % self._right_intake_amp.average)
class Auto2(): def __init__(self, robot, logger): self.logger = logger self.robot = robot self.timer = Timer() self.target_chooser = sendablechooser.SendableChooser() self.target_chooser.setDefaultOption(TargetHeight.LOW.name, TargetHeight.LOW) self.target_chooser.addOption(TargetHeight.MIDDLE.name, TargetHeight.MIDDLE) self.target_chooser.addOption(TargetHeight.HIGH.name, TargetHeight.HIGH) self.left_right_chooser = sendablechooser.SendableChooser() self.left_right_chooser.setDefaultOption(RightLeft.RIGHT.name, RightLeft.RIGHT) self.left_right_chooser.addOption(RightLeft.LEFT.name, RightLeft.LEFT) self.hab_level_chooser = sendablechooser.SendableChooser() self.hab_level_chooser.setDefaultOption(HabLevel.LEVEL1.name, HabLevel.LEVEL1) self.hab_level_chooser.addOption(HabLevel.LEVEL2.name, HabLevel.LEVEL2) SmartDashboard.putData("TargetChooser", self.target_chooser) SmartDashboard.putData("LeftRightChooser", self.left_right_chooser) SmartDashboard.putData("HabLevelChooser", self.hab_level_chooser) self.chosen_target = TargetHeight.LOW self.left_right = RightLeft.RIGHT self.hab_level = HabLevel.LEVEL1 def init(self): self.logger.info("Initializing auto 2") self.chosen_target = self.target_chooser.getSelected() self.left_right = self.left_right_chooser.getSelected() self.hab_level = self.hab_level_chooser.getSelected() self.logger.info("<Auto2> " + self.hab_level.name + ", " + self.left_right.name + ", " + self.chosen_target.name) self.state = 0 self.timer.start() def iterate(self): SmartDashboard.putNumber("Auto2 State", self.state) if self.state == 0: self.robot.harpoon.deploy_harpoon() self.timer.reset() if self.hab_level == HabLevel.LEVEL2: self.state = 1 else: self.state = 2 elif self.state == 1: self.robot.drive.drive_straight(0.4) if self.timer.get() > 1.0: self.state = 2 elif self.state == 2: self.robot.drive.drive_straight(0.0) self.robot.lift.deploy_lift() self.state = 3 elif self.state == 3: if self.robot.lift.current_state == LiftState.LIFT_DEPLOYED: self.state = 4 elif self.state == 4: pass
class Robot(TimedRobot): def robotInit(self): self.driver = DriverControl(0) self.operator = OperatorControl(1) self.dashboard = NetworkTables.getTable("SmartDashboard") self.right1 = DriveMotor(3, True) self.right2 = DriveMotor(4, True) self.right = DriveMotorGroup([self.right1, self.right2]) self.left1 = DriveMotor(1, False) self.left2 = DriveMotor(2, False) self.left = DriveMotorGroup([self.left1, self.left2]) self.drive = DriveBase(self.right, self.left, self.driver) self.driveMotors = [self.right1, self.right2, self.left1, self.left2] self.shifter = Shifter(0, 1, self.driver) self.indexer = Indexer(self.operator) self.arm = Arm(self.operator) self.lifter = Lifter(self.operator) self.intake = Intake(self.operator) def disabledInit(self): pass def autonomousInit(self): self.autonomousTimer = Timer() self.autonomousTimer.start() def teleopInit(self): pass def testInit(self): pass def robotPeriodic(self): pass def disabledPeriodic(self): pass def autonomousPeriodic(self): if (self.autonomousTimer.get() < 2): self.drive.setRaw(.5, .5) else: self.drive.setRaw(0, 0) def teleopPeriodic(self): self.indexer.update() self.shifter.update() self.drive.update() self.arm.update() self.lifter.update() self.intake.update() def testPeriodic(self): pass
class PIDController(SendableBase): """Can be used to control devices via a PID Control Loop. Creates a separate thread which reads the given :class:`.PIDSource` and takes care of the integral calculations, as well as writing the given :class:`.PIDOutput`. This feedback controller runs in discrete time, so time deltas are not used in the integral and derivative calculations. Therefore, the sample rate affects the controller's behavior for a given set of PID constants. """ kDefaultPeriod = .05 instances = 0 PIDSourceType = PIDSource.PIDSourceType # Tolerance is the type of tolerance used to specify if the PID controller # is on target. The various implementations of this such as # PercentageTolerance and AbsoluteTolerance specify types of tolerance # specifications to use. def PercentageTolerance_onTarget(self, percentage): return abs(self.getError()) < percentage / 100.0 * self.inputRange def AbsoluteTolerance_onTarget(self, value): return abs(self.getError()) < value def __init__(self, Kp, Ki, Kd, *args, **kwargs): """Allocate a PID object with the given constants for P, I, D, and F Arguments can be structured as follows: - Kp, Ki, Kd, Kf, source, output, period - Kp, Ki, Kd, source, output, period - Kp, Ki, Kd, source, output - Kp, Ki, Kd, Kf, source, output :param Kp: the proportional coefficient :type Kp: float or int :param Ki: the integral coefficient :type Ki: float or int :param Kd: the derivative coefficient :type Kd: float or int :param Kf: the feed forward term :type Kf: float or int :param source: Called to get values :type source: A function, or an object that implements :class:`.PIDSource` :param output: Receives the output percentage :type output: A function, or an object that implements :class:`.PIDOutput` :param period: the loop time for doing calculations. This particularly effects calculations of the integral and differential terms. The default is 50ms. :type period: float or int """ super().__init__(False) f_arg = ("Kf", [float, int]) source_arg = ("source", [HasAttribute("pidGet"), HasAttribute("__call__")]) output_arg = ("output", [HasAttribute("pidWrite"), HasAttribute("__call__")]) period_arg = ("period", [float, int]) templates = [[f_arg, source_arg, output_arg, period_arg], [source_arg, output_arg, period_arg], [source_arg, output_arg], [f_arg, source_arg, output_arg]] _, results = match_arglist('PIDController.__init__', args, kwargs, templates) self.P = Kp # factor for "proportional" control self.I = Ki # factor for "integral" control self.D = Kd # factor for "derivative" control self.F = results.pop("Kf", 0.0) # factor for feedforward term self.pidOutput = results.pop("output") self.origSource = results.pop("source") self.period = results.pop("period", self.kDefaultPeriod) self.filter = LinearDigitalFilter.movingAverage(self.origSource, 1) self.pidInput = self.filter self.pidInput = PIDSource.from_obj_or_callable(self.pidInput) if hasattr(self.pidOutput, 'pidWrite'): self.pidOutput = self.pidOutput.pidWrite self.maximumOutput = 1.0 # |maximum output| self.minimumOutput = -1.0 # |minimum output| self.maximumInput = 0.0 # maximum input - limit setpoint to this self.minimumInput = 0.0 # minimum input - limit setpoint to this self.inputRange = 0.0 # input range - difference between maximum and minimum self.continuous = False # do the endpoints wrap around? eg. Absolute encoder self.enabled = False # is the pid controller enabled self.prevError = 0.0 # the prior error (used to compute velocity) self.totalError = 0.0 #the sum of the errors for use in the integral calc self.setpoint = 0.0 self.prevSetpoint = 0.0 self.error = 0.0 self.result = 0.0 self.mutex = threading.RLock() self.pidWriteMutex = threading.RLock() self.pid_task = Notifier(self._calculate) self.pid_task.startPeriodic(self.period) self.setpointTimer = Timer() self.setpointTimer.start() # Need this to free on unit test wpilib reset Resource._add_global_resource(self) PIDController.instances += 1 hal.report(hal.UsageReporting.kResourceType_PIDController, PIDController.instances) self.setName("PIDController", PIDController.instances) def free(self): """Free the PID object""" super().free() # TODO: is this useful in Python? Should make TableListener weakref. self.pid_task.stop() with self.mutex: self.pidInput = None self.pidOutput = None def _calculate(self): """Read the input, calculate the output accordingly, and write to the output. This should only be called by the PIDTask and is created during initialization.""" if self.origSource is None or self.pidOutput is None: return with self.mutex: enabled = self.enabled # take snapshot of these values... if enabled: feedForward = self.calculateFeedForward() with self.mutex: input = self.pidInput.pidGet() pidSourceType = self.pidInput.getPIDSourceType() P = self.P I = self.I D = self.D minimumOutput = self.minimumOutput maximumOutput = self.maximumOutput prevError = self.prevError error = self.getContinuousError(self.setpoint - input) totalError = self.totalError # start if pidSourceType == self.PIDSourceType.kRate: if P != 0: totalError = self.clamp(totalError + error, minimumOutput / P, maximumOutput / P) result = P * totalError + D * error + feedForward else: if I != 0: totalError = self.clamp(totalError + error, minimumOutput / I, maximumOutput / I) result = P * error + I * totalError + D * (error - prevError) + \ feedForward result = self.clamp(result, minimumOutput, maximumOutput) with self.pidWriteMutex: with self.mutex: enabled = self.enabled if enabled: self.pidOutput(result) with self.mutex: self.prevError = error self.error = error self.totalError = totalError self.result = result def calculateFeedForward(self): """Calculate the feed forward term Both of the provided feed forward calculations are velocity feed forwards. If a different feed forward calculation is desired, the user can override this function and provide his or her own. This function does no synchronization because the PIDController class only calls it in synchronized code, so be careful if calling it oneself. If a velocity PID controller is being used, the F term should be set to 1 over the maximum setpoint for the output. If a position PID controller is being used, the F term should be set to 1 over the maximum speed for the output measured in setpoint units per this controller's update period (see the default period in this class's constructor). """ if self.pidInput.getPIDSourceType() == self.PIDSourceType.kRate: return self.F * self.getSetpoint() else: temp = self.F * self.getDeltaSetpoint() self.prevSetpoint = self.setpoint self.setpointTimer.reset() return temp def setPID(self, p, i, d, f=0.0): """Set the PID Controller gain parameters. Set the proportional, integral, and differential coefficients. :param p: Proportional coefficient :param i: Integral coefficient :param d: Differential coefficient :param f: Feed forward coefficient (optional, default is 0.0) """ with self.mutex: self.P = p self.I = i self.D = d self.F = f def getP(self): """Get the Proportional coefficient. :returns: proportional coefficient """ with self.mutex: return self.P def getI(self): """Get the Integral coefficient :returns: integral coefficient """ with self.mutex: return self.I def getD(self): """Get the Differential coefficient. :returns: differential coefficient """ with self.mutex: return self.D def getF(self): """Get the Feed forward coefficient. :returns: feed forward coefficient """ with self.mutex: return self.F def get(self): """Return the current PID result. This is always centered on zero and constrained the the max and min outs. :returns: the latest calculated output """ with self.mutex: return self.result def setContinuous(self, continuous=True): """Set the PID controller to consider the input to be continuous. Rather then using the max and min input range as constraints, it considers them to be the same point and automatically calculates the shortest route to the setpoint. :param continuous: Set to True turns on continuous, False turns off continuous """ if continuous and self.inputRange <= 0: raise ValueError( "No input range set when calling setContinuous().") with self.mutex: self.continuous = continuous def setInputRange(self, minimumInput, maximumInput): """Sets the maximum and minimum values expected from the input. :param minimumInput: the minimum percentage expected from the input :param maximumInput: the maximum percentage expected from the output """ with self.mutex: if minimumInput > maximumInput: raise ValueError("Lower bound is greater than upper bound") self.minimumInput = minimumInput self.maximumInput = maximumInput self.inputRange = self.maximumInput - self.minimumInput self.setSetpoint(self.setpoint) def setOutputRange(self, minimumOutput, maximumOutput): """Sets the minimum and maximum values to write. :param minimumOutput: the minimum percentage to write to the output :param maximumOutput: the maximum percentage to write to the output """ with self.mutex: if minimumOutput > maximumOutput: raise ValueError("Lower bound is greater than upper bound") self.minimumOutput = minimumOutput self.maximumOutput = maximumOutput def setSetpoint(self, setpoint): """Set the setpoint for the PIDController. :param setpoint: the desired setpoint """ with self.mutex: if self.maximumInput > self.minimumInput: if setpoint > self.maximumInput: newsetpoint = self.maximumInput elif setpoint < self.minimumInput: newsetpoint = self.minimumInput else: newsetpoint = setpoint else: newsetpoint = setpoint self.setpoint = newsetpoint def getSetpoint(self): """Returns the current setpoint of the PIDController. :returns: the current setpoint """ with self.mutex: return self.setpoint def getDeltaSetpoint(self): """Returns the change in setpoint over time of the PIDController :returns: the change in setpoint over time """ with self.mutex: t = self.setpointTimer.get() # During testing/simulation it is possible to get a divide by zero because # the threads' calls aren't strictly aligned with the master clock. if t: return (self.setpoint - self.prevSetpoint) / t else: return 0.0 def getError(self): """Returns the current difference of the input from the setpoint. :return: the current error """ with self.mutex: return self.getContinuousError(self.getSetpoint() - self.pidInput.pidGet()) def getAvgError(self): """ Returns the current difference of the error over the past few iterations. You can specify the number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is used for the onTarget() function. .. deprecated :: 2018.0.0 Use getError(), which is now already filtered. :returns: the current average of the error """ warnings.warn("use getRate instead", DeprecationWarning) with self.mutex: return self.getError() def setPIDSourceType(self, pidSourceType): """Sets what type of input the PID controller will use :param pidSourceType: the type of input """ self.pidInput.setPIDSourceType(pidSourceType) def getPIDSourceType(self): """Returns the type of input the PID controller is using :returns: the PID controller input type """ return self.pidInput.getPIDSourceType() def setAbsoluteTolerance(self, absvalue): """Set the absolute error which is considered tolerable for use with :func:`onTarget`. :param absvalue: absolute error which is tolerable in the units of the input object """ with self.mutex: self.onTarget = lambda: \ self.AbsoluteTolerance_onTarget(absvalue) def setPercentTolerance(self, percentage): """Set the percentage error which is considered tolerable for use with :func:`onTarget`. (Input of 15.0 = 15 percent) :param percentage: percent error which is tolerable """ with self.mutex: self.onTarget = lambda: \ self.PercentageTolerance_onTarget(percentage) def setToleranceBuffer(self, bufLength): """Set the number of previous error samples to average for tolerancing. When determining whether a mechanism is on target, the user may want to use a rolling average of previous measurements instead of a precise position or velocity. This is useful for noisy sensors which return a few erroneous measurements when the mechanism is on target. However, the mechanism will not register as on target for at least the specified bufLength cycles. :param bufLength: Number of previous cycles to average. :type bufLength: int .. deprecated:: 2018.0.0 Use a LinearDigitalFilter as the input """ with self.mutex: self.filter = LinearDigitalFilter.movingAverage( self.origSource, bufLength) self.pidInput = self.filter def onTarget(self): """Return True if the error is within the percentage of the total input range, determined by setTolerance. This assumes that the maximum and minimum input were set using :func:`setInput`. :returns: True if the error is less than the tolerance """ with self.mutex: return self.tolerance.onTarget() def enable(self): """Begin running the PIDController.""" with self.mutex: self.enabled = True def disable(self): """Stop running the PIDController, this sets the output to zero before stopping.""" with self.pidWriteMutex: with self.mutex: self.enabled = False self.pidOutput(0) def isEnabled(self): """Return True if PIDController is enabled.""" with self.mutex: return self.enabled def reset(self): """Reset the previous error, the integral term, and disable the controller.""" with self.mutex: self.disable() self.prevError = 0 self.totalError = 0 self.result = 0 def setP(self, p): """ Set the Proportional coefficient of the PID controller gain. :param p: Proportional coefficient """ with self.mutex: self.P = p def setI(self, i): """ Set the Integral coefficient of the PID controller gain. :param i: Integral coefficient """ with self.mutex: self.I = i def setD(self, d): """ Set the Differential coefficient of the PID controller gain. :param d: differential coefficient """ with self.mutex: self.D = d def setF(self, f): """ Set the Feed forward coefficient of the PID controller gain. :param f: feed forward coefficient """ with self.mutex: self.F = f def setEnabled(self, enable): """Set the enabled state of the PIDController.""" if enable: self.enable() else: self.disable() def initSendable(self, builder): builder.setSmartDashboardType("PIDController") builder.setSafeState(self.reset) builder.addDoubleProperty("p", self.getP, self.setP) builder.addDoubleProperty("i", self.getI, self.setI) builder.addDoubleProperty("d", self.getD, self.setD) builder.addDoubleProperty("f", self.getF, self.setF) builder.addDoubleProperty("setpoint", self.getSetpoint, self.setSetpoint) builder.addBooleanProperty("enabled", self.isEnabled, self.setEnabled) def getContinuousError(self, error): """ Wraps error around for continuous inputs. The original error is returned if continuous mode is disabled. This is an unsynchronized function. :param error: The current error of the PID controller. :return: Error for continuous inputs. """ if self.continuous and self.inputRange > 0: error %= self.inputRange if abs(error) > self.inputRange / 2: if error > 0: return error - self.inputRange else: return error + self.inputRange return error def clamp(self, value, low, high): return max(low, min(value, high))
class OpIntakeCommand(Command): def __init__ (self, intake: Intake): super().__init__ () self.requires(intake) self.intake = intake self.last_arm_state = ArmState.UP self.arm_timer = Timer() self.arm_timer_latch = False def initialize(self): self.last_arm_state = self.intake.get_arm_state() self.arm_timer.reset() self.arm_timer.stop() self.arm_timer_latch = False def execute(self): oi = OI.get() intake = self.intake if oi.arm_is_down(): intake.set_arm_state(ArmState.DOWN) else: intake.set_arm_state(ArmState.UP) if self.last_arm_state != ArmState.UP: self.arm_timer.start() self.arm_timer.reset() self.arm_timer_latch = True self.last_arm_state = self.intake.get_arm_state() if self.arm_timer.get() > 0.5: self.arm_timer_latch = False self.arm_timer.stop() if self.arm_timer_latch: intake.run_intake(0.1) else: self.arm_timer.stop() if oi.intake_is_active(): # intake.set_grab_state(GrabState.OUT) pwr = oi.get_outtake_command() if abs(pwr) < 0.05: pwr = 0 max_intake_power = 0.5 if pwr > max_intake_power: pwr = max_intake_power intake.run_intake(pwr) if intake.has_acquired_cube() and pwr > 0: oi.rumble_op() else: oi.unrumble_op() else: intake.run_intake(0) oi.unrumble_op() if oi.arm_is_open(): intake.set_grab_state(GrabState.OUT) else: intake.set_grab_state(GrabState.IN) def end(self): OI.get().unrumble_op() def isFinished(self): return False