Fix driving

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/OpticalTrackingOdometrySensor.java

@@ -21,7 +21,7 @@ public void init(HardwareMap hwMap) {
         otos.setAngularUnit(AngleUnit.DEGREES);
         otos.setLinearUnit(DistanceUnit.INCH);
         otos.calibrateImu();
-        SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(X_POSITION,0,270);
+        SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(X_POSITION,0,180);
         otos.setOffset(offset);
         otos.resetTracking();
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java

@@ -16,7 +16,7 @@
 
 public class ScoreArm extends QQMechanism{
     public static final double TEST_SPEED = 0.55;
-    public static final int CLAW_RELEASE_OFFSET = 150;
+    public static final int CLAW_RELEASE_OFFSET = 250;
     DcMotor leftMotor;
     DcMotor rightMotor;
     TouchSensor limitSwitch;
@@ -36,7 +36,7 @@ public class ScoreArm extends QQMechanism{
 
     static int INTAKE_POSITION = 0;
     static int SCORING_POSITION = 350;
-    static int PLACING_POSITION = 750;
+    static int PLACING_POSITION = 850;
 
     PIDFController pidfController = new PIDFController(kP,kI,kD,kF,max,min);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java

@@ -37,7 +37,6 @@ public void init(HardwareMap hwMap) {
         clawServo = hwMap.get(Servo.class, "claw_servo");
         wristServo = hwMap.get(Servo.class, "wrist_servo");
         colorSensor = hwMap.get(ColorRangeSensor.class, "scoreclaw_color");
-        open();
     }
 
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java

@@ -5,18 +5,20 @@
 
 @Autonomous
 public class CloseParkAuto extends QQOpMode{
-    private final int autodelay = 23;
+    private final int autodelay = 0;
 
     ElapsedTime elapsedTime = new ElapsedTime();
     private double step = 0;
     public void init(){
         super.init();
         robot.otos.setOtosPosition(13.5,-61.5,0);
+        robot.scoringClaw.close();
     }
     public void start(){
         elapsedTime.reset();
     }
     public void loop(){
+        super.loop();
         if(step == 0){
             if(elapsedTime.seconds() > autodelay){
                 step = 1;
@@ -28,12 +30,8 @@ else if(step == 1){
                 step = 2;
             }
         }else if(step == 2){
-            boolean doneDriving = nav.driveToPositionIN(62,-60,0);
-            if(doneDriving){
-                step = 3;
-            }if(step == 3){
                 robot.scoreArm.goToIntake();
             }
         }
     }
-}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java

@@ -56,9 +56,9 @@ public void loop(){
             robot.scoreArm.goToPlace();
             robot.intakeArm.transfer();
         }else if (gamepad1.dpad_up){
-            robot.intakeSlides.manualPositionChange(MANUAL_CHANGE);
+            robot.scoreArm.manualPositionChange(MANUAL_CHANGE);
         }else if (gamepad1.dpad_down){
-            robot.intakeSlides.manualPositionChange(-MANUAL_CHANGE);
+            robot.scoreArm.manualPositionChange(-MANUAL_CHANGE);
         }
         if (gamepad1.left_stick_button){
             robot.controlHub.resetGyro();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/PreloadAuto.java

@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode.ftc16072.OpModes;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+@Autonomous
+public class PreloadAuto extends QQOpMode{
+    private final int autodelay = 0;
+
+    ElapsedTime elapsedTime = new ElapsedTime();
+    private double step = 0;
+    boolean isPlacing;
+    boolean chamberContactWasPressed;
+    int contactLostPos;
+    boolean clawWasClosed;
+    public void init(){
+        super.init();
+        robot.otos.setOtosPosition(7,-61.5,0);
+        robot.scoringClaw.close();
+        robot.intakeClaw.wristFlat();
+    }
+    public void start(){
+        elapsedTime.reset();
+    }
+    public void loop(){
+        super.loop();
+        if(robot.scoreArm.isChamberContacted()){
+            robot.scoreArm.goToScoring();
+            isPlacing = true;
+        }else if(!robot.scoreArm.isChamberContacted() && isPlacing && chamberContactWasPressed){
+            contactLostPos = robot.scoreArm.getCurrentPos();
+        }else if(isPlacing && robot.scoreArm.isTimeToReleaseClaw(contactLostPos)){
+            robot.scoringClaw.open();
+            isPlacing = false;
+        }
+
+
+        if(step == 0){
+            robot.scoreArm.goToPlace();
+            if(elapsedTime.seconds() > autodelay){
+                step = 1;
+            }
+        }else if(step == 1){
+            boolean isDoneDriving = nav.driveToPositionIN(38, -61.5,0);
+            if (isDoneDriving){
+                step = 2;
+            }
+        }else if(step == 2){
+            boolean isDoneDriving = nav.driveToPositionIN(14,-109.5,0);
+            if (isDoneDriving){
+                step = 3;
+            }
+        }else if (step == 3){
+            robot.scoreArm.goToIntake();
+        }
+        chamberContactWasPressed = robot.scoreArm.isChamberContacted();
+        clawWasClosed = robot.scoringClaw.isClawClosed();
+    }
+}
+
+
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/Navigation.java

@@ -7,9 +7,9 @@
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 
 public class Navigation {
-    public static final double MAX_TRANSLATE = 1;
+    public static final double MAX_TRANSLATE = 0.3;
     public static final double MIN_TRANSLATE = -MAX_TRANSLATE;
-    public static final double MAX_ROTATE = 1;
+    public static final double MAX_ROTATE = 0.3;
     public static final double MIN_ROTATE = -MAX_ROTATE;
     Robot robot;
 
@@ -19,11 +19,11 @@ public class Navigation {
     static double TRANSLATIONAL_KF = 0;
     static double TRANSLATIONAL_TOLERANCE_THRESHOLD = 0.5;
 
-    static double ROTATIONAL_KP = 0.1;
+    static double ROTATIONAL_KP = 0.01;
     static double ROTATIONAL_KI = 0.000;
     static double ROTATIONAL_KD = 0.00;
     static double ROTATIONAL_KF = 0;
-    static double ROTATIONAL_TOLERANCE_THRESHOLD = 1;
+    static double ROTATIONAL_TOLERANCE_THRESHOLD = 3;
 
     PIDFController PIDx, PIDy, PIDh;