def end(self):
"""Called once after isFinished returns true"""
end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print("\n" + f"** Ended {self.name} at {end_time} s with a duration of {round(end_time-self.start_time,1)} s **")
SmartDashboard.putString("alert", f"** Ended {self.name} at {end_time} s with a duration of {round(end_time-self.start_time,1)} s **")
for key in self.telemetry:
SmartDashboard.putNumberArray("telemetry_" + str(key), self.telemetry[key])
self.robot.drivetrain.stop()
def dashboardPeriodic(self):
#SmartDashboard.putNumber("xError", self.getXError())
#SmartDashboard.putNumber("yError", self.getYError())
#SmartDashboard.putNumber('Camtran', self.camtran[2])
#SmartDashboard.putNumber("Distance",self.getDistance())
#SmartDashboard.putNumber("thor", self.thor)
#SmartDashboard.putNumber("Angle1", self.tx)
#SmartDashboard.putNumber("Angle2", self.getAngle2())
#SmartDashboard.putNumber("NavXAngle", self.robot.drive.getAngle())
SmartDashboard.putNumberArray("Path", self.getPath())
#SmartDashboard.putNumber("dist1", self.dist1)
#SmartDashboard.putNumber("dist2", self.dist2)
SmartDashboard.putNumber("aligned", self.aligned)
SmartDashboard.putNumber("align", self.align)
def outputToSmartDashboard(self):
"""Output values to the smart dashboard."""
lookahead = self.pursuit_points[self.last_lookahead_index].point
closest = self.pursuit_points[self.closest_point_index].point
Dash.putNumberArray("Lookahead Point", [lookahead.x, lookahead.y])
Dash.putNumber("Curvature", self.cur_curvature)
Dash.putNumberArray("Closes Point", [closest.x, closest.y])
Dash.putNumberArray(
"Target Velocities",
[self.target_velocities.x, self.target_velocities.y])