def __init__(self,
drive: Drivetrain,
waypoints: [Pose],
cruise_speed,
acc,
dist_margin=2 / 12,
lookahead_base=2,
reverse=False,
interpol_strat=InterpolationStrategy.BIARC):
super().__init__("PursuitDriveCommand", timeout=4)
self.requires(drive)
self.drive = drive
self.margin = dist_margin
self.cruise_speed = cruise_speed
self.acc = acc
self.pp_controller = PurePursuitController(
waypoints=waypoints,
lookahead_base=lookahead_base,
interpol_strat=interpol_strat,
cruise_speed=self.cruise_speed,
acc=self.acc)
cur_pose = pose_estimator.get_current_pose()
self._begin_pose = Vector2(cur_pose.x, cur_pose.y)
self._end_pose = waypoints[-1]
self.reverse = reverse
self.goal_dist = 0
self.speed = 0
def initialize(self):
poz = pose_estimator.get_current_pose()
self.target_angle = (self.lookat - poz).angle() * 180 / math.pi
if hal.isSimulation():
from pyfrc.sim import get_user_renderer
render = get_user_renderer()
if render is not None: # If running the standalone (pyplot) sim, this is None
render.draw_line([(poz.x, -poz.y + 14), (self.lookat.x, -self.lookat.y + 14)], color="#00ff00", arrow=False)
def __init__(self, left_motor: ctre.talonsrx.TalonSRX,
right_motor: ctre.talonsrx.TalonSRX, **kwargs):
'''
Represents a drivetrain that uses CANTalons and so manages those advanced features
:param left_motor:
:param right_motor:
:param kwargs:
'''
wpilib.MotorSafety.__init__(self)
self.robot_width = kwargs.pop("robot_width", 24 / 12)
self.max_turn_radius = kwargs.pop("max_radius", 10)
self.wheel_diameter = kwargs.pop("wheel_diameter", 6)
self.max_speed = kwargs.pop("max_speed", 16)
self.ahrs = AHRS.create_spi()
self.ahrs.reset()
self._left_motor = left_motor
self._right_motor = right_motor
pose.init(left_encoder_callback=self.get_left_distance,
right_encoder_callback=self.get_right_distance,
gyro_callback=self.get_heading_rads,
wheelbase=self.robot_width)
dashboard2.add_graph("Pose X", lambda: pose.get_current_pose().x)
dashboard2.add_graph("Pose Y", lambda: pose.get_current_pose().y)
self._max_output = 1
self._mode = SmartRobotDrive.Mode.PercentVbus
self.set_mode(self._mode)
# Motor safety
self.setSafetyEnabled(True)
pose.init(left_encoder_callback=self.get_left_distance,
right_encoder_callback=self.get_right_distance,
gyro_callback=self.get_heading_rads,
wheelbase=self.robot_width,
encoder_factor=1)
dashboard2.add_graph(
"Heading", lambda: pose.get_current_pose().heading * 180 / math.pi)
def initialize(self):
self.pp_controller.init()
print("Started pursuit")
cur_pose = pose_estimator.get_current_pose()
poz = pose_estimator.get_current_pose()
line_pts = []
for t in range(1000):
t0 = t / 1000
pt = self.pp_controller.path.spline.get_point(t0)
line_pts += [(pt.x, -pt.y + 14)]
dashboard2.draw(
list(map(lambda x: Vector2(x[0], -(x[1] - 14)), line_pts)))
if hal.isSimulation():
from pyfrc.sim import get_user_renderer
render = get_user_renderer()
render.draw_line(line_pts, robot_coordinates=False)
dashboard2.add_graph("CTE", self.pp_controller.get_cte)
dashboard2.add_graph("Lookahead",
lambda: self.pp_controller.current_lookahead)
dashboard2.add_graph("Goal Distance", lambda: self.goal_dist)
dashboard2.add_graph("Speed", lambda: self.speed)
def update(time):
for g, v in graphs.items():
send_message({"name": g, "time": time, "value": v()}, event="data")
for name, status in indicators.items():
send_message({"name": name, "status": status()}, event='indicator')
# Try to send the pose, but it will throw an error if it hasn't been initialized yet
try:
poze = pose_estimator.get_current_pose()
send_message(
{
"x": poze.x * 12,
"y": -poze.y * 12,
"heading": poze.heading
},
event="pose")
except ValueError:
pass
def execute(self):
poz = pose_estimator.get_current_pose()
curvature, goal, speed, goal_pt = self.pp_controller.curvature(poz)
dashboard2.draw([poz, goal_pt])
self.goal_dist = goal
self.speed = speed
min_speed = 2 # feet/s
if speed < min_speed:
speed = min_speed
speed *= (-1 if self.reverse else 1)
curvature *= (1 if self.reverse else 1)
if curvature == 0:
self.drive.straight(speed)
else:
radius = -1 / curvature
self.drive.arc(speed, radius)
def __init__(self, drive: Drivetrain, elevator: Elevator, intake: Intake):
super().__init__("ScaleOnly command")
close_waypoints = [
Vector2(0, -10),
Vector2(16, -10),
Vector2(23, -7.5)
]
far_waypoints = [
Vector2(0, -10),
Vector2(20, -10),
Vector2(20, 7),
Vector2(23, 7.5)
]
cruise = 0.6
acc = 0.6
margin = 3 / 12
lookahead = 2
drive_path_far = PursuitDriveCommand(acc=acc,
cruise_speed=cruise,
waypoints=far_waypoints,
drive=drive,
dist_margin=margin,
lookahead_base=lookahead)
drive_path_close = PursuitDriveCommand(acc=acc,
cruise_speed=cruise,
waypoints=close_waypoints,
drive=drive,
dist_margin=margin,
lookahead_base=lookahead)
drive_path_far_flipped = PursuitDriveCommand(
acc=acc,
cruise_speed=cruise,
waypoints=pursuit.flip_waypoints_y(far_waypoints),
drive=drive,
dist_margin=margin,
lookahead_base=lookahead)
drive_path_close_flipped = PursuitDriveCommand(
acc=acc,
cruise_speed=cruise,
waypoints=pursuit.flip_waypoints_y(close_waypoints),
drive=drive,
dist_margin=margin,
lookahead_base=lookahead)
drive_path_chooser = ConditionalCommand(
"StandardScaleOnlySideCondition")
drive_path_chooser.onFalse = drive_path_far
drive_path_chooser.onTrue = drive_path_close
drive_path_chooser.condition = lambda: game_data.get_scale_side(
) == game_data.Side.RIGHT
drive_path_flip_chooser = ConditionalCommand(
"FlipScaleOnlySideCondition")
drive_path_flip_chooser.onFalse = drive_path_close_flipped
drive_path_flip_chooser.onTrue = drive_path_far_flipped
drive_path_flip_chooser.condition = lambda: game_data.get_scale_side(
) == game_data.Side.RIGHT
drive_flip_chooser = ConditionalCommand("DriveFlipCondition")
drive_flip_chooser.condition = lambda: game_data.get_robot_side(
) == game_data.Side.RIGHT
drive_flip_chooser.onTrue = drive_path_chooser
drive_flip_chooser.onFalse = drive_path_flip_chooser
elevator_condition = WaitUntilConditionCommand(
lambda: pose_estimator.get_current_pose().x > 16)
elevator_to_height = MoveElevatorCommand(elevator,
ElevatorPositions.SWITCH)
elev_group = CommandGroup()
elev_group.addSequential(elevator_condition)
if not hal.isSimulation():
elev_group.addSequential(elevator_to_height)
else:
elev_group.addSequential(PrintCommand("Elevator moving"))
intake_out = MoveIntakeCommand(intake, ArmState.DOWN)
drop_cube = TimedRunIntakeCommand(intake,
time=0.5,
power=-intake.power)
drive_back = TimeDriveCommand(drive, time=0.2, power=-0.5)
spin_chooser = ConditionalCommand("SpinChooser")
spin_chooser.condition = lambda: game_data.get_scale_side(
) == game_data.Side.RIGHT
spin_chooser.onTrue = TurnToAngle(drive, 180, delta=False)
spin_chooser.onFalse = TurnToAngle(drive, -180, delta=False)
switch_path_close = [Vector2(20, -10), Vector2(19, -8)]
switch_path_far = [Vector2(20, -10), Vector2(18, 7)]
switch_path_chooser = ConditionalCommand("SwitchPathChooser")
switch_path_chooser.condition = lambda: game_data.get_own_switch_side(
) == game_data.get_robot_side()
switch_path_chooser.onTrue = PursuitDriveCommand(
drive, switch_path_close, cruise, acc)
switch_path_chooser.onFalse = PursuitDriveCommand(
drive, switch_path_far, cruise, acc)
self.addParallel(elev_group)
self.addSequential(drive_flip_chooser)
self.addSequential(intake_out)
self.addSequential(drop_cube)
self.addSequential(drive_back)
if not hal.isSimulation():
self.addParallel(MoveElevatorCommand(elevator, 0))
else:
self.addParallel(PrintCommand("Elevator moving"))
self.addSequential(spin_chooser)
self.addSequential(switch_path_chooser)
def isFinished(self):
return self.pp_controller.is_at_end(pose_estimator.get_current_pose(),
self.margin)
