class Climber: climber_motor = WPI_TalonSRX speed = 0 def setup(self): self.speed_streamer = NTStreamer(self.speed, "climber/speed", round_digits=2) def set_speed(self, speed): self.speed = speed def execute(self): self.speed_streamer.send(self.speed) self.climber_motor.set(self.speed) self.speed = 0
def setup(self): self.winch.setQuadraturePosition(0, 0) self.winch.configPeakOutputForward(1, 0) self.winch.configPeakOutputReverse(-1, 0) self.winch.configNominalOutputForward(0, 0) self.winch.configNominalOutputReverse(0, 0) self.winch.setInverted(True) position_pid_coefs = PIDCoefficients(p=0.4, i=0.007, d=0) position_pid_parameters = PIDParameters(position_pid_coefs, output_max=1, output_min=-1) self.position_pid = PIDController(position_pid_parameters, wpilib.Timer.getFPGATimestamp) self.position_streamer = NTStreamer(0.0, "elevator/position", round_digits=2)
def setup(self): self.odometry_streamer = NTStreamer(self.robot_state, "drivetrain", round_digits=2)
class Drivetrain: robot_drive = drive.DifferentialDrive compressor = Compressor rotation = 0 forward = 0 left = 0 right = 0 curvature = None robot_characteristics = RobotCharacteristics(acceleration_time=0.3, deceleration_time=1.8, max_speed=3.7, wheel_base=0.6096, encoder_ticks=1024 * 4, revolutions_to_distance=6 * math.pi * 0.02540, speed_scaling=3.7) wheel_distances = (0.0, 0.0) previous_motor_voltages = (0.0, 0.0) left_drive_motor = WPI_TalonSRX right_drive_motor = WPI_TalonSRX navx = AHRS robot_state = RobotState() def setup(self): self.odometry_streamer = NTStreamer(self.robot_state, "drivetrain", round_digits=2) def forward_at(self, speed): self.left = speed self.right = speed self.forward = speed def curve_at(self, curvature): self.curvature = curvature def tank(self, left, right): self.left = left self.right = right def set_path(self, max_speed: float, end_threshold: float, path: Path): self.robot_state = path.initial_state self._set_orientation(self.robot_state.rotation) self.left_drive_motor.setQuadraturePosition(0, 0) self.right_drive_motor.setQuadraturePosition(0, 0) self.wheel_distances = (0, 0) robot_characteristics = RobotCharacteristics( acceleration_time=self.robot_characteristics.acceleration_time, deceleration_time=self.robot_characteristics.deceleration_time, max_speed=max_speed, wheel_base=self.robot_characteristics.wheel_base, encoder_ticks=self.robot_characteristics.encoder_ticks, revolutions_to_distance=self.robot_characteristics. revolutions_to_distance, speed_scaling=self.robot_characteristics.speed_scaling) self.path_tracker = PathTracker( path, robot_characteristics, 0.1, end_threshold, self.get_odometry, lambda speed: self.forward_at(speed / self.robot_characteristics. speed_scaling), self.curve_at, None) path_points = [] for segment in path.segments: path_points.append(segment.start) path_points.append(path.segments[-1].end) def follow_path(self) -> (Completed, float): return self.path_tracker.update() def get_odometry(self) -> RobotState: return self.robot_state def get_orientation(self) -> float: return math.radians(-self.navx.getAngle()) def _set_orientation(self, orientation) -> float: self.navx.setAngleAdjustment(0) self.navx.setAngleAdjustment(-self.navx.getAngle() - math.degrees(orientation)) def _update_odometry(self): encoder_scaling = (self.robot_characteristics.encoder_ticks / self.robot_characteristics.revolutions_to_distance) current_wheel_distances = ( -self.left_drive_motor.getQuadraturePosition() / encoder_scaling, self.right_drive_motor.getQuadraturePosition() / encoder_scaling) enc_velocity = (self.right_drive_motor.getQuadratureVelocity() - self.left_drive_motor.getQuadratureVelocity()) / 2 velocity = 10 * enc_velocity / encoder_scaling self.robot_state = tank_drive_odometry(current_wheel_distances, self.wheel_distances, self.get_orientation(), self.robot_state.rotation, self.robot_state.position, velocity) self.wheel_distances = current_wheel_distances def _scale_speeds(self, vl: float, vr: float) -> (float, float): """Scales left and right motor speeds to a max of ±1.0 if either is greater """ if abs(vl) >= abs(vr) and abs(vl) > 1.0: return math.copysign(1.0, vl), math.copysign(vr / vl, vr) if abs(vr) >= abs(vl) and abs(vr) > 1.0: return math.copysign(vl / vr, vl), math.copysign(1.0, vr) return vl, vr def execute(self): self._update_odometry() self.odometry_streamer.send(self.robot_state) if self.curvature is not None: if self.curvature > 1e-4: radius = abs(1 / self.curvature) scale = interpolate(0.35, 1, 0, 0.9, radius) scale = min(scale, 1.0) self.forward *= scale v_left, v_right = tank_drive_wheel_velocities( self.robot_characteristics.wheel_base, self.forward, self.curvature) self.left, self.right = self._scale_speeds(v_left, v_right) self.robot_drive.tankDrive(self.left, self.right, squaredInputs=False) if self.left > 0.05 or self.right > 0.05: self.compressor.stop() else: self.compressor.start() self.rotation = 0 self.forward = 0 self.curvature = None self.left = 0 self.right = 0
def setup(self): self.speed_streamer = NTStreamer(self.speed, "climber/speed", round_digits=2)
class Elevator: winch = WPI_TalonSRX limit_switch = wpilib.DigitalInput speed = 0 target_position = 0 holding_position = None using_position_control = False def setup(self): self.winch.setQuadraturePosition(0, 0) self.winch.configPeakOutputForward(1, 0) self.winch.configPeakOutputReverse(-1, 0) self.winch.configNominalOutputForward(0, 0) self.winch.configNominalOutputReverse(0, 0) self.winch.setInverted(True) position_pid_coefs = PIDCoefficients(p=0.4, i=0.007, d=0) position_pid_parameters = PIDParameters(position_pid_coefs, output_max=1, output_min=-1) self.position_pid = PIDController(position_pid_parameters, wpilib.Timer.getFPGATimestamp) self.position_streamer = NTStreamer(0.0, "elevator/position", round_digits=2) def move_setpoint(self, speed: float): self.target_position += speed def hold(self): if self.holding_position is None: self.holding_position = self.get_position() self.target_position = self.holding_position def set_position(self, position: float): """Set a target position for the elevator 'position' is measured in revolutions of the elevator winch. """ self.target_position = position self.holding_position = None def get_position(self) -> float: return self.winch.getQuadraturePosition() / -4096 def reset_position(self): self.winch.setQuadraturePosition(0, 0) self.target_position = 0 def execute(self): position = self.get_position() self.position_streamer.send(position) if not self.limit_switch.get(): self.winch.setQuadraturePosition(0, 0) position = 0 if self.target_position < 0: self.target_position = 0 if self.target_position > 13: self.target_position = 13 if self.target_position is not None: if not self.using_position_control: self.position_pid.reset() self.using_position_control = True self.speed = self.position_pid.get_output(position, self.target_position) if position < 1 and self.speed < 0: self.speed = 0.1 elif position < 3.5 and self.speed < 0: self.speed = 0 if self.speed < 0: self.speed *= 0.1 else: self.speed *= 1.4 if position > 200: self.speed += 0.12 self.speed = clamp(self.speed, -1, 1) self.winch.set(self.speed) self.speed = 0
class Drivetrain: robot_drive = drive.DifferentialDrive left_drive_motor = WPI_TalonSRX right_drive_motor = WPI_TalonSRX navx = AHRS forward = 0 curvature = 0 rotation = 0 robot_characteristics = RobotCharacteristics(acceleration_time=0.3, deceleration_time=1.8, max_speed=3.7, wheel_base=0.6096, encoder_ticks=1024 * 4, revolutions_to_distance=6 * math.pi * 0.02540, speed_scaling=3.7) wheel_distances = (0.0, 0.0) robot_state = RobotState() def setup(self): self.odometry_streamer = NTStreamer(self.robot_state, "drivetrain", round_digits=2) def forward_at(self, speed): self.forward = speed def curve(self, curvature): self.curvature = curvature def turn(self, speed): self.rotation = speed def get_orientation(self) -> float: return math.radians(-self.navx.getAngle()) def _update_odometry(self): encoder_scaling = (self.robot_characteristics.encoder_ticks / self.robot_characteristics.revolutions_to_distance) current_wheel_distances = ( -self.left_drive_motor.getQuadraturePosition() / encoder_scaling, self.right_drive_motor.getQuadraturePosition() / encoder_scaling) enc_velocity = (self.right_drive_motor.getQuadratureVelocity() - self.left_drive_motor.getQuadratureVelocity()) / 2 velocity = 10 * enc_velocity / encoder_scaling self.robot_state = tank_drive_odometry(current_wheel_distances, self.wheel_distances, self.get_orientation(), self.robot_state.rotation, self.robot_state.position, velocity) self.wheel_distances = current_wheel_distances def _scale_speeds(self, vl: float, vr: float) -> (float, float): """Scales left and right motor speeds to a max of ±1.0 if either is greater """ if abs(vl) >= abs(vr) and abs(vl) > 1.0: return math.copysign(1.0, vl), math.copysign(vr / vl, vr) if abs(vr) >= abs(vl) and abs(vr) > 1.0: return math.copysign(vl / vr, vl), math.copysign(1.0, vr) return vl, vr def execute(self): self._update_odometry() self.odometry_streamer.send(self.robot_state) if self.rotation: left = -self.rotation right = self.rotation else: left, right = tank_drive_wheel_velocities( self.robot_characteristics.wheel_base, self.forward, self.curvature) left, right = self._scale_speeds(left, right) self.robot_drive.tankDrive(left, right, squaredInputs=False) self.forward = 0 self.curvature = 0 self.rotation = 0
def setup(self): self.arm_state_streamer = NTStreamer(self.arm_state, "intake/arm_state") self.wheel_speed_streamer = NTStreamer(self.wheel_speed, "intake/wheel_speed")
class Intake: l_intake_motor = WPI_VictorSPX r_intake_motor = WPI_VictorSPX solenoid = DoubleSolenoid ir = AnalogInput wrist_motor = WPI_TalonSRX wheel_speed = WheelSpeed.stopped arm_state = ArmState.closed oscillating = False oscillator = Oscillator(0.3) wrist_position = WristPosition.down wrist_pid = PIDController( PIDParameters(PIDCoefficients(p=.001, i=0, d=0)), Timer.getFPGATimestamp) cube_in = True overriding = False ir_stack = [] def setup(self): self.arm_state_streamer = NTStreamer(self.arm_state, "intake/arm_state") self.wheel_speed_streamer = NTStreamer(self.wheel_speed, "intake/wheel_speed") def toggle_arm(self): if self.arm_state == ArmState.opened or self.arm_state == ArmState.neutral: self.arm_state = ArmState.closed else: self.arm_state = ArmState.opened def open_arm(self): self.arm_state = ArmState.opened def close_arm(self): self.arm_state = ArmState.closed def neutral_arm(self): self.arm_state = ArmState.neutral def intake(self): self.wheel_speed = WheelSpeed.intake self.oscillating = True def outtake(self): self.wheel_speed = WheelSpeed.outake def hold(self): self.wheel_speed = WheelSpeed.hold def set_speed(self, speed): self.wheel_speed = speed def strong_hold(self): self.wheel_speed = WheelSpeed.strong_hold self.arm_state = ArmState.closed def slide_average(self): if len(self.ir_stack) > 25: self.ir_stack.pop(0) self.ir_stack.append(self.ir.getValue()) def get_raw_ir_value(self): return self.ir.getValue() def get_ir_value(self): if len(self.ir_stack) == 0: return self.ir.getValue() return sum(self.ir_stack) / len(self.ir_stack) def cube_is_in_range(self): return self.get_ir_value() > 490 or self.get_raw_ir_value() > 500 def toggle_has_cube(self): self.cube_in = not self.cube_in self.overriding = True def has_cube(self): if self.overriding: return self.cube_in if self.get_ir_value() < 500: self.cube_in = False return False if self.get_ir_value() > 900: self.cube_in = True return self.cube_in def reset_wrist(self): self.wrist_motor.setQuadraturePosition(WristPosition.straight_up, 0) def reset_wrist_up(self): self.wrist_motor.setQuadraturePosition(WristPosition.down, 0) def wrist_down(self): self.wrist_position = WristPosition.down def wrist_up(self): self.wrist_position = WristPosition.up def move_wrist_setpoint(self, amount): self.wrist_position -= amount * 50 def execute(self): self.wheel_speed_streamer.send(self.wheel_speed) self.arm_state_streamer.send(self.arm_state) self.slide_average() if self.oscillating: offset = 0.2 if self.oscillator() else -0.2 self.l_intake_motor.set(-self.wheel_speed + offset) self.r_intake_motor.set(self.wheel_speed + offset) else: self.l_intake_motor.set(-self.wheel_speed) self.r_intake_motor.set(self.wheel_speed) if self.arm_state == ArmState.opened: self.solenoid.set(DoubleSolenoid.Value.kForward) elif self.arm_state == ArmState.closed: self.solenoid.set(DoubleSolenoid.Value.kReverse) current_wrist_position = self.wrist_motor.getQuadraturePosition() wrist_error = self.wrist_position - current_wrist_position if self.wrist_position > WristPosition.up: self.wrist_position = WristPosition.up if self.wrist_position < WristPosition.down: self.wrist_position = WristPosition.down output = self.wrist_pid.get_output(current_wrist_position, self.wrist_position) if wrist_error < 0: output *= 0.25 self.wrist_motor.set(-output) self.wheel_speed = WheelSpeed.stopped self.oscillating = False