Created odometry for Drivetrain.java (#33)

Before issuing a pull request: 
- [ ] Ensure all constants are in the Constants class
- [ ] Update the backup OpMode if needed
- [ ] Update README.md

Author: ngarnsworthy

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java

@@ -1,11 +1,31 @@
 package org.firstinspires.ftc.teamcode;
 
+import com.arcrobotics.ftclib.geometry.Translation2d;
+import com.arcrobotics.ftclib.kinematics.wpilibkinematics.MecanumDriveKinematics;
+
 public final class Constants {
     public static class Drivetrain {
         public static final String flMotorName = "frontLeftDrive";
         public static final String frMotorName = "frontRightDrive";
         public static final String blMotorName = "backLeftDrive";
         public static final String brMotorName = "backRightDrive";
+
+        private static final double length = 0.2873375;
+        private static final  double width = 0.3444875;
+        public static final Translation2d frontLeftLocation =
+                new Translation2d(length/2, width/2);
+        public static final Translation2d frontRightLocation =
+                new Translation2d(length/2, -width/2);
+        public static final Translation2d backLeftLocation =
+                new Translation2d(-length/2, width/2);
+        public static final Translation2d backRightLocation =
+                new Translation2d(-length/2, -width/2);
+
+        public static final MecanumDriveKinematics kinematics = new MecanumDriveKinematics
+                (
+                        frontLeftLocation, frontRightLocation,
+                        backLeftLocation, backRightLocation
+                );
     }
 
     public static class Shooter {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/BackupOpMode.java

@@ -3,6 +3,8 @@
 import com.arcrobotics.ftclib.command.CommandOpMode;
 import com.arcrobotics.ftclib.gamepad.GamepadEx;
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.arcrobotics.ftclib.hardware.GyroEx;
+import com.arcrobotics.ftclib.hardware.RevIMU;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.teamcode.Constants;
@@ -24,12 +26,15 @@ public void initialize() {
         driverOp = new GamepadEx(gamepad1);
         coOp = new GamepadEx(gamepad2);
 
+        GyroEx gyroscope = new RevIMU(hardwareMap);
+
         drivetrain = new Drivetrain(
                 hardwareMap,
                 Constants.Drivetrain.flMotorName,
                 Constants.Drivetrain.frMotorName,
                 Constants.Drivetrain.blMotorName,
-                Constants.Drivetrain.brMotorName
+                Constants.Drivetrain.brMotorName,
+                gyroscope
         );
         drivetrain.setDefaultCommand(new UserDrive(drivetrain, driverOp));
         register(drivetrain);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/PrimaryOpMode.java

@@ -3,7 +3,10 @@
 import com.arcrobotics.ftclib.command.CommandOpMode;
 import com.arcrobotics.ftclib.gamepad.GamepadEx;
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.arcrobotics.ftclib.hardware.GyroEx;
+import com.arcrobotics.ftclib.hardware.RevIMU;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.IntegratingGyroscope;
 
 import org.firstinspires.ftc.teamcode.Constants;
 import org.firstinspires.ftc.teamcode.commands.RunIntake;
@@ -21,18 +24,22 @@ public class PrimaryOpMode extends CommandOpMode {
 
     private GamepadEx driverOp;
     private GamepadEx coOp;
+    private GyroEx gyroscope;
 
     @Override
     public void initialize() {
         driverOp = new GamepadEx(gamepad1);
         coOp = new GamepadEx(gamepad2);
 
+        gyroscope = new RevIMU(hardwareMap);
+
         drivetrain = new Drivetrain(
                 hardwareMap,
                 Constants.Drivetrain.flMotorName,
                 Constants.Drivetrain.frMotorName,
                 Constants.Drivetrain.blMotorName,
-                Constants.Drivetrain.brMotorName
+                Constants.Drivetrain.brMotorName,
+                gyroscope
         );
         drivetrain.setDefaultCommand(new UserDrive(drivetrain, driverOp));
         register(drivetrain);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java

@@ -2,26 +2,60 @@
 
 import com.arcrobotics.ftclib.command.SubsystemBase;
 import com.arcrobotics.ftclib.drivebase.MecanumDrive;
+import com.arcrobotics.ftclib.geometry.Pose2d;
+import com.arcrobotics.ftclib.geometry.Rotation2d;
+import com.arcrobotics.ftclib.hardware.GyroEx;
 import com.arcrobotics.ftclib.hardware.motors.Motor;
+import com.arcrobotics.ftclib.kinematics.wpilibkinematics.MecanumDriveOdometry;
+import com.arcrobotics.ftclib.kinematics.wpilibkinematics.MecanumDriveWheelSpeeds;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
+import org.firstinspires.ftc.teamcode.Constants;
+
 public class Drivetrain extends SubsystemBase {
     private final Motor fL, fR, bL, bR;
+    private final Motor.Encoder fLE, fRE, bLE, bRE;
+
     private final MecanumDrive drive;
+    private final MecanumDriveOdometry odometry;
+    private final GyroEx gyroscope;
+    private Pose2d pose;
 
-    public Drivetrain(HardwareMap hardwareMap, String flName, String frName, String blName, String brName) {
+    public Drivetrain(HardwareMap hardwareMap, String flName, String frName, String blName, String brName, GyroEx gyroscope) {
         fL = new Motor(hardwareMap, flName);
+        fLE = fL.encoder;
         fR = new Motor(hardwareMap, frName);
+        fRE = fR.encoder;
         bL = new Motor(hardwareMap, blName);
+        bLE = bL.encoder;
         bR = new Motor(hardwareMap, brName);
+        bRE = bR.encoder;
+        this.gyroscope = gyroscope;
 
         drive = new MecanumDrive(fL, fR, bL, bR);
+
+        odometry = new MecanumDriveOdometry(
+                        Constants.Drivetrain.kinematics, gyroscope.getRotation2d(),
+                        new Pose2d(0.0, 0.0, new Rotation2d()
+                        ));
+    }
+
+    @Override
+    public void periodic(){
+        MecanumDriveWheelSpeeds wheelSpeeds = new MecanumDriveWheelSpeeds(
+                        fLE.getRate(), fRE.getRate(),
+                        bLE.getRate(), bRE.getRate()
+                );
+
+        pose = odometry.updateWithTime(System.currentTimeMillis()/1000.0, gyroscope.getRotation2d(), wheelSpeeds);
     }
 
     public void move(double strafe, double forward, double rotate) {
         drive.driveRobotCentric(strafe, forward, rotate);
     }
 
+    public Pose2d getPose() {return pose;}
+
     public void stop() {
         drive.stop();
     }