Merge pull request #10 from ftc16072/DR4b

Dr4b
ftc16072 · Sep 25, 2024 · 9a9fb11 · 9a9fb11
2 parents 768716c + a219a07
commit 9a9fb11
Show file tree

Hide file tree

Showing 3 changed files with 103 additions and 2 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
@@ -0,0 +1,64 @@
+package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestTwoMotors;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.PIDFController;
+
+import java.util.Arrays;
+import java.util.List;
+
+public class DoubleReverse4Bar extends QQMechanism{
+    public static final double TEST_SPEED = 0.2;
+    DcMotor leftMotor;
+    DcMotor rightMotor;
+    public static double kP = 0.001;
+    public static double kI = 0.0;
+    public static double kD = 0.0;
+    public static double kF = 0.0;
+    public static double max =  0.8;
+    public static double min = 0.0;
+
+    int currentPos;
+    int desiredPos;
+
+
+    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.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    public void setPosition(int desiredPos){
+        this.desiredPos = desiredPos;
+    }
+    public void manualPositionChange(int changeAmount){
+        desiredPos += changeAmount;
+    }
+    @Override
+    public void update(){
+        currentPos = (leftMotor.getCurrentPosition() + rightMotor.getCurrentPosition())/2;//average left and right speeds
+        double motorPower = pidfController.calculate(desiredPos,currentPos);
+        leftMotor.setPower(motorPower);
+        rightMotor.setPower(motorPower);
+    }
+
+    @Override
+    public List<QQTest> getTests() {
+        return Arrays.asList(new TestTwoMotors("fourbar", TEST_SPEED,leftMotor,rightMotor));
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java
@@ -4,6 +4,7 @@
 
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Claw;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub;
+import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.DoubleReverse4Bar;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.OpticalTrackingOdometrySensor;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.QQMechanism;
@@ -18,6 +19,7 @@ public class Robot {
     public MecanumDrive mecanumDrive;
     public OpticalTrackingOdometrySensor otos;
     public Claw claw;
+    public DoubleReverse4Bar doubleReverse4Bar;
     public Slides slides;
     List<QQMechanism> mechanisms;
 
@@ -26,15 +28,17 @@ public Robot() {
         controlHub = new ControlHub();
         otos = new OpticalTrackingOdometrySensor();
         claw = new Claw();
+        doubleReverse4Bar = new DoubleReverse4Bar();
         slides = new Slides();
 
         mechanisms = Arrays.asList(
                 controlHub,
                 mecanumDrive,
                 otos,
                 claw,
-                slides);
-
+                slides,
+                claw,
+                doubleReverse4Bar);
     }
     public void init(HardwareMap hwMap) {
         for (QQMechanism mechanism : mechanisms) {

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
@@ -0,0 +1,33 @@
+package org.firstinspires.ftc.teamcode.ftc16072.Tests;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+public class TestTwoMotors extends QQTest{
+    DcMotor leftMotor;
+    DcMotor rightMotor;
+    double speed;
+    public TestTwoMotors(String name, double speed, DcMotor leftMotor, DcMotor rightMotor){
+        super(name);
+        this.speed = speed;
+        this.leftMotor = leftMotor;
+        this.rightMotor = rightMotor;
+    }
+    @Override
+    public void run(boolean on, Telemetry telemetry) {
+        telemetry.addData("encoder position",leftMotor.getCurrentPosition());
+        telemetry.addData("encoder position",rightMotor.getCurrentPosition());
+        if(on){
+            leftMotor.setPower(speed);
+            rightMotor.setPower(speed);
+
+        }else{
+            leftMotor.setPower(0);
+            rightMotor.setPower(0);
+
+        }
+
+    }
+
+}