Fix code

2 files changed, 379 insertions, 0 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
new file mode 100755
index 0000000..73d7cd6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java
@@ -0,0 +1,132 @@
+/* Copyright (c) 2017 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+/**
+ * This is NOT an opmode.
+ *
+ * This class can be used to define all the specific hardware for a single robot.
+ * In this case that robot is a Pushbot.
+ * See PushbotTeleopTank_Iterative and others classes starting with "Pushbot" for usage examples.
+ *
+ * This hardware class assumes the following device names have been configured on the robot:
+ * Note:  All names are lower case and some have single spaces between words.
+ *
+ * Motor channel:  Left  drive motor:        "left_drive"
+ * Motor channel:  Right drive motor:        "right_drive"
+ * Motor channel:  Manipulator drive motor:  "left_arm"
+ * Servo channel:  Servo to open left claw:  "left_hand"
+ * Servo channel:  Servo to open right claw: "right_hand"
+ */
+
+public class HardwarePushbot
+{
+    /* Public OpMode members. */
+    public DcMotor  DriveLeft   = null;
+    public DcMotor  DriveRight  = null;
+    public DcMotor  IntakeMotor = null;
+
+    public Servo GlyphClaw1 = null;
+    public Servo GlyphClaw2 = null;
+    public Servo RelicClaw = null;
+    public Servo JewelArm = null, RelicRotate = null;
+
+    //public DcMotor Testmotor = null;
+
+    public static final double MID_SERVO       =  0.5 ;
+
+
+    /* local OpMode members. */
+    HardwareMap hwMap           =  null;
+    private ElapsedTime period  = new ElapsedTime();
+
+    /* Constructor */
+    public HardwarePushbot(){
+
+    }
+
+    /* Initialize standard Hardware interfaces */
+    public void init(HardwareMap ahwMap) {
+        // Save reference to Hardware map
+        hwMap = ahwMap;
+
+        //Testmotor = hwMap.get(DcMotor.class, "Testmotor");
+        // Define and Initialize Motors
+
+        DriveLeft  = hwMap.get(DcMotor.class, "DriveLeft");
+        DriveRight = hwMap.get(DcMotor.class, "DriveRight");
+
+        IntakeMotor = hwMap.get(DcMotor.class, "RelicMotor");
+
+        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);
+
+        // Set all motors to zero power
+        DriveLeft.setPower(0);
+        DriveRight.setPower(0);
+        IntakeMotor.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);
+        IntakeMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        /* stop these motors actively */
+
+        //GlyphMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        //RelicLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+    // Define and initialize ALL installed servos.
+        /*
+        GlyphClaw1 = hwMap.get(Servo.class, "GlyphClaw1");
+        GlyphClaw2 = hwMap.get(Servo.class, "GlyphClaw2");
+        RelicClaw = hwMap.get(Servo.class, "RelicClaw");
+        JewelArm = hwMap.get(Servo.class, "JewelArm");
+        RelicRotate = hwMap.get(Servo.class, "RelicRotate");
+
+        GlyphClaw1.setPosition(.75);
+        GlyphClaw2.setPosition(.25);
+        RelicClaw.setPosition(.25);
+        JewelArm.setPosition(.85);
+        RelicRotate.setPosition(1.0);
+        */
+    }
+}
+
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
new file mode 100755
index 0000000..8dd8c14
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java
@@ -0,0 +1,247 @@
+/* Copyright (c) 2017 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DeviceInterfaceModule;
+import com.qualcomm.robotcore.util.Range;
+
+/**
+ * This OpMode uses the common Pushbot hardware class to define the devices on the robot.
+ * All device access is managed through the HardwarePushbot class.
+ * The code is structured as a LinearOpMode
+ *
+ * This particular OpMode executes a POV Game style Teleop for a PushBot
+ * In this mode the left stick moves the robot FWD and back, the Right stick turns left and right.
+ * It raises and lowers the claw using the Gampad Y and A buttons respectively.
+ * It also opens and closes the claws slowly using the left and right Bumper buttons.
+ *
+ * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
+ */
+
+@TeleOp(name="TankBot: Driver Mode", group="Pushbot")
+
+public class PushbotTeleopPOV_Linear extends LinearOpMode {
+
+    /* Declare OpMode members. */
+    HardwarePushbot robot           = new HardwarePushbot();   // Use a Pushbot's hardware
+    // could also use HardwarePushbotMatrix class.
+    final double    CLAW_SPEED      = 0.1;                   // sets rate to move servo
+    final double    FINE_SPEED      = .35;
+    final double    ROT_SPEED = .5;
+
+    static final int    BLUE_LED    = 0;     // Blue LED channel on DIM
+    static final int    RED_LED     = 1;     // Red LED Channel on DIM
+    static final double MOTOR_ZEROTHRESHOLD = .05;
+
+    static final double POINT_SPEED = .0625;
+    static final double MOVING_POINT_SPEED = .125;
+    static final double PCoef = .010;
+
+    static final int HEADING_THRESHOLD = 4;
+
+    DeviceInterfaceModule dim;
+
+    ModernRoboticsI2cGyro modernRoboticsI2cGyro;
+
+    /**
+     * Perform one cycle of closed loop heading control.
+     *
+     * @param speed     Desired speed of turn.
+     * @param angle     Absolute Angle (in Degrees) relative to last gyro reset.
+     *                  0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+     *                  If a relative angle is required, add/subtract from current heading.
+     * @param PCoeff    Proportional Gain coefficient
+     * @return
+     */
+    double onHeading(double speed, double angle, double PCoeff) {
+        double   error ;
+        double   steer = 0 ;
+        boolean  onTarget = false ;
+
+        // determine turn power based on +/- error
+        error = getError(angle);
+
+        double ret;
+
+        if (Math.abs(error) <= HEADING_THRESHOLD) {
+            ret = 0;
+        }
+        else {
+            steer = getSteer(error, PCoeff);
+            ret = steer;
+        }
+
+        // Display it for the driver.
+        telemetry.addData("Target", "%5.2f", angle);
+        telemetry.addData("Err/St", "%5.2f/%5.2f", error, steer);
+        telemetry.addData("Speed.", "%5.2f", ret);
+
+        return ret;
+    }
+
+    /**
+     * getError determines the error between the target angle and the robot's current heading
+     * @param   targetAngle  Desired angle (relative to global reference established at last Gyro Reset).
+     * @return  error angle: Degrees in the range +/- 180. Centered on the robot's frame of reference
+     *          +ve error means the robot should turn LEFT (CCW) to reduce error.
+     */
+    public double getError(double targetAngle) {
+
+        double robotError;
+
+        // calculate error in -179 to +180 range  (
+
+        robotError = targetAngle - modernRoboticsI2cGyro.getIntegratedZValue();
+        while (robotError > 180)  robotError -= 360;
+        while (robotError <= -180) robotError += 360;
+        return robotError;
+    }
+
+    /**
+     * returns desired steering force.  +/- 1 range.  +ve = steer left
+     * @param error   Error angle in robot relative degrees
+     * @param PCoeff  Proportional Gain Coefficient
+     * @return
+     */
+    public double getSteer(double error, double PCoeff) {
+        return Range.clip(error * PCoeff, -1, 1);
+    }
+
+    @Override
+    public void runOpMode() {
+        double up = 0;
+
+        //dim = hardwareMap.deviceInterfaceModule.get("dim");
+
+        /* Initialize the hardware variables.
+         * The init() method of the hardware class does all the work here
+         */
+        robot.init(hardwareMap);
+
+        // Get a reference to a Modern Robotics gyro object. We use several interfaces
+        // on this object to illustrate which interfaces support which functionality.
+        //modernRoboticsI2cGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro");
+        // If you're only interested int the IntegratingGyroscope interface, the following will suffice.
+        // gyro = hardwareMap.get(IntegratingGyroscope.class, "gyro");
+        // A similar approach will work for the Gyroscope interface, if that's all you need.
+
+        // Start calibrating the gyro. This takes a few seconds and is worth performing
+        // during the initialization phase at the start of each opMode.
+        //telemetry.log().add("Gyro Calibrating. Do Not Move!");
+        //modernRoboticsI2cGyro.calibrate();
+
+        // Wait until the gyro calibration is complete
+        /*while (!isStopRequested() && modernRoboticsI2cGyro.isCalibrating())  {
+            telemetry.addLine("calibrating...");
+            telemetry.update();
+            sleep(50);
+        }
+
+        telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start.");
+        telemetry.clear(); telemetry.update();
+*/
+
+        // Send telemetry message to signify robot waiting;
+        telemetry.addData("Say", "Hello Driver");    //
+        telemetry.update();
+
+        // Wait for the game to start (driver presses PLAY)
+        waitForStart();
+
+        /* indicate glyphmotor power */
+       // dim.setLED(BLUE_LED, false);
+
+        /* max power off */
+       // dim.setLED(RED_LED, false);
+
+        /*
+        double fine_speed = FINE_SPEED, rotation_rate = ROT_SPEED;
+        boolean max_power = false;
+        boolean last_x = false, last_b = false, last_back = false;
+        boolean glyphmotor_on = false;
+        boolean correct_direction = false;
+
+        double power[][] = new double[2][2];
+
+        int heading = modernRoboticsI2cGyro.getIntegratedZValue();
+
+        int zero_rotations = 0;
+    */
+
+        double fine_speed = .5;
+
+        // run until the end of the match (driver presses STOP)
+        while (opModeIsActive()) {
+            //robot.Testmotor.setPower(gamepad1.left_stick_x * .5);
+
+            /* y is horizontal, x is vertical... sorry */
+            double forward = gamepad1.left_stick_y;
+            double rotate = gamepad1.right_stick_x;
+
+            if(gamepad1.dpad_up)
+                forward += fine_speed;
+            if(gamepad1.dpad_down)
+                forward -= fine_speed;
+            if(gamepad1.dpad_left)
+                rotate -= fine_speed;
+            if(gamepad1.dpad_right)
+                rotate += fine_speed;
+
+            double [] powers = new double[2];
+            powers[0] = forward + rotate;
+            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)
+            {
+                powers[0] /= max;
+                powers[1] /= max;
+            }
+
+
+            robot.DriveLeft.setPower(powers[0]);
+            robot.DriveRight.setPower(powers[1]);
+            // Send telemetry message to signify robot running;
+            telemetry.addData("rotation",  "%.2f", rotate);
+            telemetry.update();
+
+            // Pace this loop so jaw action is reasonable speed.
+            sleep(50);
+        }
+    }
+}