tested

4 files changed, 82 insertions, 44 deletions

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 41c9689..6d471db 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -4,28 +4,29 @@ import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.I2C.Port;
 import frc.robot.util.IMUGyro;
 
 public class Constants 
 {
-    public static final int PortDriverController = 0;
+    public static final GenericHID controller = new GenericHID(1);
 
-    public static final double DriveMaxSpeed = 20; // m/s
+    public static final double DriveMaxSpeed = 5; // m/s
     public static final double DriveMaxAngular = Math.PI; // rad/s?
 
-    public static final double WheelRadius = 0; // in meters
-    public static final double TrackWidth = 0;  // in meters
+    public static final double WheelRadius = 0.0254; // in meters
+    public static final double TrackWidth = 0.74;  // in meters
 
     public static final double EncoderResolution = 4096;
     
     public static final IMUGyro gyro = new IMUGyro(new I2C(Port.kOnboard, 0x68));
 
-    public static final CANSparkMax motorL_leader = new CANSparkMax(40, MotorType.kBrushed);
-    public static final CANSparkMax motorL_follower = new CANSparkMax(41, MotorType.kBrushed);
-    public static final CANSparkMax motorR_leader = new CANSparkMax(42, MotorType.kBrushed);
-    public static final CANSparkMax motorR_follower = new CANSparkMax(43, MotorType.kBrushed);
+    public static final CANSparkMax motorL_leader = new CANSparkMax(11, MotorType.kBrushed);
+    public static final CANSparkMax motorL_follower = new CANSparkMax(12, MotorType.kBrushed);
+    public static final CANSparkMax motorR_leader = new CANSparkMax(13, MotorType.kBrushed);
+    public static final CANSparkMax motorR_follower = new CANSparkMax(14, MotorType.kBrushed);
 
     public static final Encoder encoderL = new Encoder(0, 1);
     public static final Encoder encoderR = new Encoder(2, 3);
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d91ff78..d2cede4 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,34 +4,53 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
 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 frc.robot.subsystems.Drivetrain;
 
 public class RobotContainer 
 {
     private final Drivetrain m_drive = new Drivetrain();
 
-    private final SlewRateLimiter m_speedlimiter = new SlewRateLimiter(3);
-    private final SlewRateLimiter m_rotlimiter = new SlewRateLimiter(3);
-
-    private final GenericHID m_controller = 
-        new GenericHID(Constants.PortDriverController);
+    private final GenericHID m_controller = Constants.controller;
 
     public RobotContainer() 
     {
         configureBindings();
     }
 
+    private double deadzone(double low, double high, double val) {
+        if(val > low && val < high) return 0.0;
+        return val;
+    }
+
     private void configureBindings() 
     {
         m_drive.setDefaultCommand(m_drive.run(() -> 
-        m_drive.drive(
-            m_speedlimiter.calculate(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed,
-            m_rotlimiter.calculate(m_controller.getRawAxis(2)) * Constants.DriveMaxAngular
-        ))); 
+        // m_drive.arcade(
+        //     deadzone(-1, 1, m_controller.getRawAxis(1) * Constants.DriveMaxSpeed),
+        //     deadzone(-1, 1, m_controller.getRawAxis(0) * Constants.DriveMaxAngular)
+        // ))); 
+        m_drive.tank(
+            deadzone(-0.09, 0.09, m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed,
+            deadzone(-0.09, 0.09, m_controller.getRawAxis(3)) * Constants.DriveMaxSpeed
+        )));
+
+        new Trigger(() -> m_controller.getRawButton(1)).onTrue(Commands.runOnce(() -> {
+            Pose2d pose = m_drive.pose2d();
+            System.out.println("x: " + pose.getX() +" y: "+ pose.getY());
+            System.out.println("yaw: " + Constants.gyro.toRotation2d().getDegrees());
+        })); 
+        new Trigger(() -> m_controller.getRawButton(2)).onTrue(Commands.runOnce(() -> {
+            System.out.println("Reset");
+            m_drive.reset();
+        })); 
+        // new Trigger(() -> m_controller.getRawButton(3)).onTrue(Commands.run(() -> {
+            // m_drive.tank(5, 5);
+        // }));
     }
 
     public Command getAutonomousCommand() 
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 8f1b975..80ed06e 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -4,6 +4,7 @@ import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.math.controller.PIDController;
 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;
@@ -25,7 +26,7 @@ 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);
+    private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1.5, 2.82, 0.47);
 
     private final PIDController m_pidL = new PIDController(1, 0, 0);
     private final PIDController m_pidR = new PIDController(1, 0, 0);
