code to reset lift

ftc16072 · Feb 20, 2024 · c4c23df · c4c23df
1 parent 5d1b5e1
commit c4c23df
Show file tree

Hide file tree

Showing 6 changed files with 35 additions and 18 deletions.
diff --git a/.../java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/UpdateArmAndLift.java b/.../java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Actions/UpdateArmAndLift.java
@@ -8,9 +8,10 @@ public class UpdateArmAndLift extends Node {
     @Override
     public State tick(QQOpMode opmode) {
         armControl.updateArm(opmode.robot);
-
+        if(opmode.robot.holdingCell.isLimitSwitchDetected()){
+            opmode.robot.lift.resetLiftPosition();
+        }
         opmode.robot.lift.update(opmode.telemetry);
-
         return State.RUNNING;
 
     }

diff --git a/...stinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfExtendSlideButtonPressed.java b/...stinspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IfExtendSlideButtonPressed.java
@@ -13,9 +13,9 @@ public State tick(QQOpMode opmode) {
         if(opmode.gamepad1.left_bumper){
             return State.SUCCESS;
         }
-        if(opmode.gamepad1.y){
-            return State.SUCCESS;
-        }
+        //if(opmode.gamepad1.y){
+          //  return State.SUCCESS;
+        //}
         if(opmode.gamepad1.b){
             return State.SUCCESS;
         }

diff --git a/...inspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IsLiftIntakePosButtonPressed.java b/...inspires/ftc/teamcode/ftc16072/BehaviorTrees/Conditions/IsLiftIntakePosButtonPressed.java
@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Conditions;
+
+import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad1;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode;
+
+public class IsLiftIntakePosButtonPressed extends Node {
+    @Override
+    public State tick(QQOpMode opmode) {
+     if(gamepad1.x){
+         return State.SUCCESS;
+     }else{
+         return State.FAILURE;
+     }
+    }
+}
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
@@ -4,6 +4,7 @@
 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.PlacePixels;
 import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Actions.ResetGyro;
@@ -207,7 +208,7 @@ public static Node root(){
                                                 new MakeNormalDrive(),
                                                 new Sequence(
                                                         new IfLeftTriggerPressed(),
-                                                        new MakeFastDrive()
+                                                        new MakeSlowDrive()
 
                                                 )
 

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
@@ -63,7 +63,6 @@ public enum LiftPositions{
     public static double K_I = 0;
     public static double K_D = 0;
 
-
     @Override
     public void init(HardwareMap hwMap) {
         rightLiftMotor = hwMap.get(DcMotorEx.class, "right_lift_motor");
@@ -170,7 +169,12 @@ public void downOnePixel(){
         setDesiredPosition(currentPosition() - PIXEL_HEIGHT);
     }
     public double currentPosition(){
-        return ((rightLiftMotor.getCurrentPosition() + leftLiftMotor.getCurrentPosition())/2.0);
+            return ((rightLiftMotor.getCurrentPosition() + leftLiftMotor.getCurrentPosition())/2.0);
+        }
+    public void resetLiftPosition(){
+    leftLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+    leftLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+    rightLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+    rightLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
     }
-
-}
+}
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
@@ -13,7 +13,6 @@
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Intake;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Mechanism;
-import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.PixelServo;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Placement;
 import org.firstinspires.ftc.teamcode.ftc16072.Util.Navigation;
 import org.firstinspires.ftc.vision.VisionPortal;
@@ -23,17 +22,15 @@
 
 public class Robot {
     List<Mechanism> mechanisms;
-    public PixelServo pixelServo;
     public MecanumDrive mecanumDrive;
     public Navigation nav;
     public ControlHub controlHub;
     //public LineDetector lineDetector;
     public Intake intake;
-
     public BackDistanceSensor backDistanceSensor;
 
     public HoldingCell holdingCell;
-    public Camera cameraFront;
+    //public Camera cameraFront;
     public Camera cameraBack;
     public Lift lift;
     public Arm arm;
@@ -43,7 +40,6 @@ public class Robot {
 
 
 
-
     public Robot() {
         //cameraFront = new Camera(Camera.CameraPosition.FRONT);
         backDistanceSensor = new BackDistanceSensor();
@@ -58,7 +54,6 @@ public Robot() {
         placement = new Placement();
         climber = new Climber();
         drone = new Drone();
-        pixelServo = new PixelServo();
 
 
 
@@ -73,8 +68,7 @@ public Robot() {
                 placement,
                 climber,
                 backDistanceSensor,
-                drone,
-                pixelServo
+                drone
 
         );
     }