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
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 get_loop_trajectory(velocity=k_max_speed_meters_per_second):
pose_points = [
geo.Pose2d(0.09, 0.02, geo.Rotation2d(0.00)),
geo.Pose2d(6.17, 0.16, geo.Rotation2d(0.00)),
geo.Pose2d(7.62, 0.87, geo.Rotation2d(1.60)),
geo.Pose2d(6.06, 1.75, geo.Rotation2d(3.14)),
geo.Pose2d(0.21, 1.70, geo.Rotation2d(3.14))
]
loop_trajectory = wpimath.trajectory.TrajectoryGenerator.generateTrajectory(
pose_points, make_config(velocity))
return loop_trajectory
def testInit(self):
self.turret.index_found = False
self.turret.on_enable()
self.track_target = False
self.run_indexer = False
self.chassis.reset_odometry(
geometry.Pose2d(1, -1, geometry.Rotation2d(math.pi)))
def teleopInit(self) -> None:
"""Initialise things for driver control."""
if not self.has_zeroed:
self.chassis.reset_odometry(
geometry.Pose2d(-3, 0, geometry.Rotation2d(math.pi)))
self.has_zeroed = True
self.chassis.disable_closed_loop()
self.chassis.disable_brake_mode()
self.indexer.shimmying = True
self.indexer.auto_retract = True
def handle_chassis_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1)
vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1)
vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1)
self.chassis.drive(vx, vz)
if joystick.getRawButtonPressed(3):
self.chassis.reset_odometry(
geometry.Pose2d(-3, 0, geometry.Rotation2d(
math.pi))) # the starting position on the field
self.target_estimator.reset()
def initialize(self):
"""Called just before this Command runs the first time."""
self.previous_time = -1
self.init_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
self.robot.drivetrain.drive.feed(
) # this initialization is taking some time now
# update gains from dash if desired
if self.dash is True:
self.kp_vel = SmartDashboard.getNumber("ramsete_kpvel",
self.kp_vel)
self.beta = SmartDashboard.getNumber("ramsete_B", self.beta)
self.zeta = SmartDashboard.getNumber("ramsete_Z", self.zeta)
self.write_telemetry = SmartDashboard.getBoolean(
"ramsete_write", self.write_telemetry)
# create controllers
self.follower = wpilib.controller.RamseteController(
self.beta, self.zeta)
self.left_controller = wpilib.controller.PIDController(
self.kp_vel, 0, self.kd_vel)
self.right_controller = wpilib.controller.PIDController(
self.kp_vel, 0, self.kd_vel)
self.left_controller.reset()
self.right_controller.reset()
#ToDo - make this selectable, probably from the dash, add the other trajectories (done)
trajectory_choice = self.path # path is passed in on constructor this time
self.velocity = float(self.robot.oi.velocity_chooser.getSelected()
) # get the velocity from the GUI
self.trajectory = drive_constants.generate_trajectory(
trajectory_choice,
self.velocity,
simulation=self.robot.isSimulation(),
save=False)
self.course = trajectory_choice
# Note - we are setting to pose to have the robot physically in the start position - usually absolute matters
if self.relative: # figure out if we want to start where we are or where the trajectory says we should be
self.start_pose = geo.Pose2d(
self.trajectory.sample(0).pose.X(),
self.trajectory.sample(0).pose.Y(),
self.robot.drivetrain.get_rotation2d())
else:
#self.start_pose = self.robot.drivetrain.get_pose()
field_x = SmartDashboard.getNumber(
'field_x',
self.trajectory.sample(0).pose.X())
field_y = SmartDashboard.getNumber(
'field_y',
self.trajectory.sample(0).pose.Y())
self.start_pose = geo.Pose2d(
field_x, field_y, self.robot.drivetrain.get_rotation2d())
self.robot.drivetrain.reset_odometry(self.start_pose)
self.robot.drivetrain.drive.feed(
) # this initialization is taking some time now
initial_state = self.trajectory.sample(0)
# these are all meters in 2021
self.previous_speeds = self.kinematics.toWheelSpeeds(
wpimath.kinematics.ChassisSpeeds(
initial_state.velocity, 0,
initial_state.curvature * initial_state.velocity))
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.getName()} on {self.course} with load time {self.start_time-self.init_time:2.2f}s"
f" (b={self.beta}, z={self.zeta}, kp_vel={self.kp_vel}) at {self.start_time} s **"
)
SmartDashboard.putString(
"alert", f"** Started {self.getName()} at {self.start_time} s **")
if self.reset_telemetry: # don't reset telemetry if we want to chain trajectories together
print(
'Time\tTrVel\tTrRot\tlspd\trspd\tram an\tram vx\tram vy\tlffw\trffw\tlpid\trpid'
)
self.telemetry.clear(
) # Aha. important not to set it to a new value (=[]) , because then it makes an instance
AutonomousRamseteSimple.time_offset = 0 # seems like an easier way to do it, but man. Globals are always ugly.
else:
pass
def to_pose(x: float, y: float, heading: float) -> geometry.Pose2d:
"""
Convert inputs into a wpilib pose object
"""
rotation = geometry.Rotation2d(heading)
return geometry.Pose2d(x, y, rotation)
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 initialize(self):
"""Called just before this Command runs the first time."""
self.previous_time = -1
self.telemetry = []
self.init_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
self.robot.drivetrain.drive.feed() # this initialization is taking some time now
# update gains from dash if desired
if self.dash is True:
self.kp_vel = SmartDashboard.getNumber("ramsete_kpvel", self.kp_vel)
self.beta = SmartDashboard.getNumber("ramsete_B", self.beta)
self.zeta = SmartDashboard.getNumber("ramsete_Z", self.zeta)
self.write_telemetry = SmartDashboard.getBoolean("ramsete_write", self.write_telemetry)
# create controllers
self.follower = wpilib.controller.RamseteController(self.beta, self.zeta)
self.left_controller = wpilib.controller.PIDController(self.kp_vel, 0 , self.kd_vel)
self.right_controller = wpilib.controller.PIDController(self.kp_vel, 0 , self.kd_vel)
self.left_controller.reset()
self.right_controller.reset()
#ToDo - test for real robot -
trajectory_choice = self.robot.oi.path_chooser.getSelected() # get path from the GUI
self.velocity = float(self.robot.oi.velocity_chooser.getSelected()) # get the velocity from the GUI
if 'z_' not in trajectory_choice: # let me try a few of the other methods if the path starts with z_
self.trajectory = drive_constants.generate_trajectory(trajectory_choice, self.velocity, simulation=self.robot.isSimulation(), save=False)
self.course = trajectory_choice
if self.robot.isReal():
self.start_pose = geo.Pose2d(self.trajectory.sample(0).pose.X(), self.trajectory.sample(0).pose.Y(),
self.robot.drivetrain.get_rotation2d())
else:
field_x = SmartDashboard.getNumber('field_x', self.trajectory.sample(0).pose.X())
field_y = SmartDashboard.getNumber('field_y', self.trajectory.sample(0).pose.Y())
self.start_pose = geo.Pose2d(field_x, field_y, self.robot.drivetrain.get_rotation2d())
self.robot.drivetrain.drive.feed() # this initialization is taking some time now
# Note - we are setting to pose to have the robot physically in the start position - usually absolute matters
if 'slalom' in trajectory_choice:
pass
#self.start_pose = geo.Pose2d(1.3, 0.66, geo.Rotation2d(0))
elif 'barrel' in trajectory_choice:
pass
# self.start_pose = geo.Pose2d(1.3, 2.40, geo.Rotation2d(0)) # may want to rotate this
elif 'bounce' in trajectory_choice:
pass
# self.start_pose = geo.Pose2d(1.3, 2.62, geo.Rotation2d(0))
elif 'student' in trajectory_choice:
pass
# self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0)) # student should put barrel, slalom or bounce in name
elif 'loop' in trajectory_choice:
self.course = 'loop'
self.trajectory = drive_constants.get_loop_trajectory(self.velocity)
self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
elif 'poses' in trajectory_choice:
self.course = 'slalom_poses'
self.trajectory = drive_constants.get_pose_trajectory(self.velocity)
self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
elif 'points' in trajectory_choice:
self.course = 'slalom_points'
self.trajectory = drive_constants.get_point_trajectory(self.velocity)
self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
elif 'z_test' in trajectory_choice:
self.course = 'test'
self.trajectory = drive_constants.get_test_trajectory(self.velocity)
self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
else:
pass
#self.start_pose = geo.Pose2d(0, 0, geo.Rotation2d(0))
self.robot.drivetrain.reset_odometry(self.start_pose)
initial_state = self.trajectory.sample(0)
# these are all meters in 2021
self.previous_speeds = self.kinematics.toWheelSpeeds(wpimath.kinematics.ChassisSpeeds(
initial_state.velocity, 0, initial_state.curvature*initial_state.velocity))
self.start_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print("\n" + f"** Started {self.getName()} on {self.course} with load time {self.start_time-self.init_time:2.2f}s"
f" (b={self.beta}, z={self.zeta}, kp_vel={self.kp_vel}) at {self.start_time} s **")
SmartDashboard.putString("alert", f"** Started {self.getName()} at {self.start_time} s **")
print('Time\tTr Vel\tTr Rot\tlspd\trspd\tram ang\tram vx\tram vy\tlffw\trffw\tlpid\trpid')
def get_pose_trajectory(velocity=k_max_speed_meters_per_second):
pose_points = [
geo.Pose2d(0.32, 0.01, geo.Rotation2d(0.00)),
geo.Pose2d(1.25, 0.47, geo.Rotation2d(1.04)),
geo.Pose2d(2.16, 1.71, geo.Rotation2d(0.69)),
geo.Pose2d(4.74, 1.91, geo.Rotation2d(0.00)),
geo.Pose2d(5.74, 1.06, geo.Rotation2d(-1.21)),
geo.Pose2d(6.06, 0.31, geo.Rotation2d(0.00)),
geo.Pose2d(7.41, 0.21, geo.Rotation2d(0.64)),
geo.Pose2d(7.52, 1.50, geo.Rotation2d(2.30)),
geo.Pose2d(6.97, 1.82, geo.Rotation2d(3.12)),
geo.Pose2d(6.10, 1.48, geo.Rotation2d(-2.05)),
geo.Pose2d(5.87, 0.84, geo.Rotation2d(-2.04)),
geo.Pose2d(5.30, 0.17, geo.Rotation2d(-2.55)),
geo.Pose2d(3.77, -0.05, geo.Rotation2d(3.12)),
geo.Pose2d(1.81, 0.33, geo.Rotation2d(2.38)),
geo.Pose2d(1.12, 1.36, geo.Rotation2d(2.30)),
geo.Pose2d(0.22, 1.69, geo.Rotation2d(-3.14)),
geo.Pose2d(-0.15, 1.76, geo.Rotation2d(3.14))
]
pose_trajectory = wpimath.trajectory.TrajectoryGenerator.generateTrajectory(
pose_points, make_config(velocity))
return pose_trajectory