Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /* Copyright (c) 2014 Qualcomm Technologies Inc
- 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 Qualcomm Technologies Inc 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 com.qualcomm.ftcrobotcontroller.opmodes;
- import com.qualcomm.robotcore.eventloop.opmode.OpMode;
- import com.qualcomm.robotcore.hardware.DcMotor;
- import com.qualcomm.robotcore.hardware.Servo;
- import com.qualcomm.robotcore.util.Range;
- /**
- * TeleOp Mode
- * <p>
- * Enables control of the robot via the gamepad
- */
- public class FourWheelTankDriveAndArm8782 extends OpMode {
- DcMotor motorRightFront;
- DcMotor motorLeftFront;
- DcMotor motorRightRear;
- DcMotor motorLeftRear;
- Servo arm;
- final static double ARM_MIN_RANGE = 0.00;
- final static double ARM_MAX_RANGE = 1.00;
- // starting position of the arm servo.
- double armPosition = 0.5;
- // amount to change the arm servo position.
- double armDelta = 0.1;
- /**
- * Constructor
- */
- public FourWheelTankDriveAndArm8782() {
- }
- /*
- * Code to run when the op mode is first enabled goes here
- *
- * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
- */
- @Override
- public void init() {
- /*
- * Use the hardwareMap to get the dc motors by name. Note
- * that the names of the devices must match the names used when you
- * configured your robot and created the configuration file.
- *
- * For this bot we assume there are four motors:
- * "1" is on the left front corner of the bot.
- * "3" is on the left rear corner of the bot.
- * "2" is on the right front corner of the bot.
- * "4" is on the right rear corner of the bot.
- */
- motorLeftFront = hardwareMap.dcMotor.get("FtcDcMotor1");
- motorLeftRear = hardwareMap.dcMotor.get("FtcDcMotor3");
- motorRightFront = hardwareMap.dcMotor.get("FtcDcMotor2");
- motorRightRear = hardwareMap.dcMotor.get("FtcDcMotor4");
- motorLeftFront.setDirection(DcMotor.Direction.REVERSE);
- motorLeftRear.setDirection(DcMotor.Direction.REVERSE);
- arm = hardwareMap.servo.get("FtcServo1");
- }
- /*
- * This method will be called repeatedly in a loop
- *
- * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
- */
- @Override
- public void loop() {
- /*
- * Gamepad 1
- *
- * Gamepad 1 controls the motors via the left stick
- */
- // tank drive
- // note that if y equal -1 then joystick is pushed all of the way forward.
- float left = -gamepad1.left_stick_y;
- float right = -gamepad1.right_stick_y;
- // clip the right/left values so that the values never exceed +/- 1
- left = Range.clip(left, -1, 1);
- right = Range.clip(right, -1, 1);
- // scale the joystick value to make it easier to control
- // the robot more precisely at slower speeds.
- left = (float)scaleInput(left);
- right = (float)scaleInput(right);
- // write the values to the motors
- motorLeftFront.setPower(left);
- motorLeftRear.setPower(left);
- motorRightFront.setPower(right);
- motorRightRear.setPower(right);
- // update the position of the arm.
- if (gamepad1.y) {
- // if the Y button is pushed on gamepad1, decrease the position of
- // the arm servo.
- armPosition += armDelta;
- }
- if (gamepad1.a) {
- // if the A button is pushed on gamepad1, increment the position of
- // the arm servo.
- armPosition -= armDelta;
- }
- armPosition = Range.clip(armPosition, ARM_MIN_RANGE, ARM_MAX_RANGE);
- // write position values to the wrist and claw servo
- arm.setPosition(armPosition);
- /*
- * Send telemetry data back to driver station. Note that if we are using
- * a legacy NXT-compatible motor controller, then the getPower() method
- * will return a null value. The legacy NXT-compatible motor controllers
- * are currently write only.
- */
- telemetry.addData("Text", "*** Robot Data***");
- telemetry.addData("left tgt pwr", "left pwr: " + String.format("%.2f", left));
- telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));
- telemetry.addData("arm pos" , "arm pos: " + String.format("%.2d", armPosition));
- }
- /*
- * Code to run when the op mode is first disabled goes here
- *
- * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#stop()
- */
- @Override
- public void stop() {
- }
- /*
- * This method scales the joystick input so for low joystick values, the
- * scaled value is less than linear. This is to make it easier to drive
- * the robot more precisely at slower speeds.
- */
- double scaleInput(double dVal) {
- double[] scaleArray = { 0.0, 0.05, 0.09, 0.10, 0.12, 0.15, 0.18, 0.24,
- 0.30, 0.36, 0.43, 0.50, 0.60, 0.72, 0.85, 1.00, 1.00 };
- // get the corresponding index for the scaleInput array.
- int index = (int) (dVal * 16.0);
- // index should be positive.
- if (index < 0) {
- index = -index;
- }
- // index cannot exceed size of array minus 1.
- if (index > 16) {
- index = 16;
- }
- // get value from the array.
- double dScale = 0.0;
- if (dVal < 0) {
- dScale = -scaleArray[index];
- } else {
- dScale = scaleArray[index];
- }
- // return scaled value.
- return dScale;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement