class SetDistanceToBox(Command):
"""
Drive until the robot is the given distance away from the box. Uses a local
PID controller to run a simple PID loop that is only enabled while this
command is running. The input is the averaged values of the left and right
encoders.
"""
def __init__(self, robot, distance):
super().__init__()
self.robot = robot
self.requires(self.robot.drivetrain)
self.pid = PIDController(
-2,
0,
0,
)
self.pid.setSetpoint(distance)
def initialize(self):
"""Called just before this Command runs the first time"""
# Get everything in a safe starting state.
self.robot.drivetrain.reset()
self.pid.reset()
self.pid.enable()
def execute(self):
"""Called repeatedly when this Command is scheduled to run"""
def isFinished(self):
"""Make this return true when this Command no longer needs to run execute()"""
return self.pid.onTarget()
def end(self):
"""Called once after isFinished returns true"""
# Stop PID and the wheels
self.pid.disable()
self.robot.drivetrain.driveManual(0, 0)
def interrupted(self):
"""Called when another command which requires one or more of the same
subsystems is scheduled to run"""
self.end()
class Drive:
"""
Handle robot drivetrain.
All drive interaction must go through this class.
"""
limelight: Limelight
train: wpilib.drive.DifferentialDrive
navx: navx.AHRS
left_motors: wpilib.SpeedControllerGroup
right_motors: wpilib.SpeedControllerGroup
y = will_reset_to(0.0)
rot = will_reset_to(0.0)
aligning = will_reset_to(False)
deadband = will_reset_to(0.1)
auto = will_reset_to(False)
left_voltage = will_reset_to(0)
right_voltage = will_reset_to(0)
speed_constant = will_reset_to(1.05)
rotational_constant = will_reset_to(0.8)
squared_inputs = False
angle_p = ntproperty('/align/kp', 0.04)
angle_i = ntproperty('/align/ki', 0)
angle_d = ntproperty('/align/kd', 0.0)
angle_reported = ntproperty('/align/angle', 0)
angle_to = ntproperty('/align/angle_to', 0)
def setup(self):
"""
Run setup code on the injected variables (train)
"""
self.angle_controller = PIDController(self.angle_p, self.angle_i,
self.angle_d)
self.angle_controller.setTolerance(2, 1)
self.angle_controller.enableContinuousInput(0, 360)
self.angle_setpoint = None
self.calculated_pid = False
def set_target(self, angle: float, relative=True):
if relative and angle is not None:
self.angle_setpoint = (self.angle + angle) % 360
self.angle_to = self.angle_setpoint
else:
self.angle_setpoint = angle
if angle is not None:
self.angle_controller.setSetpoint(self.angle_setpoint)
else:
self.calculated_pid = False
self.angle_controller.reset()
def align(self):
self.aligning = True
self.deadband = 0
def voltageDrive(self, left_voltage, right_voltage):
self.left_voltage = left_voltage
self.right_voltage = right_voltage
self.auto = True
self.deadband = 0
def move(self, y: float, rot: float):
"""
Move robot.
:param y: Speed of motion in the y direction. [-1..1]
:param rot: Speed of rotation. [-1..1]
"""
self.y = y
self.rot = rot
@property
def target_locked(self):
# self.logger.info(f'SET {self.angle_controller.getSetpoint()} ANG {self.navx.getAngle()}')
return self.angle_controller.atSetpoint() and self.calculated_pid
@property
def angle(self):
raw = self.navx.getAngle()
while raw < 0:
raw += 360
return raw % 360
def execute(self):
"""
Handle driving.
"""
self.train.setDeadband(self.deadband)
self.angle_reported = self.angle
if self.aligning and self.angle_setpoint is not None:
self.right_motors.setInverted(False)
if self.angle_controller.atSetpoint() and self.calculated_pid:
self.train.arcadeDrive(0, 0, squareInputs=False)
return
# Use new network tables variables for testing
self.angle_controller.setP(self.angle_p)
self.angle_controller.setD(self.angle_d)
self.angle_controller.setI(self.angle_i)
output = self.angle_controller.calculate(self.angle)
self.logger.info(
f'Output: {output}, Error: {self.angle_controller.getPositionError()}'
)
# Manual I-term zone (15 degrees)
if abs(self.angle_controller.getPositionError()) <= 3:
self.angle_controller.setI(self.angle_i)
# Minumum and Maximum effect of integrator on output
self.angle_controller.setIntegratorRange(-0.05, 0.05)
else:
self.angle_controller.setI(0)
self.angle_controller.setIntegratorRange(0, 0)
if output < 0:
output = max(output, -0.35)
else:
output = min(output, 0.35)
self.train.arcadeDrive(0, output, squareInputs=False)
self.calculated_pid = True
elif self.auto:
self.right_motors.setInverted(True)
self.right_motors.setVoltage(self.right_voltage)
self.left_motors.setVoltage(self.left_voltage)
else:
self.right_motors.setInverted(False)
self.train.arcadeDrive(
self.speed_constant * self.y,
self.rotational_constant * self.rot,
squareInputs=self.squared_inputs,
)
if not self.limelight.targetExists():
self.limelight.target_state = 0
elif self.target_locked:
self.limelight.target_state = 2
else:
self.limelight.target_state = 1
class Turret:
'''
The object thats responsible for managing the shooter
'''
def __init__(self):
self.clockwise_limit_switch = DigitalInput(
TURRET_CLOCKWISE_LIMIT_SWITCH)
self.counterclockwise_limit_switch = DigitalInput(
TURRET_COUNTERCLOCKWISE_LIMIT_SWITCH)
self.turn_motor = SparkMax(TURRET_TURN_MOTOR)
self.turn_pid = PIDController(0.4, 0.001, 0.02)
self.shoot_motor_1 = Falcon(TURRET_SHOOT_MOTORS[0])
self.shoot_motor_2 = Falcon(TURRET_SHOOT_MOTORS[1])
self.timer = Timer()
self.limelight = Limelight()
def set_target_angle(self, angle):
'''
Sets the target angle of the turret. This will use a PID to turn the
turret to the target angle.
'''
target_encoder = angle_to_encoder(angle)
self.turn_pid.setSetpoint(target_encoder)
def update(self):
'''
This is used to continuously update the turret's event loop.
All this manages as of now is the turrets PID controller.
The shoot motor is constantly running at a low percentage until we need it.
'''
motor_speed = self.turn_pid.calculate(
self.limelight.get_target_screen_x())
if self.clockwise_limit_switch.get() and motor_speed < 0:
self.turn_motor.set_percent_output(motor_speed)
elif self.counterclockwise_limit_switch.get() and motor_speed > 0:
self.turn_motor.set_percent_output(motor_speed)
def shoot(self):
'''
The wheel to shoot will rev up completely before balls start feeding
in from the singulator.
'''
# One of the motors will be reversed, so make sure the shoot motor has the correct ID!
speed = self.shoot_motor_1.get_percent_output()
if speed < 1:
speed += 0.02
elif speed > 1:
speed -= 0.02
self.shoot_motor_1.set_percent_output(speed)
self.shoot_motor_2.set_percent_output(-speed)
def idle(self):
'''
Resets the motors back to their default state.
'''
speed = self.shoot_motor_1.get_percent_output()
if speed < 0.5:
speed += 0.02
elif speed > 0.5:
speed -= 0.02
self.shoot_motor_1.set_percent_output(speed)
self.shoot_motor_2.set_percent_output(-speed)
def is_full_speed(self):
'''
Returns if the motor is at full speed or not.
'''
self.timer.get() > 0.4