Skip to content

Commit

Permalink
Swerve note lineup looks good
Browse files Browse the repository at this point in the history
  • Loading branch information
@Inspirol
Inspirol committed Apr 10, 2024
1 parent e6ee9ae commit 289a8dd
Show file tree
Hide file tree
Showing 5 changed files with 74 additions and 44 deletions.
44 changes: 27 additions & 17 deletions command/controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,8 @@
WaitCommand,
ParallelRaceGroup,
WaitUntilCommand,
ConditionalCommand
ConditionalCommand,
ParallelDeadlineGroup
)
from commands2.command import Command
from wpimath.geometry import Pose2d, Rotation2d # noqa
Expand Down Expand Up @@ -442,26 +443,35 @@ def __init__(self, drivetrain: Drivetrain, wrist: Wrist, intake: Intake, limelig
),
SequentialCommandGroup(
ParallelCommandGroup(
SetWristIdle(wrist),
DriveSwerveNoteLineup(drivetrain, limelight),
),
ParallelCommandGroup(
SequentialCommandGroup(
InstantCommand(
lambda: drivetrain.set_robot_centric(
(-config.object_detection_intaking_drivetrain_speed * drivetrain.max_vel, 0), 0)
),
IntakeStageNote(wrist, intake),
ParallelDeadlineGroup(
WaitUntilCommand(lambda: intake.detect_note()),
InstantCommand(
lambda: drivetrain.set_robot_centric((0, 0), 0)
)
),
SequentialCommandGroup(
IntakeStageNote(wrist, intake),
IntakeStageIdle(wrist, intake)
DriveSwerveNoteLineup(drivetrain, limelight)
)
)
),
# SequentialCommandGroup(
# ParallelCommandGroup(
# SetWristIdle(wrist),
# DriveSwerveNoteLineup(drivetrain, limelight),
# ),
# ParallelCommandGroup(
# SequentialCommandGroup(
# InstantCommand(
# lambda: drivetrain.set_robot_centric(
# (-config.object_detection_intaking_drivetrain_speed * drivetrain.max_vel, 0), 0)
# ),
# WaitUntilCommand(lambda: intake.detect_note()),
# InstantCommand(
# lambda: drivetrain.set_robot_centric((0, 0), 0)
# )
# ),
# SequentialCommandGroup(
# IntakeStageNote(wrist, intake),
# IntakeStageIdle(wrist, intake)
# )
# )
# ),
lambda: intake.detect_note() or wrist.detect_note_first() or wrist.detect_note_second()
)
)
Expand Down
33 changes: 25 additions & 8 deletions command/drivetrain.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -296,16 +296,25 @@ def __init__(self, subsystem: Drivetrain, LimeLight: Limelight):
self.target_exists = False
self.target_constrained = False
# self.v_pid = PIDController(.09, 0, 0.1)
self.h_pid = PIDController(.07, 0, 0.1)
self.v_constant = 3
self.h_pid = PIDController(.03, 0, 00)
self.o_pid = PIDController(
config.drivetrain_rotation_P,
config.drivetrain_rotation_I,
config.drivetrain_rotation_D
)
self.v_constant = 2.5
self.is_pipeline: bool = False
self.nt = ntcore.NetworkTableInstance.getDefault().getTable('drivetrain pid tune')

self.gyro_lock = 0

def initialize(self):
# self.limelight.set_pipeline_mode(config.LimelightPipeline.neural)
# self.v_pid.reset()
self.o_pid.enableContinuousInput(radians(-180), radians(180))
self.o_pid.reset()
self.h_pid.reset()
self.gyro_lock = 0
self.target_exists = False
# self.v_pid.setTolerance(config.object_detection_ty_threshold)
self.h_pid.setTolerance(config.object_detection_tx_threshold)

Expand All @@ -319,9 +328,16 @@ def execute(self):
# print(self.limelight.table.getNumber('tv', 3))
self.drivetrain.set_robot_centric((0,0),0)
return

if self.limelight.target_exists() and self.target_exists == False:
self.target_exists = True
self.gyro_lock = self.drivetrain.get_heading().radians()
else:
self.target_exists = False
return
# self.nt.putBoolean('see target', True)
# print("target")
tx, ty, ta = self.limelight.get_target(True)
tx, ty, ta = self.limelight.get_target()

self.nt.putNumber('tx target', config.object_detection_tx)
self.nt.putNumber('ty target', config.object_detection_ty)
Expand All @@ -332,22 +348,23 @@ def execute(self):



