Merge pull request ftctechnh#1 from cporter/vv/initial

Code we've been working on after school

TeamCode/src/main/java/com/suitbots/vv/ArcadeTeleop.java

@@ -0,0 +1,29 @@
+package com.suitbots.vv;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+@TeleOp(name = "Teleop: Arcade", group = "Teleops")
+public class ArcadeTeleop extends OpMode {
+    private TankRobot robot;
+    private Controller g1;
+
+    public void init() {
+        robot = new TankRobot();
+        robot.initHardware(hardwareMap);
+        g1 = new Controller(gamepad1);
+    }
+
+    public void loop() {
+        g1.update();
+
+        double scale = 1.0 * (g1.leftBumper() ? .5 : 1.) * (g1.rightBumper() ? .5 : 1.);
+
+        double lp = - g1.left_stick_y + g1.left_stick_x - g1.right_stick_y + g1.right_stick_x;
+        double rp = - g1.left_stick_y - g1.left_stick_x - g1.right_stick_y - g1.right_stick_x;
+        double m = Math.max(1.0, Math.max(Math.abs(lp), Math.abs(rp)));
+
+        robot.setDriveMotors(lp * scale / m, rp * scale / m);
+        robot.setSpinner(g1.left_trigger - g1.right_trigger);
+    }
+}

TeamCode/src/main/java/com/suitbots/vv/AutonomousBase.java

@@ -0,0 +1,101 @@
+package com.suitbots.vv;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import java.util.Locale;
+
+public abstract class AutonomousBase extends LinearOpMode {
+    protected TankRobot robot;
+
+    protected void initialize() throws InterruptedException {
+        robot = new TankRobot();
+        robot.initHardware(hardwareMap);
+        robot.calibrateGyro();
+        int count = 0;
+        while(robot.isGyroCalibrating()) {
+            telemetry.addData("Gyro", String.format(Locale.US, "Calibrating (%d)", count++));
+            telemetry.update();
+            Thread.sleep(500, 0);
+        }
+        telemetry.addData("Gyro", "Calibrated");
+        telemetry.update();
+    }
+
+    private void sleep() throws InterruptedException {
+        // idle();
+        Thread.sleep(50, 0);
+    }
+
+    public static final int TICKS_PER_REV = 1140;
+    public void fwd(double inches) throws InterruptedException {
+        final int ticks = (int)(TICKS_PER_REV * inches / robot.WHEEL_RADIUS_IN);
+        robot.resetDriveEncoders();
+        robot.pushRunMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.setDriveMotorsEncoderTarget(ticks);
+        robot.setDriveMotors(.5, .5);
+        while (opModeIsActive() && robot.motorsAreBusy()) {
+            sleep();
+        }
+        robot.setDriveMotors(0.0, 0.0);
+        robot.popRunMode();
+    }
+
+    public void fwdSquares(double squares) throws InterruptedException {
+        fwd(24.0 * squares);
+    }
+
+    public void fwdMeters(double meters) throws InterruptedException {
+        fwd(39.3701 * meters);
+    }
+
+    public void turn(int degrees) throws InterruptedException {
+        turn(degrees, true);
+    }
+
+    public void turn(int _degrees, boolean reset_heading) throws InterruptedException {
+        final double P = .1;
+        robot.pushRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        if (reset_heading) {
+            robot.resetGyro();
+        }
+        if (0 < _degrees) {
+            robot.setDriveMotors(P, -P);
+        } else {
+            robot.setDriveMotors(-P, P);
+        }
+        final int degrees = Math.abs(_degrees) % 360;
+        while (opModeIsActive() && Math.abs(robot.getHeading()) < Math.abs(degrees)) {
+            sleep();
+        }
+        robot.setDriveMotors(0.0, 0.0);
+        robot.popRunMode();
+    }
+
+    public static final double LINE_SPEED = .4;
+    public static final double LINE_THRESHOLD = 3.0;
+    public void driveToWhiteLine() throws InterruptedException {
+        robot.pushRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.setDriveMotors(LINE_SPEED, LINE_SPEED);
+        while (opModeIsActive() && LINE_THRESHOLD > robot.getLineReading()) {
+            sleep();
+        }
+        robot.setDriveMotors(0.0, 0.0);
+        robot.popRunMode();
+    }
+
+    public static double WALL_SPEED = .2;
+    public static final double WALL_DISTANCE_THRESHOLD = 10.0;
+    public static double WALL_LIGHT_THRESHOLD = 50.0;
+    public void driveToWall() throws InterruptedException {
+        robot.pushRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.setDriveMotors(WALL_SPEED, WALL_SPEED);
+        while (opModeIsActive() &&
+                (WALL_DISTANCE_THRESHOLD < robot.getAcousticRangeCM()
+                        || WALL_LIGHT_THRESHOLD < robot.getRangeLightDetected())) {
+            idle();
+        }
+        robot.setDriveMotors(0.0, 0.0);
+        robot.popRunMode();
+    }
+}

