unfortunately large commit for ri3w prep

ftc16072 · Sep 27, 2024 · 4a74afa · 4a74afa
1 parent 69535c9
commit 4a74afa
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 17 deletions.
diff --git a/...e/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/DoubleReverse4Bar.java b/...e/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/DoubleReverse4Bar.java
@@ -12,33 +12,33 @@
 import java.util.List;
 
 public class DoubleReverse4Bar extends QQMechanism{
-    public static final double TEST_SPEED = 0.2;
+    public static final double TEST_SPEED = 0.55;
+    public static double TEST_SPEED_OFF = 0.4;
     DcMotor leftMotor;
     DcMotor rightMotor;
-    public static double kP = 0.001;
+    public static double kP = 0.01;
     public static double kI = 0.0;
     public static double kD = 0.0;
-    public static double kF = 0.0;
+    public static double kF = 0.4;
     public static double max =  0.8;
     public static double min = 0.0;
 
-    int currentPos;
-    int desiredPos;
+    public int currentPos;
+    public int desiredPos;
+    public double motorPower;
 
 
     PIDFController pidfController = new PIDFController(kP,kI,kD,kF,max,min);
 
-    public int INTAKE_POS = 0;//TODO: make real
-
-    public int PLACE_POS = 600; //TODO: make real :)
-
     @Override
     public void init(HardwareMap hwMap) {
         leftMotor = hwMap.get(DcMotor.class, "left_4bar_motor");
         rightMotor = hwMap.get(DcMotor.class, "right_4bar_motor");
         leftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
-        leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         leftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         rightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
     }
@@ -53,12 +53,16 @@ public void manualPositionChange(int changeAmount){
     public void update(){
         currentPos = (leftMotor.getCurrentPosition() + rightMotor.getCurrentPosition())/2;//average left and right speeds
         double motorPower = pidfController.calculate(desiredPos,currentPos);
+        this.motorPower = motorPower;
         leftMotor.setPower(motorPower);
         rightMotor.setPower(motorPower);
     }
 
     @Override
     public List<QQTest> getTests() {
-        return Arrays.asList(new TestTwoMotors("fourbar", TEST_SPEED,leftMotor,rightMotor));
+        return Arrays.asList(
+                new TestTwoMotors("fourbar up", TEST_SPEED,leftMotor,rightMotor),
+                new TestTwoMotors("four bar stop", TEST_SPEED_OFF,leftMotor,rightMotor)
+        );
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Slides.java
@@ -12,7 +12,7 @@
 
 @Config
 public class Slides extends QQMechanism {
-    public static double SLIDE_BACK_POSITION = 0.125;
+    public static double SLIDE_BACK_POSITION = 0.1;
     public static double SLIDE_FRONT_POSITION = 1.0;
     public static double SLIDE_MIDDLE_POSITION = 0.5;
 

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RI3WTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RI3WTeleOp.java
@@ -9,7 +9,7 @@
 public class RI3WTeleOp extends QQOpMode{
 
     public static final double TRIGGER_THRESHOLD = 0.5;
-    public static final int MANUAL_CHANGE = 15;
+    public static final int MANUAL_CHANGE = 5;
 
 
     @Override
@@ -18,6 +18,9 @@ public void loop() {
         double forward = -gamepad1.left_stick_y;
         double left = gamepad1.left_stick_x;
         double rotate = gamepad1.right_stick_x;
+        telemetry.addData("dr4b desired pos", robot.doubleReverse4Bar.desiredPos);
+        telemetry.addData("dr4b current pos", robot.doubleReverse4Bar.currentPos);
+        telemetry.addData("dr4b power", robot.doubleReverse4Bar.motorPower);
 
         if (gamepad1.left_trigger > TRIGGER_THRESHOLD && gamepad1.right_trigger > TRIGGER_THRESHOLD){
             robot.mecanumDrive.setSpeed(MecanumDrive.Speed.TURBO);
@@ -49,6 +52,9 @@ else if (gamepad1.dpad_left){
         else if (gamepad1.right_bumper){
             robot.claw.close();
         }
+        if (gamepad1.b){
+            robot.controlHub.resetGyro();
+        }
 
 
     }

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestTwoMotors.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestTwoMotors.java
@@ -1,10 +1,12 @@
 package org.firstinspires.ftc.teamcode.ftc16072.Tests;
 
+import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-
+@Config
 public class TestTwoMotors extends QQTest{
+    public static double STOP_POWER = 0;
     DcMotor leftMotor;
     DcMotor rightMotor;
     double speed;
@@ -23,8 +25,8 @@ public void run(boolean on, Telemetry telemetry) {
             rightMotor.setPower(speed);
 
         }else{
-            leftMotor.setPower(0);
-            rightMotor.setPower(0);
+            leftMotor.setPower(STOP_POWER);
+            rightMotor.setPower(STOP_POWER);
 
         }