def setup(self):
# empty waypoints are used for trajectories with only two points
self.start_poses = [
to_pose(-3.459, -1.7, math.pi),
to_pose(-8.163, -1.7, math.pi),
to_pose(-4.168, -1.7, math.pi),
]
self.end_poses = [
to_pose(-8.163, -1.7, math.pi),
to_pose(-4.168, -1.7, math.pi),
to_pose(-6.062 - 1, 0.248, -1.178),
]
self.waypoints = [
[
geometry.Translation2d(-7.077, -1.7),
geometry.Translation2d(-7.992, -1.7),
],
[],
[],
]
self.expected_balls = [3, 0, 2]
self.reversed = [False, True, False]
self.trajectory_config = trajectory.TrajectoryConfig(maxVelocity=2.5,
maxAcceleration=1)
self.trajectory_max = 3
super().setup()
def get_test_trajectory(velocity=k_max_speed_meters_per_second):
start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
end_pose = geo.Pose2d(4, 0, geo.Rotation2d(0))
midpoints = [geo.Translation2d(1.5, 0.5), geo.Translation2d(2.5, -0.5)]
test_trajectory = wpimath.trajectory.TrajectoryGenerator.generateTrajectory(
start_pose, midpoints, end_pose, make_config(velocity))
return test_trajectory
def setup(self):
# TODO add correct headings
# empty waypoints are used for trajectories with only two points
self.start_poses = [
to_pose(-3.459, -1.7, math.pi),
to_pose(-6.165, 0.582, 0),
to_pose(-4.698, 2.359, 0),
]
self.end_poses = [
to_pose(-6.179, 2.981, 0),
to_pose(-4.698, 2.359, 0),
to_pose(-8.163, 0.705, 0),
]
self.waypoints = [
[
geometry.Translation2d(-4.971, 1.154),
geometry.Translation2d(-5.616, 1.978),
],
[],
[geometry.Translation2d(-5.668, 0.988)],
]
self.trajectory_config = trajectory.TrajectoryConfig(maxVelocity=1.5,
maxAcceleration=1)
self.expected_balls = [5, 0, 3]
self.reversed = [False, True, False]
self.trajectory_max = 3
super().setup()
def setup(self):
self.start_poses = [to_pose(-3.459, -1.7, math.pi)]
self.end_poses = [to_pose(-8.163, -1.7, math.pi)]
self.waypoints = [[
geometry.Translation2d(-7.077, -1.7),
geometry.Translation2d(-7.992, -1.7)
]]
self.expected_balls = [3]
self.reversed = [False]
self.trajectory_config = trajectory.TrajectoryConfig(maxVelocity=2.5,
maxAcceleration=1)
self.trajectory_max = 1
super().setup()
def setup(self):
self.start_poses = [to_pose(0, 0, math.pi)]
self.end_poses = [to_pose(-2, 0, math.pi)]
self.waypoints = [[geometry.Translation2d(-1, math.pi)]]
self.trajectory_config = trajectory.TrajectoryConfig(maxVelocity=1,
maxAcceleration=1)
self.expected_balls = [0]
self.reversed = [False]
self.trajectory_max = 1
super().setup()
class ShootMoveShootBase(AutonomousStateMachine):
shooter_controller: ShooterController
chassis: Chassis
indexer: Indexer
shooter: Shooter
# has_zeroed: bool
TARGET_POSITION = geometry.Translation2d(0, 0)
def __init__(self) -> None:
super().__init__()
self.controller = controller.RamseteController()
tolerance = to_pose(0.1, 0.1, math.pi / 18)
self.controller.setTolerance(tolerance)
self.trajectory_config = trajectory.TrajectoryConfig(maxVelocity=1,
maxAcceleration=1)
self.gen = trajectory.TrajectoryGenerator()
self.trajectory_num = 0
def setup(self):
self.trajectory_config.setKinematics(self.chassis.kinematics)
self.trajectory_config.addConstraint(
constraint.DifferentialDriveVoltageConstraint(
self.chassis.ff_calculator, self.chassis.kinematics, 10))
self.end_ranges = []
self.paths = []
for i in range(len(self.start_poses)):
self.trajectory_config.setReversed(self.reversed[i])
path = self.gen.generateTrajectory(
self.start_poses[i],
self.waypoints[i],
self.end_poses[i],
self.trajectory_config,
)
self.paths.append(path)
self.end_ranges.append((self.TARGET_POSITION -
self.end_poses[i].translation()).norm())
self.path = self.paths[0]
def on_enable(self) -> None:
self.chassis.reset_odometry(self.start_poses[0])
self.indexer.lower_intake()
self.indexer.enable_intaking()
self.trajectory_num = 0
# self.has_zeroed = True
super().on_enable()
@state(first=True)
def shoot(self, initial_call, state_tm) -> None:
"""
Shoot all balls that we currently have
"""
if initial_call:
if self.trajectory_num == 0:
self.shooter.set_range(3)
self.shooter_controller.fired_count = 0
self.shooter_controller.engage()
self.shooter_controller.fire_input()
if self.has_fired_balls():
if self.trajectory_num >= len(self.paths):
self.done()
else:
self.next_state("move")
@state
def move(self, initial_call, state_tm) -> None:
"""
Follow the trajectory defined by our waypoints
"""
if initial_call:
self.path = self.paths[self.trajectory_num]
self.shooter.set_range(self.end_ranges[self.trajectory_num])
if (state_tm > self.path.totalTime()
# or self.has_collected_balls()
):
# this needs to be overidden in the subclasses
print(f"Calculated path time: {self.path.totalTime()}")
self.chassis.drive(0, 0)
self.next_state("shoot")
self.trajectory_num += 1
return
pos = self.chassis.get_pose()
goal = self.path.sample(state_tm)
speeds = self.controller.calculate(pos, goal)
self.chassis.drive(speeds.vx, speeds.omega)
def has_collected_balls(self) -> bool:
"""
Has the robot collected all the balls for the current trajectory?
"""
ball_target = self.expected_balls[self.trajectory_num]
if ball_target == 0:
return False
if self.indexer.balls_loaded() >= ball_target:
return True
else:
return False
def has_fired_balls(self) -> bool:
balls_to_fire = self.expected_balls[self.trajectory_num - 1]
if self.trajectory_num == 0:
balls_to_fire = 3
# print(f"Expecting to fire {balls_to_fire} balls")
return self.shooter_controller.fired_count >= balls_to_fire
def __init__(self, physics_controller: PhysicsInterface):
self.physics_controller = physics_controller # mandatory
self.counter = 0
self.x, self.y = 0, 0
self.obstacles = 'slalom' # initialize only, this is read from the dash periodically
self.hood_position = 30
# -------- INITIALIZE HARDWARE ---------------
if self.sparkmax: # use the sparkmax drivetrain objects, access their position and velocity attributes
self.l_spark = simlib.SimDeviceSim('SPARK MAX [3]')
self.r_spark = simlib.SimDeviceSim('SPARK MAX [1]')
print(
f'** SparkMax allows access to: **\n{self.r_spark.enumerateValues()} '
)
self.l_spark_position = self.l_spark.getDouble('Position')
self.r_spark_position = self.r_spark.getDouble('Position')
self.l_spark_velocity = self.l_spark.getDouble('Velocity')
self.r_spark_velocity = self.r_spark.getDouble('Velocity')
self.l_spark_output = self.l_spark.getDouble('Applied Output')
self.r_spark_output = self.r_spark.getDouble('Applied Output')
else: # use the generic wpilib motors and encoders
self.l_motor = simlib.PWMSim(1)
self.r_motor = simlib.PWMSim(3)
self.l_encoder = simlib.EncoderSim.createForChannel(0)
self.r_encoder = simlib.EncoderSim.createForChannel(2)
# update units from drive constants
self.l_encoder.setDistancePerPulse(
drive_constants.k_encoder_distance_per_pulse_m)
self.r_encoder.setDistancePerPulse(
drive_constants.k_encoder_distance_per_pulse_m)
# hood IO
self.hood_encoder = simlib.EncoderSim.createForChannel(4)
self.hood_encoder.setDistancePerPulse(1 / 1024)
self.limit_low = simlib.DIOSim(6)
self.limit_hi = simlib.DIOSim(7)
self.hood_motor = simlib.PWMSim(5)
self.shooter_feed = simlib.PWMSim(6)
#self.shooter_hood = simlib.PWMSim(5)
# NavX (SPI interface) - no idea why the "4" is there, seems to be the default name generated by the navx code
self.navx = simlib.SimDeviceSim("navX-Sensor[4]")
self.navx_yaw = self.navx.getDouble("Yaw")
# set up two simulated encoders to see how they effect the robot
self.l_distance, self.r_distance = 0, 0
self.l_distance_old, self.r_distance_old = 0, 0 # used for calculating encoder rates
# -------- INITIALIZE FIELD SETTINGS ---------------
# keep us on the field - set x,y limits for driving
field_size = 'competition'
if field_size == 'competition':
self.x_limit, self.y_limit = 15.97 + 0.5, 8.21 + 0.5 # meters for a 52.4x26.9' field PLUS 0.5 meter border
self.x, self.y = 4, 8.21 / 2
else: # at home autonomous challenge for 2021
self.x_limit, self.y_limit = 9.114, 4.572 # meters for a 30x15' DO NOT INCLUDE BORDER. To that with the imgui.ini.
# Set our position on the field
position = 'slalom' # set this to put the robot on the field
if position == 'slalom':
self.x, self.y = 1.1, 0.68
elif position == 'barrel':
self.x, self.y = 1.1, 2.3 - .25
elif position == 'bounce':
self.x, self.y = 1.1, 2.5 - .25
else:
self.x, self.y = 0, 0
self.field_offset = geo.Transform2d(geo.Translation2d(0.25, 0.25),
geo.Rotation2d(0))
# is all this necessary?
initial_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
final_pose = geo.Pose2d(self.x, self.y, geo.Rotation2d(0))
initial_position_transform = geo.Transform2d(initial_pose, final_pose)
self.physics_controller.move_robot(initial_position_transform)
# -------- INITIALIZE ROBOT MODEL ---------------
# Change these parameters to fit your robot!
practice_weight = 20 # lightweight practice bot, WCD
bumper_width = 3.25 * units.inch
""" # this is the theory version - when you don't have a drivetrain to emulate
self.drivetrain = tankmodel.TankModel.theory(
motor_cfgs.MOTOR_CFG_FALCON_500, # motor configuration
practice_weight * units.lbs, # robot mass
drive_constants.k_gear_ratio, # drivetrain gear ratio
2, # motors per side
18/2 * units.inch, # robot wheelbase, 27 in = 0.69 meters, but in half for WCD!
27 * units.inch + bumper_width * 2, # robot width
29 * units.inch + bumper_width * 2, # robot length
drive_constants.k_wheel_diameter_in * units.inch, # wheel diameter
)"""
# characterized values of the drivetrain - 2021 0306 CJH
self.drivetrain = tankmodel.TankModel(
motor_config=motor_cfgs.MOTOR_CFG_775PRO,
robot_mass=practice_weight * units.lbs,
x_wheelbase=drive_constants.k_robot_wheelbase *
units.meters, # need to cut this in half for WCD
robot_width=drive_constants.k_robot_width *
units.meters, # measured wheel to wheel
robot_length=drive_constants.k_robot_length *
units.meters, # measured across entire frame
l_kv=drive_constants.kv_volt_seconds_per_meter * units.volts *
units.seconds / units.meter,
l_ka=drive_constants.ka_volt_seconds_squared_per_meter *
units.volts * units.seconds * units.seconds / units.meter,
l_vi=drive_constants.ks_volts * units.volts,
r_kv=drive_constants.kv_volt_seconds_per_meter * units.volts *
units.seconds / units.meter,
r_ka=drive_constants.ka_volt_seconds_squared_per_meter *
units.volts * units.seconds * units.seconds / units.meter,
r_vi=drive_constants.ks_volts * units.volts,
timestep=5 * units.ms)
def update_sim(self, now: float, tm_diff: float) -> None:
"""
Called when the simulation parameters for the program need to be
updated.
:param now: The current time as a float
:param tm_diff: The amount of time that has passed since the last
time that this function was called
"""
# Simulate the drivetrain and update encoders
if self.sparkmax:
#self.l_spark_output.set(0.333) # need the -1 to 1 motor value, and I have to fake it
#self.r_spark_output.set(0.999)
l_motor = self.l_spark_output.get()
r_motor = self.r_spark_output.get()
else:
l_motor = self.l_motor.getSpeed(
) # these are PWM speeds of -1 to 1
r_motor = self.r_motor.getSpeed(
) # going forward, left should be positive and right is negative
# get a new location based on motor movement
transform = self.drivetrain.calculate(l_motor, r_motor, tm_diff)
pose = self.physics_controller.move_robot(
transform) # includes inertia
# keep us on the simulated field - reverse the transform if we try to go out of bounds
sim_padding = 0.25 # 0.25 # let us go a bit outside but not get lost
bad_move = False # see if we are out of bounds or hitting a barrier
if (pose.translation().x < -sim_padding
or pose.translation().x > self.x_limit + sim_padding
or pose.translation().y < -sim_padding
or pose.translation().y > self.y_limit + sim_padding):
bad_move = True
# allowing the user to change the obstacles
pylon_points = []
if 'slalom' in self.obstacles:
pylon_points = drive_constants.slalom_points
if 'barrel' in self.obstacles:
pylon_points = drive_constants.barrel_points
if 'bounce' in self.obstacles:
pylon_points = drive_constants.bounce_points
if any([
drive_constants.distance(pose, i) <
drive_constants.k_track_width_meters / 2 for i in pylon_points
]):
bad_move = True
# this resets the move if we are hitting a barrier
# ToDo: stop the motion as well. Perhaps in addition to moving back we should redo transform w/ no motor speed
if bad_move:
curr_x, curr_y = transform.translation().x, transform.translation(
).y
# in 2021 library they added an inverse() to transforms so this could all be one line
new_transform = geo.Transform2d(geo.Translation2d(-curr_x, curr_y),
transform.rotation())
pose = self.physics_controller.move_robot(new_transform)
# Update encoders - need use the pose so it updates properly for coasting to a stop
# ToDo: get the actual values, probably from wheelspeeds?
# have to only work with deltas otherwise we can't reset the encoder from the real code
# damn it, the SparkMax and the standard encoders have different calls
if self.sparkmax:
self.l_spark_position.set(
self.l_spark_position.get() +
(self.drivetrain.l_position - self.l_distance_old) * 0.3105)
self.r_spark_position.set(
self.r_spark_position.get() +
(self.drivetrain.r_position - self.r_distance_old) * 0.3105)
self.l_spark_velocity.set(
0.31 * (self.drivetrain.l_position - self.l_distance_old) /
tm_diff)
self.r_spark_velocity.set(
0.31 * (self.drivetrain.r_position - self.r_distance_old) /
tm_diff)
else:
self.l_encoder.setDistance(
self.l_encoder.getDistance() +
(self.drivetrain.l_position - self.l_distance_old) * 0.3105)
self.r_encoder.setDistance(
self.r_encoder.getDistance() -
(self.drivetrain.r_position - self.r_distance_old) *
0.3105) # negative going forward
self.l_encoder.setRate(
0.31 * (self.drivetrain.l_position - self.l_distance_old) /
tm_diff)
self.r_encoder.setRate(
-0.31 * (self.drivetrain.r_position - self.r_distance_old) /
tm_diff) # needs to be negitive going fwd
self.l_distance_old = self.drivetrain.l_position
self.r_distance_old = self.drivetrain.r_position
self.x, self.y = pose.translation().x, pose.translation().y
# Update the navx gyro simulation
# -> FRC gyros like NavX are positive clockwise, but the returned pose is positive counter-clockwise
self.navx_yaw.set(-pose.rotation().degrees())
# ---------------- HOOD ELEVATOR UPDATES ---------------------------
# update 'position' (use tm_diff so the rate is constant) - this is for simulating an elevator, arm etc w/ limit switches
self.hood_position += self.hood_motor.getSpeed(
) * tm_diff * 30 - 0.05 # inserting gravity!
# update limit switches based on position
if self.hood_position <= 30:
switch1 = True
switch2 = False
if self.hood_position < 29.5: # don't let gravity sag too much
self.hood_position = 29.5
elif self.hood_position > 50:
switch1 = False
switch2 = True
else:
switch1 = False
switch2 = False
# set values here - nice way to emulate the robot's state
self.limit_low.setValue(switch1)
self.limit_hi.setValue(switch2)
self.hood_encoder.setDistance(round(self.hood_position, 2))
self.counter += 1
if self.counter % 5 == 0:
SmartDashboard.putNumber('field_x', round(self.x, 2))
SmartDashboard.putNumber('field_y', round(self.y, 2))
SmartDashboard.putNumber('hood', round(self.hood_position, 1))
if self.counter % 50 == 0:
self.obstacles = SmartDashboard.getData(
'obstacles').getSelected() # read the obstacles from the dash
def get_point_trajectory(velocity=k_max_speed_meters_per_second):
start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
end_pose = geo.Pose2d(0, 1.76, geo.Rotation2d(0))
midpoints = [
geo.Translation2d(0.32, 0.01),
geo.Translation2d(1.25, 0.47),
geo.Translation2d(2.16, 1.71),
geo.Translation2d(4.74, 1.91),
geo.Translation2d(5.74, 1.06),
geo.Translation2d(6.06, 0.31),
geo.Translation2d(7.41, 0.21),
geo.Translation2d(7.52, 1.50),
geo.Translation2d(6.97, 1.82),
geo.Translation2d(6.10, 1.48),
geo.Translation2d(5.87, 0.84),
geo.Translation2d(5.30, 0.17),
geo.Translation2d(3.77, -0.05),
geo.Translation2d(1.81, 0.33),
geo.Translation2d(1.12, 1.36),
geo.Translation2d(0.22, 1.69),
geo.Translation2d(-0.15, 1.76)
]
slalom_point_trajectory = wpimath.trajectory.TrajectoryGenerator.generateTrajectory(
start_pose, midpoints, end_pose, make_config(velocity))
return slalom_point_trajectory