Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Profiled PID for controlling the rotation of targeting commands #42

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Profiled PID for controlling the rotation of targeting commands #42

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 18 additions & 8 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,8 @@ import com.team4099.robot2023.util.driver.DriverProfile
import edu.wpi.first.wpilibj.RobotBase
import edu.wpi.first.wpilibj2.command.Command
import org.littletonrobotics.junction.Logger
import org.team4099.lib.controller.PIDController
import org.team4099.lib.controller.ProfiledPIDController
import org.team4099.lib.controller.TrapezoidProfile
import org.team4099.lib.units.Velocity
import org.team4099.lib.units.derived.Angle
import org.team4099.lib.units.derived.Radian
Expand All @@ -36,7 +37,7 @@ class TargetAngleCommand(
val targetAngle: () -> Angle
) : Command() {

private var thetaPID: PIDController<Radian, Velocity<Radian>>
private var thetaPID: ProfiledPIDController<Radian, Velocity<Radian>>
val thetakP =
LoggedTunableValue(
"Pathfollow/thetaAmpkP",
Expand All @@ -62,10 +63,13 @@ class TargetAngleCommand(
addRequirements(drivetrain)

thetaPID =
PIDController(
ProfiledPIDController(
thetakP.get(),
thetakI.get(),
thetakD.get(),
TrapezoidProfile.Constraints(
DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX
)
)

if (!(RobotBase.isSimulation())) {
Expand All @@ -75,29 +79,35 @@ class TargetAngleCommand(
thetakD.initDefault(DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD)

thetaPID =
PIDController(
ProfiledPIDController(
DrivetrainConstants.PID.TELEOP_ALIGN_PID_KP,
DrivetrainConstants.PID.TELEOP_ALIGN_PID_KI,
DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD
DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD,
TrapezoidProfile.Constraints(
DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX
)
)
} else {
thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP)
thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI)
thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD)

thetaPID =
PIDController(
ProfiledPIDController(
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP,
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI,
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD,
TrapezoidProfile.Constraints(
DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX
)
)
}

thetaPID.enableContinuousInput(-PI.radians, PI.radians)
}

override fun initialize() {
thetaPID.reset() // maybe do first for x?
thetaPID.reset(0.degrees) // maybe do first for x?
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why 0 degrees?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what would we reset it to then? the current gyro angle?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

you're not resetting the angle, you're resetting the error it sees. we don't want error from the previous path to carry on, but rather recalculate in our execute function

/*
if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) {
thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get())
Expand Down
36 changes: 27 additions & 9 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,8 @@ import com.team4099.robot2023.util.DebugLogger
import com.team4099.robot2023.util.driver.DriverProfile
import edu.wpi.first.wpilibj.RobotBase
import edu.wpi.first.wpilibj2.command.Command
import org.team4099.lib.controller.PIDController
import org.team4099.lib.controller.ProfiledPIDController
import org.team4099.lib.controller.TrapezoidProfile
import org.team4099.lib.units.Velocity
import org.team4099.lib.units.derived.Radian
import org.team4099.lib.units.derived.degrees
Expand All @@ -35,7 +36,7 @@ class TargetNoteCommand(
val limelight: LimelightVision
) : Command() {

private var thetaPID: PIDController<Radian, Velocity<Radian>>
private var thetaPID: ProfiledPIDController<Radian, Velocity<Radian>>
val thetakP =
LoggedTunableValue(
"NoteAlignment/noteThetakP",
Expand All @@ -61,10 +62,13 @@ class TargetNoteCommand(
addRequirements(drivetrain)

thetaPID =
PIDController(
ProfiledPIDController(
thetakP.get(),
thetakI.get(),
thetakD.get(),
TrapezoidProfile.Constraints(
DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX
)
)

if (!(RobotBase.isSimulation())) {
Expand All @@ -74,32 +78,46 @@ class TargetNoteCommand(
thetakD.initDefault(DrivetrainConstants.PID.LIMELIGHT_THETA_KD)

thetaPID =
PIDController(
ProfiledPIDController(
DrivetrainConstants.PID.LIMELIGHT_THETA_KP,
DrivetrainConstants.PID.LIMELIGHT_THETA_KI,
DrivetrainConstants.PID.LIMELIGHT_THETA_KD
DrivetrainConstants.PID.LIMELIGHT_THETA_KD,
TrapezoidProfile.Constraints(
DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX
)
)
} else {
thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP)
thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI)
thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD)

thetaPID =
PIDController(
ProfiledPIDController(
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP,
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI,
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD,
TrapezoidProfile.Constraints(
DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX
)
)
}

thetaPID.enableContinuousInput(-PI.radians, PI.radians)
}

override fun initialize() {
thetaPID.reset()
thetaPID.reset(0.degrees)

if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

please add tunable numbers for steering vel max and accel max as well

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

for the rest of the commands as well

thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get())
thetaPID =
ProfiledPIDController(
thetakP.get(),
thetakI.get(),
thetakD.get(),
TrapezoidProfile.Constraints(
DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX
)
)
}
}

Expand Down
26 changes: 18 additions & 8 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,8 @@ import edu.wpi.first.math.filter.MedianFilter
import edu.wpi.first.wpilibj.RobotBase
import edu.wpi.first.wpilibj2.command.Command
import org.littletonrobotics.junction.Logger
import org.team4099.lib.controller.PIDController
import org.team4099.lib.controller.ProfiledPIDController
import org.team4099.lib.controller.TrapezoidProfile
import org.team4099.lib.units.Velocity
import org.team4099.lib.units.base.inMeters
import org.team4099.lib.units.derived.Radian
Expand Down Expand Up @@ -40,7 +41,7 @@ class TargetSpeakerCommand(
val drivetrain: Drivetrain,
val vision: Vision
) : Command() {
private var thetaPID: PIDController<Radian, Velocity<Radian>>
private var thetaPID: ProfiledPIDController<Radian, Velocity<Radian>>
val thetakP =
LoggedTunableValue(
"Pathfollow/thetaAmpkP",
Expand Down Expand Up @@ -70,10 +71,13 @@ class TargetSpeakerCommand(
addRequirements(drivetrain)

thetaPID =
PIDController(
ProfiledPIDController(
thetakP.get(),
thetakI.get(),
thetakD.get(),
TrapezoidProfile.Constraints(
DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX
)
)

if (!(RobotBase.isSimulation())) {
Expand All @@ -83,29 +87,35 @@ class TargetSpeakerCommand(
thetakD.initDefault(DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD)

thetaPID =
PIDController(
ProfiledPIDController(
DrivetrainConstants.PID.TELEOP_ALIGN_PID_KP,
DrivetrainConstants.PID.TELEOP_ALIGN_PID_KI,
DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD
DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD,
TrapezoidProfile.Constraints(
DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX
)
)
} else {
thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP)
thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI)
thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD)

thetaPID =
PIDController(
ProfiledPIDController(
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP,
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI,
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD
DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD,
TrapezoidProfile.Constraints(
DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX
)
)
}

thetaPID.enableContinuousInput(-PI.radians, PI.radians)
}

override fun initialize() {
thetaPID.reset() // maybe do first for x?
thetaPID.reset(0.degrees) // maybe do first for x?
angleMedianFilter.reset()
/*
if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ object ControlBoard {
val climbAlignLeft = Trigger { driver.dPadLeft }
val climbAlignRight = Trigger { driver.dPadRight }

val targetSpeaker = Trigger { driver.xButton } // TODO: switch back to climbAlignLeft
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

doesn't belong in this commit

val targetSpeaker = Trigger { driver.xButton }
val climbAutoAlign = Trigger { driver.bButton }

// week0 controls
Expand Down
Loading