added pid stuff for pivot Code made by @ooshcoding aka Aashi Bhatt

GirlsOfSteelRobotics · Dec 13, 2024 · ec7c5b4 · ec7c5b4
1 parent 4ffa35f
commit ec7c5b4
Show file tree

Hide file tree

Showing 2 changed files with 75 additions and 15 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/Pivot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Subsystems/Pivot.java
@@ -1,27 +1,64 @@
 package org.firstinspires.ftc.teamcode.Subsystems;
 
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
+import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
+import com.acmerobotics.roadrunner.ftc.RawEncoder;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
+@Config
 public class Pivot {
     private static final double TICKS_PER_INCH_PIVOT = 1;
+    //prolly have to change
 
     private static final double INITIAL_HEIGHT = 0;
+    public static  double KP = 0.005;
+    //have to find the KP slop
+    public static double LOW_BASKET_SCORING_ANGLE = 150;
+    //have to tune this to find this
+    public static boolean ENFORCE_LIMITS = true;
+    //need this for the limit on the top baskets and the low baskets
+    //have to do PID for these values also to find the ticks
+    public static double INTAKE_ANGLE = 270;
+    public static double LOWER_LIMIT = 0;
+    public static double UPPER_LIMIT = 94;
+    //have to do pid to make sure that this is all right
+    public static double UP_POWER = .75;
+    public static double DOWN_POWER = -0.2;
 
+    private static double tickOffset;
     private final DcMotor pivotMotor;
+    OverflowEncoder encoder;
 
     public Pivot(HardwareMap hardwareMap) {
         pivotMotor = hardwareMap.get(DcMotor.class, "pivotMotor");
         pivotMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        DcMotorEx hackEncoderMotor = hardwareMap.get(DcMotorEx.class, "rightBack");//get the name of the motor
+        encoder = new OverflowEncoder(new RawEncoder(hackEncoderMotor));
     }
 
     public void goUp() {
-        pivotMotor.setPower(-1);
+        if(ENFORCE_LIMITS && getAngle()>UPPER_LIMIT ) {
+            pivotMotor.setPower(0);
+        } else {
+            pivotMotor.setPower(0.5);
+            //might have to change later based on how heavy it is
+        }
+//        pivotMotor.setPower(UP_POWER);
     }
 
     public void goDown() {
-        pivotMotor.setPower(1);
+       if (ENFORCE_LIMITS && getAngle()<LOWER_LIMIT) {
+           pivotMotor.setPower(0);
+       } else {
+           pivotMotor.setPower(-0.2);
+           //might have to change direction idk yet
+       }
+//        pivotMotor.setPower(DOWN_POWER);
     }
 
     public void stop() {
@@ -32,4 +69,22 @@ public double getHeight () {
         return pivotMotor.getCurrentPosition() / TICKS_PER_INCH_PIVOT;
     }
 
+    public double getAngle() {
+        PositionVelocityPair ticks = encoder.getPositionAndVelocity();
+        return (ticks.position - tickOffset) * TICKS_PER_INCH_PIVOT;
+    }
+
+//    public boolean goToAngle(double angle){
+//        double error = getAngle()-angle;
+//        double power = KP * error;
+//        //idk ask pj for help on this one
+//    }
+    public double getPower() {
+        return pivotMotor.getPower();
+    }
+
+    public void zeroEncoder() {
+        tickOffset = encoder.getPositionAndVelocity().position;
+    }
+
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/teleop/teleop.java
@@ -28,42 +28,47 @@ public void runOpMode() throws InterruptedException {
 
             Pose2d pose = drive.getPose();
 
-            telemetry.addData("x", pose.position.x);
-            telemetry.addData("y", pose.position.y);
-            telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble()));
-            telemetry.addData("extension", slide.getHeight());
-            telemetry.addData("degrees", pivot.getHeight());
-            telemetry.update();
-
 
             if(gamepad1.start && gamepad1.back) {
                 drive.zeroWheels();
             }
-
-            if(gamepad1.a) {
+            //linear slide
+            if(gamepad1.right_trigger > 0.5) {
                 slide.goUp();
-            } else if (gamepad1.b) {
+            } else if (gamepad1.left_trigger>0.5) {
                 slide.goDown();
             } else {
                 slide.stop();
             }
             //need encoders!!!!!
 
-            if(gamepad1.x) {
+            if(gamepad1.b) {
                 claw.openClaw();
-            } else if(gamepad1.y) {
+            } else if(gamepad1.x) {
                 claw.closeClaw();
             } else {
                 claw.stopClaw();
             }
-
+            //pivot
             if(gamepad1.left_bumper){
                 pivot.goUp();
+                telemetry.addData("Going Up", true);
             } else if (gamepad1.right_bumper){
                 pivot.goDown();
+                telemetry.addData("Going Down", true);
             } else {
                 pivot.stop();
             }
+
+            telemetry.addData("x", pose.position.x);
+            telemetry.addData("y", pose.position.y);
+            telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble()));
+            telemetry.addData("extension", slide.getHeight());
+            telemetry.addData("degrees", pivot.getHeight());
+            telemetry.addData("arm power", pivot.getPower());
+            telemetry.addData("arm angle", pivot.getAngle());
+            telemetry.update();
+
         }