Skip to content

Commit

Permalink
auto work
Browse files Browse the repository at this point in the history
  • Loading branch information
@sswadkar
sswadkar committed Dec 16, 2023
1 parent a62ce75 commit f3625c0
Show file tree
Hide file tree
Showing 9 changed files with 352 additions and 61 deletions.
3 changes: 3 additions & 0 deletions src/main/kotlin/com/team4099/lib/trajectory/CustomHolonomicDriveController.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Pose2d
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.kinematics.ChassisSpeeds
import edu.wpi.first.math.trajectory.Trajectory
import org.littletonrobotics.junction.Logger

/**
* This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain
Expand Down Expand Up @@ -95,6 +96,8 @@ class CustomHolonomicDriveController(
val xFeedback = m_xController.calculate(currentPose.x, poseRef.x)
val yFeedback = m_yController.calculate(currentPose.y, poseRef.y)
val thetaFeedback = m_thetaController.calculate(currentPose.rotation.radians, angleRef.radians)
Logger.getInstance().recordOutput("Pathfollow/thetaFeedbackRadians", thetaFeedback)
Logger.getInstance().recordOutput("Pathfollow/appliedThetaFeedback", angleVelocityRefRadians + thetaFeedback)

// Return next output.
return ChassisSpeeds.fromFieldRelativeSpeeds(
Expand Down
53 changes: 40 additions & 13 deletions src/main/kotlin/com/team4099/robot2023/auto/mode/LimelightTestingAuto.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,17 +42,27 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super
null,
0.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(
FieldConstants.StagingLocations.translations[3]!!.x-intakeOffset.get() - 1.meters,
FieldConstants.StagingLocations.translations[3]!!.y-intakeOffset.get()
)
.translation2d,
null,
0.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(
FieldConstants.StagingLocations.translations[3]!!.x-intakeOffset.get(),
FieldConstants.StagingLocations.translations[3]!!.y-intakeOffset.get()
)
.translation2d,
0.0.degrees.inRotation2ds,
null,
0.0.degrees.inRotation2ds
),
)
}
},
isAuto = true
),
WaitCommand(1.5).andThen(superstructure.groundIntakeConeCommand())
),
Expand All @@ -68,7 +78,7 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super
FieldConstants.StagingLocations.translations[3]!!.y- intakeOffset.get()
)
.translation2d,
0.0.degrees.inRotation2ds,
null,
0.0.degrees.inRotation2ds
),
Waypoint(
Expand All @@ -80,48 +90,64 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super
Translation2d(
endingPosX.get(),
FieldConstants.Grids.nodeFirstY +
FieldConstants.Grids.nodeSeparationY * 7
FieldConstants.Grids.nodeSeparationY * 5
)
.translation2d,
null,
180.0.degrees.inRotation2ds
)
)
},
keepTrapping = true
keepTrapping = true,
isAuto = true
)
),
superstructure.prepScoreCommand(
Constants.Universal.GamePiece.CUBE, Constants.Universal.NodeTier.HIGH
),
superstructure.score(),
WaitCommand(0.5),
WaitCommand(2.0),
ParallelCommandGroup(
DrivePathCommand(
drivetrain,
{
listOf(
Waypoint(
Translation2d(1.9.meters, 3.31.meters).translation2d,
Translation2d(
endingPosX.get(),
FieldConstants.Grids.nodeFirstY +
FieldConstants.Grids.nodeSeparationY * 5
)
.translation2d,
null,
0.0.degrees.inRotation2ds
180.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(2.9.meters, 4.8.meters).translation2d,
null,
0.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(
FieldConstants.StagingLocations.translations[2]!!.x-intakeOffset.get() - 1.meters,
FieldConstants.StagingLocations.translations[2]!!.y-intakeOffset.get()
)
.translation2d,
null,
0.0.degrees.inRotation2ds
),
Waypoint(
Translation2d(
FieldConstants.StagingLocations.translations[2]!!.x-intakeOffset.get(),
FieldConstants.StagingLocations.translations[2]!!.y-intakeOffset.get()
)
.translation2d,
0.0.degrees.inRotation2ds,
null,
0.0.degrees.inRotation2ds
),
)
}
},
isAuto = true
),
WaitCommand(1.5).andThen(superstructure.groundIntakeConeCommand())
),
Expand All @@ -137,7 +163,7 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super
FieldConstants.StagingLocations.translations[2]!!.y- intakeOffset.get()
)
.translation2d,
0.0.degrees.inRotation2ds,
null,
0.0.degrees.inRotation2ds
),
Waypoint(
Expand All @@ -149,15 +175,16 @@ class LimelightTestingAuto(val drivetrain: Drivetrain, val superstructure: Super
Translation2d(
endingPosX.get(),
FieldConstants.Grids.nodeFirstY +
FieldConstants.Grids.nodeSeparationY * 7
FieldConstants.Grids.nodeSeparationY * 5
)
.translation2d,
null,
180.0.degrees.inRotation2ds
)
)
},
keepTrapping = true
keepTrapping = true,
isAuto = true
)
)
)
Expand Down
13 changes: 6 additions & 7 deletions src/main/kotlin/com/team4099/robot2023/commands/AutoAlignAndIntakeCommand.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,15 +9,12 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.subsystems.limelight.LimelightVision
import com.team4099.robot2023.subsystems.superstructure.Superstructure
import edu.wpi.first.math.kinematics.ChassisSpeeds
import edu.wpi.first.wpilibj2.command.Command
import edu.wpi.first.wpilibj2.command.Commands
import edu.wpi.first.wpilibj2.command.Commands.runOnce
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
import edu.wpi.first.wpilibj2.command.WaitCommand
import org.littletonrobotics.junction.Logger
import org.team4099.lib.controller.PIDController
import org.team4099.lib.geometry.Pose2d
import org.team4099.lib.geometry.Pose3d
import org.team4099.lib.geometry.Translation2d
import org.team4099.lib.kinematics.ChassisAccels
import org.team4099.lib.units.Velocity
Expand Down Expand Up @@ -74,21 +71,21 @@ class AutoAlignAndIntakeCommand(
val thetakP =
LoggedTunableValue(
"AutoAlign/alignkP",
DrivetrainConstants.PID.AUTO_THETA_PID_KP,
DrivetrainConstants.PID.TELEOP_THETA_PID_KP,
Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree })
)
val thetakI =
LoggedTunableValue(
"AutoAlign/alignkI",
DrivetrainConstants.PID.AUTO_THETA_PID_KI,
DrivetrainConstants.PID.TELEOP_THETA_PID_KI,
Pair(
{ it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds }
)
)
val thetakD =
LoggedTunableValue(
"AutoAlign/alignkD",
DrivetrainConstants.PID.AUTO_THETA_PID_KD,
DrivetrainConstants.PID.TELEOP_THETA_PID_KD,
Pair(
{ it.inDegreesPerSecondPerDegreesPerSecond },
{ it.degrees.perSecond.perDegreePerSecond }
Expand Down Expand Up @@ -147,7 +144,9 @@ class AutoAlignAndIntakeCommand(
Logger.getInstance().recordOutput("AutoAlign/TargetPose", limelight.targetGamePiecePose?.pose3d)
Logger.getInstance().recordOutput("AutoAlign/Tx", limelight.targetGamePieceTx?.inDegrees ?: 0.0)

val xFeedback = xPID.calculate(drivetrain.odometryPose.purelyTranslateBy(intakeOffsetTranslation).x, limelight.targetGamePiecePose?.x ?: 0.0.meters)
val error = limelight.targetGamePiecePose?.toPose2d()?.relativeTo(drivetrain.odometryPose.purelyTranslateBy(intakeOffsetTranslation))

val xFeedback = xPID.calculate(error?.translation?.magnitude?.meters ?: 0.0.meters, 0.0.meters)
val yFeedback = yPID.calculate(drivetrain.odometryPose.purelyTranslateBy(intakeOffsetTranslation).y, limelight.targetGamePiecePose?.y ?: 0.0.meters)
val thetaFeedback = thetaPID.calculate(limelight.targetGamePieceTx ?: 0.0.degrees, 0.0.degrees)

Expand Down
Loading

0 comments on commit f3625c0

Please sign in to comment.