imu added

4 files changed, 232 insertions, 43 deletions

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f517f08..41c9689 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -1,37 +1,32 @@
 package frc.robot;
 
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.AnalogGyro;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
+import frc.robot.util.IMUGyro;
 
 public class Constants 
 {
-    // other constants
-
     public static final int PortDriverController = 0;
 
-    public static final double DriveMaxSpeed = 0;
-    public static final double DriveMaxAngular = 0;
+    public static final double DriveMaxSpeed = 20; // 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 EncoderResolution = 4096;
-
-    // public static final DifferentialDrive drive = new DifferentialDrive(...);
     
-    public static final AnalogGyro gyro = new AnalogGyro(...);
-
-    public static final PWMSparkMax motorL_leader = new PWMSparkMax(...);
-    public static final PWMSparkMax motorL_follower = new PWMSparkMax(...);
-    public static final PWMSparkMax motorR_leader = new PWMSparkMax(...);
-    public static final PWMSparkMax motorR_follower = new PWMSparkMax(...);
-
+    public static final IMUGyro gyro = new IMUGyro(new I2C(Port.kOnboard, 0x68));
 
-    public static final Encoder encoderL = new Encoder(...);
-    public static final Encoder encoderR = new Encoder(...);
+    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 SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(...);
+    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 e08bf8d..d91ff78 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -5,9 +5,9 @@
 package frc.robot;
 
 import edu.wpi.first.math.filter.SlewRateLimiter;
+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.CommandXboxController;
 import frc.robot.subsystems.Drivetrain;
 
 public class RobotContainer 
@@ -17,8 +17,8 @@ public class RobotContainer
     private final SlewRateLimiter m_speedlimiter = new SlewRateLimiter(3);
     private final SlewRateLimiter m_rotlimiter = new SlewRateLimiter(3);
 
-    private final CommandXboxController m_dcontroller = 
-        new CommandXboxController(Constants.PortDriverController);
+    private final GenericHID m_controller = 
+        new GenericHID(Constants.PortDriverController);
 
     public RobotContainer() 
     {
@@ -27,12 +27,11 @@ public class RobotContainer
 
     private void configureBindings() 
     {
-        // drivetrain controls
         m_drive.setDefaultCommand(m_drive.run(() -> 
-            m_drive.drive(
-                m_speedlimiter.calculate(m_dcontroller.getLeftY()) * Constants.DriveMaxSpeed,
-                m_rotlimiter.calculate(m_dcontroller.getRightX()) * Constants.DriveMaxAngular
-        )));
+        m_drive.drive(
+            m_speedlimiter.calculate(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed,
+            m_rotlimiter.calculate(m_controller.getRawAxis(2)) * Constants.DriveMaxAngular
+        ))); 
     }
 
     public Command getAutonomousCommand() 
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 14c9d50..8f1b975 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -1,30 +1,31 @@
 package frc.robot.subsystems;
 
+import com.revrobotics.CANSparkMax;
+
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 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.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.util.IMUGyro;
 
 public class Drivetrain extends SubsystemBase 
 {
-    private AnalogGyro m_gyro = Constants.gyro;
+    private IMUGyro m_gyro = Constants.gyro;
 
-    private PWMSparkMax m_motorL_leader = Constants.motorL_leader;
-    private PWMSparkMax m_motorL_follower = Constants.motorL_follower;
-    private PWMSparkMax m_motorR_leader = Constants.motorR_leader;
-    private PWMSparkMax m_motorR_follower = Constants.motorR_follower;
+    private CANSparkMax m_motorL_leader = Constants.motorL_leader;
+    private CANSparkMax m_motorL_follower = Constants.motorL_follower;
+    private CANSparkMax m_motorR_leader = Constants.motorR_leader;
+    private CANSparkMax m_motorR_follower = Constants.motorR_follower;
 
     private Encoder m_encoderL = Constants.encoderL;
     private Encoder m_encoderR = Constants.encoderR;
 
-    private SimpleMotorFeedforward m_feedforward = Constants.feedforward;
+    private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1);
 
     private final PIDController m_pidL = new PIDController(1, 0, 0);
     private final PIDController m_pidR = new PIDController(1, 0, 0);
@@ -36,10 +37,10 @@ public class Drivetrain extends SubsystemBase
 
     public Drivetrain()
     {
-        m_gyro.reset();        
+        m_gyro.reset();
 
-        m_motorL_leader.addFollower(m_motorL_follower);
-        m_motorR_leader.addFollower(m_motorR_follower);
+        m_motorR_follower.follow(m_motorR_leader);
+        m_motorL_follower.follow(m_motorL_leader);
 
         // perhaps invert
         // m_motorR_leader.setInverted(true);
@@ -51,7 +52,7 @@ public class Drivetrain extends SubsystemBase
         m_encoderR.reset();
 
         m_odometry = new DifferentialDriveOdometry(
-            m_gyro.getRotation2d(),
+            m_gyro.toRotation2d(),
             Constants.encoderL.getDistance(),
             Constants.encoderR.getDistance());
     }
@@ -79,10 +80,12 @@ public class Drivetrain extends SubsystemBase
     @Override
     public void periodic() 
     {
+        m_gyro.update();
+
         m_odometry.update(
-            Constants.gyro.getRotation2d(),
-            Constants.encoderL.getDistance(),
-            Constants.encoderR.getDistance());
+            m_gyro.toRotation2d(),
+            m_encoderL.getDistance(),
+            m_encoderR.getDistance());
     }
 
     @Override
diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java
new file mode 100644
index 0000000..1ea1983
--- /dev/null
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -0,0 +1,192 @@
+package frc.robot.util;
+ 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.Timer;
+ 
+public class IMUGyro 
+{
+    public I2C mpu;
+ 
+    public double pitch;
+    public double roll;
+    public double yaw;
+ 
+    private double gyroOffsets[] = new double[3];
+    
+    // for deltatime
+    private double prev_time = Timer.getFPGATimestamp();
+ 
+    private final int PWR_MGMT_1 = 0x6B;
+    private final int CONFIG_ADDR = 0x1A;
+    private final int SMPLRT_DIV = 0x19;
+ 
+    private final int GYRO_ADDR = 0x43;
+    private final int GYRO_CONFIG = 0x1B;
+ 
+    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_ACCL = (1/16.384);
+ 
+    public IMUGyro(I2C gyro) 
+    {
+        mpu = gyro;
+        reset();
+
+        mpu.write(CONFIG_ADDR, 0b00000110);
+        mpu.write(SMPLRT_DIV, 0b00010011);
+        mpu.write(GYRO_CONFIG, 0b00001000);
+        mpu.write(ACCL_CONFIG, 0b00000000);
+    }
+ 
+    public void reset()
+    {
+        mpu.write(PWR_MGMT_1, 0b10001000);
+
+        pitch = 0;
+        roll = 0;
+        yaw = 0;
+    }
+ 
+    public void update()
+    {
+        double cur_time = Timer.getFPGATimestamp();
+        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];
+    }
+ 
+    public Rotation2d toRotation2d()
+    {
+        return new Rotation2d(yaw % 360);
+    }
+ 
+    public void calibrateGyro(int iter)
+    {
+        gyroOffsets = new double[3];
+
+        double x = 0, y = 0, z = 0;
+        for(int i = 0; i < iter; i++) 
+        {
+            double[] rates = gyroRates();
+            x += rates[0];
+            y += rates[1];
+            z += rates[2];
+        }
+ 
+        gyroOffsets[0] = x/iter;
+        gyroOffsets[1] = y/iter;
+        gyroOffsets[2] = z/iter;
+    }
+ 
+    private double[] gyroRates()
+    {
+        return scaledRates(GYRO_ADDR, SCALE_GYRO);
+    }
+ 
+    private double[] acclRates()
+    {
+        return scaledRates(ACCL_ADDR, SCALE_ACCL);
+    }
+ 
+    private double[] scaledRates(int addr, double scale)
+    {
+        byte[] buffer = new byte[6];
+        mpu.read(addr, 6, buffer);
+        double dx = decode(buffer[0], buffer[1]) * scale - gyroOffsets[0];
+        double dy = decode(buffer[2], buffer[3]) * scale - gyroOffsets[1];
+        double dz = decode(buffer[4], buffer[5]) * scale - gyroOffsets[2];
+        return new double[] {dx, dy, dz};
+    }
+ 
+    private double decode(byte first, byte second) 
+    {
+        return ((first << 8) | second);
+    }
+
+    // mahony quaternion
+    private double[] q = {1.0, 0.0, 0.0, 0.0};
+    // mahony integral term
+    private double[] i = new double[3];
+
+    private double[] fromQuaternion(double[] q)
+    {
+        return new double[] {
+            Math.atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2])),
+            Math.asin(2.0 * (q[0] * q[2] - q[1] * q[3])),
+            -Math.atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3]))
+        };
+    }
+
+    private void mahony(double Ki, double Kp, double[] gRates, double[] aRates, double dt)
+    {
+        double gx = gRates[0], gy = gRates[1], gz = gRates[2];
+        double ax = aRates[0], ay = aRates[1], az = aRates[2];
+
+        double tmp = ax * ax + ay * ay + az * az;
+        if(tmp > 0.0) 
+        {
+            tmp = Math.sqrt(tmp);
+            ax /= tmp; ay /= tmp; az /= tmp;
+
+            // estimated direction of gravity in the body frame
+            double vx = q[1] * q[3] - q[0] * q[2];
+            double vy = q[0] * q[1] + q[2] * q[3];
+            double vz = q[0] * q[0] - 0.5f + q[3] * q[3];
+
+            // error is cross product
+            double ex = (ay * vz - az * vy);
+            double ey = (az * vx - ax * vz);
+            double ez = (ax * vy - ay * vx);
+
+            // integral feedback
+            if(Ki > 0.0) {
+                i[0] += Ki * ex * dt;
+                i[1] += Ki * ey * dt;
+                i[2] += Ki * ez * dt;
+                gx += i[0];  
+                gy += i[1];
+                gz += i[2];
+            }
+
+            gx += Kp * ex;
+            gy += Kp * ey;
+            gz += Kp * ez;            
+        }
+
+        // integrate quaternion
+        dt /= 2; // ??
+        gx *= dt; gy *= dt; gz *= dt;
+        
+        double qa = q[0], qb = q[1], qc = q[2], qd = q[3];
+        q[0] += (-qb * gx - qc * gy - qd * gz);
+        q[1] += (qa * gx + qc * gz - qd * gy);
+        q[2] += (qa * gy - qb * gz + qd * gx);
+        q[3] += (qa * gz + qb * gy - qc * gx);
+
+        // renormalize
+        double qnorm_factor = Math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+        q[0] /= qnorm_factor;
+        q[1] /= qnorm_factor;
+        q[2] /= qnorm_factor;
+        q[3] /= qnorm_factor;
+    }
+}
+