aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorFranklin Wei <me@fwei.tk>2018-12-08 18:05:01 -0500
committerFranklin Wei <me@fwei.tk>2018-12-08 18:05:01 -0500
commit69b2ebd2c65c49a3ac82662a62ee80c91134d3e2 (patch)
treeaea81179f10c3713125a0db3894ee59e6b3ec2dd
parent1df279274ca16aa67ec2b502bafe6eeab3bede7a (diff)
downloadftc-69b2ebd2c65c49a3ac82662a62ee80c91134d3e2.zip
ftc-69b2ebd2c65c49a3ac82662a62ee80c91134d3e2.tar.gz
ftc-69b2ebd2c65c49a3ac82662a62ee80c91134d3e2.tar.bz2
ftc-69b2ebd2c65c49a3ac82662a62ee80c91134d3e2.tar.xz
Fix code
-rwxr-xr-xTeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java132
-rwxr-xr-xTeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java247
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);
+ }
+ }
+}