TeamCode/src/main/java/com/suitbots/vv/Controller.java

@@ -0,0 +1,61 @@
+package com.suitbots.vv;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+public class Controller {
+    private Gamepad gamepad;
+
+    private int dpad_up, dpad_down, dpad_left, dpad_right;
+    private int x, y, a, b;
+    private int left_bumper, right_bumper;
+
+    public double left_stick_x, right_stick_x, left_stick_y, right_stick_y;
+    public double left_trigger, right_trigger;
+
+    public Controller(Gamepad g) {
+        gamepad = g;
+    }
+
+    public void update() {
+        if (gamepad.x) { ++x; } else { x = 0; }
+        if (gamepad.y) { ++y; } else { y = 0; }
+        if (gamepad.a) { ++a; } else { a = 0; }
+        if (gamepad.b) { ++b; } else { b = 0; }
+        if (gamepad.dpad_up) { ++dpad_up; } else { dpad_up = 0; }
+        if (gamepad.dpad_down) { ++dpad_down; } else { dpad_down = 0; }
+        if (gamepad.dpad_left) { ++dpad_left; } else { dpad_left = 0; }
+        if (gamepad.dpad_right) { ++dpad_right; } else { dpad_right = 0; }
+        if (gamepad.left_bumper) { ++left_bumper; } else { left_bumper = 0; }
+        if (gamepad.right_bumper) { ++right_bumper; } else { right_bumper = 0; }
+
+        left_stick_x = gamepad.left_stick_x;
+        left_stick_y = gamepad.left_stick_y;
+        right_stick_x = gamepad.right_stick_x;
+        right_stick_y = gamepad.right_stick_y;
+        left_trigger = gamepad.left_trigger;
+        right_trigger = gamepad.right_trigger;
+    }
+
+    public boolean dpadUp() { return 0 < dpad_up; }
+    public boolean dpadDown() { return 0 < dpad_down; }
+    public boolean dpadLeft() { return 0 < dpad_left; }
+    public boolean dpadRight() { return 0 < dpad_right; }
+    public boolean X() { return 0 < x; }
+    public boolean Y() { return 0 < y; }
+    public boolean A() { return 0 < a; }
+    public boolean B() { return 0 < b; }
+    public boolean leftBumper() { return 0 < left_bumper; }
+    public boolean rightBumper() { return 0 < right_bumper; }
+
+    public boolean dpadUpOnce() { return 1 == dpad_up; }
+    public boolean dpadDownOnce() { return 1 == dpad_down; }
+    public boolean dpadLeftOnce() { return 1 == dpad_left; }
+    public boolean dpadRightOnce() { return 1 == dpad_right; }
+    public boolean XOnce() { return 1 == x; }
+    public boolean YOnce() { return 1 == y; }
+    public boolean AOnce() { return 1 == a; }
+    public boolean BOnce() { return 1 == b; }
+    public boolean leftBumperOnce() { return 1 == left_bumper; }
+    public boolean rightBumperOnce() { return 1 == right_bumper; }
+
+}

TeamCode/src/main/java/com/suitbots/vv/ForwardOneSecond.java

@@ -0,0 +1,32 @@
+package com.suitbots.vv;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+@Autonomous(name = "Fwd One Sec.")
+@Disabled
+public class ForwardOneSecond extends LinearOpMode {
+    public void runOpMode() throws InterruptedException {
+        DcMotor left = hardwareMap.dcMotor.get("left");
+        DcMotor right = hardwareMap.dcMotor.get("right");
+        right.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        waitForStart();
+
+        long t0 = System.currentTimeMillis();
+        left.setPower(1.0);
+        right.setPower(1.0);
+        while (opModeIsActive()) {
+            long t1 = System.currentTimeMillis();
+            if (1000 < (t1 - t0)) {
+                break;
+            }
+            idle();
+        }
+        left.setPower(0.0);
+        right.setPower(0.0);
+    }
+}

TeamCode/src/main/java/com/suitbots/vv/SensorsTest.java

