def end(self, message='Ended'):
"""Called once after isFinished returns true"""
end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(f"** {message} {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **")
SmartDashboard.putString("alert", f"** Ended {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **")
self.robot.drivetrain.stop()
self.write_telemetry = SmartDashboard.getBoolean("ramsete_write", self.write_telemetry)
if self.write_telemetry:
location = Path.cwd() if self.robot.isSimulation() else Path('/home/lvuser/py/') # it's not called robot on the robot
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
file_name = timestamp + '_'+ self.course + f'_kpv_{self.kp_vel:2.1f}'.replace('.','p') + f'_vel_{str(self.velocity).replace(".","p")}' +'.pkl'
pickle_file = location / 'sim' / 'data' / file_name
with open(pickle_file.absolute(), 'wb') as fp:
out_dict = {'TIMESTAMP':timestamp,'DATA':self.telemetry, 'COURSE':self.course, 'VELOCITY':self.velocity,
'KP_VEL':self.kp_vel, 'KD_VEL':self.kd_vel, 'BETA':self.beta, 'ZETA':self.zeta}
pickle.dump(out_dict, fp)
print(f'*** Wrote ramsete command data to {file_name} ***')
else:
print(f'*** Skipping saving of telemetry to disk ***')
def end(self, message='Ended'):
"""Called once after isFinished returns true"""
end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
elapsed_time = end_time - self.start_time + self.time_offset
print(
f"** {message} {self.getName()} at {end_time} s after {round(elapsed_time,1)} s **"
)
SmartDashboard.putString(
"alert",
f"** Ended {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **"
)
self.robot.drivetrain.stop()
AutonomousRamseteSimple.time_offset += end_time - self.start_time # maintain running time in string of commands BUT don't reassign or it makes an instance
# ToDo: update this so it only happens at the end of a list of commands. May need a new variable.
self.write_telemetry = SmartDashboard.getBoolean(
"ramsete_write", self.write_telemetry)
if self.write_telemetry:
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
file_name = timestamp + '_' + self.course + f'_kpv_{self.kp_vel:2.1f}'.replace(
'.',
'p') + f'_vel_{str(self.velocity).replace(".","p")}' + '.pkl'
pickle_file = Path.cwd() / 'sim' / 'data' / file_name
with open(pickle_file.absolute(), 'wb') as fp:
out_dict = {
'TIMESTAMP': timestamp,
'DATA': self.telemetry,
'COURSE': self.course,
'VELOCITY': self.velocity,
'KP_VEL': self.kp_vel,
'KD_VEL': self.kd_vel,
'BETA': self.beta,
'ZETA': self.zeta
}
pickle.dump(out_dict, fp)
print(f'*** Wrote ramsete command data to {file_name} ***')
else:
print(
f'*** Skipping saving of {len(self.telemetry)} telemetry points to disk ***'
)
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 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 initializeBoolean(label: str, defaultValue: bool) -> bool:
value: bool = SmartDashboard.getBoolean(label, defaultValue)
SmartDashboard.putBoolean(label, value)
return value
def teleopPeriodic(self):
try:
if self.robot_mode == RobotModes.MANUAL_DRIVE:
if robotmap.drivetrain_enabled:
if self.subsystem_drivetrain.drive_mode == drivetrain.DrivetrainMode.MANUAL_DRIVE_CURVATURE:
self.subsystem_drivetrain.curvature_drive(
self.oi.controller_driver.getY()**3,
-self.oi.controller_driver.getX()**3)
if self.subsystem_drivetrain.drive_mode == drivetrain.DrivetrainMode.MANUAL_DRIVE_ARCADE:
self.subsystem_drivetrain.arcade_drive(
self.oi.controller_driver.getY()**3,
-self.oi.controller_driver.getX()**3)
if self.navx.isConnected(
) and self.oi.check_drivetrain_straight(
self.oi.controller_driver.getX(),
self.oi.controller_driver.getY()):
self.subsystem_drivetrain.drive_to_angle(
self.oi.controller_driver.getY()**3)
elif self.subsystem_drivetrain.drive_mode == drivetrain.DrivetrainMode.ASSIST_DRIVE_ARCADE:
self.subsystem_drivetrain.drive_mode = drivetrain.DrivetrainMode.MANUAL_DRIVE_CURVATURE
if robotmap.shooter_enabled:
target_shooter_speed = dash.getNumber(
"target shooter velocity", 0)
shooter_enabled = dash.getBoolean("shooter enabled", False)
if shooter_enabled:
self.subsystem_shooter.enable_at_speed(
self.oi.controller_driver.getY(
wpilib.XboxController.Hand.kLeft))
else:
self.subsystem_shooter.disable()
dash.putNumber("shooter speed error",
self.subsystem_shooter.get_error())
if robotmap.loader_enabled:
if self.oi.controller_driver.getPOV() == 0: # up on dpad
self.subsystem_loader.position = LoaderPosition.UP
if self.oi.controller_driver.getPOV(
) == 180: # down on dpad
self.subsystem_loader.position = LoaderPosition.DOWN
if self.oi.controller_driver.getXButtonPressed():
self.subsystem_loader.enabled = not self.subsystem_loader.enabled
if self.oi.controller_driver.getBButtonPressed():
self.robot_mode = RobotModes.AUTO_TARGETTING
elif self.robot_mode == RobotModes.AUTO_TARGETTING:
# this is the control loop that will deal with automatically shooting into a target
# inputs: distance from target, horizontal offset from target
# outputs: speed of shooter, turning speed of robot, linear speed of robot
# the speed of the shooter is controlled by the distance
# the linear speed of the robot is controlled by the distance
# the turning speed of the robot is controlled by the horizontal offset of the robot
if not robotmap.limelight_enabled:
# we can't do anything in this case.
self.robot_mode = RobotModes.MANUAL_DRIVE
distance_to_target = self.limelight.get_distance_trig()
x_offset = self.limelight.get_horizontal_angle_offset()
drivetrain_on_target = True
if robotmap.drivetrain_enabled:
# use proportional techniques to make the robot drive into a good position to shoot
angular_error = 0
distance_error = 0
if abs(x_offset
) > robotmap.auto_targetting_alignment_tolerance:
drivetrain_on_target = False
angular_error = 0 - x_offset
if distance_to_target > robotmap.auto_targetting_max_distance:
drivetrain_on_target = False
distance_error = distance_to_target - robotmap.auto_targetting_max_distance
if distance_to_target < robotmap.auto_targetting_min_distance:
drivetrain_on_target = False
distance_to_target = distance_to_target - robotmap.auto_targetting_min_distance
drivetrain_rotation = drivetrain.DrivetrainConstants.K_PROPORTIONAL_TURNING * angular_error
drivetrain_power = drivetrain.DrivetrainConstants.K_PROPORTIONAL_DRIVING * distance_error
self.subsystem_drivetrain.arcade_drive(
drivetrain_power, drivetrain_rotation)
if robotmap.shooter_enabled:
# set the shooter to an appropriate speed if the drivetrain is on target
if drivetrain_on_target:
self.subsystem_shooter.enabled = True
self.subsystem_shooter.shoot_from_distance(
distance_to_target, angle=45)
else:
self.subsystem_shooter.enabled = False
if robotmap.loader_enabled:
# TODO: we don't have a working loader right now so (logically) there's no code here
pass
if self.oi.controller_driver.getBButtonPressed():
self.robot_mode = RobotModes.MANUAL_DRIVE
if robotmap.limelight_enabled:
dash.putString("distance to target",
self.limelight.get_distance_trig())
dash.putString("horizontal angle to target",
self.limelight.get_horizontal_angle_offset())
dash.putString("vertical angle to target",
self.limelight.get_vertical_angle_offset())
dash.putNumber("PID Output",
self.subsystem_drivetrain.turn_pid.get())
except:
self.onException()
