more subsystems

1 files changed, 32 insertions, 16 deletions

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 557c7e3..5756650 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -2,14 +2,14 @@ package frc.robot.subsystems;
 
 import com.revrobotics.CANSparkMax;
 
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
-import edu.wpi.first.math.proto.Trajectory;
+import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Velocity;
@@ -25,6 +25,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import static edu.wpi.first.units.MutableMeasure.mutable;
 import static edu.wpi.first.units.Units.Volts;
 
+import org.littletonrobotics.junction.Logger;
+
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.MetersPerSecond;
 
@@ -43,10 +45,10 @@ public class Drivetrain extends SubsystemBase
     private Encoder m_encoderL = Constants.encoderL;
     private Encoder m_encoderR = Constants.encoderR;
 
-    private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1.1, 3.16, 0.17);
+    private SimpleMotorFeedforward m_feedforward = Constants.motor_feedforward; 
 
-    private final PIDController m_pidL = new PIDController(1, 0, 0);
-    private final PIDController m_pidR = new PIDController(1, 0, 0);
+    private ProfiledPIDController m_pidL = new ProfiledPIDController(0.2, 0, 0, Constants.motor_constraints);
+    private ProfiledPIDController m_pidR = new ProfiledPIDController(0.2, 0, 0, Constants.motor_constraints);
     
     private DifferentialDriveKinematics m_kinematics = 
         new DifferentialDriveKinematics(Constants.TrackWidth);
@@ -89,7 +91,7 @@ public class Drivetrain extends SubsystemBase
         m_motorR_follower.follow(m_motorR_leader);
         m_motorL_follower.follow(m_motorL_leader);
         
-        m_motorR_leader.setInverted(true);
+        m_motorL_leader.setInverted(true);
         m_encoderL.setReverseDirection(true);
 
         m_encoderL.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
@@ -119,6 +121,9 @@ public class Drivetrain extends SubsystemBase
 
     public void speed(DifferentialDriveWheelSpeeds speeds) 
     {
+        Logger.recordOutput(getName() + "/leftSpeed", speeds.leftMetersPerSecond);
+        Logger.recordOutput(getName() + "/rightSpeed", speeds.rightMetersPerSecond);
+
         voltage(
             m_pidL.calculate(m_encoderL.getRate(), speeds.leftMetersPerSecond)
               + m_feedforward.calculate(speeds.leftMetersPerSecond),
@@ -129,6 +134,9 @@ public class Drivetrain extends SubsystemBase
 
     public void voltage(double leftVolts, double rightVolts)
     {
+        Logger.recordOutput(getName() + "/leftVolts", leftVolts);
+        Logger.recordOutput(getName() + "/rightVolts", rightVolts);
+
         m_motorL_leader.setVoltage(leftVolts);
         m_motorR_leader.setVoltage(rightVolts);
     }
@@ -159,34 +167,42 @@ public class Drivetrain extends SubsystemBase
             m_gyro.toRotation2d(),
             m_encoderL.getDistance(),
             m_encoderR.getDistance());
-    }
 
-    @Override
-    public void simulationPeriodic() {
-        // This method will be called once per scheduler run during simulation
+        Logger.recordOutput(getName() + "/gyro", m_gyro.toRotation2d().getDegrees());
+        Logger.recordOutput(getName() + "/pose", pose2d());
+
+        Logger.recordOutput(getName() + "/encoderL", m_encoderL.getRate());
+        Logger.recordOutput(getName() + "/encoderR", m_encoderR.getRate());
     }
 
-    public Command trajectory(Trajectory trajectory)
+    public Command followTrajectory(Trajectory trajectory)
     {
-        // TOOD: Implement
-        // return this.runOnce(...)   
+        // RamseteCommand ramsete = new RamseteCommand(
+        //     trajectory, this::pose2d, 
+        //     new RamseteController(0, 0), 
+        //     m_feedforward, m_kinematics, 
+        //     () -> new DifferentialDriveWheelSpeeds(m_encoderL.getRate(), m_encoderR.getRate()), 
+        //     m_pidR, m_pidL, this::voltage, this);
+
+        // return Commands.runOnce(() -> this.reset())
+        //                .andThen(ramsete)
+        //                .andThen(Commands.runOnce(() -> voltage(0, 0)));
         return Commands.none();
     }
 
     public Command sysIdQausistatic(SysIdRoutine.Direction direction)
     {
         return Commands.race(m_sysIdRoutine.quasistatic(direction),
-            Commands.runOnce(() -> {
+            this.runOnce(() -> {
                 while(m_encoderL.getDistance() < 6 && 
                       m_encoderR.getDistance() < 6);
             }));
-        
     }
 
     public Command sysIdDynamic(SysIdRoutine.Direction direction)
     {
         return Commands.race(m_sysIdRoutine.dynamic(direction),
-        Commands.runOnce(() -> {
+        this.runOnce(() -> {
             while(m_encoderL.getDistance() < 6 && 
                   m_encoderR.getDistance() < 6);
         }));