sysid and trajectory things

7 files changed, 123 insertions, 78 deletions

diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json
new file mode 100644
index 0000000..8644b73
--- /dev/null
+++ b/.DataLogTool/datalogtool.json
@@ -0,0 +1,6 @@
+{
+  "download": {
+    "localDir": "/home/atanas",
+    "serverTeam": "10.90.89.2"
+  }
+}
diff --git a/.SysId/sysid.json b/.SysId/sysid.json
new file mode 100644
index 0000000..0967ef4
--- /dev/null
+++ b/.SysId/sysid.json
@@ -0,0 +1 @@
+{}
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 8be11f2..8743373 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -25,5 +25,6 @@
       }

     },

   ],

-  "java.test.defaultConfig": "WPIlibUnitTests"

+  "java.test.defaultConfig": "WPIlibUnitTests",

+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"

 }

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1ff70bb..c4c2329 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -6,6 +6,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.I2C;
@@ -15,21 +16,25 @@ import frc.robot.util.IMUGyro;
 public class Constants
 {
     public static final GenericHID controller = new GenericHID(0);
+    public static final double InputSmoothing = 0.1;
 
     public static final double DriveMaxSpeed = 5; // m/s
+    public static final double DriveMaxAcceleration = 5; // m/s^2
     public static final double DriveMaxAngular = Math.PI; // rad/s?
 
+    public static final double AutonomousMaxSpeed = 0.5; // m/s
+    public static final double AutonomousMaxAcceleration = 0.25; // m/s^2
+
     // for Drivetrain
-    public static final double WheelDiameter = 0.1524; // in meters
-    public static final double TrackWidth = 0.685;  // in meters
-    public static final double EncoderResolution = 4096 / 24;
+    public static final double WheelDiameter = Units.inchesToMeters(6); // in meters
+    public static final double TrackWidth = 0.58;  // in meters
+    public static final double EncoderResolution = 16;
 
     public static final IMUGyro gyro = new IMUGyro(new I2C(Port.kOnboard, 0x68));
 
     public static final TrapezoidProfile.Constraints motor_constraints = 
-        new TrapezoidProfile.Constraints(DriveMaxSpeed, 4.5);
-    public static final SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(1.1, 3.5);
-    // public static final SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(0, 0, 0);
+        new TrapezoidProfile.Constraints(DriveMaxSpeed, DriveMaxAcceleration);
+    public static final SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(0.54082, 2.4705);
 
     public static final CANSparkMax motorL_leader = new CANSparkMax(11, MotorType.kBrushed);
     public static final CANSparkMax motorL_follower = new CANSparkMax(12, MotorType.kBrushed);
@@ -64,12 +69,12 @@ public class Constants
     public static final double ChainEncoderScale = 2600;
 
     public static final TrapezoidProfile.Constraints chain_constraints = 
-        new TrapezoidProfile.Constraints(0, 0);
+        new TrapezoidProfile.Constraints(0.5, 0.5);
 
-    public static final double ChainkP = 0.0;
+    public static final double ChainkP = 45.0;
     public static final double ChainkI = 0.0;
     public static final double ChainkD = 0.0;
     
-    public static final CANSparkMax chain = null;//new CANSparkMax(70, MotorType.kBrushed);
-    public static final Encoder chain_encoder = null;//new Encoder(3, 4);
+    public static final CANSparkMax chain = new CANSparkMax(50, MotorType.kBrushed);
+    public static final Encoder chain_encoder = new Encoder(8, 9);
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cbaceea..87c4e60 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,16 +4,25 @@
 
 package frc.robot;
 
-import org.littletonrobotics.junction.Logger;
+import java.util.List;
 
+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.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+
 import frc.robot.subsystems.Drivetrain;
 
 public class RobotContainer 
 {
     private final Drivetrain m_drive = new Drivetrain();
+    // private final Chain m_chain = new Chain();
 
     private final GenericHID m_controller = Constants.controller;
 
@@ -22,6 +31,20 @@ public class RobotContainer
         configureBindings();
     }
 
+    private double[] correctInputs(double in1, double in2)  
+    {
+        double p1 = processInput(in1);
+        double p2 = processInput(in2);
+
+        if(Math.signum(p1) != Math.signum(p2)) 
+            return new double[] {p1, p2};
+
+        double diff = Math.abs(p1 - p2) * Constants.InputSmoothing;
+
+        return (p1 > p2) ? new double[] {p1 - diff, p2 + diff}
+                         : new double[] {p1 + diff, p2 - diff};
+    }
+
     private double processInput(double val) 
     {  
         double input = deadzone(-0.06, 0.06, val);
@@ -35,37 +58,36 @@ public class RobotContainer
     }
 
     private void configureBindings() 
-    {
-         m_drive.setDefaultCommand(m_drive.run(() -> 
-         m_drive.tank(
-             processInput(m_controller.getRawAxis(2)) * Constants.DriveMaxSpeed,
-             processInput(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed
-         )));
-
-        // m_drive.setDefaultCommand(m_drive.run(() -> {
-        //     Logger.recordOutput("inputL", processInput(m_controller.getRawAxis(2)));
-        //     Logger.recordOutput("inputR", processInput(m_controller.getRawAxis(1)));
-        // }));
-        // m_controller.b().onTrue(Commands.runOnce(() -> {
-        //     m_drive.reset();
-        // })); 
-
-        // m_controller.x().and(m_controller.leftBumper())
-        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
-        //             System.out.println("Quasistatic Forward")),
-        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
-        // m_controller.x().and(m_controller.rightBumper())
-        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
-        //             System.out.println("Quasistatic Reverse")),
-        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
-        // m_controller.y().and(m_controller.leftBumper())
-        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
-        //             System.out.println("Dynamic Forward")),
-        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
-        // m_controller.y().and(m_controller.rightBumper())
-        //     .onTrue(Commands.sequence(Commands.runOnce(() -> 
-        //             System.out.println("Dynamic Reverse")),
-        //         m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
+    {   
+        m_drive.setDefaultCommand(m_drive.run(() -> {
+            double[] inputs = correctInputs(
+                    m_controller.getRawAxis(2),
+                    m_controller.getRawAxis(1));
+            m_drive.tank(
+                inputs[0] * Constants.DriveMaxSpeed,
+                inputs[1] * Constants.DriveMaxSpeed);
+        }));
+
+        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+            new Pose2d(0, 0, new Rotation2d(0)),
+            // List.of(),
+            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+            new Pose2d(3, 0, new Rotation2d(0)), 
+            m_drive.trajectoyConfig);
+
+        new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
+
+        // m_chain.setDefaultCommand(m_chain.run(() ->
+        // m_chain.atPercentage(
+        //     processInput(m_controller.getRawAxis(2))
+        // )));
+
+
+        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kForward));
+        // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kReverse));
+        // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kForward));
+        // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kReverse));
+
     }
 
     public Command getAutonomousCommand() 
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 5756650..5f34119 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -2,7 +2,9 @@ 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.RamseteController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -10,15 +12,17 @@ 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.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Velocity;
 import edu.wpi.first.units.Voltage;
 import edu.wpi.first.units.MutableMeasure;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
@@ -47,8 +51,10 @@ public class Drivetrain extends SubsystemBase
 
     private SimpleMotorFeedforward m_feedforward = Constants.motor_feedforward; 
 
-    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 final double kP = 0.363;
+
+    private ProfiledPIDController m_pidL = new ProfiledPIDController(kP, 0, 0, Constants.motor_constraints);
+    private ProfiledPIDController m_pidR = new ProfiledPIDController(kP, 0, 0, Constants.motor_constraints);
     
     private DifferentialDriveKinematics m_kinematics = 
         new DifferentialDriveKinematics(Constants.TrackWidth);
@@ -59,30 +65,39 @@ public class Drivetrain extends SubsystemBase
     private final MutableMeasure<Voltage> m_voltage = mutable(Volts.of(0));
     private final MutableMeasure<Distance> m_distance = mutable(Meters.of(0));
     private final MutableMeasure<Velocity<Distance>> m_velocity = mutable(MetersPerSecond.of(0));
+
     private final SysIdRoutine m_sysIdRoutine = new SysIdRoutine(
         new SysIdRoutine.Config(), 
         new SysIdRoutine.Mechanism(
             (Measure<Voltage> volts) -> {
-                m_motorL_leader.set(-volts.in(Volts));
-                m_motorR_leader.set(-volts.in(Volts));
+                m_motorL_leader.setVoltage(volts.in(Volts));
+                m_motorR_leader.setVoltage(volts.in(Volts));
             },
             log -> {
                 log.motor("drive-left")
                     .voltage(
                         m_voltage.mut_replace(
-                            m_motorL_leader.get() * RobotController.getBatteryVoltage(), Volts)
+                            m_motorL_leader.getAppliedOutput() * m_motorL_leader.getBusVoltage(), Volts)
                     )
                     .linearPosition(m_distance.mut_replace(m_encoderL.getDistance(), Meters))
                     .linearVelocity(m_velocity.mut_replace(m_encoderL.getRate(), MetersPerSecond));
                 log.motor("driver-right")
-                .voltage(
+                    .voltage(
                         m_voltage.mut_replace(
-                            m_motorR_leader.get() * RobotController.getBatteryVoltage(), Volts)
+                            m_motorR_leader.getAppliedOutput() * m_motorR_follower.getBusVoltage(), Volts)
                     )
                     .linearPosition(m_distance.mut_replace(m_encoderR.getDistance(), Meters))
                     .linearVelocity(m_velocity.mut_replace(m_encoderR.getRate(), MetersPerSecond));
+                
             }, this));
 
