more stuffHEADmaster

2 files changed, 27 insertions, 10 deletions

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java
index c4b2160..652a9b5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotAutonomousv1.java
@@ -242,8 +242,9 @@ public class TankbotAutonomousv1 extends LinearOpMode {
                 gyroDriveRelative(DRIVE_SPEED, 20, 0);
                 gyroTurnRelative(TURN_SPEED, (45 - 5));
                 gyroDriveRelative(DRIVE_SPEED, 14, 0);
-                gyroTurnRelative(TURN_SPEED, 90);
-                gyroDriveRelative(DRIVE_SPEED, 35, 0);
+                gyroTurnRelative(TURN_SPEED, -90);
+                gyroDriveRelative(DRIVE_SPEED, -35, 0);
+                gyroTurnRelative(TURN_SPEED, 180);
                 break;
             case 1:
                 /* TODO */
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java
index fac3eb4..2f46c72 100755
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankbotDriver.java
@@ -145,8 +145,11 @@ public class TankbotDriver extends LinearOpMode {
         waitForStart();
 
         double fine_speed = .5;
+        boolean turbo = false;
+        boolean last_x = false;
 
         int liftpos = robot.LiftMotor.getCurrentPosition();
+        boolean last_lift = false;
 
         // run until the end of the match (driver presses STOP)
         while (opModeIsActive()) {
@@ -159,6 +162,13 @@ public class TankbotDriver extends LinearOpMode {
             if(Math.abs(rotate) < DEAD_ZONE)
                 rotate = 0;
 
+            boolean x = gamepad1.x;
+            if(x && !last_x)
+                turbo = !turbo;
+            last_x = x;
+
+            fine_speed = turbo ? 1 : .5;
+
             if(gamepad1.dpad_up)
                 forward += fine_speed;
             if(gamepad1.dpad_down)
@@ -188,30 +198,35 @@ public class TankbotDriver extends LinearOpMode {
             if(gamepad1.left_bumper)
                 intake -= .5;
 
-            /* 2240 counts/rev */
+            /* 1120 counts/rev */
             int dlift = 0;
             boolean no_lift = false;
 
             if(gamepad1.a)
-                dlift += 2240/5;
+                dlift += 2240/10;
             else if(gamepad1.y)
-                dlift -= 2240/5;
+                dlift -= 2240/10;
             else
                 no_lift = true;
 
             boolean limit_reached = !robot.liftLimit.getState();
-            if(limit_reached || no_lift) {
-                robot.LiftMotor.setTargetPosition(robot.LiftMotor.getCurrentPosition());
+            if(limit_reached || (no_lift && last_lift)) {
+                /* stop movement */
+                liftpos = robot.LiftMotor.getCurrentPosition();
             }
             else {
                 liftpos += dlift;
-                robot.LiftMotor.setTargetPosition(liftpos);
             }
 
+            /* always max power */
+            robot.LiftMotor.setPower(1);
+            robot.LiftMotor.setTargetPosition(liftpos);
+
+            last_lift = !no_lift;
+
             robot.IntakeMotor.setPower(intake);
             robot.DriveLeft.setPower(powers[0]);
             robot.DriveRight.setPower(powers[1]);
-            robot.LiftMotor.setPower(1);
 
             // Send telemetry message to signify robot running;
             telemetry.addData("rotation",  "%.2f", rotate);
@@ -219,10 +234,11 @@ public class TankbotDriver extends LinearOpMode {
             telemetry.addData("forward", "%.3f", forward);
             telemetry.addData("Drive positions", "%d %d", robot.DriveLeft.getCurrentPosition(), robot.DriveRight.getCurrentPosition());
             telemetry.addData("heading", getHeading(true));
+            telemetry.addData("Turbo mode", "%s", turbo ? "ON!" : "off");
             telemetry.update();
 
             // Pace this loop so jaw action is reasonable speed.
-            sleep(50);
+            sleep(20);
         }
     }
 }