class Drive:
"""
Handle robot drivetrain.
All drive interaction must go through this class.
"""
train: wpilib.drive.DifferentialDrive
y = will_reset_to(0)
rot = will_reset_to(0)
SPEED_CONSTANT = 0.5
ROT_CONSTANT = 0.75
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
def execute(self):
"""
Handle driving.
"""
self.train.arcadeDrive(self.y * self.SPEED_CONSTANT,
self.rot * self.ROT_CONSTANT)
class Drivetrain:
drive_l1: ctre.WPI_TalonSRX
drive_l2: ctre.VictorSPX
drive_l3: ctre.VictorSPX
drive_r1: ctre.WPI_TalonSRX
drive_r2: ctre.VictorSPX
drive_r3: ctre.VictorSPX
speed = magicbot.will_reset_to(0)
rotation = magicbot.will_reset_to(0)
def setup(self):
self.drive_l2.follow(self.drive_l1)
self.drive_l3.follow(self.drive_l1)
self.drive_r2.follow(self.drive_r1)
self.drive_r3.follow(self.drive_r1)
self.drive_l1.setNeutralMode(self.drive_l1.NeutralMode.Brake)
self.drive_r1.setNeutralMode(self.drive_r1.NeutralMode.Brake)
self.ddrive = wpilib.drive.DifferentialDrive(self.drive_l1,
self.drive_r1)
# def on_enable(self):
# self.x = 0
# self.y = 0
def drive(self, speed, rotation):
self.speed = -speed
self.rotation = -rotation
def autoAlign(self, rotation):
self.rotation = rotation
def execute(self):
self.ddrive.arcadeDrive(self.speed, self.rotation, False)
class Drive:
"""
Handle robot drivetrain.
All drive interaction must go through this class.
"""
train: wpilib.drive.DifferentialDrive
y = will_reset_to(0)
rot = will_reset_to(0)
speed_constant = tunable(1.05)
rotational_constant = tunable(0.5)
squared_inputs = tunable(False)
def setup(self):
"""
Run setup code on the injected variables (train)
"""
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
def execute(self):
"""
Handle driving.
"""
self.train.arcadeDrive(self.speed_constant * self.y,
self.rotational_constant * self.rot,
squareInputs=self.squared_inputs)
class Drive:
drive_train: wpilib.drive.DifferentialDrive
navx: navx.AHRS
right_encoder: ExternalEncoder
left_encoder: ExternalEncoder
y = will_reset_to(0)
rotation = will_reset_to(0)
squared = True
def drive(self, y, rot, squared=True):
self.y = y
self.rotation = rot
self.squared = squared
def execute(self):
self.drive_train.arcadeDrive(-self.y, -self.rotation, self.squared)
self.update_sd()
def reset_encoders(self):
self.left_encoder.zero()
self.right_encoder.zero()
def update_sd(self):
wpilib.SmartDashboard.putData("Drive/Left Drive Encoder",
self.left_encoder.encoder)
wpilib.SmartDashboard.putData("Drive/Right Drive Encoder",
self.right_encoder.encoder)
wpilib.SmartDashboard.putNumber("Drive/Navx", self.navx.getYaw())
class DriveTrain:
'''
Simple magicbot drive object
'''
#wall_p = tunable(-1.8)
#distance = tunable(0)
#analog = tunable(0)
#tx = tunable(0)
#ty = tunable(0)
#offset = tunable(1.0)
#MaxY = tunable(0.8)
#ultrasonic = wpilib.AnalogInput
drive: wpilib.drive.DifferentialDrive
gyro: wpilib.ADXRS450_Gyro
x = will_reset_to(0)
y = will_reset_to(0)
def __init__(self):
self.moveDeg = None
self.saveAng = None
# This stores the information of moveDeg for the second time
# distance is .98
def move(self, y, x):
self.y = y
self.x = x -0.135
def move2(self, deg):
self.moveDeg = deg
def rotate(self, x):
self.x = x
def execute(self):
self.tx = self.x
self.ty = self.y
if self.moveDeg != None:
if self.saveAng == None:
self.saveAng = self.gyro.getAngle()
wantAngle = self.saveAng + self.moveDeg
angle = self.gyro.getAngle()
error = angle - wantAngle
print(error)
output = (error * 0.01)
output = max(min(0.5, output), -0.5)
self.x = output
self.moveDeg = None
self.drive.arcadeDrive(self.y, self.x, True)
class Arm:
"""
Single Jointed arm for that holds the hatch and cargo mechs
"""
arm_motor = WPI_TalonSRX
arm_limit = wpilib.DigitalInput
reset_angle = will_reset_to(180)
kp = tunable(1)
ki = tunable(0)
kd = tunable(0)
kf = tunable(0)
target_angle = will_reset_to(180)
zeroed = False
# Constants
armRatio = 24 / 84
def setup(self):
self.arm_controller = ArmPID()
self.blahhhh = ElevatorPID(self.kp, self.ki, self.kd, self.kf)
self.arm_motor.clearStickyFaults()
self.zeroed = False
def set(self, angle):
self.target_angle = angle
self.arm_controller.resetIntegrator()
def angle(self, ticks):
return ticks * (360 * self.armRatio) / 4096
def ticks(self, angle):
return int(angle * (4096 / (360 * self.armRatio)))
def execute(self):
if not self.zeroed:
self.arm_motor.set(0.2)
if self.arm_limit.get():
self.zeroed = True
self.arm_motor.setSelectedSensorPosition(
self.ticks(self.reset_angle)
)
return # exits out of the function if we aren't zeroed
self.arm_motor.set(
self.arm_controller.update(
self.kp,
self.ki,
self.kd,
self.kf,
self.target_angle,
self.angle(self.arm_motor.getSelectedSensorPosition()),
)
)
class Align:
limelight: Limelight
drive: Drive
aligning = will_reset_to(False)
angle = will_reset_to(0)
def execute(self):
if self.aligning:
self.drive.align(self.angle, True)
def align(self, angle):
self.aligning = True
self.angle = angle
class Drive:
"""
Handle robot drivetrain.
All drive interaction must go through this class.
"""
train: wpilib.drive.DifferentialDrive
y = will_reset_to(0)
rot = will_reset_to(0)
speed_constant = tunable(1.05)
rotational_constant = tunable(0.5)
squared_inputs = tunable(False)
fine_movement = will_reset_to(False)
fine_speed_multiplier = tunable(0.5)
fine_rotation_multiplier = tunable(0.5)
def __init__(self):
self.enabled = False
def setup(self):
"""
Set input threshold.
"""
self.train.setDeadband(0.1)
def move(self, y: float, rot: float, fine_movement: bool = False):
"""
Move robot.
:param y: Speed of motion in the y direction. [-1..1]
:param rot: Speed of rotation. [-1..1]
:param fine_movement: Decrease speeds for precise motion.
"""
self.y = y
self.rot = rot
self.fine_movement = fine_movement
def execute(self):
"""
Handle driving.
"""
self.train.arcadeDrive(
self.speed_constant * self.y *
(self.fine_speed_multiplier if self.fine_movement else 1),
self.rotational_constant * self.rot *
(self.fine_rotation_multiplier if self.fine_movement else 1),
squaredInputs=self.squared_inputs)
class Hang:
winch_motor: ctre.WPI_TalonFX
kracken_hook_latch: wpilib.DoubleSolenoid
WINCH_SPEED = 0.5
winch_desired_output = will_reset_to(0.0)
def __init__(self) -> None:
self.fire_hook = False
def on_disable(self) -> None:
# stop motors
self.winch_motor.stopMotor()
def execute(self) -> None:
# solenoid
if self.fire_hook:
self.kracken_hook_latch.set(wpilib.DoubleSolenoid.Value.kForward)
# drive winch
self.winch_motor.set(self.winch_desired_output)
def raise_hook(self) -> None:
# fire solenoid
self.fire_hook = True
def winch(self) -> None:
# drive motor
self.winch_desired_output = self.WINCH_SPEED
def pay_out(self) -> None:
# drive motor backwards to release cable in pits
self.winch_desired_output = -self.WINCH_SPEED
class Cargo:
lift_piston = wpilib.Solenoid
motor = ctre.TalonSRX
motor_speed = will_reset_to(0)
is_in_position = tunable(False)
motor_input_speed = tunable(-0.6)
motor_output_speed = tunable(1)
def lift(self):
self.is_in_position = False
def lower(self):
self.is_in_position = True
def intake(self):
self.motor_speed = self.motor_input_speed
def outtake(self):
self.motor_speed = self.motor_output_speed
def execute(self):
self.motor.set(ctre.ControlMode.PercentOutput, self.motor_speed)
class Intake:
"""
Operate robot intake.
"""
intake_wheels: wpilib.SpeedControllerGroup
_intake_wheel_speed = will_reset_to(0)
def spin(self, speed: float = SPEED_MULTIPLIER):
"""
Set the speed of intake wheels.
:param speed: The requested speed, between -1 and 1.
"""
self._intake_wheel_speed = speed
def pull(self):
"""
Pull cube into intake.
"""
self.spin(-SPEED_MULTIPLIER)
def push(self):
"""
Push cube out of intake.
"""
self.spin(SPEED_MULTIPLIER)
def execute(self):
"""
Run intake motors.
"""
self.intake_wheels.set(self._intake_wheel_speed)
class Component2:
component1 = Component1
some_motor = wpilib.Talon
# This is changed to the value in robot.py
SOME_CONSTANT = int
# This gets reset after each invocation of execute()
did_something = will_reset_to(False)
def on_enable(self):
"""Called when the robot enters teleop or autonomous mode"""
self.logger.info(
"Robot is enabled: I have SOME_CONSTANT=%s", self.SOME_CONSTANT
)
def do_something(self):
self.did_something = True
def execute(self):
if self.did_something:
self.some_motor.set(1)
else:
self.some_motor.set(0)
class Drive:
lMotor = wpilib.Talon
rMotor = wpilib.Talon
# This is changed to the value in robot.py
SOME_CONSTANT = int
# This gets reset after each invocation of execute()
started_driving = will_reset_to(False)
def on_enable(self):
'''Called when the robot enters teleop or autonomous mode'''
self.logger.info("Robot is enabled: I have SOME_CONSTANT=%s",
self.SOME_CONSTANT)
def start_driving(self, speed, lTurn, rTurn):
self.started_driving = True
self.driving_speed = speed
self.lTurning_speed = lTurn
self.rTurning_speed = rTurn
def execute(self):
if self.started_driving:
self.lMotor.setSpeed(-self.driving_speed +
self.lTurning_speed * 0.2)
self.rMotor.setSpeed(self.driving_speed +
self.rTurning_speed * 0.2)
else:
self.lMotor.setSpeed(0)
self.rMotor.setSpeed(0)
class Fork:
fork_switch: wpilib.DigitalInput
arm_motor: ctre.WPI_TalonSRX
position = will_reset_to(0)
last_position = 0
fork_switch_on = tunable(False)
def execute(self):
# TODO: the limit switch seems to be broken on the robot :(
self.fork_switch_on = self.fork_switch.get()
# if the limit switch is on and you last opened it,
# and you are trying to open it
#if self.fork_switch_on and self.position and self.last_position == self.position:
# self.position = 0
#elif self.position != 0: # if nothing is being pressed - do this
# self.last_position = self.position
self.arm_motor.set(self.position)
#self.arm_motor.set(0)
# @timed_state(state_tm)
def open(self):
self.position = -1
def close(self):
self.position = 1
class Drivetrain:
shifter: wpilib.Solenoid
train: wpilib.drive.DifferentialDrive
forward_speed = will_reset_to(0)
turn_speed = will_reset_to(0)
kP = tunable(1)
kI = tunable(0)
kD = tunable(0)
drive_multiplier = tunable(.7)
turn_multiplier = tunable(.5)
high_gear = tunable(False)
currentSpeed = tunable(0)
integral = tunable(0)
previous_error = tunable(0)
def execute(self):
self.shifter.set(self.high_gear)
self.train.arcadeDrive(self.forward_speed, self.turn_speed)
def PID(self, targetSpeed):
'''
Uses the current speed alongside the target speed to create a smooth ramping
'''
error = self.currentSpeed - targetSpeed
self.integral = self.integral + (error * .02)
derivative = (error - self.previous_error) / .02
return self.kP * error + self.kI * self.integral + self.kD * derivative
def shift_toggle(self):
self.high_gear = not self.high_gear
def move(self, speed, turn):
self.forward_speed = -speed * self.drive_multiplier
self.turn_speed = turn * self.turn_multiplier
def movePID(self, speed, turn):
self.forward_speed = self.PID(speed)
self.turn_speed = turn
class Intake:
intake_motor: VictorSP
speed = will_reset_to(0)
def spin(self, speed: float):
self.speed = speed
def execute(self):
self.intake_motor.set(self.speed)
class Intake:
intake_motor: WPI_VictorSPX
intake_motor_lower: CANSparkMax
intake_switch: wpilib.DigitalInput
intake_solenoid: wpilib.DoubleSolenoid
balls_collected = ntproperty("/components/intake/ballsCollected", 0)
speed = will_reset_to(0.0)
speed_lower = will_reset_to(0.0)
solenoid_state = will_reset_to(wpilib.DoubleSolenoid.Value.kForward)
previous_limit = False
def spin(self, speed, speed_lower: float):
self.spin_inside(speed)
self.spin_lower(speed_lower)
def spin_lower(self, speed_lower: float):
self.speed_lower = speed_lower
def spin_inside(self, speed: float):
self.speed = speed
def extend(self):
self.set_solenoid(True)
def retract(self):
self.set_solenoid(False)
def set_solenoid(self, extended: bool):
self.solenoid_state = wpilib.DoubleSolenoid.Value.kReverse if extended else wpilib.DoubleSolenoid.Value.kForward
def execute(self):
self.intake_motor.set(self.speed)
self.intake_motor_lower.set(self.speed_lower)
if self.intake_switch.get():
if self.intake_switch.get() is not self.previous_limit:
self.balls_collected += 1
self.previous_limit = self.intake_switch.get()
self.intake_solenoid.set(self.solenoid_state)
class Intake:
left_intake_motor: wpilib.Spark
right_intake_motor: wpilib.Spark
speed = will_reset_to(0)
def set_speed(self, new_speed: float):
self.speed = new_speed
def execute(self):
self.left_intake_motor.set(self.speed)
self.right_intake_motor.set(self.speed)
class Intake:
left_motor: ctre.WPI_TalonSRX
right_motor: ctre.WPI_TalonSRX
intake_speed = will_reset_to(0)
kIntakeSpeed = tunable(.7)
def execute(self):
self.left_motor.set(self.intake_speed)
self.right_motor.set(-self.intake_speed)
def suck(self):
self.intake_speed = -self.kIntakeSpeed
def push(self):
self.intake_speed = self.kIntakeSpeed
class Intake:
intake_motor: ctre.WPI_TalonSRX
speed = magicbot.will_reset_to(0)
magnitude = magicbot.tunable(0.5)
#TO DO: Find the correct directions for the intake and outake.
def ballIn(self):
self.speed = self.magnitude
def ballOut(self):
self.speed = -self.magnitude
def execute(self):
self.intake_motor.set(self.speed)
class Winch:
"""
Operate robot winch.
"""
winch_motors: wpilib.SpeedControllerGroup
_climb_speed = will_reset_to(0)
def run(self, speed: float = 1):
"""
Set the motor speed of each climbing motor.
:param speed: The requested speed, between -1 and 1.
"""
self._climb_speed = speed
def execute(self):
"""
Run climbing motors.
"""
self.winch_motors.set(self._climb_speed)
def __init__(self):
# Current drive mode, this changes when a control calls its drive function
self.drive_mode = will_reset_to(DriveMode.TANK)
# Rotation, negative turns to the left, also known as z
# Used for both
self.rotation = will_reset_to(0)
# Speed, positive is forward (joystick must be inverted)
# Used for both
self.y = will_reset_to(0)
# Horizontal speed
# Mecanum only
self.x = will_reset_to(0)
self.fod = will_reset_to(False)
self.adjusted = will_reset_to(True)
class CargoManipulator:
"""
Operate cargo manipulation system.
"""
# cargo_intake_motors: wpilib.SpeedControllerGroup
left_cargo_intake_motor: WPI_TalonSRX
pull_speed = (0.5)
light_pull_speed = (0.2)
push_speed = (1.0)
intake_speed = will_reset_to(0)
def pull(self):
"""
"""
self.intake_speed = -self.pull_speed
def pull_lightly(self):
"""
Pull at a lower speed, to hold in ball.
"""
self.intake_speed = -self.light_pull_speed
def push(self):
"""
Move lift downward.
Used when controlling arm through buttons.
"""
self.intake_speed = self.push_speed
def execute(self):
"""
Run elevator motors.
"""
# self.cargo_intake_motors.set(-self.intake_speed)
self.left_cargo_intake_motor.set(self.intake_speed)
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 ShooterController(StateMachine):
"""Statemachine for high level control of the shooter and injector"""
VERBOSE_LOGGING = True
chassis: Chassis
indexer: Indexer
shooter: Shooter
turret: Turret
target_estimator: TargetEstimator
led_screen: LEDScreen
range_finder: RangeFinder
fire_command = will_reset_to(False)
MAX_MISALIGNMENT = 0.2 # m from centre of target
MIN_SCAN_PERIOD = 3.0
TARGET_LOST_TO_SCAN = 0.5
CAMERA_TO_LIDAR = 0.15
def __init__(self) -> None:
super().__init__()
self.spin_command = False
self.distance = None
self.time_of_last_scan: Optional[float] = None
self.time_target_lost: Optional[float] = None
self.disabled_flash: int = 0
self.fired_count: int = 0
def execute(self) -> None:
super().execute()
self.update_LED()
def update_LED(self) -> None:
# Flash if turret and shooter are disabled
if self.shooter.disabled:
if self.disabled_flash < 25:
self.led_screen.set_bottom_row(255, 0, 0)
self.led_screen.set_middle_row(255, 0, 0)
self.led_screen.set_top_row(255, 0, 0)
else:
self.led_screen.set_bottom_row(0, 0, 0)
self.led_screen.set_middle_row(0, 0, 0)
self.led_screen.set_top_row(0, 0, 0)
self.disabled_flash = (self.disabled_flash + 1) % 50
return
if self.shooter.is_ready():
self.led_screen.set_bottom_row(0, 255, 0)
else:
self.led_screen.set_bottom_row(255, 0, 0)
if self.indexer.is_ready():
self.led_screen.set_middle_row(0, 255, 0)
else:
self.led_screen.set_middle_row(255, 0, 0)
if self.target_estimator.is_ready():
if self.aimed_at_target():
self.led_screen.set_top_row(0, 255, 0)
else:
self.led_screen.set_top_row(255, 128, 0)
else:
self.led_screen.set_top_row(255, 0, 0)
@state(first=True)
def startup(self, initial_call) -> None:
"""
Wait for the turret to complete it's inital slew before doing anything
"""
if initial_call:
# target_angle = self.chassis.find_power_port_angle()
self.turret.slew(0)
if self.turret.is_ready():
self.next_state("searching")
@state
def searching(self) -> None:
"""
The vision system does not have a target, we try to find one using odometry
"""
if self.target_estimator.is_ready():
# print(f"searching -> tracking {self.vision.get_vision_data()}")
self.time_target_lost = None
self.next_state("tracking")
else:
# Scan starting straight downrange.
time_now = time.monotonic()
# Start a scan only if it's been a minimum time since we lost the target
if (self.time_target_lost is None or
(time_now - self.time_target_lost) > self.TARGET_LOST_TO_SCAN):
# Start a scan only if it's been a minimum time since we started
# a scan. This allows the given scan to grow enough to find the
# target so that we don't just start the scan over every cycle.
if (self.time_of_last_scan is None
or (time_now - self.time_of_last_scan) >
self.MIN_SCAN_PERIOD):
self.turret.scan(-self.chassis.get_heading())
self.time_of_last_scan = time_now
# TODO test this
# target_angle = self.chassis.angle_to_target()
# self.turret.scan(target_angle)
@state
def tracking(self) -> None:
"""
Aiming towards a vision target and spinning up flywheels
"""
# collect data only once per loop
if not self.target_estimator.is_ready():
self.time_target_lost = time.monotonic()
self.next_state("searching")
# print(f"tracking -> searching {self.vision.get_vision_data()}")
else:
target_data = self.target_estimator.get_data()
if abs(target_data.angle) > self.find_allowable_angle():
# print(f"Telling turret to slew by {target_data.angle}")
self.turret.slew(target_data.angle)
if self.turret.is_ready():
self.shooter.set_range(target_data.distance)
if self.ready_to_fire() and self.fire_command:
self.next_state("firing")
@state(must_finish=True)
def firing(self, initial_call) -> None:
"""
Positioned to fire, inject and expel a single ball
"""
if initial_call:
self.shooter.fire()
self.fired_count += 1
elif not self.shooter.is_firing():
self.next_state("tracking")
def fire_input(self) -> None:
"""
Called by robot.py to indicate the fire button has been pressed
"""
self.fire_command = True
@feedback
def ready_to_fire(self) -> bool:
return (self.shooter.is_ready() and self.aimed_at_target()
and self.indexer.is_ready()
and self.target_estimator.is_ready()
and self.turret.is_ready())
@feedback
def aimed_at_target(self) -> bool:
target_data = self.target_estimator.get_data()
return abs(target_data.angle) < self.find_allowable_angle()
def find_allowable_angle(self) -> float:
"""
Find the maximum angle by which the turret can be misaligned to still score a hit
Currently does not consider angle from target, assumes we are at max distance
"""
# dist: the maximum value of our lookup table
dist = self.shooter.ranges[-1]
angle = math.atan(self.MAX_MISALIGNMENT / dist)
return angle
class Drivetrain:
motorr1: rev.CANSparkMax
motorr2: rev.CANSparkMax
motorl1: rev.CANSparkMax
motorl2: rev.CANSparkMax
using_vision = will_reset_to(False)
vision_dist_kP = tunable(0.15)
vision_dist_kI = tunable(0)
vision_dist_kD = tunable(0)
vision_turn_kP = tunable(0.055)
vision_turn_kI = tunable(0.001)
vision_turn_kD = tunable(0)
vision_turn_integral_range = tunable(0.1)
forward = will_reset_to(0)
turn = will_reset_to(0)
twist_power = will_reset_to(0)
tank_drive: bool
twist: bool
vision_integral = 0
intake_is_front = True
turn_multiplier = tunable(0.7)
def vision_aim(self, x: float, y: float, aim_x=True, aim_y=True):
self.using_vision = True
# out = self.vision_pid.calculate(x)
# print(out)
self.vision_integral += x
p = self.vision_turn_kP * x
i = self.vision_turn_kI * self.vision_integral
out = min(.25, max(p + i, -.25))
self.turn = out
self.twist_power = self.turn
if abs(x) < self.vision_turn_integral_range:
self.vision_integral = 0
# self.forward = min(.5, max(self.vision_dist_kP * y, -.5))
def reset_integral(self):
self.vision_integral = 0
def setup(self):
self.motorr1.restoreFactoryDefaults()
self.motorr2.restoreFactoryDefaults()
self.motorl1.restoreFactoryDefaults()
self.motorl2.restoreFactoryDefaults()
self.motorr2.follow(self.motorr1)
self.motorl2.follow(self.motorl1)
self.drive = wpilib.drive.DifferentialDrive(self.motorl1, self.motorr1)
# self.vision_pid = wpilib.controller.PIDController(0.006, 0.001, 0.0009)
# self.vision_pid.setSetpoint(0)
# self.vision_pid.calculate(0)
# self.vision_pid.setTolerance(0.1)
def arcadeDrive(self, forward, turn, twist):
self.forward = forward * (-1 if self.intake_is_front else 1)
self.turn = turn
self.twist_power = twist
def tankDrive(self, left, right):
if self.intake_is_front:
self.forward = left
self.turn = right
else:
self.forward = right
self.turn = left
def execute(self):
if not self.using_vision:
self.reset_integral()
if self.tank_drive:
# Reusing forward and turn as left and right to reduce memory usage
self.drive.tankDrive(self.forward, self.turn)
else:
if self.twist:
self.drive.arcadeDrive(self.forward, self.twist_power, True)
wpilib.SmartDashboard.putNumber("turnSpeed", self.twist_power)
else:
self.drive.arcadeDrive(self.forward, self.turn, True)
wpilib.SmartDashboard.putNumber("turnSpeed", self.turn)
wpilib.SmartDashboard.putNumber("forwardSpeed", self.forward)
class Chassis:
left_rear: rev.CANSparkMax
left_front: rev.CANSparkMax
right_rear: rev.CANSparkMax
right_front: rev.CANSparkMax
imu: NavX
open_loop = magicbot.tunable(False)
vx = magicbot.will_reset_to(0.0)
vz = magicbot.will_reset_to(0.0)
def setup(self) -> None:
self.left_front.setInverted(False)
self.right_front.setInverted(True)
self.disable_brake_mode()
self.left_rear.follow(self.left_front)
self.right_rear.follow(self.right_front)
self.left_encoder = rev.CANEncoder(self.left_front)
self.right_encoder = rev.CANEncoder(self.right_front)
rev_to_m = WHEEL_CIRCUMFERENCE / GEAR_RATIO
for enc in (self.left_encoder, self.right_encoder):
enc.setPositionConversionFactor(rev_to_m)
enc.setVelocityConversionFactor(rev_to_m / 60)
self.left_pid: rev.CANPIDController = self.left_front.getPIDController(
)
self.right_pid: rev.CANPIDController = self.right_front.getPIDController(
)
self.ff_calculator = SimpleMotorFeedforwardMeters(kS=0.194,
kV=2.79,
kA=0.457)
for pid in (self.left_pid, self.right_pid):
# TODO: needs tuning
pid.setP(5.19 / 10)
pid.setI(0)
pid.setD(0)
pid.setIZone(0)
pid.setFF(0)
pid.setOutputRange(-1, 1)
self.kinematics = DifferentialDriveKinematics(TRACK_WIDTH)
self.odometry = DifferentialDriveOdometry(self._get_heading())
# default_position on the field
self.reset_odometry(Pose2d(-3, 0, Rotation2d(math.pi)))
def execute(self) -> None:
# XXX: https://github.com/robotpy/robotpy-wpilib/issues/635
chassis_speeds = ChassisSpeeds()
chassis_speeds.vx = self.vx
chassis_speeds.omega = self.vz
speeds = self.kinematics.toWheelSpeeds(chassis_speeds)
left_ff = self.ff_calculator.calculate(speeds.left)
right_ff = self.ff_calculator.calculate(speeds.right)
if self.open_loop:
self.left_front.setVoltage(left_ff)
self.right_front.setVoltage(right_ff)
else:
self.left_pid.setReference(speeds.left,
rev.ControlType.kVelocity,
arbFeedforward=left_ff)
self.right_pid.setReference(speeds.right,
rev.ControlType.kVelocity,
arbFeedforward=right_ff)
self.odometry.update(
self._get_heading(),
self.left_encoder.getPosition(),
self.right_encoder.getPosition(),
)
def drive(self, vx: float, vz: float) -> None:
"""Sets the desired robot velocity.
Arguments:
vx: Forwards linear velocity in m/s.
vz: Angular velocity in rad/s, anticlockwise positive.
"""
self.vx = vx
self.vz = vz
def disable_closed_loop(self) -> None:
"""Run the drivetrain in open loop mode (feedforward only)."""
self.open_loop = True
def enable_closed_loop(self) -> None:
"""Run the drivetrain in velocity closed loop mode."""
self.open_loop = False
def enable_brake_mode(self) -> None:
for motor in (
self.left_front,
self.left_rear,
self.right_front,
self.right_rear,
):
motor.setIdleMode(rev.IdleMode.kBrake)
def disable_brake_mode(self) -> None:
for motor in (
self.left_front,
self.left_rear,
self.right_front,
self.right_rear,
):
motor.setIdleMode(rev.IdleMode.kCoast)
def _get_heading(self) -> Rotation2d:
"""Get the current heading of the robot from the IMU, anticlockwise positive."""
return Rotation2d(self.imu.getYaw())
def get_pose(self) -> Pose2d:
"""Get the current position of the robot on the field."""
return self.odometry.getPose()
@magicbot.feedback
def get_heading(self) -> float:
"""Get the current heading of the robot."""
return self.get_pose().rotation().radians()
@magicbot.feedback
def get_left_velocity(self) -> float:
return self.left_encoder.getVelocity()
@magicbot.feedback
def get_right_velocity(self) -> float:
return self.right_encoder.getVelocity()
def reset_odometry(self, pose: Pose2d) -> None:
"""Resets the odometry to start with the given pose.
This is intended to be called at the start of autonomous.
"""
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
self.odometry.resetPosition(pose, self._get_heading())
def find_field_angle(self, field_point: Translation2d) -> float:
"""Return the theoretical angle (in radians) to the target based on odometry"""
pose = self.get_pose()
rel = field_point - pose.translation()
rel_heading = Rotation2d(rel.y, rel.x) + pose.rotation()
return rel_heading.radians()
def find_power_port_angle(self) -> float:
return self.find_field_angle(POWER_PORT_POSITION)
class Shooter:
limelight_state = will_reset_to(1)
motor_rpm = will_reset_to(0)
feeder_motor_speed = will_reset_to(0)
# target_rpm = tunable(-3500)
feed_speed_setpoint = tunable(-1)
rpm_error = tunable(300)
x_aim_error = tunable(1.2)
y_aim_error = tunable(2)
motor: PIDSparkMax
feeder_motor: TalonSRX
camera_state: int
@property
def target_rpm(self):
_, y = self.aim()
return -1 * (5.232 * pow(y, 2) - 70.76 * y + 3300)
def setup(self):
"""
Runs right after the createObjects method is run.
Sets up all the networktables values and configures
the shooter motor PID values
"""
self.limelight = limelight.Limelight()
self.log()
# Shooter motor configuration
self.motor.fromKu(0.0008, 0.6) # P = 0.03, I = 0.05, D = 0.125
self.motor.setFF(1 / 5880)
# self.motor.setFF(0)
# self.motor.setPID(.0008, 0, 0)
# self.motor.setPID(0.0015, 0.008, 0.005)
self.motor._motor_pid.setIZone(0.5)
self.motor.motor.setSmartCurrentLimit(100)
@feedback
def aim(self) -> Tuple[float, float]:
"""
Will return the distances from the crosshair
"""
# self.limelight_state = 3
# self.camera_state = 0
x = self.limelight.horizontal_offset
y = self.limelight.vertical_offset
return (x, y)
@property
def is_aimed(self):
"""
Test whether the target is within a tolerance
"""
x, y = self.aim()
if abs(x) > self.x_aim_error:
return False
# if abs(y) > self.y_aim_error:
# return False
return True
@property
def is_ready(self):
"""
Returns whether the current rpm of the motor is within
a certain range, specified by the `rpm_error` property
"""
return (self.rpm_error + self.motor_rpm > self.motor.rpm >
-self.rpm_error + self.motor_rpm)
def shoot(self):
"""
Sets the shooter to start moving toward the setpoint
"""
self.motor_rpm = self.target_rpm
def feed(self):
"""
Start the feeder to move the power cells towards the flywheel
"""
self.feeder_motor_speed = self.feed_speed_setpoint
def backdrive(self):
"""
Start the feeder to move the power cells towards the flywheel
"""
self.feeder_motor_speed = 1
def execute(self):
self.limelight.light(self.limelight_state)
# self.limelight.pipeline(self.limelight_state)
if abs(self.motor_rpm) < 200:
self.motor.stop()
else:
self.motor.set(self.motor_rpm)
if (abs(self.motor_rpm) > 500
and abs(self.motor.rpm) > abs(self.motor_rpm) - self.rpm_error
) or self.feeder_motor_speed:
if not self.feeder_motor_speed:
self.feed()
self.feeder_motor.set(ControlMode.PercentOutput,
self.feeder_motor_speed)
else:
self.feeder_motor.set(ControlMode.PercentOutput, 0)
# self.feeder_motor.set(ControlMode.PercentOutput, self.feeder_motor_speed)
self.log()
def log(self):
"""
Get values relating to the shooter and post them
to the dashboard for logging reasons.
"""
wpilib.SmartDashboard.putBoolean(
"limelightLightState",
True if self.limelight_state == 3 else False)
wpilib.SmartDashboard.putBoolean("shooterReady", self.is_ready)
# wpilib.SmartDashboard.putBoolean("isAimed", self.is_aimed)
wpilib.SmartDashboard.putBoolean("targetsFound",
self.limelight.valid_targets)
wpilib.SmartDashboard.putNumber("shooterSpeedTarget",
abs(self.motor_rpm))
wpilib.SmartDashboard.putNumber("shooterMotorSpeed",
abs(self.motor.rpm))
wpilib.SmartDashboard.putNumber("shooterOutput",
self.motor.motor.getAppliedOutput())
class ShooterController(StateMachine):
"""Statemachine for high level control of the shooter and injector"""
VERBOSE_LOGGING = True
chassis: Chassis
indexer: Indexer
shooter: Shooter
turret: Turret
target_estimator: TargetEstimator
led_screen: LEDScreen
range_finder: RangeFinder
fire_command = will_reset_to(False)
TARGET_RADIUS = (3 * 12 +
3.25) / 2 * 0.0254 # Circumscribing radius of target
BALL_RADIUS = 7 / 2 * 0.0254
# convert from freedom units
CENTRE_ACCURACY = (
0.1 # maximum distance the centre of the ball will be from our target point
)
TOTAL_RADIUS = BALL_RADIUS + CENTRE_ACCURACY
OFFSET = TOTAL_RADIUS / math.sin(math.pi / 3)
TRUE_TARGET_RADIUS = TARGET_RADIUS - OFFSET
TARGET_POSITION = wpilib.geometry.Pose2d(
0, -2.404, wpilib.geometry.Rotation2d(math.pi))
# in field co ordinates
MIN_SCAN_PERIOD = 3.0
TARGET_LOST_TO_SCAN = 0.5
def __init__(self) -> None:
super().__init__()
self.spin_command = False
self.distance = None
self.time_of_last_scan: Optional[float] = None
self.time_target_lost: Optional[float] = None
def execute(self) -> None:
super().execute()
self.update_LED()
def update_LED(self) -> None:
if self.shooter.is_ready():
self.led_screen.set_bottom_row(0, 255, 0)
else:
self.led_screen.set_bottom_row(255, 0, 0)
if self.indexer.is_ready():
self.led_screen.set_middle_row(0, 255, 0)
else:
self.led_screen.set_middle_row(255, 0, 0)
if self.target_estimator.is_ready():
if self.turret.is_ready():
self.led_screen.set_top_row(0, 255, 0)
else:
self.led_screen.set_top_row(255, 128, 0)
else:
self.led_screen.set_top_row(255, 0, 0)
@state(first=True)
def searching(self) -> None:
"""
The vision system does not have a target, we try to find one using odometry
"""
if self.target_estimator.is_ready():
# print(f"searching -> tracking {self.vision.get_vision_data()}")
self.time_target_lost = None
self.next_state("tracking")
else:
# Scan starting straight downrange. TODO: remove this if the above
# seems worthwhile
time_now = time.monotonic()
# Start a scan only if it's been a minimum time since we lost the target
if (self.time_target_lost is None or
(time_now - self.time_target_lost) > self.TARGET_LOST_TO_SCAN):
# Start a scan only if it's been a minimum time since we started
# a scan. This allows the given scan to grow enough to find the
# target so that we don't just start the scan over every cycle.
if (self.time_of_last_scan is None
or (time_now - self.time_of_last_scan) >
self.MIN_SCAN_PERIOD):
self.turret.scan(-self.chassis.get_heading())
self.time_of_last_scan = time_now
@state
def tracking(self) -> None:
"""
Aiming towards a vision target and spinning up flywheels
"""
# collect data only once per loop
if not self.target_estimator.is_ready():
self.time_target_lost = time.monotonic()
self.next_state("searching")
# print(f"tracking -> searching {self.vision.get_vision_data()}")
else:
target_data = self.target_estimator.get_data()
# if abs(target_data.angle) > self.find_allowable_angle(target_data.distance):
# print(f"Telling turret to slew by {target_data.angle}")
self.turret.slew(target_data.angle)
if self.turret.is_ready():
self.shooter.set_range(target_data.distance)
if self.ready_to_fire() and self.fire_command:
self.next_state("firing")
@state(must_finish=True)
def firing(self, initial_call) -> None:
"""
Positioned to fire, inject and expel a single ball
"""
if initial_call:
self.shooter.fire()
elif not self.shooter.is_firing():
self.next_state("tracking")
def fire_input(self) -> None:
"""
Called by robot.py to indicate the fire button has been pressed
"""
self.fire_command = True
@feedback
def ready_to_fire(self) -> bool:
return (self.shooter.is_ready() and self.turret.is_ready()
and self.indexer.is_ready())
def find_allowable_angle(self, dist: float) -> float:
"""
Find the maximum angle by which the turret can be misaligned to still score a hit
Currently does not consider angle from target
dist: planar distance from the target
"""
dist = min(dist, 14.0)
angle = math.atan(self.TRUE_TARGET_RADIUS / dist)
# print(f"angle tolerance +- {angle} true target radius{self.TRUE_TARGET_RADIUS}")
return angle
def setup(self):
self.extend = will_reset_to(False)
self.knee_speed = will_reset_to(0)
self.drive_speed = will_reset_to(0)
