Merge branch 'develop' of https://github.com/Blu-Cru/Deocde into develop

Author: acurra3398

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/drivetrain/localization/Octoquad.java

@@ -41,7 +41,7 @@ public Octoquad(OctoQuad octoquad){
         OctoQuad.LocalizerDataBlock info = octoquad.readLocalizerData();
         pose = new Pose2d(info.posX_mm, info.posY_mm, info.heading_rad);
         vel = new Pose2d(info.velX_mmS, info.velY_mmS, info.velHeading_radS);
-
+        Robot.getInstance().positionHistory.add(pose, vel);
     }
 
     @Override
@@ -50,6 +50,7 @@ public void read() {
         //dividing for unit conversion
         pose = new Pose2d(info.posX_mm / 25.4, info.posY_mm / 25.4, info.heading_rad);
         vel = new Pose2d(info.velX_mmS / 25.4, info.velY_mmS / 25.4, info.velHeading_radS);
+        Robot.getInstance().positionHistory.add(pose, vel);
     }
     /**
      * returns pose in x,y,h in inch,inch,radian

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/outtake/TagCamera.java

@@ -28,7 +28,7 @@
 
 import java.util.ArrayList;
 
-public class    TagCamera implements BluSubsystem, Subsystem {
+public class TagCamera implements BluSubsystem, Subsystem {
     AprilTagProcessor tags;
     VisionPortal portal;
     AprilTagDetection detection;
@@ -82,6 +82,8 @@ public TagCamera(){
     @Override
     public void init() {
         read();
+        xFilter.setVal(Robot.getInstance().sixWheelDrivetrain.getPos().getX());
+        yFilter.setVal(Robot.getInstance().sixWheelDrivetrain.getPos().getY());
     }
 
     @Override
@@ -155,6 +157,7 @@ public void read() {
                 // now that we know offsets we can assume we havent changed off that much
                 botposeOnTheMove = new Pose2d(Robot.getInstance().sixWheelDrivetrain.getPos().vec().addNotInPlace(offset),
                         Robot.getInstance().sixWheelDrivetrain.getPos().getH());
+                xFilter.update(Robot.getInstance().sixWheelDrivetrain.getPos().getX(), botposeOnTheMove.getX());
 
                 //non-vector code
                 /*double camToTagFieldX = dx * Math.cos(angle) - dy * Math.sin(angle);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/subsytems/outtake/turret/Turret.java

@@ -36,9 +36,9 @@ public class Turret implements BluSubsystem, Subsystem {
         public static double kP = 0.028;
     public static double kI = 0.02;
     public static double kD = 0.0018;
-    public static double kPTags = 0.0011;
+    public static double kPTags = 0.00125;
     public static double kITags = 0;
-    public static double kDTags = 0.000215;
+    public static double kDTags = 0.0002125;
 
     public static double acceptableError = 0.5;
     public static double powerClip = 1;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/common/util/KalmanFilter.java

@@ -20,27 +20,27 @@ public KalmanFilter(double x, double Q, double R, double P, double K) {
         this.P = P;
         this.K = K;
     }
-    public void update(double input) {
-        double predictedinput = input-lastinput;
+    public void update(double firstInput, double secondInput) {
+        double predictedinput = firstInput-lastinput;
         u = predictedinput; // The delta
         x = x_previous + u;
 
         P = p_previous + Q;
 
         K = P/(P + R);
 
-        z = input; // The known current input
+        z = secondInput; // The known current input
 
         x = x + K * (z - x);
 
         P = (1 - K) * P;
 
         x_previous = x;
         p_previous = P;
-        lastinput = input;
+        lastinput = firstInput;
     }
 
-    public void update(double input, double givenlastinput) {
+    /*public void update(double input, double givenlastinput) {
         double predictedinput = input-givenlastinput;
         u = predictedinput; // The delta
         x = x_previous + u;
@@ -58,6 +58,9 @@ public void update(double input, double givenlastinput) {
         x_previous = x;
         p_previous = P;
         lastinput = input;
+    }*/
+    public void setVal(double val){
+        this.x = val;
     }
 
     public double get() {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/Tele.java

@@ -54,9 +54,9 @@ public enum State{
     public void initialize(){
         reportTelemetry = true;
         robot.clear();
+        addSixWheel();
         robot.addTurretCam();
         //addLLTagDetector();
-        addSixWheel();
         addIntake();
         addElevator();
         addTransfer();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blucru/opmodes/test/TurretLockOnGoalTest.java

@@ -11,14 +11,19 @@
 public class TurretLockOnGoalTest extends BluLinearOpMode {
     public void initialize(){
         robot.clear();
-        robot.addTurretCam();
         addSixWheel();
-        addTurret();
+        robot.addTurretCam();
+        //addLLTagDetector();
+        addIntake();
+        addElevator();
+        addTransfer();
         addShooter();
+        addTurret();
+        addTilt();
         Globals.multiTelemetry = new MultipleTelemetry(telemetry);
     }
 
-    public void periodic(){
+    public void periodic() throws InterruptedException {
         if (gamepad1.a){
             turret.lockOnGoal();
         }
@@ -35,5 +40,6 @@ public void periodic(){
         if (driver1.pressedLeftBumper()){
             Globals.setAlliance(Alliance.BLUE);
         }
+        //Thread.sleep(10);
     }
 }