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