def teleopPeriodic(self):
a = 0
while self.isOperatorControl() and self.isEnabled():
self.controls.periodic()
self.limelight.mutiPipeline()
self.intake.periodic()
self.lift.periodic()
b = self.lift.getEncoderVal()
if a < b:
a = b
SmartDashboard.putNumber("pipeline id",
self.limelight.getPipeline())
SmartDashboard.putBoolean("inake hasCube", self.intake.hasCube())
#drive.periodic()
#SmartDashboard.putNumber("liftRTS hz", self.liftRTS.getHz())
SmartDashboard.putNumber("0", self.a0.getAverageVoltage())
SmartDashboard.putNumber("1", self.a1.getAverageVoltage())
SmartDashboard.putNumber("2", self.a2.getAverageVoltage())
SmartDashboard.putNumber("3", self.a3.getAverageVoltage())
SmartDashboard.putNumber("Lift", a)
SmartDashboard.putNumber("Drive Right Dist",
self.drive.getRightDistance())
SmartDashboard.putNumber("Drive Left Dist",
self.drive.getLeftDistance())
SmartDashboard.putNumber("pitch", self.navx.getPitch())
def periodic(self) -> None:
"""Perform necessary periodic updates"""
self.counter += 1
if self.counter % 5 == 0:
# pass
# ten per second updates
SmartDashboard.putNumber('elevation',
self.hood_encoder.getDistance())
SmartDashboard.putNumber('rpm',
self.flywheel_encoder.getVelocity())
watch_axis = False
if watch_axis:
self.hood_scale = 0.2
self.hood_offset = 0.0
power = self.hood_scale * (self.robot.oi.stick.getRawAxis(2) -
0.5) + self.hood_offset
self.robot.shooter.change_elevation(power)
maintain_elevation = True
if maintain_elevation:
self.error = self.get_angle() - self.hood_setpoint
pid_out = self.hood_controller.calculate(self.error)
output = 0.03 + pid_out
SmartDashboard.putNumber('El PID', pid_out)
self.change_elevation(output)
if self.counter % 50 == 0:
SmartDashboard.putBoolean("hood_low", self.limit_low.get())
SmartDashboard.putBoolean("hood_high", self.limit_high.get())
def dashboardPeriodic(self):
#commented out some values. DON'T DELETE THESE VALUES
#SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition())
SmartDashboard.putNumber("Wrist Angle", self.getAngle())
#SmartDashboard.putNumber("Output", self.out)
#self.F = SmartDashboard.getNumber("F Gain", 0)
#self.wristUpVolts = SmartDashboard.getNumber("Wrist Up Volts", 0)
#self.wristDownVolts = SmartDashboard.getNumber("Wrist Down Volts", 0)
SmartDashboard.putBoolean("Limit Switch",
self.wrist.isFwdLimitSwitchClosed())
SmartDashboard.putBoolean("Limit Switch Reverse",
self.wrist.isRevLimitSwitchClosed())
def robotPeriodic(self):
SmartDashboard.putBoolean('Fwd closed',
self.motor.isFwdLimitSwitchClosed())
SmartDashboard.putBoolean('Rev closed',
self.motor.isRevLimitSwitchClosed())
SmartDashboard.putNumber('Motor position',
self.motor.getQuadraturePosition())
SmartDashboard.putNumber("Motor power",
self.motor.getMotorOutputPercent())
SmartDashboard.putNumber("Motor volts",
self.motor.getMotorOutputVoltage())
SmartDashboard.putNumber("Motor current",
self.motor.getOutputCurrent())
def robotPeriodic(self):
self.limelight.readLimelightData()
SmartDashboard.putBoolean("teleop", self.teleop)
if (self.dashboard): self.updateDashboardPeriodic()
'''currAlign = self.joystick0.getRawButton(map.autoAlign)
if currAlign and currAlign != self.lastAlign:
AutoAlign(self.limelight.getPath()[0],self.limelight.getPath()[1],self.limelight.getPath()[2],self.limelight.getPath()[3]).start()
if not currAlign and currAlign != self.lastAlign:
AutoAlign(self.limelight.getPath()[0],self.limelight.getPath()[1],self.limelight.getPath()[2],self.limelight.getPath()[3]).cancel()
self.SetSpeedDT.start()
self.lastAlign = currAlign'''
'''currStraightAlign = self.joystick0.getRawButton(map.straightAlign)
def updateDashboard(self):
SmartDashboard.putBoolean("Portcullis Upper Right Limit Switch",
self.rightAtTop())
SmartDashboard.putBoolean("Portcullis Upper Left Limit Switch",
self.leftAtTop())
SmartDashboard.putBoolean("Portcullis Lower Right Limit Switch",
self.rightAtBottom())
SmartDashboard.putBoolean("Portcullis Lower Left Limit Switch",
self.leftAtBottom())
def iterate(self, robot_mode, simulation, pilot_stick, copilot_stick):
self.robot_mode = robot_mode
lift_position = self.lift_talon.getAnalogInRaw()
if lift_position > self.lift_stow_position + 5:
SmartDashboard.putBoolean("Creep", True)
else:
SmartDashboard.putBoolean("Creep", False)
if robot_mode == RobotMode.TEST:
if self.test_lift_pneumatic.getSelected() == 1:
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
else:
self.lift_pneumatic_extend.set(True)
self.lift_pneumatic_retract.set(False)
#need to check these separately so we don't disable the mechanism completely if we end up one tick outside our allowable range
if lift_position > self.min_lift_position or lift_position < self.max_lift_position:
self.lift_talon.set(
ControlMode.PercentOutput,
-1.0 * pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS))
elif lift_position < self.min_lift_position:
#allow upward motion
self.lift_talon.set(
ControlMode.PercentOutput,
max(
-1.0 *
pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0))
elif lift_position > self.max_lift_position:
#allow downward motion
self.lift_talon.set(
ControlMode.PercentOutput,
min(
-1.0 *
pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0))
else:
self.lift_talon.set(ControlMode.PercentOutput, 0.0)
else:
self.iterate_state_machine(pilot_stick, copilot_stick)
SmartDashboard.putString("Lift State", self.current_state.name)
SmartDashboard.putString("Lift Preset", self.current_lift_preset.name)
SmartDashboard.putNumber("Lift Setpoint", self.lift_setpoint)
SmartDashboard.putNumber("Lift Position", lift_position)
def dashboardPeriodic(self):
#self.wristUp = self.getNumber("WristUpSpeed" , 0.5)
#self.wristDown = self.getNumber("WristDownSpeed" , 0.2)
#self.wristUpVolts = self.getNumber("WristUpVoltage" , 5)
#self.wristDownVolts = self.getNumber("WristDownVoltage" , 2)
#self.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain)
#self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity)
SmartDashboard.putNumber("Wrist Position",
self.wrist.getQuadraturePosition())
SmartDashboard.putData("PID Controller", self.cargoController)
SmartDashboard.putNumber("Wrist Angle", self.getAngle())
SmartDashboard.putNumber("Wrist Power", self.input)
SmartDashboard.putNumber("Wrist Power2", self.input2)
SmartDashboard.putString("Last Cargo Command", self.lastCargoCommand)
SmartDashboard.putBoolean("Wrist PinState Quad A",
self.wrist.getPinStateQuadA())
SmartDashboard.putBoolean("Wrist PinState Quad B",
self.wrist.getPinStateQuadB())
self.F = SmartDashboard.getNumber("F Gain", 0)
def execute(self) -> None:
(ball_detected, rotation_offset,
distance) = self.vision.getBallValues()
SmartDashboard.putBoolean('/DriveToBall/ball_detected', ball_detected)
if ball_detected or abs(rotation_offset) > 5:
SmartDashboard.putNumber('/DriveToBall/rotation_offset',
rotation_offset)
SmartDashboard.putNumber('/DriveToBall/distance', distance)
speed_ratio = abs(rotation_offset) / 30
added_speed = 0.1
min_speed = 0.2
speed = min_speed + added_speed * speed_ratio
angle_is_negative = rotation_offset < 0
output = -speed if angle_is_negative else speed
self.drivetrain.arcade_drive(0, output)
def dashboardPeriodic(self):
#commented out some values. DON'T DELETE THESE VALUES
#SmartDashboard.putNumber("Hatch Current", self.wheels.getOutputCurrent())
#SmartDashboard.putNumber("Power", self.outPower)
SmartDashboard.putBoolean("Has Hatch", self.hasHatch)
SmartDashboard.putBoolean("Slider Out", self.slider.get())
SmartDashboard.putBoolean("Kicker Out", self.kicker.get())
def log(self):
self.counter += 1
if self.counter % 10 == 0:
# start keeping track of where the robot is with an x and y position (only good for WCD)
try:
distance = 0.5 * (self.sparkneo_encoder_1.getPosition() -
self.sparkneo_encoder_3.getPosition())
except:
print(
f"Problem: encoder position returns {self.sparkneo_encoder_1.getPosition()}"
)
distance = 0
self.x = self.x + (distance - self.previous_distance) * math.sin(
math.radians(self.robot.navigation.get_angle()))
self.y = self.y + (distance - self.previous_distance) * math.cos(
math.radians(self.robot.navigation.get_angle()))
self.previous_distance = distance
# send values to the dash to make sure encoders are working well
#SmartDashboard.putNumber("Robot X", round(self.x, 2))
#SmartDashboard.putNumber("Robot Y", round(self.y, 2))
for ix, encoder in enumerate(self.encoders):
SmartDashboard.putNumber(
f"Position Enc{str(int(1+ix))}",
round(encoder.getPosition() - self.encoder_offsets[ix], 2))
SmartDashboard.putNumber(f"Velocity Enc{str(int(1+ix))}",
round(encoder.getVelocity(), 2))
SmartDashboard.putNumber(
"Current M1",
round(self.spark_neo_left_front.getOutputCurrent(), 2))
SmartDashboard.putNumber(
"Current M3",
round(self.spark_neo_right_front.getOutputCurrent(), 2))
SmartDashboard.putBoolean('AccLimit', self.is_limited)
if self.counter % 1000 == 0:
# self.display_PIDs()
SmartDashboard.putString(
"alert",
f"Position: ({round(self.x, 1)},{round(self.y, 1)}) Time: {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)}"
)
def robotPeriodic(self):
wpilib.SmartDashboard.putNumber("YawAngle", self.m_imu.getAngle())
wpilib.SmartDashboard.putNumber("XCompAngle",
self.m_imu.getXComplementaryAngle())
wpilib.SmartDashboard.putNumber("YCompAngle",
self.m_imu.getYComplementaryAngle())
wpilib.SmartDashboard.getBoolean("RunCal", False)
wpilib.SmartDashboard.getBoolean("RunCal", False)
wpilib.SmartDashboard.getBoolean("RunCal", False)
wpilib.SmartDashboard.getBoolean("RunCal", False)
self.m_yawSelected = self.m_yawChooser.GetSelected()
self.m_yawSelected = KYAW_DEFAULT
# Set IMU settings
if (self.m_configCal):
self.m_imu.configCalTime(ADIS16470CalibrationTime._8s)
self.m_configCal = SmartDashboard.putBoolean("ConfigCal", False)
if (self.m_reset):
self.m_imu.Reset()
self.m_reset = SmartDashboard.putBoolean("Reset", False)
if (self.m_runCal):
self.m_imu.Calibrate()
self.m_runCal = SmartDashboard.putBoolean("RunCal", False)
# Read the desired yaw axis from the dashboard
if (self.m_yawSelected == "X-Axis"):
self.m_yawActiveAxis = ADIS16470_IMU.IMUAxis.kX
elif (self.m_yawSelected == "Y-Axis"):
self.m_yawActiveAxis = ADIS16470_IMU.IMUAxis.kY
else:
self.m_yawActiveAxis = ADIS16470_IMU.IMUAxis.kZ
# Set the desired yaw axis from the dashboard
if (self.m_setYawAxis):
self.m_imu.SetYawAxis(self.m_yawActiveAxis)
self.m_setYawAxis = SmartDashboard.putBoolean("SetYawAxis", False)
def on_iteration(self, time_elapsed):
dash.putBoolean("On Target",
self.subsystem_drivetrain.is_done_turning())
dash.putNumber("Distance from target",
self.subsystem_drivetrain.turn_pid.getError())
turn_kP, turn_kI, turn_kD = dashboard_utils.get_pid_values(
robotmap.dashboard_pid_drivetrain_turn)
self.subsystem_drivetrain.update_pid(turn_kP, turn_kI, turn_kD)
dash.putNumber("total error",
self.subsystem_drivetrain.turn_pid.totalError)
dash.putNumber(
"integral term", self.subsystem_drivetrain.turn_pid.I *
self.subsystem_drivetrain.turn_pid.totalError)
dash.putNumber("current angle",
self.subsystem_drivetrain.navx.getAngle())
dash.putNumber("PID Output", self.subsystem_drivetrain.turn_pid.get())
self.subsystem_drivetrain.turn_to_angle(
dash.getNumber("Turn Position", 0))
def robotPeriodic(self):
SmartDashboard.putNumber("driver/current_distance", RobotMap.driver_component.current_distance)
SmartDashboard.putNumber("driver/left_encoder",
RobotMap.driver_component.left_encoder_motor.getSelectedSensorPosition(0))
SmartDashboard.putNumber("driver/right_encoder",
RobotMap.driver_component.right_encoder_motor.getSelectedSensorPosition(0))
SmartDashboard.putNumber("driver/gyro", RobotMap.driver_component.driver_gyro.getAngle())
SmartDashboard.putNumber("lifter/current_position", RobotMap.lifter_component.current_position)
SmartDashboard.putNumber("lifter/current_elevator_position", RobotMap.lifter_component.current_elevator_position)
SmartDashboard.putNumber("lifter/current_carriage_position", RobotMap.lifter_component.current_carriage_position)
SmartDashboard.putBoolean("lifter/carriage_top_switch", RobotMap.lifter_component.carriage_top_switch.get())
SmartDashboard.putBoolean("lifter/carriage_bottom_switch", RobotMap.lifter_component.carriage_bottom_switch.get())
SmartDashboard.putBoolean("lifter/elevator_bottom_switch", RobotMap.lifter_component.elevator_bottom_switch.get())
SmartDashboard.putNumber("gripper/gripper_pot", RobotMap.gripper_component.pot.get())
class AutonomousRamsete(Command):
"""Attempting to translate the Ramsete command from commands V2 into a V1 version since robotpy doesn't have this command yet """
# constants for ramsete follower and velocity PID controllers - don't want a different copy for each command
beta = drive_constants.ramsete_B
zeta = drive_constants.ramsete_Zeta
kp_vel = drive_constants.kp_drive_vel
kd_vel = 0
velocity = drive_constants.k_max_speed_meters_per_second
write_telemetry = False
dash = True # ToDo: decide if I ever want to hide this
if dash is True:
SmartDashboard.putNumber("ramsete_kpvel", kp_vel)
SmartDashboard.putNumber("ramsete_B", beta)
SmartDashboard.putNumber("ramsete_Z", zeta)
SmartDashboard.putBoolean("ramsete_write", write_telemetry)
def __init__(self, robot, timeout=50):
Command.__init__(self, name='auto_ramsete')
self.robot = robot
self.requires(robot.drivetrain)
self.setTimeout(timeout)
self.previous_time = -1
self.previous_speeds = None
self.use_PID = True
self.counter = 0
self.telemetry = []
self.trajectory = None
self.feed_forward = drive_constants.feed_forward
self.kinematics = drive_constants.drive_kinematics
self.course = drive_constants.course
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 execute(self) -> None:
current_time = self.timeSinceInitialized()
dt = current_time - self.previous_time
if self.previous_time < 0:
self.robot.drivetrain.tank_drive_volts(0, 0)
self.previous_time = current_time
return
# get the robot's current field pose, current trajectory point, and feed to the ramsete controller
pose = self.robot.drivetrain.get_pose()
sample = self.trajectory.sample(current_time)
ramsete = self.follower.calculate(pose, sample)
target_wheel_speeds = self.kinematics.toWheelSpeeds(ramsete)
left_speed_setpoint = target_wheel_speeds.left
right_speed_setpoint = target_wheel_speeds.right
v_limit = 10 # for some reason the ffwd is putting out monster values of voltage - probably an error on my part in the characterization
if self.use_PID:
left_feed_forward = self.feed_forward.calculate(left_speed_setpoint, (left_speed_setpoint - self.previous_speeds.left)/dt)
left_feed_forward = v_limit if left_feed_forward > v_limit else left_feed_forward
left_feed_forward = -v_limit if left_feed_forward < -v_limit else left_feed_forward
right_feed_forward = self.feed_forward.calculate(right_speed_setpoint, (right_speed_setpoint - self.previous_speeds.right)/dt)
right_feed_forward = v_limit if right_feed_forward > v_limit else right_feed_forward
right_feed_forward = -v_limit if right_feed_forward < -v_limit else right_feed_forward
#ws_left, ws_right = self.robot.drivetrain.get_wheel_speeds().left, self.robot.drivetrain.get_wheel_speeds().right
ws_left, ws_right = self.robot.drivetrain.get_rate(self.robot.drivetrain.l_encoder), -self.robot.drivetrain.get_rate(self.robot.drivetrain.r_encoder)
left_output_pid = self.left_controller.calculate(ws_left, left_speed_setpoint)
right_output_pid = self.right_controller.calculate(ws_right, right_speed_setpoint)
# 100% sure that these signs are right - see plots. Lots of issues here. Need to sort out where you correct for encoder signs.
pid_sign = 1
left_output = pid_sign*left_output_pid + left_feed_forward
right_output = pid_sign*right_output_pid + right_feed_forward
else: # ToDo - fix this to just be the feed forwards and test it
left_output = left_speed_setpoint
right_output = right_speed_setpoint
self.robot.drivetrain.tank_drive_volts(left_output, -right_output)
self.previous_speeds = target_wheel_speeds
self.previous_time = current_time
self.robot.drivetrain.drive.feed() # should this be in tank drive?
if self.counter % 5 == 0: # ten times per second update the telemetry array
telemetry_data = {'TIME':current_time, 'RBT_X':pose.X(), 'RBT_Y':pose.Y(), 'RBT_TH':pose.rotation().radians(),
'RBT_VEL':self.robot.drivetrain.get_average_encoder_rate(),
'RBT_RVEL':ws_right, 'RBT_LVEL':ws_left,
'TRAJ_X':sample.pose.X(), 'TRAJ_Y':sample.pose.Y(), 'TRAJ_TH':sample.pose.rotation().radians(), 'TRAJ_VEL':sample.velocity,
'RAM_VELX':ramsete.vx, 'RAM_LVEL_SP':left_speed_setpoint, 'RAM_RVEL_SP':right_speed_setpoint,
'RAM_OM':ramsete.omega, 'LFF':left_feed_forward, 'RFF':right_feed_forward, 'LPID': left_output_pid, 'RPID':right_output_pid}
self.telemetry.append(telemetry_data)
if self.counter % 50 == 0: # once per second send data to the console
out_string = f'{current_time:4.2f}\t{self.trajectory.sample(current_time).velocity:4.1f}\t{self.trajectory.sample(current_time).pose.rotation().radians():4.2f}\t'
out_string += f'{left_speed_setpoint:4.2f}\t{right_speed_setpoint:4.2f}\t{ramsete.omega:4.2f}\t{ramsete.vx:4.2f}\t{ramsete.vy:4.2f}\t'
out_string += f'{left_feed_forward:4.2f}\t{right_feed_forward:4.2f}\t{left_output_pid:4.2f}\t{right_output_pid:4.2f}'
print(out_string)
self.counter += 1
def isFinished(self) -> bool: # ToDo: investigate the different end methods
return self.isTimedOut() or self.timeSinceInitialized() > self.trajectory.totalTime()
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 interrupted(self):
self.end(message='Interrupted')
def dashboardPeriodic(self):
SmartDashboard.putBoolean("Fully Extended Front",
self.isFullyExtendedFront())
SmartDashboard.putBoolean("Fully Extended Back",
self.isFullyExtendedBack())
SmartDashboard.putBoolean("Fully Retracted Front",
self.isFullyRetractedFront())
SmartDashboard.putBoolean("Fully Retracted Back",
self.isFullyRetractedBack())
SmartDashboard.putBoolean("Front Over Ground",
self.isFrontOverGround())
SmartDashboard.putBoolean("Back Over Ground", self.isBackOverGround())
SmartDashboard.putNumber("self.state", self.state)
self.climbSpeed = SmartDashboard.getNumber("ClimbSpeed",
self.climbSpeed)
self.kP = SmartDashboard.getNumber("Climber kP", self.kP)
self.backHold = SmartDashboard.getNumber("BackHold", self.backHold)
self.frontHold = SmartDashboard.getNumber("FrontHold", self.frontHold)
SmartDashboard.putNumber("Lean", self.getLean())
SmartDashboard.putNumber("FloorSensor", self.backUp())
SmartDashboard.putBoolean("Auto Climb Indicator",
self.autoClimbIndicator)
SmartDashboard.putNumber("Retract Start", self.frontRetractStart)
SmartDashboard.putNumber("Wheels Start", self.wheelsStart2)
def robotPeriodic(self):
self.limelight.readLimelightData()
SmartDashboard.putBoolean("teleop", self.teleop)
if (self.dashboard): self.updateDashboardPeriodic()
def disabledPeriodic(self):
Scheduler.getInstance().run()
#Tanklift data
SmartDashboard.putBoolean("Front IR", subsystems.drivelift.frontIR.state)
SmartDashboard.putBoolean("Back IR", subsystems.drivelift.backIR.state)
#Elevator Data
SmartDashboard.putNumber("Elevator Ticks", subsystems.elevator.elevatorEncoder.get())
SmartDashboard.putNumber("Elevator Height", subsystems.elevator.elevatorEncoder.getDistance())
SmartDashboard.putNumber("Limit Switch", subsystems.elevator.btmLimitSwitch.get())
#Game Objective States
SmartDashboard.putBoolean("Ramp Tog", subsystems.ramp.rampToggle)
SmartDashboard.putBoolean("Hatch Tog", subsystems.hatchgrab.hatchToggle)
SmartDashboard.putBoolean("Suction Extend", subsystems.hatchgrab.hatchExtendToggle)
SmartDashboard.putBoolean("Cargo Tog", subsystems.cargograb.cargoToggle)
SmartDashboard.putNumber("R Servo Angle", subsystems.cargograb.rightServo.getAngle())
SmartDashboard.putNumber("L Servo Angle", subsystems.cargograb.leftServo.getAngle())
SmartDashboard.putBoolean("Relay State", subsystems.hatchgrab.hatchGrabExtendSol.get())
#Driveline data
SmartDashboard.putNumber("Left Speed", subsystems.driveline.leftSpdCtrl.get())
SmartDashboard.putNumber("Right Speed", subsystems.driveline.rightSpdCtrl.get())
SmartDashboard.putNumber("Right Sensors", subsystems.driveline.rSensor.getVoltage())
SmartDashboard.putNumber("Left Sensors", subsystems.driveline.lSensor.getVoltage())
SmartDashboard.putNumber("Sensor Dif", subsystems.driveline.lineSensorCompare)
def robotPeriodic(self):
#self.limelight.readLimelightData()
if (self.dashboard): self.updateDashboardPeriodic()
SmartDashboard.putBoolean("Passed Hatch", self.drive.isCargoPassed())
def initializeBoolean(label: str, defaultValue: bool) -> bool:
value: bool = SmartDashboard.getBoolean(label, defaultValue)
SmartDashboard.putBoolean(label, value)
return value
