Merge pull request #18 from Merridew1/DrivetrainPID

Drivetrain pid
Merridew1 · Dec 9, 2024 · 27ec070 · 27ec070
2 parents b847cc0 + 1a6590a
commit 27ec070
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -57,6 +57,7 @@ public static final class DriveTrain {
             public static double P = 0;
             public static double I = 0;
             public static double D = 0;
+            public static double MAX_VELOCITY = 0;
         }
     }
 

diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java
@@ -2,6 +2,7 @@
 
 import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -14,6 +15,10 @@
 public class Drivetrain extends SubsystemBase {
     private DrivetrainIO io;
     private DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged();
+    PIDController driveTrainRightPidController = new PIDController(Constants.PID.DriveTrain.P,
+        Constants.PID.DriveTrain.I, Constants.PID.DriveTrain.D);
+    PIDController driveTrainLeftPIDController = new PIDController(0, 0, 0);
+    SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0, 0);
 
     /**
      * Create Wrist Intake Subsystem
@@ -29,8 +34,11 @@ public void periodic() {
     }
 
     public void drive(Double lPower, double rPower) {
-        io.setDrivePower(driveTrainPidController.calculate(lPower),
-            driveTrainPidController.calculate(rPower));
+        io.setDrivePower(
+            driveTrainLeftPIDController.calculate(inputs.leftVelocity,
+                lPower * Constants.PID.DriveTrain.MAX_VELOCITY),
+            driveTrainRightPidController.calculate(inputs.rightVelocity,
+                rPower * Constants.PID.DriveTrain.MAX_VELOCITY));
         Logger.recordOutput("Drivetrain/leftPower", lPower);
         Logger.recordOutput("Drivetrain/rightPower", rPower);
     }
@@ -39,8 +47,6 @@ public Command driveCommand(CommandXboxController controller) {
         return run(() -> drive(controller.getLeftY(), controller.getRightY()));
     }
 
-    PIDController driveTrainPidController = new PIDController(Constants.PID.DriveTrain.P,
-        Constants.PID.DriveTrain.I, Constants.PID.DriveTrain.D);
 
 }
 
diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java
@@ -13,6 +13,8 @@ public interface DrivetrainIO {
     @AutoLog
     public static class DrivetrainIOInputs {
         public Rotation2d gyroYaw = new Rotation2d();
+        public double rightVelocity;
+        public double leftVelocity;
     }
 
     /** Updates the set of loggable inputs. */

diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems.drive;
 
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.Constants;
@@ -13,6 +15,8 @@ public class DrivetrainReal implements DrivetrainIO {
     TalonFX left2 = new TalonFX(Constants.Motors.DriveTrain.BACK_LEFT_MOTOR_ID, "CANivore");
     TalonFX right1 = new TalonFX(Constants.Motors.DriveTrain.FRONT_RIGHT_MOTOR_ID, "CANivore");
     TalonFX right2 = new TalonFX(Constants.Motors.DriveTrain.BACK_RIGHT_MOTOR_ID, "CANivore");
+    StatusSignal<Double> rightVelocity = right1.getVelocity();
+    StatusSignal<Double> leftVelocity = left1.getVelocity();
 
     /**
      * Drivetrain Real
@@ -25,17 +29,20 @@ public DrivetrainReal() {
     @Override
     public void updateInputs(DrivetrainIOInputs inputs) {
         inputs.gyroYaw = Rotation2d.fromDegrees(0);
+        inputs.leftVelocity = leftVelocity.getValueAsDouble();
+        inputs.rightVelocity = rightVelocity.getValueAsDouble();
+
     }
 
     /**
      * Drive Voltage
      */
     @Override
     public void setDrivePower(double lPower, double rPower) {
-        left1.set(lPower);
-        left2.set(lPower);
-        right1.set(rPower);
-        right2.set(rPower);
+        left1.setControl(new DutyCycleOut(rPower));
+        left2.setControl(new DutyCycleOut(lPower));
+        right1.setControl(new DutyCycleOut(rPower));
+        right2.setControl(new DutyCycleOut(rPower));
     }
 
 }