Fixes, template auto

GearUp12499-org · Nov 6, 2023 · 58abf2b · 58abf2b
1 parent 6f15e8e
commit 58abf2b
Show file tree

Hide file tree

Showing 4 changed files with 42 additions and 15 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GenericDRAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GenericDRAuto.java
@@ -3,10 +3,23 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
+import org.firstinspires.ftc.teamcode.abstractions.Claw;
+import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration;
+
+import dev.aether.collaborative_multitasking.MultitaskScheduler;
+
 @Autonomous
 public class GenericDRAuto extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
+        RobotConfiguration robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap);
+        MultitaskScheduler scheduler = new MultitaskScheduler();
+        Claw claw = new Claw(scheduler, robot.clawGrab(), robot.clawRotate(), robot.getClawLock());
+
+        GenericDRAutoB backing = new GenericDRAutoB(robot, scheduler, claw);
 
+        while (opModeIsActive()) {
+            scheduler.tick();
+        }
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GenericDRAutoB.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GenericDRAutoB.kt
@@ -5,11 +5,12 @@ import dev.aether.collaborative_multitasking.Task
 import dev.aether.collaborative_multitasking.ext.maxDuration
 import org.firstinspires.ftc.teamcode.abstractions.Claw
 import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration
+import org.firstinspires.ftc.teamcode.utility.MotorPowers
 
 class GenericDRAutoB(
     private val robotConfig: RobotConfiguration,
+    private val scheduler: MultitaskScheduler,
     private val claw: Claw,
-    private val scheduler: MultitaskScheduler
 ) {
     // TODO: INCOMPLETE
     private val driveMotors = robotConfig.driveMotors()
@@ -22,7 +23,7 @@ class GenericDRAutoB(
         const val TurnDuration = 750 // ms
     }
 
-    fun pos1(): Task {
+    private fun forwardPart(): Task {
         return scheduler.task {
             +robotConfig.driveMotorLock
             onStart { ->
@@ -32,12 +33,25 @@ class GenericDRAutoB(
             onFinish { ->
                 driveMotors.setAll(0.0)
             }
-        }.then(claw.release())
+        }
     }
 
-    fun turn(fac: Double): Task {
+    fun left(): Task = forwardPart().then(turn(TurnSpeed)).then(claw.release())
+    fun center(): Task = forwardPart().then(claw.release())
+    fun right(): Task = forwardPart().then(turn(-TurnSpeed)).then(claw.release())
+
+    private fun turn(fac: Double, duration: Int = TurnDuration): Task {
         return scheduler.task {
             +robotConfig.driveMotorLock
+            onStart { ->
+                MotorPowers(
+                    1 * fac,
+                    -1 * fac,
+                    1 * fac,
+                    -1 * fac
+                ).apply(driveMotors)
+            }
+            maxDuration(duration)
         }
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp.java
@@ -120,7 +120,8 @@ public void runOpMode() {
             driveMotors.frontRight.setPower(frontRightPower * balFR);
             driveMotors.backRight.setPower(backRightPower * balBR);
 
-            int SLIDE_LIM = 3000;
+            int LONG_SLIDE_LIM = 3000;
+            int SHORT_SLIDE_LIM = 1500;
             int MOTION_PER_CYCLE = 20;
 
             // when B is pressed, reset all the lifts to the first preset
@@ -132,26 +133,26 @@ public void runOpMode() {
             }
 
             // use dpad up and down to move the left lift
-//            if (gamepad2.dpad_up) targetLeft += MOTION_PER_CYCLE;
-//            if (gamepad2.dpad_down) targetLeft -= MOTION_PER_CYCLE;
+            if (gamepad2.dpad_up) targetLeft += MOTION_PER_CYCLE;
+            if (gamepad2.dpad_down) targetLeft -= MOTION_PER_CYCLE;
 
             // gamepad 1 dpad up/down is for endgame truss scaling
             // moves the right lift, and synchronizes the left lift with it
-            if (gamepad1.dpad_up || gamepad2.dpad_up) {
+            if (gamepad1.dpad_up) {
                 targetRight += MOTION_PER_CYCLE;
                 targetLeft = targetRight;
             }
-            if (gamepad1.dpad_down || gamepad2.dpad_down) {
+            if (gamepad1.dpad_down) {
                 targetRight -= MOTION_PER_CYCLE;
                 targetLeft = targetRight;
             }
 
             // don't let the lifts go out of bounds
             // (this will cause the motors to break down)
             if (targetLeft < 0) targetLeft = 0;
-            if (targetLeft > SLIDE_LIM) targetLeft = SLIDE_LIM;
+            if (targetLeft > LONG_SLIDE_LIM) targetLeft = LONG_SLIDE_LIM;
             if (targetRight < 0) targetRight = 0;
-            if (targetRight > SLIDE_LIM) targetRight = SLIDE_LIM;
+            if (targetRight > SHORT_SLIDE_LIM) targetRight = SHORT_SLIDE_LIM;
 
             robot.liftLeft().setTargetPosition(targetLeft);
             robot.liftRight().setTargetPosition(targetRight);
@@ -183,7 +184,7 @@ public void runOpMode() {
                 drone.setPower(0);
             }
             // Gets the average ticks of both the slide motors --> Ticks for perfect hang position is 1340 ticks use hangTarget variable
-            double slideTicks = (liftRight.getCurrentPosition() + liftLeft.getCurrentPosition())/2.0;;
+            double slideTicks = (liftRight.getCurrentPosition() + liftLeft.getCurrentPosition())/2.0;
 
             if(gamepad1.y){
                 robot.liftRight().setTargetPosition(hangTarget);

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/abstractions/Dumper.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/abstractions/Dumper.kt
@@ -16,9 +16,8 @@ class Dumper(
     private val lock = config.dumperLock
 
     companion object {
-        // TODO get values
-        const val ROTATE_IDLE = 0.540
-        const val ROTATE_DUMP = 0.338
+        const val ROTATE_IDLE = 0.531
+        const val ROTATE_DUMP = 0.316
         const val UNLATCHED = 0.4
         const val LATCHED = 0.78