@@ -38,14 +39,13 @@ public class Drivetrain extends SubsystemBase
     public Drivetrain()
     {
         m_gyro.reset();
+        m_gyro.calibrateGyro(10000);
 
         m_motorR_follower.follow(m_motorR_leader);
         m_motorL_follower.follow(m_motorL_leader);
+        m_motorR_leader.setInverted(true);
 
-        // perhaps invert
-        // m_motorR_leader.setInverted(true);
-
-        m_encoderL.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
+        m_encoderL.setDistancePerPulse(-2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
         m_encoderR.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
         
         m_encoderL.reset();
@@ -59,11 +59,17 @@ public class Drivetrain extends SubsystemBase
 
     // speed - linear velocity in m/s
     // rot   - angular velocity in rad/s
-    public void drive(double speed, double rot) 
+    public void arcade(double speed, double rot) 
     {
         speed(m_kinematics.toWheelSpeeds(new ChassisSpeeds(speed, 0.0, rot)));
     }
 
+    // in m/s
+    public void tank(double left, double right) 
+    {
+        speed(new DifferentialDriveWheelSpeeds(left, right));
+    }
+
     public void speed(DifferentialDriveWheelSpeeds speeds) 
     {
         m_motorL_leader.setVoltage(
@@ -77,6 +83,24 @@ public class Drivetrain extends SubsystemBase
         );
     }
 
+    public Pose2d pose2d() 
+    {
+        System.out.println("Distance: "+m_encoderL.getDistance());
+        System.out.println("Distance: "+m_encoderR.getDistance());
+        return m_odometry.getPoseMeters();
+    }
+
+    public void reset() 
+    {
+        m_gyro.reset();
+        m_encoderL.reset();
+        m_encoderR.reset();
+        m_odometry = new DifferentialDriveOdometry(
+            m_gyro.toRotation2d(),
+            Constants.encoderL.getDistance(),
+            Constants.encoderR.getDistance());
+    }    
+
     @Override
     public void periodic() 
     {
diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java
index 1ea1983..3860b85 100644
--- a/src/main/java/frc/robot/util/IMUGyro.java
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -12,7 +12,11 @@ public class IMUGyro
     public double roll;
     public double yaw;
  
-    private double gyroOffsets[] = new double[3];
+    private double gyroOffsets[] = {
+        0.312449302886265,
+        0.04883417185589417,
+        -0.12945104079301352
+    };
     
     // for deltatime
     private double prev_time = Timer.getFPGATimestamp();
@@ -27,7 +31,7 @@ public class IMUGyro
     private final int ACCL_ADDR = 0x3B;
     private final int ACCL_CONFIG = 0x1C;
  
-    private final double SCALE_GYRO = (Math.PI/(180*16.384));
+    private final double SCALE_GYRO = (Math.PI/(180*16.384) * 2 /*??*/);
     private final double SCALE_ACCL = (1/16.384);
  
     public IMUGyro(I2C gyro) 
@@ -35,15 +39,15 @@ public class IMUGyro
         mpu = gyro;
         reset();
 
-        mpu.write(CONFIG_ADDR, 0b00000110);
-        mpu.write(SMPLRT_DIV, 0b00010011);
-        mpu.write(GYRO_CONFIG, 0b00001000);
-        mpu.write(ACCL_CONFIG, 0b00000000);
+        // mpu.write(CONFIG_ADDR, 0b000000);
+        mpu.write(SMPLRT_DIV, 0b00000010);
+        // mpu.write(GYRO_CONFIG, 0b00011000);
+        // mpu.write(ACCL_CONFIG, 0b00000000);
     }
  
     public void reset()
     {
-        mpu.write(PWR_MGMT_1, 0b10001000);
+        mpu.write(PWR_MGMT_1, 0b000001000);
 
         pitch = 0;
         roll = 0;
@@ -56,21 +60,11 @@ public class IMUGyro
         double dt = cur_time - prev_time;
         prev_time = cur_time;
 
-        // double[] gRates = gyroRates();
-
-        // roll = gRates[0] * dt;
-        // pitch = gRates[1] * dt;
-        // yaw = gRates[2] * dt;
-
         double[] gRates = gyroRates();
-        double[] aRates = acclRates();
-
-        mahony(30, 0, gRates, aRates, dt);
-        double[] angles = fromQuaternion(q);
 
-        roll = angles[0]; 
-        pitch = angles[1]; 
-        yaw = angles[2];
+        roll += gRates[0] * dt;
+        pitch += gRates[1] * dt;
+        yaw += gRates[2] * dt;
     }
  
     public Rotation2d toRotation2d()