diff options
author | Franklin Wei <me@fwei.tk> | 2018-12-08 18:05:01 -0500 |
---|---|---|
committer | Franklin Wei <me@fwei.tk> | 2018-12-08 18:05:01 -0500 |
commit | 69b2ebd2c65c49a3ac82662a62ee80c91134d3e2 (patch) | |
tree | aea81179f10c3713125a0db3894ee59e6b3ec2dd | |
parent | 1df279274ca16aa67ec2b502bafe6eeab3bede7a (diff) | |
download | ftc-69b2ebd2c65c49a3ac82662a62ee80c91134d3e2.zip ftc-69b2ebd2c65c49a3ac82662a62ee80c91134d3e2.tar.gz ftc-69b2ebd2c65c49a3ac82662a62ee80c91134d3e2.tar.bz2 ftc-69b2ebd2c65c49a3ac82662a62ee80c91134d3e2.tar.xz |
Fix code
-rwxr-xr-x | TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwarePushbot.java | 132 | ||||
-rwxr-xr-x | TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PushbotTeleopPOV_Linear.java | 247 |
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); + } + } +} |