Merge pull request #65 from ftc16072/Teleopfixes

Teleopfixes
ftc16072 · Feb 17, 2024 · 715303b · 715303b
2 parents f0cd075 + 7194ab0
commit 715303b
Show file tree

Hide file tree

Showing 13 changed files with 45 additions and 177 deletions.
diff --git a/...java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SpinInIntakeMotor.java b/...java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SpinInIntakeMotor.java
diff --git a/...ava/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SpinOutIntakeMotor.java b/...ava/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/SpinOutIntakeMotor.java
diff --git a/...n/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/StopIntakeMotor.java b/...n/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/StopIntakeMotor.java
diff --git a/...main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/UpdateIntake.java b/...main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/UpdateIntake.java
@@ -8,7 +8,7 @@ public class UpdateIntake extends Node {
     @Override
     public State tick(QQOpMode opmode) {
         if (opmode.gamepad1.right_trigger > TRIGGER_THRESHOLD){
-            opmode.robot.intake.intake();
+            opmode.robot.intake.intake(opmode.gamepad1.right_trigger);
         }
         else if (opmode.gamepad1.x){
             opmode.robot.intake.eject();

diff --git a/...rg/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfEjectButtonPressed.java b/...rg/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfEjectButtonPressed.java
diff --git a/...g/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfIntakeButtonPressed.java b/...g/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfIntakeButtonPressed.java
diff --git a/.../main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/PushbotTeleop.java b/.../main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/PushbotTeleop.java
@@ -1,39 +1,11 @@
 package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees;
 
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.AddTelemetry;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ClampOnPixel;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveFieldRelative;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeFastDrive;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeNormalDrive;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeSlowDrive;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveArmAndLift;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveLiftToIntakePosition;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveLiftToPixelGrabPosition;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePixels;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePixels2;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ResetGyro;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetLiftPosition;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinInIntakeMotor;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinOutIntakeMotor;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.StopIntakeMotor;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateArmAndLift;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateClimber;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreNotSlidesExtended;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreSlidesExtended;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1Pixel;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1or2Pixels;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has2Pixels;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.HasLessThan2Pixels;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfEjectButtonPressed;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfIntakeButtonPressed;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLeftTriggerPressed;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLiftAtBottom;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLiftToPixelGrabPosButtonPressed;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsControllerDriving;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Failover;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateIntake;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Parallel;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Sequence;
 
 /*
 http://behaviortrees.ftcteams.com/
@@ -94,23 +66,9 @@
 public class PushbotTeleop {
     public static Node root(){
         return new Parallel(5,
-                new Failover(
-                        new Sequence(
-                                new IfIntakeButtonPressed(),
-                                new SpinInIntakeMotor()
-                        ),
-                        new Sequence(
-                                new IfEjectButtonPressed(), // might want to consider removing this and automatically ejecting
-                                new SpinOutIntakeMotor()
-
-                        ),
-                        new StopIntakeMotor()
-
-                ),
+                new UpdateIntake(),
                 new ResetGyro(),
-
                 new DriveFieldRelative(),
-
                 new UpdateClimber()
 
         );

diff --git a/...src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TeleopTree.java b/...src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Trees/TeleopTree.java
@@ -1,37 +1,20 @@
 package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Trees;
 
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.AddTelemetry;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ClampOnPixel;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DriveFieldRelative;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.DroneRelease;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeFastDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeNormalDrive;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MakeSlowDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveArmAndLift;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveLiftToIntakePosition;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.MoveLiftToPixelGrabPosition;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePixels;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.PlacePixels2;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ResetGyro;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.RumbleGamepad;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SetLiftPosition;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinInIntakeMotor;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.SpinOutIntakeMotor;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.StopIntakeMotor;
-
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateArmAndLift;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateClimber;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreNotSlidesExtended;
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.UpdateIntake;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.AreSlidesExtended;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1Pixel;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has1or2Pixels;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.Has2Pixels;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.HasLessThan2Pixels;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfEjectButtonPressed;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfIntakeButtonPressed;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLeftTriggerPressed;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLiftAtBottom;
-import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IfLiftToPixelGrabPosButtonPressed;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsBackboardInRange;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.IsControllerDriving;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions.ifDroneReleaseButtonPressed;
@@ -175,17 +158,7 @@ public static Node root(){
                         new StopIntakeMotor()
 
                  */
-                new Failover(
-                        new Sequence(
-                                new IfEjectButtonPressed(),
-                                new SpinOutIntakeMotor()
-                        ),
-                        new Sequence(
-                                new IfIntakeButtonPressed(),
-                                new SpinInIntakeMotor()
-                        ),
-                        new StopIntakeMotor()
-                ),
+                new UpdateIntake(),
 
                 /*new Sequence(
                                 new SetLiftPosition()

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Intake.java
@@ -29,8 +29,8 @@ public List<QQtest> getTests() {
                 new TestMotor("Eject", EJECT_SPEED, intakeMotor));
 
     }
-    public void intake(){
-        intakeMotor.setPower(INTAKE_SPEED);
+    public void intake(double amount){
+        intakeMotor.setPower(INTAKE_SPEED * amount);
     }
     public void eject(){
         intakeMotor.setPower(EJECT_SPEED);

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Lift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Lift.java
@@ -8,6 +8,7 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.ftc16072.QQTest.QQtest;
+import org.firstinspires.ftc.teamcode.ftc16072.QQTest.TestMotor;
 import org.firstinspires.ftc.teamcode.ftc16072.QQTest.TestTwoMotor;
 
 import java.util.Arrays;
@@ -38,7 +39,7 @@ public enum LiftPositions{
 
     private LiftPositions manipulatorPosition;
     private static final int LIFT_POSITION_SAFETY_BOTTOM = -50;
-    private static final int LIFT_POSITION_SAFETY_TOP = 343455; //TODO need to fix
+    private static final int LIFT_POSITION_SAFETY_TOP = 2600; //TODO: find actual
 
     private static final int PIXEL_GRAB_POSITION=0;
 
@@ -58,11 +59,9 @@ public enum LiftPositions{
     private double desiredPosition;
     private double sumOfErrors;
     private double lastError;
-    public static double K_P = 0.007;
-    public static double K_I = 0.0001;
-    public static double K_D = 0.2;
-    public double motorPower;
-
+    public static double K_P = 0.002;
+    public static double K_I = 0;
+    public static double K_D = 0;
 
 
     @Override
@@ -84,6 +83,13 @@ public void init(HardwareMap hwMap) {
 
     }
     private void setDesiredPosition(double newPosition){
+        if(newPosition > LIFT_POSITION_SAFETY_TOP){
+            newPosition = LIFT_POSITION_SAFETY_TOP;
+        }
+        if(newPosition < LIFT_POSITION_SAFETY_BOTTOM){
+            newPosition = LIFT_POSITION_SAFETY_BOTTOM;
+        }
+
         desiredPosition = newPosition;
         sumOfErrors = 0;
         lastError = 0;
@@ -94,16 +100,27 @@ public boolean areSlidesExtendedPastBoundary(){
 
         }
     public void update(Telemetry telemetry){
-
+        double motorPower;
         double error;
         error = getDesiredPosition() - currentPosition();
+        telemetry.addData("Desired Pos", getDesiredPosition());
+        telemetry.addData("Curr Pos", currentPosition());
+
         sumOfErrors = sumOfErrors + error;
 
-        //motorPower =K_P * error + K_I * sumOfErrors + K_D * (error - lastError);
-        motorPower = K_P *error;
+        motorPower =K_P * error + K_I * sumOfErrors + K_D * (error - lastError);
+
         lastError = error;
         telemetry.addData("error", error);
 
+        /*
+        if (Math.abs(motorPower) < 0.1){
+            motorPower = 0;
+        }
+        */
+
+        telemetry.addData("lift power: ",motorPower);
+
         rightLiftMotor.setPower(motorPower);
         leftLiftMotor.setPower(motorPower);
     }
@@ -112,24 +129,17 @@ public void update(Telemetry telemetry){
     public List<QQtest> getTests() {
         return Arrays.asList(
                 new TestTwoMotor("lift", leftLiftMotor, rightLiftMotor, 0.5),
-                new TestTwoMotor("downLift", leftLiftMotor, rightLiftMotor, -0.5)
+                new TestTwoMotor("downLift", leftLiftMotor, rightLiftMotor, -0.5),
+                new TestMotor("right lift motor", 0.5, rightLiftMotor),
+                new TestMotor("left lift motor", 0.5, leftLiftMotor)
                 );
     }
 
     public void manualLiftUp(){
-        setDesiredPosition(currentPosition() + MANUAL_CHANGE);
-        if(getDesiredPosition() > LIFT_POSITION_SAFETY_TOP){
-            desiredPosition = LIFT_POSITION_SAFETY_TOP;
-        }
-    }public void manualLiftDown(){
-        setDesiredPosition(currentPosition()-MANUAL_CHANGE);
-        if(getDesiredPosition() < LIFT_POSITION_SAFETY_BOTTOM){
-            setDesiredPosition(LIFT_POSITION_SAFETY_BOTTOM);
-
-        }
+        setDesiredPosition(getDesiredPosition() + MANUAL_CHANGE);
     }
-    public double getPower(){
-        return motorPower;
+    public void manualLiftDown(){
+        setDesiredPosition(getDesiredPosition() - MANUAL_CHANGE);
     }
     public double getDesiredPosition(){
         return desiredPosition;

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java
@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.ftc16072.OpModes;
 
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -22,6 +24,8 @@ abstract public class QQOpMode extends OpMode {
      * every opmode initializes robot with hardware map
       */
     public void init(){
+        FtcDashboard dashboard = FtcDashboard.getInstance();
+        telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
         robot.init(hardwareMap);
     }
 

diff --git a/...ode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TestingRobotTeleOp.java b/...ode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/TestingRobotTeleOp.java
@@ -7,15 +7,6 @@
 @TeleOp
 @Config
 public class TestingRobotTeleOp extends QQOpMode {
-    FtcDashboard dashboard;
-
-    public void init(){
-        robot.init(hardwareMap);
-        dashboard = FtcDashboard.getInstance();
-        telemetry = dashboard.getTelemetry();
-
-
-    }
     @Override
     public void loop() {
         if (gamepad1.a){
@@ -53,7 +44,6 @@ else if(gamepad2.dpad_down){
             robot.climber.stop();
         }
 
-        telemetry.addData("lift motor power ",robot.lift.getPower());
         telemetry.addData("lift desired Position", robot.lift.getDesiredPosition());
         telemetry.addData("lift current position", robot.lift.currentPosition());
         robot.lift.update(telemetry);

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/LiftControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/LiftControl.java
@@ -15,7 +15,8 @@ public boolean atPosition(Lift.LiftPositions liftPosition){
     }
     public void manualLiftUp(){
         lift.manualLiftUp();
-    }public void manualLiftDown(){
+    }
+    public void manualLiftDown(){
         lift.manualLiftDown();
     }