@@ -0,0 +1,44 @@
+package com.suitbots.vv;
+
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+@TeleOp(name = "Sensor Test")
+public class SensorsTest extends AutonomousBase {
+    public void runOpMode() throws InterruptedException {
+        initialize();
+        Controller g1 = new Controller(gamepad1);
+
+        waitForStart();
+
+        while(opModeIsActive()) {
+            g1.update();
+            robot.setDriveMotors(- g1.left_stick_y, -g1.right_stick_y);
+
+            telemetry.addData("Heading", robot.getHeading());
+            telemetry.addData("Range/A", robot.getAcousticRangeCM());
+            telemetry.addData("Range/O", robot.getRangeLightDetected());
+            telemetry.addData("Line", robot.getLineReading());
+            telemetry.update();
+            if (g1.AOnce()) {
+                robot.resetGyro();
+            } else if (g1.dpadUpOnce()) {
+                fwd(12);
+            } else if (g1.dpadDownOnce()) {
+                fwd(-12);
+            } else if (g1.dpadLeftOnce()) {
+                turn(-45);
+            } else if (g1.dpadRightOnce()) {
+                turn(45);
+            }
+        }
+    }
+}

TeamCode/src/main/java/com/suitbots/vv/TankRobot.java

@@ -0,0 +1,104 @@
+package com.suitbots.vv;
+
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+public class TankRobot {
+    private DcMotor lm, rm, spinner;
+    private ModernRoboticsI2cGyro gyro;
+    private ModernRoboticsI2cRangeSensor range;
+    private OpticalDistanceSensor line;
+
+    private DcMotor.RunMode mode_stack[] = new DcMotor.RunMode[10];
+    private int mode_stack_pos = 0;
+
+    private DcMotor.RunMode currentRunMode() {
+        return mode_stack[mode_stack_pos];
+    }
+
+    private void setCurrentRunMode() {
+        lm.setMode(currentRunMode());
+        rm.setMode(currentRunMode());
+    }
+
+    public void initHardware(HardwareMap hardwareMap) {
+        lm = hardwareMap.dcMotor.get("l1");
+        rm = hardwareMap.dcMotor.get("r1");
+        rm.setDirection(DcMotorSimple.Direction.REVERSE);
+        spinner = hardwareMap.dcMotor.get("spinner");
+        gyro = (ModernRoboticsI2cGyro) hardwareMap.gyroSensor.get("gyro");
+        if (null == gyro) {
+            throw new RuntimeException("Could not load gyro sensor");
+        }
+        range = (ModernRoboticsI2cRangeSensor)
+                hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "range");
+        line = hardwareMap.opticalDistanceSensor.get("line");
+        mode_stack[mode_stack_pos] = DcMotor.RunMode.RUN_USING_ENCODER;
+        setCurrentRunMode();
+    }
+
+    public void calibrateGyro() { gyro.calibrate(); }
+    public boolean isGyroCalibrating() {
+        return gyro.isCalibrating();
+    }
+
+    public void resetDriveEncoders() {
+        pushRunMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        popRunMode();
+    }
+
+    public void pushRunMode(DcMotor.RunMode m) {
+        mode_stack[++mode_stack_pos] = m;
+        setCurrentRunMode();
+    }
+
+    public void popRunMode() {
+        --mode_stack_pos;
+        setCurrentRunMode();
+    }
+
+    public void setDriveMotors(double l, double r) {
+        lm.setPower(l);
+        rm.setPower(r);
+    }
+
+    public void setDriveMotorsEncoderTarget(int ticks) {
+        lm.setTargetPosition(ticks);
+        rm.setTargetPosition(ticks);
+    }
+
+    public boolean motorsAreBusy() {
+        return lm.isBusy() || rm.isBusy();
+    }
+    public void setSpinner(double p) {
+        spinner.setPower(p);
+    }
+
+    public static final double WHEEL_RADIUS_IN = 4.0 * Math.PI;
+
+    public void resetGyro() {
+        gyro.resetZAxisIntegrator();
+    }
+    public int getHeading() {
+        return gyro.getIntegratedZValue();
+    }
+    public double getLineReading() {
+        return line.getLightDetected();
+    }
+    public double getAcousticRangeInches() {
+        return range.getDistance(DistanceUnit.INCH);
+    }
+    public double getAcousticRangeCM() {
+        return range.getDistance(DistanceUnit.CM);
+    }
+
+    public double getRangeLightDetected() {
+        return range.getRawLightDetected();
+    }
+}