more new stuff

2 files changed, 46 insertions, 8 deletions

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java
index e5b3376..5d8902b 100755
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java
@@ -30,6 +30,7 @@
 package org.firstinspires.ftc.teamcode;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.util.ElapsedTime;
@@ -57,6 +58,7 @@ public class HardwarePushbot
     public DcMotor  DriveLeft   = null;
     public DcMotor  DriveRight  = null;
     public DcMotor  IntakeMotor = null;
+    public DcMotor  LiftMotor = null;
 
     public Servo GlyphClaw1 = null;
     public Servo GlyphClaw2 = null;
@@ -67,6 +69,7 @@ public class HardwarePushbot
 
     public static final double MID_SERVO       =  0.5 ;
 
+    public DigitalChannel liftLimit = null;
 
     /* local OpMode members. */
     HardwareMap hwMap           =  null;
@@ -87,30 +90,39 @@ public class HardwarePushbot
 
         DriveLeft  = hwMap.get(DcMotor.class, "DriveLeft");
         DriveRight = hwMap.get(DcMotor.class, "DriveRight");
-
         IntakeMotor = hwMap.get(DcMotor.class, "IntakeMotor");
+        LiftMotor = hwMap.get(DcMotor.class, "LiftMotor");
+
+        // get a reference to our digitalTouch object.
+        liftLimit = hwMap.get(DigitalChannel.class, "LiftLimit");
+
+        // set the digital channel to input.
+        liftLimit.setMode(DigitalChannel.Mode.INPUT);
+
 
         DriveLeft.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motorsF
         DriveRight.setDirection(DcMotor.Direction.REVERSE);// .setDirection(DcMotor.Direction.REVERSE);// Set to FORWARD if using AndyMark motors
         IntakeMotor.setDirection(DcMotor.Direction.FORWARD);
+        LiftMotor.setDirection(DcMotor.Direction.FORWARD);
 
         // Set all motors to zero power
         DriveLeft.setPower(0);
         DriveRight.setPower(0);
         IntakeMotor.setPower(0);
-
+        LiftMotor.setPower(0);
 
         // Set all motors to run without encoders.
         // May want to use RUN_USING_ENCODERS if encoders are installed.
-        DriveLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        DriveRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        DriveLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        DriveRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         IntakeMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        LiftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
 
         /* stop these motors actively */
 
         //GlyphMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
-        //RelicLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        LiftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
     // Define and initialize ALL installed servos.
         /*
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java
index 232a201..cd76df2 100755
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java
@@ -206,13 +206,19 @@ public class PushbotTeleopPOV_Linear extends LinearOpMode {
             //robot.Testmotor.setPower(gamepad1.left_stick_x * .5);
 
             /* y is horizontal, x is vertical... sorry */
-            double forward = gamepad1.left_stick_y;
+            double forward = -gamepad1.left_stick_y;
             double rotate = gamepad1.right_stick_x;
+            final double DEAD_ZONE = .05;
+
+            if(Math.abs(forward) < DEAD_ZONE)
+                forward = 0;
+            if(Math.abs(rotate) < DEAD_ZONE)
+                rotate = 0;
 
             if(gamepad1.dpad_up)
-                forward -= fine_speed;
-            if(gamepad1.dpad_down)
                 forward += fine_speed;
+            if(gamepad1.dpad_down)
+                forward -= fine_speed;
             if(gamepad1.dpad_left)
                 rotate -= fine_speed;
             if(gamepad1.dpad_right)
@@ -223,6 +229,7 @@ public class PushbotTeleopPOV_Linear extends LinearOpMode {
             powers[1] = forward + rotate;
 
 
+
             // Normalize the values so neither exceed +/- 1.0
             double max = Math.max(Math.abs(powers[0]), Math.abs(powers[1]));
             if (max > 1.0)
@@ -237,11 +244,30 @@ public class PushbotTeleopPOV_Linear extends LinearOpMode {
             if(gamepad1.left_bumper)
                 intake -= .5;
 
+            /* 2240 counts/rev */
+            int dlift = 0;
+
+            if(gamepad1.a)
+                dlift += 2240/5;
+            if(gamepad1.y)
+                dlift -= 2240/5;
+
+            int liftpos = robot.LiftMotor.getCurrentPosition();
+
+            boolean limit_reached = !robot.liftLimit.getState();
+            if(limit_reached)
+                dlift = 0;
+            robot.LiftMotor.setTargetPosition(liftpos + dlift);
+
             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);
+            telemetry.addData("liftposition", "%d", robot.LiftMotor.getCurrentPosition());
+            telemetry.addData("forward", "%.3f", forward);
             telemetry.update();
 
             // Pace this loop so jaw action is reasonable speed.