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
