diff options
author | Franklin Wei <me@fwei.tk> | 2018-12-20 22:06:41 -0500 |
---|---|---|
committer | Franklin Wei <me@fwei.tk> | 2018-12-20 22:06:41 -0500 |
commit | 9606b23963a5dea4fbe4afde74cd9c2ff9060de4 (patch) | |
tree | 05beaa03792030aa9fd94cc26e4ecc7fdb54a2f2 | |
parent | 2378d7c5e23d97720b75c75092249d9af776d33c (diff) | |
download | ftc-9606b23963a5dea4fbe4afde74cd9c2ff9060de4.zip ftc-9606b23963a5dea4fbe4afde74cd9c2ff9060de4.tar.gz ftc-9606b23963a5dea4fbe4afde74cd9c2ff9060de4.tar.bz2 ftc-9606b23963a5dea4fbe4afde74cd9c2ff9060de4.tar.xz |
more new stuff
-rwxr-xr-x | TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java | 22 | ||||
-rwxr-xr-x | TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java | 32 |
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. |