+    public final TrajectoryConfig trajectoyConfig = 
+        new TrajectoryConfig(
+            Constants.AutonomousMaxSpeed,
+            Constants.AutonomousMaxAcceleration)
+        .setKinematics(m_kinematics);
+
+
     public Drivetrain()
     {
         m_gyro.reset();
@@ -94,8 +109,8 @@ public class Drivetrain extends SubsystemBase
         m_motorL_leader.setInverted(true);
         m_encoderL.setReverseDirection(true);
 
-        m_encoderL.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
-        m_encoderR.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
+        m_encoderL.setDistancePerPulse(Math.PI * Constants.WheelDiameter / 10.71 / Constants.EncoderResolution);
+        m_encoderR.setDistancePerPulse(Math.PI * Constants.WheelDiameter / 10.71 / Constants.EncoderResolution);
         
         m_encoderL.reset();
         m_encoderR.reset();
@@ -170,41 +185,36 @@ public class Drivetrain extends SubsystemBase
 
         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());
+ 
+        Logger.recordOutput(getName() + "/encoderRateL", m_encoderL.getRate());
+        Logger.recordOutput(getName() + "/encoderRateR", m_encoderR.getRate());
+        Logger.recordOutput(getName() + "/encoderDL", m_encoderL.getDistance());
+        Logger.recordOutput(getName() + "/encoderDR", m_encoderR.getDistance());
     }
 
     public Command followTrajectory(Trajectory trajectory)
     {
-        // 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();
+        RamseteCommand ramsete = new RamseteCommand(
+            trajectory, this::pose2d, 
+            new RamseteController(2.0, 0.7), 
+            m_feedforward, m_kinematics, 
+            () -> new DifferentialDriveWheelSpeeds(m_encoderL.getRate(), m_encoderR.getRate()), 
+            new PIDController(kP, 0, 0),
+            new PIDController(kP, 0, 0), 
+            this::voltage, this);
+
+        return Commands.runOnce(() -> this.reset())
+                       .andThen(ramsete)
+                       .andThen(Commands.runOnce(() -> voltage(0, 0)));
     }
 
     public Command sysIdQausistatic(SysIdRoutine.Direction direction)
     {
-        return Commands.race(m_sysIdRoutine.quasistatic(direction),
-            this.runOnce(() -> {
-                while(m_encoderL.getDistance() < 6 && 
-                      m_encoderR.getDistance() < 6);
-            }));
+        return m_sysIdRoutine.quasistatic(direction);
     }
 
     public Command sysIdDynamic(SysIdRoutine.Direction direction)
     {
-        return Commands.race(m_sysIdRoutine.dynamic(direction),
-        this.runOnce(() -> {
-            while(m_encoderL.getDistance() < 6 && 
-                  m_encoderR.getDistance() < 6);
-        }));
+        return m_sysIdRoutine.dynamic(direction);
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java
index aa4563b..580af8b 100644
--- a/src/main/java/frc/robot/util/IMUGyro.java
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -65,7 +65,7 @@ public class IMUGyro
  
     public Rotation2d toRotation2d()
     {
-        return new Rotation2d(yaw % 360);
+        return new Rotation2d(-yaw);
     }
  
     public void calibrateGyro(int iter)