if self.target_exists == False and self.target_exists:
self.target_exists = True
# if self.target_exists == False and self.target_exists:
# self.target_exists = True

# print("Tracking...")

# dy = self.v_pid.calculate(ty, config.object_detection_ty)
dx = self.h_pid.calculate(tx, config.object_detection_tx)
omega = self.o_pid.calculate(self.drivetrain.get_heading().radians(), self.gyro_lock)

dy = self.v_constant ** (-(abs(self.h_pid.getPositionError()) / 10)) if abs(self.h_pid.getPositionError()) > 0 else 1
dy = self.v_constant ** (-(abs(self.h_pid.getPositionError()) / 30)) if abs(self.h_pid.getPositionError()) > 0 else 1



# dx *= states.drivetrain_controlled_vel * config.object_detection_drivetrain_speed_dx
dy *= states.drivetrain_controlled_vel * config.object_detection_drivetrain_speed_dy

self.drivetrain.set_robot_centric((dy, -dx), 0)
self.drivetrain.set_robot_centric((-dy, -dx), -omega)


def isFinished(self):
Expand Down
2 changes: 1 addition & 1 deletion config.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -229,7 +229,7 @@ class LimelightPosition:
back_left_move_id = 11
back_left_turn_id = 14
back_left_encoder_port = AnalogEncoder(1 if comp_bot.get() else 0)
back_left_encoder_zeroed_pos = 0.221 if comp_bot.get() else 0.458
back_left_encoder_zeroed_pos = 0.221 if comp_bot.get() else 0.181

back_right_move_id = 18
back_right_turn_id = 16
Expand Down
34 changes: 17 additions & 17 deletions oi/OI.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,26 +74,26 @@ def map_controls():
command.IntakeIdle(Robot.intake)
)

# Keymap.Intake.AUTO_INTAKE.onTrue(
# command.AutoPickupNote(Robot.drivetrain, Robot.wrist, Robot.intake, Sensors.limelight_intake)
# ).onFalse(
# commands2.ParallelCommandGroup(
# # command.IntakeStageIdle(Robot.wrist, Robot.intake),
# commands2.ConditionalCommand(
# command.IntakeStageNote(Robot.wrist, Robot.intake).andThen(
# command.IntakeStageIdle(Robot.wrist, Robot.intake)),
# command.IntakeStageIdle(Robot.wrist, Robot.intake),
# lambda: Robot.intake.detect_note() or Robot.wrist.detect_note_first()
# ),
# command.DriveSwerveCustom(Robot.drivetrain)
# )
# )

Keymap.Intake.AUTO_INTAKE.onTrue(
command.DriveSwerveNoteLineup(Robot.drivetrain, Sensors.limelight_intake)
command.AutoPickupNote(Robot.drivetrain, Robot.wrist, Robot.intake, Sensors.limelight_intake)
).onFalse(
command.DriveSwerveCustom(Robot.drivetrain)
commands2.ParallelCommandGroup(
# command.IntakeStageIdle(Robot.wrist, Robot.intake),
commands2.ConditionalCommand(
command.IntakeStageNote(Robot.wrist, Robot.intake).andThen(
command.IntakeStageIdle(Robot.wrist, Robot.intake)),
command.IntakeStageIdle(Robot.wrist, Robot.intake),
lambda: Robot.intake.detect_note() or Robot.wrist.detect_note_first()
),
command.DriveSwerveCustom(Robot.drivetrain)
)
)

# Keymap.Intake.AUTO_INTAKE.onTrue(
# command.DriveSwerveNoteLineup(Robot.drivetrain, Sensors.limelight_intake)
# ).onFalse(
# command.DriveSwerveCustom(Robot.drivetrain)
# )

Keymap.Elevator.ELEVATOR_HIGH.onTrue(
command.SetElevator(Robot.elevator, config.Giraffe.kElevatorHigh.height)
Expand Down
5 changes: 4 additions & 1 deletion robot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -222,8 +222,11 @@ def teleopInit(self):
command.DriveSwerveCustom(Robot.drivetrain),
)
)
# self.scheduler.schedule(
# command.DeployIntake(Robot.intake).andThen(command.IntakeIdle(Robot.intake))
# )
self.scheduler.schedule(
command.DeployIntake(Robot.intake).andThen(command.IntakeIdle(Robot.intake))
command.IntakeIdle(Robot.intake)
)

if Robot.wrist.note_in_feeder():
Expand Down

0 comments on commit 289a8dd

Please sign in to comment.