Merge remote-tracking branch 'origin/branch-cooper' into branch-carlos
This commit is contained in:
31
HARDWARE.md
Normal file
31
HARDWARE.md
Normal file
@ -0,0 +1,31 @@
|
|||||||
|
# FTC Hardware Config 2023
|
||||||
|
|
||||||
|
**DISCLAIMER:** View the robot like this.
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
Configuration Name: **cometBoTsChassis2023**
|
||||||
|
|
||||||
|
There are two robots: 14493-DS, and FTC-992M.
|
||||||
|
|
||||||
|
Below are the following configurations for our robots
|
||||||
|
|
||||||
|
| physical port | hub | robot part | robot part location | robot software config name |
|
||||||
|
|---------------|-----------|----------------------------|-------------------------------|----------------------------|
|
||||||
|
| motor0 | control | UltraPlanetary HD hex motor | right front leg frame | Drive front rt |
|
||||||
|
| motor1 | control | UltraPlanetary HD hex motor | right back leg frame | Drive back rt |
|
||||||
|
| motor2 | control | UltraPlanetary HD hex motor | left front leg frame | Drive front lt |
|
||||||
|
| motor3 | control | UltraPlanetary HD hex motor | left back leg frame | Drive back lt |
|
||||||
|
| I2C B0 | control | Color sensor V3 | Left outside leg frame | color left |
|
||||||
|
| I2C B1 | control | Color sensor V3 | Right outside leg frame | color right |
|
||||||
|
| digital01 | control | Digital device | arm frame back right | axle encoder |
|
||||||
|
| I2C B0 | expansion | 2m distance sensor | Middle Back outside leg frame | distance |
|
||||||
|
| motor0 | expansion | UltraPlanetary HD hex motor | left back arm frame | arm raise |
|
||||||
|
| motor1 | expansion | Core Hex Motor | right back arm frame | hang |
|
||||||
|
| Servo 0 | expansion | Servo | on arm | wrist |
|
||||||
|
| Servo 1 | expansion | Servo | on arm | gripper |
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -0,0 +1,459 @@
|
|||||||
|
/* 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.eventloop.opmode.Autonomous;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.hardware.ColorSensor;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||||
|
import com.qualcomm.robotcore.hardware.Servo;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This file illustrates the concept of driving a path based on encoder counts.
|
||||||
|
* The code is structured as a LinearOpMode
|
||||||
|
*
|
||||||
|
* The code REQUIRES that you DO have encoders on the wheels,
|
||||||
|
* otherwise you would use: RobotAutoDriveByTime;
|
||||||
|
*
|
||||||
|
* This code ALSO requires that the drive Motors have been configured such that a positive
|
||||||
|
* power command moves them forward, and causes the encoders to count UP.
|
||||||
|
*
|
||||||
|
* The desired path in this example is:
|
||||||
|
* - Drive forward for 48 inches
|
||||||
|
* - Spin right for 12 Inches
|
||||||
|
* - Drive Backward for 24 inches
|
||||||
|
* - Stop and close the claw.
|
||||||
|
*
|
||||||
|
* The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS)
|
||||||
|
* that performs the actual movement.
|
||||||
|
* This method assumes that each movement is relative to the last stopping place.
|
||||||
|
* There are other ways to perform encoder based moves, but this method is probably the simplest.
|
||||||
|
* This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile
|
||||||
|
*
|
||||||
|
* Use Android Studio 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
|
||||||
|
*/
|
||||||
|
|
||||||
|
@Autonomous(name="Robot: Auto Drive By Encoder", group="Robot")
|
||||||
|
//@Disabled
|
||||||
|
public class Autonomoustest extends LinearOpMode {
|
||||||
|
|
||||||
|
/* Declare OpMode members. */
|
||||||
|
private DcMotor leftDrive = null;
|
||||||
|
private DcMotor rightDrive = null;
|
||||||
|
private DcMotor backrightDrive = null;
|
||||||
|
private DcMotor backleftDrive = null;
|
||||||
|
private ColorSensor colorRight = null;
|
||||||
|
private ColorSensor colorLeft = null;
|
||||||
|
private Servo wrist = null;
|
||||||
|
private Servo gripper = null;
|
||||||
|
private DcMotor arm = null;
|
||||||
|
|
||||||
|
|
||||||
|
private ElapsedTime runtime = new ElapsedTime();
|
||||||
|
|
||||||
|
// Calculate the COUNTS_PER_INCH for your specific drive train.
|
||||||
|
// Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV
|
||||||
|
// For external drive gearing, set DRIVE_GEAR_REDUCTION as needed.
|
||||||
|
// For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear.
|
||||||
|
// This is gearing DOWN for less speed and more torque.
|
||||||
|
// For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation.
|
||||||
|
static final double COUNTS_PER_MOTOR_REV = 537.6; // eg: TETRIX Motor Encoder
|
||||||
|
static final double DRIVE_GEAR_REDUCTION = 1.0; // No External Gearing.
|
||||||
|
static final double WHEEL_DIAMETER_INCHES = 3.77953; // For figuring circumference
|
||||||
|
static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
|
||||||
|
(WHEEL_DIAMETER_INCHES * Math.PI);
|
||||||
|
static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
|
||||||
|
static final double DRIVE_SPEED = 0.2;
|
||||||
|
static final double TURN_SPEED = 0.4;
|
||||||
|
|
||||||
|
static final double LONG_TIMEOUT = 1000;
|
||||||
|
static final double DEGREE_TOO_DISTANCE = 0.21944444444;
|
||||||
|
static final double ARM_SPEED = .1;
|
||||||
|
static final double TICKS_TO_DEGREES = 0.07462686567;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode()
|
||||||
|
{
|
||||||
|
hardwareinit();
|
||||||
|
|
||||||
|
// Send telemetry message to indicate successful Encoder reset
|
||||||
|
/* telemetry.addData("Starting at", "%7d :%7d",
|
||||||
|
leftDrive.getCurrentPosition(),
|
||||||
|
rightDrive.getCurrentPosition(),
|
||||||
|
backleftDrive.getCurrentPosition(),
|
||||||
|
backrightDrive.getCurrentPosition());*/
|
||||||
|
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
// Wait for the game to start (driver presses PLAY)
|
||||||
|
waitForStart();
|
||||||
|
{
|
||||||
|
//executeAuto();
|
||||||
|
testGripper();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// Step through each leg of the path,
|
||||||
|
// Note: Reverse movement is obtained by setting a negative distance (not speed)
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//
|
||||||
|
|
||||||
|
public void driveForward(double distance)
|
||||||
|
{
|
||||||
|
leftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); // S1: Forward 47 Inches with 5 Sec timeout
|
||||||
|
}
|
||||||
|
|
||||||
|
public void straightLeft(double distance)
|
||||||
|
{
|
||||||
|
leftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
backrightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void straightRight(double distance)
|
||||||
|
{
|
||||||
|
leftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
rightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
backleftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void turnLeft(double degrees)
|
||||||
|
{
|
||||||
|
leftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
backleftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
double turning_distance = degrees * DEGREE_TOO_DISTANCE;
|
||||||
|
encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void turnRight(double degrees) {
|
||||||
|
leftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
rightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
backrightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
double turning_distance = degrees * DEGREE_TOO_DISTANCE;
|
||||||
|
encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
|
||||||
|
}
|
||||||
|
|
||||||
|
public int readColorRight() {
|
||||||
|
telemetry.addData("Clear", colorRight.alpha());
|
||||||
|
telemetry.addData("Red ", colorRight.red());
|
||||||
|
telemetry.addData("Green", colorRight.green());
|
||||||
|
telemetry.addData("Blue ", colorRight.blue());
|
||||||
|
int bluenumber = colorRight.blue();
|
||||||
|
return bluenumber;
|
||||||
|
}
|
||||||
|
|
||||||
|
public int readColorLeft() {
|
||||||
|
telemetry.addData("Clear Left", colorLeft.alpha());
|
||||||
|
telemetry.addData("Red left ", colorLeft.red());
|
||||||
|
telemetry.addData("Green left", colorLeft.green());
|
||||||
|
telemetry.addData("Blue left", colorLeft.blue());
|
||||||
|
|
||||||
|
int bluenumber = colorLeft.blue();
|
||||||
|
return bluenumber;
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
public void raisearm(int degrees) {
|
||||||
|
armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT);
|
||||||
|
|
||||||
|
}
|
||||||
|
public void hardwareinit()
|
||||||
|
{
|
||||||
|
leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt");
|
||||||
|
rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt");
|
||||||
|
backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt");
|
||||||
|
backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt");
|
||||||
|
colorRight = hardwareMap.get(ColorSensor.class, "color right");
|
||||||
|
colorLeft = hardwareMap.get(ColorSensor.class, "color left");
|
||||||
|
gripper = hardwareMap.get(Servo.class, "gripper");
|
||||||
|
arm = hardwareMap.get(DcMotor.class, "arm raise");
|
||||||
|
wrist = hardwareMap.get(Servo.class, "wrist");
|
||||||
|
// To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
|
||||||
|
// When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
|
||||||
|
// Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips
|
||||||
|
leftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
arm.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
|
||||||
|
leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
backleftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
backrightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
|
||||||
|
|
||||||
|
leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
}
|
||||||
|
public void testWrist()
|
||||||
|
{
|
||||||
|
wrist.setPosition(0);
|
||||||
|
sleep(3000);
|
||||||
|
wrist.setPosition(1);
|
||||||
|
sleep(3000);
|
||||||
|
}
|
||||||
|
public void testGripper()
|
||||||
|
{
|
||||||
|
gripper.setPosition(0);
|
||||||
|
sleep(3000);
|
||||||
|
gripper.setPosition(1);
|
||||||
|
sleep(3000);
|
||||||
|
}
|
||||||
|
public void executeAuto()
|
||||||
|
{
|
||||||
|
|
||||||
|
raisearm(100);
|
||||||
|
wrist.setPosition(0);
|
||||||
|
driveForward(28);
|
||||||
|
int blueleft = readColorLeft();
|
||||||
|
int blueright = readColorRight();
|
||||||
|
if (blueleft > 75)
|
||||||
|
{
|
||||||
|
//telemetry.addData("color sensor","left");
|
||||||
|
if(blueleft > blueright)
|
||||||
|
telemetry.addData("color sensor","left");
|
||||||
|
turnLeft(90);
|
||||||
|
straightLeft(2);
|
||||||
|
driveForward(11);
|
||||||
|
driveForward(-30);
|
||||||
|
straightLeft(32);
|
||||||
|
turnLeft(10);
|
||||||
|
driveForward(18);
|
||||||
|
driveForward(-31);
|
||||||
|
double backboard = 34;
|
||||||
|
straightRight(backboard);
|
||||||
|
raisearm(80);
|
||||||
|
driveForward(-1);
|
||||||
|
gripper.setPosition(0);
|
||||||
|
terminateOpModeNow();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
if (blueright > 75)
|
||||||
|
{
|
||||||
|
//telemetry.addData("color sensor", "right");
|
||||||
|
if(blueleft < blueright)
|
||||||
|
telemetry.addData("color sensor","right");
|
||||||
|
straightRight(11.5);
|
||||||
|
driveForward(-17);
|
||||||
|
turnLeft(90);
|
||||||
|
straightLeft(15);
|
||||||
|
driveForward(8);
|
||||||
|
driveForward(-26);
|
||||||
|
double backboard = 22.5;
|
||||||
|
straightRight(backboard);
|
||||||
|
raisearm(80);
|
||||||
|
driveForward(-5);
|
||||||
|
gripper.setPosition(1);
|
||||||
|
terminateOpModeNow();
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
else
|
||||||
|
telemetry.addData("position","center");
|
||||||
|
driveForward(7);
|
||||||
|
driveForward(-7);
|
||||||
|
straightRight(11.5);
|
||||||
|
driveForward(-17);
|
||||||
|
turnLeft(90);
|
||||||
|
straightLeft(15);
|
||||||
|
driveForward(8);
|
||||||
|
driveForward(-26);
|
||||||
|
double backboard = 29;
|
||||||
|
straightRight(backboard);
|
||||||
|
raisearm(80);
|
||||||
|
driveForward(-5);
|
||||||
|
gripper.setPosition(1);
|
||||||
|
telemetry.update();
|
||||||
|
sleep(250);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//Values were created from robot with wheel issues 9/28/23
|
||||||
|
|
||||||
|
telemetry.addData("Path", "Complete");
|
||||||
|
telemetry.update();
|
||||||
|
sleep(1000); // pause to display final telemetry message.
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Method to perform a relative move, based on encoder counts.
|
||||||
|
* Encoders are not reset as the move is based on the current position.
|
||||||
|
* Move will stop if any of three conditions occur:
|
||||||
|
* 1) Move gets to the desired position
|
||||||
|
* 2) Move runs out of time
|
||||||
|
* 3) Driver stops the opmode running.
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
public void encoderDrive(double speed,
|
||||||
|
double leftInches, double rightInches,
|
||||||
|
double timeoutS) {
|
||||||
|
int newLeftTarget;
|
||||||
|
int newRightTarget;
|
||||||
|
int newBackLeftTarget;
|
||||||
|
int newbackRightTarget;
|
||||||
|
|
||||||
|
|
||||||
|
if (opModeIsActive()) {
|
||||||
|
|
||||||
|
// Determine new target position, and pass to motor controller
|
||||||
|
newLeftTarget = leftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH);
|
||||||
|
newRightTarget = rightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
|
||||||
|
newBackLeftTarget = backleftDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
|
||||||
|
newbackRightTarget = backrightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH);
|
||||||
|
leftDrive.setTargetPosition(newLeftTarget);
|
||||||
|
rightDrive.setTargetPosition(newRightTarget);
|
||||||
|
backrightDrive.setTargetPosition(newbackRightTarget);
|
||||||
|
backleftDrive.setTargetPosition(newBackLeftTarget);
|
||||||
|
|
||||||
|
// Turn On RUN_TO_POSITION
|
||||||
|
leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
backrightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
backleftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
|
||||||
|
// reset the timeout time and start motion.
|
||||||
|
runtime.reset();
|
||||||
|
leftDrive.setPower(Math.abs(speed));
|
||||||
|
rightDrive.setPower(Math.abs(speed));
|
||||||
|
backrightDrive.setPower(Math.abs(speed));
|
||||||
|
backleftDrive.setPower(Math.abs(speed));
|
||||||
|
|
||||||
|
// keep looping while we are still active, and there is time left, and both motors are running.
|
||||||
|
// Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
|
||||||
|
// its target position, the motion will stop. This is "safer" in the event that the robot will
|
||||||
|
// always end the motion as soon as possible.
|
||||||
|
// However, if you require that BOTH motors have finished their moves before the robot continues
|
||||||
|
// onto the next step, use (isBusy() || isBusy()) in the loop test.
|
||||||
|
while (opModeIsActive() &&
|
||||||
|
(runtime.seconds() < timeoutS) &&
|
||||||
|
(leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy() && backrightDrive.isBusy())) {
|
||||||
|
|
||||||
|
// Display it for the driver.
|
||||||
|
telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget);
|
||||||
|
telemetry.addData("Currently at", " at %7d :%7d",
|
||||||
|
leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backrightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition());
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
leftDrive.setPower(0);
|
||||||
|
rightDrive.setPower(0);
|
||||||
|
backrightDrive.setPower(0);
|
||||||
|
backleftDrive.setPower(0);
|
||||||
|
|
||||||
|
|
||||||
|
// Turn off RUN_TO_POSITION
|
||||||
|
leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
|
||||||
|
sleep(250); // optional pause after each move.
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void armEncoder(double speed,
|
||||||
|
double Inches, double timeoutS) {
|
||||||
|
int newarmTarget;
|
||||||
|
|
||||||
|
|
||||||
|
if (opModeIsActive()) {
|
||||||
|
|
||||||
|
// Determine new target position, and pass to motor controller
|
||||||
|
newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
|
||||||
|
arm.setTargetPosition(newarmTarget);
|
||||||
|
|
||||||
|
// Turn On RUN_TO_POSITION
|
||||||
|
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
|
||||||
|
// reset the timeout time and start motion.
|
||||||
|
runtime.reset();
|
||||||
|
arm.setPower(Math.abs(speed));
|
||||||
|
|
||||||
|
// keep looping while we are still active, and there is time left, and both motors are running.
|
||||||
|
// Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
|
||||||
|
// its target position, the motion will stop. This is "safer" in the event that the robot will
|
||||||
|
// always end the motion as soon as possible.
|
||||||
|
// However, if you require that BOTH motors have finished their moves before the robot continues
|
||||||
|
// onto the next step, use (isBusy() || isBusy()) in the loop test.
|
||||||
|
while (opModeIsActive() &&
|
||||||
|
(runtime.seconds() < timeoutS) &&
|
||||||
|
(arm.isBusy())) {
|
||||||
|
|
||||||
|
// Display it for the driver.
|
||||||
|
telemetry.addData("Running to", " %7d", newarmTarget);
|
||||||
|
telemetry.addData("Currently at", " at %7d",
|
||||||
|
arm.getCurrentPosition());
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
arm.setPower(0);
|
||||||
|
|
||||||
|
|
||||||
|
// Turn off RUN_TO_POSITION
|
||||||
|
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,171 @@
|
|||||||
|
/* Copyright (c) 2021 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.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.util.ElapsedTime;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This file contains an example of a Linear "OpMode".
|
||||||
|
* An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
|
||||||
|
* The names of OpModes appear on the menu of the FTC Driver Station.
|
||||||
|
* When a selection is made from the menu, the corresponding OpMode is executed.
|
||||||
|
*
|
||||||
|
* This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot.
|
||||||
|
* This code will work with either a Mecanum-Drive or an X-Drive train.
|
||||||
|
* Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html
|
||||||
|
* Note that a Mecanum drive must display an X roller-pattern when viewed from above.
|
||||||
|
*
|
||||||
|
* Also note that it is critical to set the correct rotation direction for each motor. See details below.
|
||||||
|
*
|
||||||
|
* Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously.
|
||||||
|
* Each motion axis is controlled by one Joystick axis.
|
||||||
|
*
|
||||||
|
* 1) Axial: Driving forward and backward Left-joystick Forward/Backward
|
||||||
|
* 2) Lateral: Strafing right and left Left-joystick Right and Left
|
||||||
|
* 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left
|
||||||
|
*
|
||||||
|
* This code is written assuming that the right-side motors need to be reversed for the robot to drive forward.
|
||||||
|
* When you first test your robot, if it moves backward when you push the left stick forward, then you must flip
|
||||||
|
* the direction of all 4 motors (see code below).
|
||||||
|
*
|
||||||
|
* Use Android Studio 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="Basic: Omni Linear OpMode", group="Linear Opmode")
|
||||||
|
@Disabled
|
||||||
|
public class BasicOmniOpMode_Linear extends LinearOpMode {
|
||||||
|
|
||||||
|
// Declare OpMode members for each of the 4 motors.
|
||||||
|
private ElapsedTime runtime = new ElapsedTime();
|
||||||
|
private DcMotor leftFrontDrive = null;
|
||||||
|
private DcMotor leftBackDrive = null;
|
||||||
|
private DcMotor rightFrontDrive = null;
|
||||||
|
private DcMotor rightBackDrive = null;
|
||||||
|
|
||||||
|
public class run{
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() {
|
||||||
|
|
||||||
|
// Initialize the hardware variables. Note that the strings used here must correspond
|
||||||
|
// to the names assigned during the robot configuration step on the DS or RC devices.
|
||||||
|
leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive");
|
||||||
|
leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive");
|
||||||
|
rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive");
|
||||||
|
rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive");
|
||||||
|
|
||||||
|
// ########################################################################################
|
||||||
|
// !!! IMPORTANT Drive Information. Test your motor directions. !!!!!
|
||||||
|
// ########################################################################################
|
||||||
|
// Most robots need the motors on one side to be reversed to drive forward.
|
||||||
|
// The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft)
|
||||||
|
// If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure
|
||||||
|
// that your motors are turning in the correct direction. So, start out with the reversals here, BUT
|
||||||
|
// when you first test your robot, push the left joystick forward and observe the direction the wheels turn.
|
||||||
|
// Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward
|
||||||
|
// Keep testing until ALL the wheels move the robot forward when you push the left joystick forward.
|
||||||
|
leftFrontDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
leftBackDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
rightFrontDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
rightBackDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
|
||||||
|
// Wait for the game to start (driver presses PLAY)
|
||||||
|
telemetry.addData("Status", "Initialized");
|
||||||
|
telemetry.update();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
runtime.reset();
|
||||||
|
|
||||||
|
// run until the end of the match (driver presses STOP)
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
double max;
|
||||||
|
|
||||||
|
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
|
||||||
|
double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value
|
||||||
|
double lateral = gamepad1.left_stick_x;
|
||||||
|
double yaw = gamepad1.right_stick_x;
|
||||||
|
|
||||||
|
// Combine the joystick requests for each axis-motion to determine each wheel's power.
|
||||||
|
// Set up a variable for each drive wheel to save the power level for telemetry.
|
||||||
|
double leftFrontPower = axial + lateral + yaw;
|
||||||
|
double rightFrontPower = axial - lateral - yaw;
|
||||||
|
double leftBackPower = axial - lateral + yaw;
|
||||||
|
double rightBackPower = axial + lateral - yaw;
|
||||||
|
|
||||||
|
// Normalize the values so no wheel power exceeds 100%
|
||||||
|
// This ensures that the robot maintains the desired motion.
|
||||||
|
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
|
||||||
|
max = Math.max(max, Math.abs(leftBackPower));
|
||||||
|
max = Math.max(max, Math.abs(rightBackPower));
|
||||||
|
|
||||||
|
if (max > 1.0) {
|
||||||
|
leftFrontPower /= max;
|
||||||
|
rightFrontPower /= max;
|
||||||
|
leftBackPower /= max;
|
||||||
|
rightBackPower /= max;
|
||||||
|
}
|
||||||
|
|
||||||
|
// This is test code:
|
||||||
|
//
|
||||||
|
// Uncomment the following code to test your motor directions.
|
||||||
|
// Each button should make the corresponding motor run FORWARD.
|
||||||
|
// 1) First get all the motors to take to correct positions on the robot
|
||||||
|
// by adjusting your Robot Configuration if necessary.
|
||||||
|
// 2) Then make sure they run in the correct direction by modifying the
|
||||||
|
// the setDirection() calls above.
|
||||||
|
// Once the correct motors move in the correct direction re-comment this code.
|
||||||
|
|
||||||
|
/*
|
||||||
|
leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad
|
||||||
|
leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad
|
||||||
|
rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad
|
||||||
|
rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Send calculated power to wheels
|
||||||
|
leftFrontDrive.setPower(leftFrontPower);
|
||||||
|
rightFrontDrive.setPower(rightFrontPower);
|
||||||
|
leftBackDrive.setPower(leftBackPower);
|
||||||
|
rightBackDrive.setPower(rightBackPower);
|
||||||
|
|
||||||
|
// Show the elapsed game time and wheel power.
|
||||||
|
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||||
|
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
|
||||||
|
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}}
|
||||||
@ -0,0 +1,258 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||||
|
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||||
|
import com.qualcomm.robotcore.hardware.Servo;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@TeleOp( name = "fullRobotControl")
|
||||||
|
public class FullRobotControl extends OpMode {
|
||||||
|
public double axial;
|
||||||
|
public double lateral;
|
||||||
|
public double yaw;
|
||||||
|
DcMotor frontRight;
|
||||||
|
DcMotor backRight;
|
||||||
|
DcMotor frontLeft;
|
||||||
|
DcMotor backLeft;
|
||||||
|
DcMotor armMotor;
|
||||||
|
Servo gripper;
|
||||||
|
Servo wrist;
|
||||||
|
DcMotor arm;
|
||||||
|
public ElapsedTime runtime = new ElapsedTime();
|
||||||
|
static final double TICKS_TO_DEGREES = 0.07462686567;
|
||||||
|
static final double COUNTS_PER_MOTOR_REV = 537.6;
|
||||||
|
static final double DRIVE_GEAR_REDUCTION = 1.0;
|
||||||
|
static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* this function takes a long milliseconds parameter and sleeps
|
||||||
|
* @param millis milliseconds to sleep
|
||||||
|
*/
|
||||||
|
public void sleepmillis(long millis) {
|
||||||
|
try {
|
||||||
|
Thread.sleep(millis);
|
||||||
|
} catch (Exception e) {
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* stops all drive motors
|
||||||
|
*/
|
||||||
|
public void off() {
|
||||||
|
frontRight.setPower(0);
|
||||||
|
backRight.setPower(0);
|
||||||
|
frontLeft.setPower(0);
|
||||||
|
backLeft.setPower(0);
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* User defined init method
|
||||||
|
* This method will be called once when the INIT button is pressed.
|
||||||
|
*/
|
||||||
|
|
||||||
|
public void init() {
|
||||||
|
|
||||||
|
|
||||||
|
telemetry.addData("Status","In Init()");
|
||||||
|
telemetry.update();
|
||||||
|
frontRight = hardwareMap.dcMotor.get("Drive front rt");
|
||||||
|
backRight = hardwareMap.dcMotor.get("Drive back rt");
|
||||||
|
frontLeft = hardwareMap.dcMotor.get("Drive front lt");
|
||||||
|
backLeft = hardwareMap.dcMotor.get("Drive back lt");
|
||||||
|
armMotor = hardwareMap.dcMotor.get("armMotor");
|
||||||
|
gripper = hardwareMap.servo.get("gripper");
|
||||||
|
gripper.setPosition(1);
|
||||||
|
wrist = hardwareMap.servo.get("wrist");
|
||||||
|
arm = hardwareMap.dcMotor.get("arm raise");
|
||||||
|
wrist.setPosition(1);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* User defined init_loop method
|
||||||
|
* This method will be called repeatedly when the INIT button is pressed.
|
||||||
|
* This method is optional. By default this method takes no action.
|
||||||
|
*/
|
||||||
|
public void init_loop(){
|
||||||
|
// Wait for the game to start (driver presses PLAY)
|
||||||
|
telemetry.addData("Status", "Initialized");
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* User defined start method.
|
||||||
|
* This method will be called once when the PLAY button is first pressed.
|
||||||
|
* This method is optional. By default this method takes not action. Example usage: Starting another thread.
|
||||||
|
*/
|
||||||
|
public void start() {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* User defined stop method
|
||||||
|
* This method will be called when this op mode is first disabled.
|
||||||
|
* The stop method is optional. By default this method takes no action.
|
||||||
|
*/
|
||||||
|
public void stop(){
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* User defined loop method.
|
||||||
|
* This method will be called repeatedly in a loop while this op mode is running
|
||||||
|
*/
|
||||||
|
double num = 2;
|
||||||
|
String speed = "plaid";
|
||||||
|
public void loop() {
|
||||||
|
|
||||||
|
frontLeft.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
backLeft.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
frontRight.setDirection(DcMotor.Direction.FORWARD);
|
||||||
|
backRight.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
|
||||||
|
if(gamepad1.a){
|
||||||
|
num = 2.5;
|
||||||
|
speed = "medium";
|
||||||
|
}
|
||||||
|
if(gamepad1.b){
|
||||||
|
num = 2;
|
||||||
|
speed = "fast";
|
||||||
|
}
|
||||||
|
if(gamepad1.x){
|
||||||
|
num = 1.75;
|
||||||
|
speed = "Ludicrous";
|
||||||
|
}
|
||||||
|
if(gamepad1.y ){
|
||||||
|
num = 1.5;
|
||||||
|
speed = "plaid";
|
||||||
|
}
|
||||||
|
axial = -gamepad1.left_stick_y/num; // Note: pushing stick forward gives negative value
|
||||||
|
lateral = gamepad1.left_stick_x/num;
|
||||||
|
yaw = gamepad1.right_stick_x/(num+0.5);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Combine the joystick requests for each axis-motion to determine each wheel's power.
|
||||||
|
// Set up a variable for each drive wheel to save the power level for telemetry.
|
||||||
|
double leftFrontPower = axial + lateral + yaw;
|
||||||
|
double rightFrontPower = axial - lateral - yaw;
|
||||||
|
double leftBackPower = axial - lateral + yaw;
|
||||||
|
double rightBackPower = axial + lateral - yaw;
|
||||||
|
double armPower = gamepad1.right_stick_y/3;
|
||||||
|
// Normalize the values so no wheel power exceeds 100%
|
||||||
|
// This ensures that the robot maintains the desired motion.
|
||||||
|
double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
|
||||||
|
max = Math.max(max, Math.abs(leftBackPower));
|
||||||
|
max = Math.max(max, Math.abs(rightBackPower));
|
||||||
|
if (max > 1.0) {
|
||||||
|
leftFrontPower /= max;
|
||||||
|
rightFrontPower /= max;
|
||||||
|
leftBackPower /= max;
|
||||||
|
rightBackPower /= max;
|
||||||
|
}
|
||||||
|
if(gamepad1.dpad_down){
|
||||||
|
wrist.setPosition(1);
|
||||||
|
|
||||||
|
}
|
||||||
|
if(gamepad1.dpad_up){
|
||||||
|
|
||||||
|
wrist.setPosition(0.5);
|
||||||
|
}
|
||||||
|
if(gamepad1.left_bumper){
|
||||||
|
gripper.setPosition(0.25);
|
||||||
|
}
|
||||||
|
if(gamepad1.right_bumper){
|
||||||
|
gripper.setPosition(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
int armState = 0;
|
||||||
|
if(gamepad2.x)
|
||||||
|
{
|
||||||
|
armState = 1;
|
||||||
|
raisearm(0);
|
||||||
|
}
|
||||||
|
if(gamepad2.y)
|
||||||
|
{
|
||||||
|
armState = 2;
|
||||||
|
raisearm(30);
|
||||||
|
}
|
||||||
|
if(gamepad2.y)
|
||||||
|
{
|
||||||
|
armState = 2;
|
||||||
|
raisearm(200);
|
||||||
|
}
|
||||||
|
if(gamepad2.right_bumper)
|
||||||
|
{
|
||||||
|
armState =+ 1;
|
||||||
|
}
|
||||||
|
if(gamepad2.left_bumper)
|
||||||
|
{
|
||||||
|
armState =- 1;
|
||||||
|
}
|
||||||
|
if(armState == 1)
|
||||||
|
{
|
||||||
|
raisearm(0);
|
||||||
|
}
|
||||||
|
if(armState == 2)
|
||||||
|
{
|
||||||
|
raisearm(30);
|
||||||
|
}
|
||||||
|
if(armState == 1)
|
||||||
|
{
|
||||||
|
raisearm(200);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
frontLeft.setPower(leftFrontPower);
|
||||||
|
frontRight.setPower(rightFrontPower);
|
||||||
|
backLeft.setPower(leftBackPower);
|
||||||
|
backRight.setPower(rightBackPower);
|
||||||
|
armMotor.setPower(armPower);
|
||||||
|
// Show the elapsed game time and wheel power
|
||||||
|
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||||
|
telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
|
||||||
|
telemetry.addData("Back left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
|
||||||
|
telemetry.addData("Speed", speed);
|
||||||
|
|
||||||
|
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
public void raisearm(int degrees)
|
||||||
|
{
|
||||||
|
armEncoder(.2, degrees*TICKS_TO_DEGREES);
|
||||||
|
|
||||||
|
}
|
||||||
|
public void armEncoder(double speed,
|
||||||
|
double Inches) {
|
||||||
|
int newarmTarget;
|
||||||
|
|
||||||
|
|
||||||
|
// Determine new target position, and pass to motor controller
|
||||||
|
newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
|
||||||
|
arm.setTargetPosition(newarmTarget);
|
||||||
|
|
||||||
|
// Turn On RUN_TO_POSITION
|
||||||
|
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
|
||||||
|
// reset the timeout time and start motion.
|
||||||
|
runtime.reset();
|
||||||
|
arm.setPower(Math.abs(speed));
|
||||||
|
|
||||||
|
|
||||||
|
arm.setPower(0);
|
||||||
|
|
||||||
|
|
||||||
|
// Turn off RUN_TO_POSITION
|
||||||
|
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
@ -0,0 +1,4 @@
|
|||||||
|
package org.firstinspires.ftc.teamcode;
|
||||||
|
|
||||||
|
public class TestCode {
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user