diff --git a/gradlew b/gradlew old mode 100755 new mode 100644 diff --git a/src/main/kotlin/frc/team449/subsystems/drive/DriveSubsystem.kt b/src/main/kotlin/frc/team449/subsystems/drive/DriveSubsystem.kt index c74898d..04ddc6a 100644 --- a/src/main/kotlin/frc/team449/subsystems/drive/DriveSubsystem.kt +++ b/src/main/kotlin/frc/team449/subsystems/drive/DriveSubsystem.kt @@ -1,25 +1,14 @@ package frc.team449.subsystems.drive -import choreo.trajectory.SwerveSample import edu.wpi.first.math.MathUtil -import edu.wpi.first.math.controller.PIDController import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.geometry.Translation2d import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.wpilibj2.command.Subsystem - /** A drivetrain that uses closed-loop velocity control. */ interface DriveSubsystem : Subsystem { - - private val xController: PIDController - get() = PIDController(10.0, 0.0, 0.0) - private val yController: PIDController - get()=PIDController(10.0, 0.0, 0.0) - private val headingController: PIDController - get()= PIDController(7.5, 0.0, 0.0) - var heading: Rotation2d get() = Rotation2d(MathUtil.angleModulus(this.pose.rotation.radians)) set(value) { @@ -28,27 +17,6 @@ interface DriveSubsystem : Subsystem { var pose: Pose2d - fun getPose(): Pose2d { - return Pose2d(pose.x , pose.y , heading) - } - fun followTrajectory(sample: SwerveSample) { - // Get the current pose of the robot - val pose: Pose2d = getPose(); - - // Generate the next speeds for the robot - val speeds: ChassisSpeeds = ChassisSpeeds( - sample.vx + xController.calculate(pose.x, sample.x), - sample.vy + yController.calculate(pose.y, sample.y), - sample.omega + headingController.calculate(pose.rotation.radians, sample.heading) - ) - - // Apply the generated speeds - driveFieldRelative(speeds) - } - - fun driveFieldRelative(speeds: ChassisSpeeds) - - /** Sets the drivetrain's desired speeds. */ fun set(desiredSpeeds: ChassisSpeeds)