better teleop for robotica

svhsrobotics · Feb 8, 2024 · e214ba0 · e214ba0
1 parent 3269636
commit e214ba0
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 25 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java
@@ -3,6 +3,7 @@
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.control.PIDCoefficients;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
@@ -55,11 +56,11 @@ public static double getMotorVelocityF(double ticksPerSecond) {
     private final AprilTagCamera[] cameras;
 
     private final Servo purpleServo;
-    public final DcMotor armMotor;
+    public final DcMotorEx armMotor;
     public final Servo wristServo;
     public final AxonServo intakeServo;
     public final Servo droneServo;
-    public final DcMotor hangMotor;
+    public final DcMotorEx hangMotor;
     public final DcMotor intakeMotor;
 
     public RoboticaBot(HardwareMap hardwareMap) {
@@ -71,13 +72,13 @@ public RoboticaBot(HardwareMap hardwareMap) {
 
 
         purpleServo = hardwareMap.get(Servo.class, "purple");
-        armMotor = hardwareMap.get(DcMotor.class, "arm");
+        armMotor = hardwareMap.get(DcMotorEx.class, "arm");
         armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         wristServo = hardwareMap.get(Servo.class, "cup_pivot");
         intakeServo = new AxonServo("intake_servo", "intake_analog_2", hardwareMap);
         droneServo = hardwareMap.get(Servo.class, "drone");
-        hangMotor = hardwareMap.get(DcMotor.class, "hang");
+        hangMotor = hardwareMap.get(DcMotorEx.class, "hang");
         intakeMotor = hardwareMap.get(DcMotor.class, "intake");
 
         // TODO: Reverse Motors, encoders & such

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java
@@ -10,7 +10,6 @@
 import org.firstinspires.ftc.teamcode.drive.Robot;
 import org.firstinspires.ftc.teamcode.drive.RoboticaBot;
 import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;
-import org.firstinspires.ftc.teamcode.util.Debouncer;
 import org.firstinspires.ftc.teamcode.util.GlobalOpMode;
 
 @TeleOp
@@ -32,10 +31,13 @@ public void runOpMode() throws InterruptedException {
         }
 
         double wristPose = 0;
-        Debouncer iDebouncer = new Debouncer();
-        Debouncer debouncer2 = new Debouncer();
+        //Debouncer iDebouncer = new Debouncer();
+        //Debouncer debouncer2 = new Debouncer();
         double purplePose = .5;
-        int armPos = ((RoboticaBot) robot).armMotor.getCurrentPosition();
+        int armPos = 0;
+        if (robot.getClass() == RoboticaBot.class) {
+            armPos = ((RoboticaBot) robot).armMotor.getCurrentPosition();
+        }
 
         while (opModeIsActive()) {
             android.util.Log.i("TELEOP", "LOOP");
@@ -97,26 +99,23 @@ public void runOpMode() throws InterruptedException {
                 android.util.Log.i("TELEOP", "LOOP2A");
             } else if (robot.getClass() == RoboticaBot.class) {
                 RoboticaBot rrobot = (RoboticaBot) robot;
-//                if (iDebouncer.update(gamepad1.a)) {
-//                     rrobot.readyForIntake();
-//                     armPos = -240;
-//                }
-//                if (debouncer2.update(gamepad1.b)) {
-//                    rrobot.dropOff();
-//                }
+
                 android.util.Log.i("TELEOP", "LOOP2B");
 
-                if (gamepad1.left_trigger > 0.1) {
-                    armPos = rrobot.armMotor.getCurrentPosition() + (int)(gamepad1.left_trigger * 40);
-                } else if (gamepad1.right_trigger > 0.1) {
-                    armPos = rrobot.armMotor.getCurrentPosition() - (int)(gamepad1.right_trigger * 40);
-                } else {
+                if (gamepad1.left_trigger + gamepad1.right_trigger > 0.05) {
+                    rrobot.armMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+                    rrobot.armMotor.setVelocity((gamepad1.left_trigger - gamepad1.right_trigger) * 0.4);
                     armPos = rrobot.armMotor.getCurrentPosition();
+                } else {
+                    //armPos = rrobot.armMotor.getCurrentPosition();
+                    rrobot.armMotor.setTargetPosition(armPos);
+                    rrobot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+                    rrobot.armMotor.setPower(0.3);
                 }
 
-                rrobot.armMotor.setTargetPosition(armPos);
-                rrobot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-                rrobot.armMotor.setPower(0.3);
+//                rrobot.armMotor.setTargetPosition(armPos);
+//                rrobot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+//                rrobot.armMotor.setPower(0.3);
 
 
                 //rrobot.armMotor.setPower((gamepad1.left_trigger*0.5)-((gamepad1.right_trigger*0.5)));
@@ -133,9 +132,9 @@ public void runOpMode() throws InterruptedException {
 
 
                 if (gamepad1.left_bumper) {
-                    wristPose += 0.025;
+                    wristPose += 0.05;
                 } else if (gamepad1.right_bumper) {
-                    wristPose -= 0.025;
+                    wristPose -= 0.05;
                 }
                 if (wristPose < 0) wristPose = 0;
                 if (wristPose > 1) wristPose = 1;