def teleopPeriodic(self):
if subsystems_enabled["limelight"]:
dash.putNumber("Trig Calculated Distance: ", self.limelight_component.get_distance_trig(
config_data['limelight']['target_height']
))
if subsystems_enabled["navx"]:
if self.driver.get_navx_reset():
self.navx_component.reset()
put_tuple("Displacement", self.navx_component.displacement, int)
put_tuple("Velocity", self.navx_component.velocity, int)
put_tuple("Acceleration", self.navx_component.acceleration, int)
if subsystems_enabled["shooter"]:
shooter_power = -self.driver.get_shooter_axis()
if self.driver.get_shooter_full():
shooter_power = -1
self.shooter_component.power = shooter_power
if subsystems_enabled["neo"]:
neo_kP = dash.getNumber("Shooter kP", 0)
neo_kF = dash.getNumber("Shooter kF", 0)
self.neo_component.update_pid(neo_kP, neo_kF)
neo_power = dash.getNumber("Target RPM", 0)
self.neo_component.set_rpm(neo_power)
if self.driver.get_update_telemetry():
dash.putNumber("Target RPM", 0)
dash.putNumber("Shooter kP", 0)
dash.putNumber("Shooter kF", 0)
dash.putNumber("Shooter RPM", self.neo_component.get_raw_velocity())
def dashboardPeriodic(self):
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())
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.climbSpeedFront = SmartDashboard.getNumber(
"ClimbSpeedFront", self.climbSpeedFront)
self.climbSpeedBack = SmartDashboard.getNumber("ClimbSpeedBack",
self.climbSpeedBack)
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 initialize(self) -> None:
"""
Read in and apply current drive choices.
"""
self.rampTicks = int(SmartDashboard.getNumber(self.rampTicksLabel, self.rampTicks))
self.cruiseTicks = int(SmartDashboard.getNumber(self.cruiseTicksLabel, self.cruiseTicks))
self.leftPower = self.leftGain * SmartDashboard.getNumber(self.fixedLeftLabel, self.kDefaultPower)
self.rightPower = self.rightGain * SmartDashboard.getNumber(self.fixedRightLabel, self.kDefaultPower)
self.mode = self.rampUp
self.ticks = 0
def teleDrive(self, xSpeed, zRotation):
if self.robot.oi.getController1().B().get():
scale = SmartDashboard.getNumber("creep_mult", 0.3)
xSpeed = xSpeed * scale
zRotation = zRotation * scale
self.differential_drive.arcadeDrive(xSpeed, zRotation, False)
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **"
)
SmartDashboard.putString(
"alert",
f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **"
)
if self.source == 'dashboard':
self.setpoint = SmartDashboard.getNumber('z_angle', 1)
# may want to break if no valid setpoint is passed
self.start_angle = self.robot.drivetrain.navx.getAngle()
if self.absolute: # trust the navx to be correctly oriented to the field
self.setpoint = self.setpoint - self.start_angle
if self.setpoint > 180:
self.setpoint = -(360 - self.setpoint)
if self.setpoint < -180:
self.setpoint = (360 + self.setpoint)
self.error = 0
self.prev_error = 0
def scale(self):
self.drive(self.SCALE0 - self.SHAKE)
self.turnCalc(self.SCALE_TURN)
self.parallelLift(SmartDashboard.getNumber("max_lift_inches", 79) - 2)
self.drive(self.SCALE1)
self.drive(self.SCALE2)
self.shoot()
self.drive(-self.SCALE2)
def test_mode(self):
if self.test_wrist.getSelected() == 1:
self.wrist_up.set(True)
self.wrist_down.set(False)
else:
self.wrist_up.set(False)
self.wrist_down.set(True)
if self.test_claw.getSelected() == 1:
self.claw_close.set(False)
self.claw_open.set(True)
else:
self.claw_close.set(True)
self.claw_open.set(False)
self.left_grab.set(ctre.ControlMode.PercentOutput, SmartDashboard.getNumber("TestClawMotors", 0))
self.right_grab.set(ctre.ControlMode.PercentOutput, -SmartDashboard.getNumber("TestClawMotors", 0))
def initialize(self):
#print("CMD TurnToHeading initialize called")
if self.useSmartDashboardValues:
self.target = SmartDashboard.getNumber("Turn Target", 0.0)
self.tolerance = SmartDashboard.getNumber("Turn Tolerance", 500.0)
self.tolerance = SmartDashboard.getNumber("Turn minSpeed", 0.0)
self.scaleSpeed = SmartDashboard.getNumber("Turn scaleSpeed", 0.5)
self.kP = SmartDashboard.getNumber("Turn P", 0.05)
self.kI = SmartDashboard.getNumber("Turn I", 0.001)
self.kD = SmartDashboard.getNumber("Turn D", 0.01)
subsystems.driveline.turnPID.minSpeed = self.minSpeed
subsystems.driveline.turnPID.scaleSpeed = self.scaleSpeed
subsystems.driveline.pidTurnController.setOutputRange(-1.0, 1.0)
subsystems.driveline.pidTurnController.setInputRange(-180, 180)
subsystems.driveline.pidTurnController.setContinuous(True)
subsystems.driveline.pidTurnController.setPID(self.kP, self.kI,
self.kD)
subsystems.driveline.pidTurnController.setSetpoint(self.target)
subsystems.driveline.pidTurnController.setAbsoluteTolerance(
self.tolerance)
self.turnRateSamples = [
self.steadyRate
] * self.numSamples # biasing to neutral to begin
print(
"CMD TurnToHeading Starting({}) - target: {}, current: {}, P: {}, I: {}, D: {}, tolerance: {}, steadyRate: {}, scaleSpeed: {}"
.format(int(round(time.time() * 1000)), self.target,
robotmap.sensors.ahrs.getYaw(), self.kP, self.kI, self.kD,
self.tolerance, self.steadyRate, self.scaleSpeed))
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print("\n" + f"** Started {self.getName()} at {self.start_time} s **")
SmartDashboard.putString("alert", f"** Started {self.getName()} at {self.start_time} s **")
if self.use_dash:
self.kp = SmartDashboard.getNumber('kp_vel', self.kp)
self.kd = SmartDashboard.getNumber('kd_vel', self.kd)
self.left_speed_setpoint = SmartDashboard.getNumber('l_vel', self.left_speed_setpoint)
self.right_speed_setpoint = SmartDashboard.getNumber('r_vel', self.right_speed_setpoint)
self.left_controller = wpilib.controller.PIDController(self.kp, 0 , self.kd, period=0.02)
self.right_controller = wpilib.controller.PIDController(self.kp, 0 , self.kd, period=0.02)
self.previous_time = -1
self.left_controller.reset()
self.right_controller.reset()
def initialize(self):
self.DT.zeroEncoders()
# GETTING VALUE FROM DASHBOARD IF P,I,D ARGUMENTS ARE NOT GIVEN WHEN COMMAND IS CALLED
if self.pCheck == 1000:
p = SmartDashboard.getNumber("DriveStraight_P", 0.1)
else:
p = self.pCheck
if self.iCheck == 1000:
i = SmartDashboard.getNumber("DriveStraight_I", 0)
else:
i = self.iCheck
if self.dCheck == 1000:
d = SmartDashboard.getNumber("DriveStraight_D", 0.4)
else:
d = self.dCheck
angleP = SmartDashboard.getNumber('DriveStraightAngle_P', 0.025)
angleI = SmartDashboard.getNumber('DriveStraightAngle_I', 0.0)
angleD = SmartDashboard.getNumber('DriveStraightAngle_D', 0.01)
self.DT.setGains(p, i, d, 0)
self.DT.setGainsAngle(angleP, angleI, angleD, 0)
self.distance = self.distance + self.DT.getAvgDistance()
self.DT.setCombined(distance=self.distance, angle=self.angle)
def initialize(self):
"""Called just before this Command runs the first time."""
# allow setpoint to be controlled by the dashboard
if self.source == 'dashboard':
setpoint = SmartDashboard.getNumber('z_distance', 1)
self.setpoint_sign = math.copysign(1, setpoint)
self.setpoint = (math.fabs(setpoint)) # correction for overshoot
# self.robot.drivetrain.reset_encoders() # does not work in sim mode
self.start_pos = self.robot.drivetrain.get_average_encoder_distance()
if self.k_dash:
self.kp, self.kd = SmartDashboard.getNumber(
'zdrive_kp',
self.kp), SmartDashboard.getNumber('zdrive_kd', self.kd)
self.ki, self.period = SmartDashboard.getNumber(
'zdrive_ki',
self.ki), SmartDashboard.getNumber('zdrive_per', self.kperiod)
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.getName()} with setpoint {self.setpoint} (kp={self.kp}, ki={self.ki}, kd={self.kd}) at {self.start_time} s **"
)
SmartDashboard.putString(
"alert",
f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **"
)
self.has_finished = False
self.controller = wpilib.controller.PIDController(self.kp,
self.ki,
self.kd,
period=self.kperiod)
self.controller.setSetpoint(self.setpoint)
self.controller.setTolerance(self.tolerance)
self.controller.reset()
def dashboardPeriodic(self):
#commented out some values. DON'T DELETE THESE VALUES
SmartDashboard.putNumber("Left Counts", self.leftEncoder.get())
SmartDashboard.putNumber("Left Distance",
self.leftEncoder.getDistance())
SmartDashboard.putNumber("Right Counts", self.rightEncoder.get())
#SmartDashboard.putNumber("Right Distance", self.rightEncoder.getDistance())
SmartDashboard.putNumber("DT_DistanceAvg", self.getAvgDistance())
SmartDashboard.putNumber("DT_DistanceLeft", self.getDistance()[0])
SmartDashboard.putNumber("DT_DistanceRight", self.getDistance()[1])
SmartDashboard.putNumber("DT_Angle", self.getAngle())
#SmartDashboard.putNumber("DT_PowerLeft", self.left.get())
#SmartDashboard.putNumber("DT_PowerRight", self.right.get())
#SmartDashboard.putNumber("DT_VelocityLeft", self.getVelocity()[0])
#SmartDashboard.putNumber("DT_VelocityRight", self.getVelocity()[1])
#SmartDashboard.putNumber("DT_CounLeft", self.getRaw()[0])
#SmartDashboard.putNumber("DT_CountRight", self.getRaw()[1])
#SmartDashboard.putNumber("angle correction", self.anglePID)
#SmartDashboard.putNumber("DriveAmps",self.getOutputCurrent())
#self.mode = SmartDashboard.getData("Mode", "Tank")
self.k = SmartDashboard.getNumber("K Value", 1)
self.sensitivity = SmartDashboard.getNumber("sensitivity", 1)
self.driveStyle = SmartDashboard.getString("DriveStyle", "Tank")
def initialize(self):
"""Called just before this Command runs the first time."""
if self.from_dashboard:
self.setpoint = SmartDashboard.getNumber('Auto Rotation', 0)
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.name} with setpoint {self.setpoint} at {self.start_time} s **"
)
SmartDashboard.putString(
"alert",
f"** Started {self.name} with setpoint {self.setpoint} at {self.start_time} s **"
)
self.start_angle = self.robot.navigation.get_angle()
self.error = 0
self.prev_error = 0
def initialize(self):
self.DT.zeroEncoders()
p = SmartDashboard.getNumber("DriveStraight_P", 0.1)
i = SmartDashboard.getNumber("DriveStraight_I", 0)
d = SmartDashboard.getNumber("DriveStraight_D", 0.4)
angleP = SmartDashboard.getNumber('DriveStraightAngle_P', 0.025)
angleI = SmartDashboard.getNumber('DriveStraightAngle_I', 0.0)
angleD = SmartDashboard.getNumber('DriveStraightAngle_D', 0.01)
self.DT.setGains(p, 0, d, 0)
self.DT.setGainsAngle(angleP, angleI, angleD, 0)
StartAngle = self.DT.getAngle()
self.DT.setMode("Combined", name=None, distance=self.setpoint, angle=StartAngle)
def __init__(self, robot, setpoint=None, timeout=None, source=None):
"""The constructor"""
#super().__init__()
Command.__init__(self, name='auto_pid')
# Signal that we require ExampleSubsystem
self.requires(robot.drivetrain)
self.source = source
self.k_dash = False
self.start_time = 0
self.teaching = True # switch to True if you want to learn about PID controllers
# allow setpoint to be controlled by the dashboard
if source == 'dashboard':
setpoint = SmartDashboard.getNumber('z_distance', 1)
self.k_dash = True
else:
if setpoint is None:
setpoint = 2
# wpilib PID controller apparently does not like negative setpoints, so use this to allow going backwards
self.setpoint_sign = math.copysign(1, setpoint)
self.setpoint = math.fabs(setpoint) # correction for overshoot
if timeout is None:
self.timeout = 5
else:
self.timeout = timeout
self.setTimeout(timeout)
self.robot = robot
self.has_finished = False
self.error = 0
# using these k-values
self.kp = 1.8
self.kd = 4.0
self.ki = 0.00
self.kperiod = 0.1
self.tolerance = 0.1
if self.k_dash:
SmartDashboard.putNumber('zdrive_kp', self.kp)
SmartDashboard.putNumber('zdrive_ki', self.ki)
SmartDashboard.putNumber('zdrive_kd', self.kd)
SmartDashboard.putNumber('zdrive_per', self.kperiod)
def initialize(self):
"""Called just before this Command runs the first time."""
if self.from_dashboard:
self.setpoint = SmartDashboard.getNumber('Auto Distance',0)
self.start_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print("\n" + f"** Started {self.name} with setpoint {self.setpoint} and control_type {self.control_type} at {self.start_time} s **")
SmartDashboard.putString("alert", f"** Started {self.name} with setpoint {self.setpoint} and control_type {self.control_type} at {self.start_time} s **")
if self.control_type == 'position':
self.robot.drivetrain.goToSetPoint(self.setpoint)
elif self.control_type == 'velocity':
self.robot.drivetrain.set_velocity(self.setpoint)
else:
pass
self.has_arrived = False
self.telemetry = {'time':[], 'position':[], 'velocity':[], 'current':[], 'output':[]}
self.counter = 0
self.extra_time = 0.5 # amount of time to give it to slow down after we reach setpoint
self.setTimeout(self.initial_timeout) # needs to be in initialize because we modify it in execute - dashboard instance keeps getting reused
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 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 teleopPeriodic(self):
sd.putNumber("Neo Encoder", self.neo0.getEncoder().getPosition())
speed = sd.getNumber("Neo Power", 0)
self.neo0.set(speed)
self.neo1.set(speed)
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 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 dashboardPeriodic(self):
SmartDashboard.putNumber("Cargomech Wrist Output", self.out)
self.wristPowerSet = SmartDashboard.getNumber("Wrist Power Set", 0)
def get(self, default_value=None):
if default_value == None:
return dash.getNumber(self.key, self.value)
else:
return dash.getNumber(self.key, default_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()
def initializeNumber(label: str, defaultValue: float) -> float:
value: float = SmartDashboard.getNumber(label, defaultValue)
SmartDashboard.putNumber(label, value)
return value
def getNumber(self, key, defVal):
val = SmartDashboard.getNumber(key, None)
if val == None:
val = defVal
SmartDashboard.putNumber(key, val)
return val
def get_pid(key):
p = dash.getNumber(f"{key}_kP", 0)
i = dash.getNumber(f"{key}_kI", 0)
d = dash.getNumber(f"{key}_kD", 0)
return PIDValue(p, i, d)
def initialize(self):
p = SmartDashboard.getNumber("DriveAngle_P", 0.02)
i = SmartDashboard.getNumber("DriveAngle_I", 0)
d = SmartDashboard.getNumber("DriveAngle_D", 0.01)
self.DT.setGainsAngle(p, 0, d, 0)
self.DT.setAngle(self.angle)
def teleopPeriodic(self):
self.neo.set(sd.getNumber("Neo Power", 0.5))
