Compare commits
73 Commits
master
...
branch-coo
Author | SHA1 | Date | |
---|---|---|---|
05cc0c6785 | |||
a27996882b | |||
4dc9b182a7 | |||
b956adb95a | |||
cb87dceeb8 | |||
4855f69efc | |||
913544f4fd | |||
e40dd11624 | |||
7d28e8bd60 | |||
d025c7b106 | |||
8108e5c42f | |||
84aba36915 | |||
13eebf51b8 | |||
93edbbf45f | |||
76ca6437ed | |||
96345a151c | |||
c71019e090 | |||
6943172487 | |||
aecb6122b3 | |||
c5bc5df6a3 | |||
76eac94686 | |||
e6e8a657d7 | |||
a260b373de | |||
0d131c1b1e | |||
a6ea0fc529 | |||
e3a3bdfb3b | |||
58f738e28d | |||
7c1a0923e2 | |||
4b6fd66770 | |||
8aef2037c5 | |||
36d3af564e | |||
e991c38b72 | |||
81e0825fea | |||
a9c0d443eb | |||
d666d5e9ac | |||
20680c3011 | |||
17f4fa47dc | |||
7a861d562e | |||
2b638e1953 | |||
8db63f829c | |||
6f5e08e0cd | |||
162922a164 | |||
387cca2ace | |||
089ca0cf3d | |||
9a4561b29c | |||
8d880cd330 | |||
55eacfc391 | |||
45bab85f09 | |||
aa2d8a2b8a | |||
d08f00ce12 | |||
ba5a1102ed | |||
e3469f56c4 | |||
18418825d2 | |||
eb81e1b4b6 | |||
180f2bd873 | |||
69f9264228 | |||
9564e5a539 | |||
35dbc1cc15 | |||
ab728d975c | |||
780f5fdde9 | |||
7aa982d2bf | |||
e65767b2da | |||
71f2eafbd6 | |||
ed1868c682 | |||
82cecba8f1 | |||
9aab8fadf1 | |||
c3d8dc6b11 | |||
ee86476ca3 | |||
70ac99c5b3 | |||
c3da4e525d | |||
dc530b52b7 | |||
7b1b952a6c | |||
ffd94c28eb |
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 |
|
||||
| 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 |
|
||||
| motor3 | expansion | Digital device | arm frame back right | axle encoder |
|
||||
| Servo 0 | expansion | Servo | on arm | wrist |
|
||||
| Servo 1 | expansion | Servo | on arm | gripper |
|
||||
|
||||
|
||||
|
||||
|
||||
|
17
Jenkinsfile
vendored
Normal file
17
Jenkinsfile
vendored
Normal file
@ -0,0 +1,17 @@
|
||||
pipeline {
|
||||
agent {
|
||||
docker { image 'mingc/android-build-box' }
|
||||
}
|
||||
stages {
|
||||
stage('Gradle Build') {
|
||||
steps {
|
||||
sh './gradlew clean build'
|
||||
}
|
||||
}
|
||||
}
|
||||
post {
|
||||
always {
|
||||
archiveArtifacts artifacts: 'TeamCode/build/outputs/apk/debug/*.apk', fingerprint: true
|
||||
}
|
||||
}
|
||||
}
|
4
NOTES.md
Normal file
4
NOTES.md
Normal file
@ -0,0 +1,4 @@
|
||||
- Refactor of code
|
||||
- Possibly establish patterns
|
||||
- Also, establish github for students as an element of a professional portfolio (laura)
|
||||
-
|
@ -1,4 +1,4 @@
|
||||
## NOTICE
|
||||
## SCDS NOTICE
|
||||
|
||||
This repository contains the public FTC SDK for the CENTERSTAGE (2023-2024) competition season.
|
||||
|
||||
|
@ -26,8 +26,4 @@ android {
|
||||
dependencies {
|
||||
implementation project(':FtcRobotController')
|
||||
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
|
||||
|
||||
implementation 'org.apache.commons:commons-math3:3.6.1'
|
||||
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
|
||||
implementation 'com.acmerobotics.roadrunner:core:0.5.6'
|
||||
}
|
||||
}
|
@ -0,0 +1,195 @@
|
||||
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 = "ArmState")
|
||||
public class
|
||||
ArmStates extends OpMode {
|
||||
|
||||
DcMotor arm;
|
||||
Servo wrist;
|
||||
static final double TICKS_TO_DEGREES = 0.07462686567;
|
||||
static final double COUNTS_PER_MOTOR_REV = 537.6;
|
||||
static final double DRIVE_GEAR_REDUCTION = 60;
|
||||
static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.75 * 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() {
|
||||
arm.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();
|
||||
arm = hardwareMap.dcMotor.get("arm raise");
|
||||
wrist = hardwareMap.servo.get("wrist");
|
||||
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
public int armState = 0;
|
||||
|
||||
public void loop() {
|
||||
int currentArmState = armState;
|
||||
|
||||
telemetry.addData("state", armState);
|
||||
telemetry.addData("arm pos", arm.getCurrentPosition());
|
||||
telemetry.update();
|
||||
//Something is wrong, the arm is going the wrong way. Maybe it's just my robot, but you should look at it
|
||||
if (gamepad2.x) {
|
||||
armState = 1;
|
||||
}
|
||||
if (gamepad2.y) {
|
||||
armState = 2;
|
||||
}
|
||||
if (gamepad2.b) {
|
||||
armState = 3;
|
||||
}
|
||||
if (gamepad2.a)
|
||||
{
|
||||
armState = 0;
|
||||
}
|
||||
if (gamepad2.right_bumper) {
|
||||
armState += 1;
|
||||
}
|
||||
if (gamepad2.left_bumper) {
|
||||
armState -= 1;
|
||||
}
|
||||
if (armState != currentArmState) {
|
||||
updateState();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public void armDriveWithEncoder(double speed,
|
||||
int ticks) {
|
||||
|
||||
// Determine new target position, and pass to motor controller
|
||||
arm.setTargetPosition(ticks);
|
||||
|
||||
// Turn On RUN_TO_POSITION
|
||||
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
// reset the timeout time and start motion.
|
||||
arm.setPower(Math.abs(speed));
|
||||
|
||||
while //(opModeIsActive() &&
|
||||
//(runtime.seconds() < timeoutS) &&
|
||||
(arm.isBusy()) {
|
||||
telemetry.addData("Running to", " %7d ", ticks);
|
||||
telemetry.addData("Currently at", " %7d", arm.getCurrentPosition());
|
||||
telemetry.update();
|
||||
}
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
arm.setPower(0);
|
||||
|
||||
// Turn off RUN_TO_POSITION
|
||||
arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
}
|
||||
|
||||
|
||||
public void raisearm(int degrees) {
|
||||
arm.setDirection(DcMotor.Direction.REVERSE);
|
||||
armDriveWithEncoder(.05, degrees);
|
||||
}
|
||||
|
||||
public void updateState() {
|
||||
if (armState == 0) {
|
||||
// arm state 0 means on the ground with gripper down
|
||||
wrist.setPosition(.5);
|
||||
raisearm(0);
|
||||
telemetry.addData("state", 0);
|
||||
}
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
arm.setPower(0);
|
||||
if (armState == 1) {
|
||||
// arm state 1 is for driving
|
||||
raisearm(200);
|
||||
telemetry.addData("state", 1);
|
||||
}
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
arm.setPower(0);
|
||||
if (armState == 2) {
|
||||
raisearm(697);
|
||||
wrist.setPosition(0);
|
||||
raisearm(697);
|
||||
telemetry.addData("state", 2);
|
||||
}
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
arm.setPower(.0);
|
||||
if (armState == 3) {
|
||||
raisearm(780);
|
||||
telemetry.addData("state", 3);
|
||||
telemetry.addData("arm pos", arm.getCurrentPosition());
|
||||
}
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
arm.setPower(0);
|
||||
}
|
||||
}
|
@ -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=" CR file", 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();
|
||||
}
|
||||
}}
|
518
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java
Normal file
518
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java
Normal file
@ -0,0 +1,518 @@
|
||||
/* 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 android.annotation.SuppressLint;
|
||||
|
||||
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.DistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
/**
|
||||
* 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="Blue", group="Robot")
|
||||
//@Disabled
|
||||
public class Blue extends LinearOpMode {
|
||||
|
||||
/* Declare OpMode members. */
|
||||
private DcMotor leftDrive = null;
|
||||
private DcMotor rightDrive = null;
|
||||
private DcMotor backrightDrive = null;
|
||||
private DcMotor backleftDrive = null;
|
||||
private DistanceSensor distanceRight = null;
|
||||
private DistanceSensor distanceLeft = null;
|
||||
private Servo wrist = null;
|
||||
private Servo gripper = null;
|
||||
private DcMotor arm = null;
|
||||
private DistanceSensor distance = 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.3;
|
||||
|
||||
static final double DRIVE_SPEED_SLOW = 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();
|
||||
|
||||
}
|
||||
|
||||
// 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, false); // 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, true);
|
||||
}
|
||||
|
||||
public void straightLeftOnPower(double speed) {
|
||||
leftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backrightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
leftDrive.setPower(speed * 1.05);
|
||||
rightDrive.setPower(speed * 1.05);
|
||||
backrightDrive.setPower(speed);
|
||||
backleftDrive.setPower(speed);
|
||||
}
|
||||
|
||||
|
||||
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, true);
|
||||
}
|
||||
|
||||
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, false);
|
||||
}
|
||||
|
||||
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, false);
|
||||
}
|
||||
|
||||
public void straighten(Double distance)
|
||||
|
||||
{
|
||||
driveForward(0);
|
||||
double D1 = distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
driveForward(distance);
|
||||
double D2 = distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
double rad = Math.atan2(D1 - D2, distance);
|
||||
double degrees = Math.toDegrees(rad);
|
||||
turnRight(degrees);
|
||||
telemetry.addData("d1", D1);
|
||||
telemetry.addData("d2", D2);
|
||||
telemetry.addData("Calibration deg", degrees);
|
||||
telemetry.update();
|
||||
sleep(1000);
|
||||
}
|
||||
|
||||
public void centerLeft()
|
||||
{
|
||||
double leftDistance = distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
straightLeft(leftDistance - 3);
|
||||
telemetry.addData("leftDistance",leftDistance);
|
||||
telemetry.addData("moving left x inches",leftDistance - 3);
|
||||
telemetry.update();
|
||||
straightLeft(0.0);
|
||||
sleep(1000);
|
||||
}
|
||||
|
||||
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");
|
||||
distanceRight = hardwareMap.get(DistanceSensor.class, "color right");
|
||||
distanceLeft = hardwareMap.get(DistanceSensor.class, "color left");
|
||||
gripper = hardwareMap.get(Servo.class, "gripper");
|
||||
arm = hardwareMap.get(DcMotor.class, "arm raise");
|
||||
wrist = hardwareMap.get(Servo.class, "wrist");
|
||||
distance = hardwareMap.get(DistanceSensor.class, "distance");
|
||||
sleep(1000);
|
||||
// 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.5);
|
||||
|
||||
}
|
||||
@SuppressLint("SuspiciousIndentation")
|
||||
public void executeAuto()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
int distright = (int)distanceRight.getDistance(DistanceUnit.INCH);
|
||||
telemetry.addData("right dist", distright);
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
// arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
// driveForward(26);
|
||||
// sleep(500);
|
||||
//
|
||||
// int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
// int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
|
||||
// telemetry.addData("color left sensor",distanceleft);
|
||||
// telemetry.addData("color right sensor",distanceright);
|
||||
// telemetry.update();
|
||||
|
||||
|
||||
// if (distanceleft < 7)
|
||||
// {
|
||||
// telemetry.addData("postion","left");
|
||||
// telemetry.update();
|
||||
// turnLeft(90);
|
||||
// straightLeft(2);
|
||||
// driveForward(6.5);
|
||||
// raisearm(45);
|
||||
// arm.setPower(0);
|
||||
// sleep(500);
|
||||
// driveForward(-20);
|
||||
//
|
||||
// do {
|
||||
// straightLeftOnPower(DRIVE_SPEED_SLOW);
|
||||
// distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
//
|
||||
// } while (distanceleft >= 4);
|
||||
// straighten(12.0);
|
||||
// centerLeft();
|
||||
// driveForward(88);
|
||||
// sleep(1000);
|
||||
// wrist.setPosition(.465);
|
||||
// gripper.setPosition(1);
|
||||
// sleep(1000);
|
||||
// driveForward(-3);
|
||||
// terminateOpModeNow();
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
// }
|
||||
// if (distanceright < 7)
|
||||
// {
|
||||
// telemetry.addData("postion", "right");
|
||||
// telemetry.update();
|
||||
// straightRight(12);
|
||||
// raisearm(80);
|
||||
// arm.setPower(0);
|
||||
// driveForward(-22);
|
||||
// turnLeft(90);
|
||||
// do {
|
||||
// straightLeftOnPower(DRIVE_SPEED_SLOW);
|
||||
// distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
//
|
||||
// } while (distanceleft >= 6);
|
||||
// straighten(12.0);
|
||||
// centerLeft();
|
||||
// driveForward(98);
|
||||
// sleep(1000);
|
||||
// wrist.setPosition(.465);
|
||||
// gripper.setPosition(1);
|
||||
// sleep(1000);
|
||||
// driveForward(-3);
|
||||
// terminateOpModeNow();
|
||||
//
|
||||
//
|
||||
// }
|
||||
// else
|
||||
// telemetry.addData("postion","center");
|
||||
// telemetry.update();
|
||||
// driveForward(3.5);
|
||||
// raisearm(80);
|
||||
// arm.setPower(0);
|
||||
// driveForward(-8);
|
||||
// turnLeft(90);
|
||||
// driveForward(-12);
|
||||
// do {
|
||||
// straightLeftOnPower(DRIVE_SPEED_SLOW);
|
||||
// distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
//
|
||||
// } while (distanceleft >= 6);
|
||||
// straighten(12.0);
|
||||
// centerLeft();
|
||||
// driveForward(88);
|
||||
// sleep(1000);
|
||||
// wrist.setPosition(.465);
|
||||
// gripper.setPosition(1);
|
||||
// sleep(1000);
|
||||
// driveForward(-3);
|
||||
// terminateOpModeNow();
|
||||
|
||||
|
||||
|
||||
//Values were created from robot with wheel issues 9/28/23
|
||||
// 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, boolean addJuice) {
|
||||
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) (leftInches * 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();
|
||||
if(addJuice) {
|
||||
leftDrive.setPower(Math.abs(speed * 1.05));
|
||||
rightDrive.setPower(Math.abs(speed * 1.05));
|
||||
} else {
|
||||
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,468 @@
|
||||
/* 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.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
/**
|
||||
* 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="Blue (Backstage)", group="Robot")
|
||||
//@Disabled
|
||||
public class BlueBackStage extends LinearOpMode {
|
||||
|
||||
/* Declare OpMode members. */
|
||||
private DcMotor leftDrive = null;
|
||||
private DcMotor rightDrive = null;
|
||||
private DcMotor backrightDrive = null;
|
||||
private DcMotor backleftDrive = null;
|
||||
private DistanceSensor distanceRight = null;
|
||||
private DistanceSensor distanceLeft = 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);
|
||||
double DRIVE_SPEED = 0.5;
|
||||
|
||||
static final double DRIVE_SPEED_SLOW = .25;
|
||||
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();
|
||||
|
||||
}
|
||||
|
||||
// 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, false); // S1: Forward 47 Inches with 5 Sec timeout
|
||||
}
|
||||
|
||||
public void driveForwardSpeed(double distance, double speed) {
|
||||
leftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backrightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
encoderDrive(speed, distance, distance, LONG_TIMEOUT, false); // 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, true);
|
||||
}
|
||||
|
||||
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, true);
|
||||
}
|
||||
|
||||
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, false);
|
||||
}
|
||||
|
||||
|
||||
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");
|
||||
distanceRight = hardwareMap.get(DistanceSensor.class, "color right");
|
||||
distanceLeft = hardwareMap.get(DistanceSensor.class, "color left");
|
||||
gripper = hardwareMap.get(Servo.class, "gripper");
|
||||
arm = hardwareMap.get(DcMotor.class, "arm raise");
|
||||
wrist = hardwareMap.get(Servo.class, "wrist");
|
||||
sleep(1000);
|
||||
// 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.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backleftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backrightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
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() {
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
driveForward(26);
|
||||
sleep(500);
|
||||
int distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
int distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
|
||||
if (distanceleft < 7) {
|
||||
telemetry.addData("position", "left");
|
||||
telemetry.update();
|
||||
straightLeft(13.5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-15.5);
|
||||
turnRight(90);
|
||||
straightRight(15);
|
||||
driveForward(-18);
|
||||
DRIVE_SPEED = .25;
|
||||
straightLeft(22.25);
|
||||
driveForward(-.30);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(1);
|
||||
sleep(500);
|
||||
driveForward(6);
|
||||
raisearm(-50);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
straightLeft(35);
|
||||
driveForward(-22);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
}
|
||||
if (distanceright < 7) {
|
||||
telemetry.addData("position", "right");
|
||||
telemetry.update();
|
||||
turnRight(88);
|
||||
driveForward(5.25);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-38);
|
||||
straightLeft(7.5);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(1);
|
||||
driveForward(7.5);
|
||||
raisearm(-50);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
straightLeft(11);
|
||||
driveForward(-10);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
} else {
|
||||
telemetry.addData("position", "center");
|
||||
telemetry.update();
|
||||
driveForward(5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-8);
|
||||
straightLeft(11.5);
|
||||
driveForward(-15);
|
||||
turnRight(90);
|
||||
straightRight(15);
|
||||
driveForward(-18);
|
||||
DRIVE_SPEED = .25;
|
||||
straightLeft(29);
|
||||
driveForward(-2);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(1);
|
||||
driveForward(6);
|
||||
raisearm(-50);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
straightLeft(28);
|
||||
driveForward(-25);
|
||||
terminateOpModeNow();
|
||||
}
|
||||
|
||||
|
||||
//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 timeout, boolean addJuice) {
|
||||
//
|
||||
// if(leftInches < 4) {
|
||||
// encodedDriver(speed, leftInches, rightInches, timeout, addJuice);
|
||||
//
|
||||
// } else {
|
||||
// // first n-4 is 50% speed (regional tournament speed)
|
||||
// encodedDriver(DRIVE_SPEED, leftInches-10, rightInches-4, timeout, addJuice);
|
||||
// // trailing result is 30% speed (base speed)
|
||||
// encodedDriver(.3, 10, 10, timeout, addJuice);
|
||||
//
|
||||
// }
|
||||
//}
|
||||
|
||||
public void encoderDrive(double speed,
|
||||
double leftInches, double rightInches,
|
||||
double timeoutS, boolean addJuice) {
|
||||
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();
|
||||
if(addJuice) {
|
||||
leftDrive.setPower(Math.abs(speed * 1.05));
|
||||
rightDrive.setPower(Math.abs(speed * 1.05));
|
||||
} else {
|
||||
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,478 @@
|
||||
/* 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 android.annotation.SuppressLint;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
/**
|
||||
* 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="Blue (Backstage) Clone", group="Robot")
|
||||
//@Disabled
|
||||
public class BlueBackStageClone extends LinearOpMode {
|
||||
|
||||
/* Declare OpMode members. */
|
||||
private DcMotor leftDrive = null;
|
||||
private DcMotor rightDrive = null;
|
||||
private DcMotor backrightDrive = null;
|
||||
private DcMotor backleftDrive = null;
|
||||
private DistanceSensor distanceRight = null;
|
||||
private DistanceSensor distanceLeft = null;
|
||||
private Servo wrist = null;
|
||||
private Servo gripper = null;
|
||||
private DcMotor arm = null;
|
||||
private DistanceSensor distance = 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.3;
|
||||
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();
|
||||
|
||||
}
|
||||
|
||||
// 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 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");
|
||||
distanceRight = hardwareMap.get(DistanceSensor.class, "color right");
|
||||
distanceLeft = hardwareMap.get(DistanceSensor.class, "color left");
|
||||
gripper = hardwareMap.get(Servo.class, "gripper");
|
||||
arm = hardwareMap.get(DcMotor.class, "arm raise");
|
||||
wrist = hardwareMap.get(Servo.class, "wrist");
|
||||
distance = hardwareMap.get(DistanceSensor.class, "distance");
|
||||
sleep(1000);
|
||||
// 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.5);
|
||||
|
||||
}
|
||||
@SuppressLint("SuspiciousIndentation")
|
||||
public void executeAuto()
|
||||
{
|
||||
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
driveForward(26);
|
||||
sleep(500);
|
||||
|
||||
int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
|
||||
telemetry.addData("color left sensor",distanceleft);
|
||||
telemetry.addData("color right sensor",distanceright);
|
||||
telemetry.update();
|
||||
if (distanceleft < 7)
|
||||
{
|
||||
telemetry.addData("postion","left");
|
||||
telemetry.update();
|
||||
turnLeft(90);
|
||||
straightLeft(2);
|
||||
driveForward(5.5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-21);
|
||||
straightLeft(34);
|
||||
driveForward(-10);
|
||||
straightRight(35);
|
||||
driveForward(-5);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(1);
|
||||
sleep(500);
|
||||
driveForward(4.5);
|
||||
|
||||
raisearm(-50);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
|
||||
/* need to review */
|
||||
straightLeft(11);
|
||||
|
||||
driveForward(1.5);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
}
|
||||
if (distanceright < 7)
|
||||
{
|
||||
telemetry.addData("postion", "right");
|
||||
telemetry.update();
|
||||
straightRight(12);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-15.5);
|
||||
turnLeft(90);
|
||||
straightLeft(15);
|
||||
driveForward(-20.5);
|
||||
straightRight(19);
|
||||
driveForward(-1.5);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(1);
|
||||
driveForward(8.5);
|
||||
|
||||
raisearm(-50);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
|
||||
/* need to review */
|
||||
straightLeft(29);
|
||||
|
||||
driveForward(-10);
|
||||
|
||||
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
}
|
||||
else
|
||||
telemetry.addData("postion","center");
|
||||
telemetry.update();
|
||||
driveForward(3.5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-8);
|
||||
straightRight(11.5);
|
||||
driveForward(-15);
|
||||
turnLeft(90);
|
||||
straightLeft(15);
|
||||
driveForward(-18);
|
||||
straightRight(29);
|
||||
turnRight(10);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
|
||||
gripper.setPosition(1);
|
||||
driveForward(5);
|
||||
|
||||
/* added from bluebackstage */
|
||||
raisearm(-50);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
straightLeft(19);
|
||||
driveForward(-10);
|
||||
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
|
||||
//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) (leftInches * 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);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
@ -92,34 +92,34 @@ public class ChassisControl extends OpMode {
|
||||
* User defined loop method.
|
||||
* This method will be called repeatedly in a loop while this op mode is running
|
||||
*/
|
||||
double num = 1;
|
||||
String speed = "";
|
||||
double num = 3;
|
||||
String speed = "slow";
|
||||
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 = 3;
|
||||
speed = "slow";
|
||||
}
|
||||
if(gamepad1.b){
|
||||
num = 2.5;
|
||||
speed = "medium";
|
||||
}
|
||||
if(gamepad1.y){
|
||||
num = 2;
|
||||
speed = "fast";
|
||||
}
|
||||
if(gamepad1.x){
|
||||
num = 1.5;
|
||||
speed = "Ludicrous";
|
||||
}
|
||||
if(gamepad1.x && gamepad1.y){
|
||||
num = 1;
|
||||
speed = "plaid";
|
||||
}
|
||||
// if(gamepad1.a){
|
||||
// num = 3;
|
||||
// speed = "slow";
|
||||
// }
|
||||
//// if(gamepad1.b){
|
||||
//// num = 2.5;
|
||||
//// speed = "medium";
|
||||
//// }
|
||||
//// if(gamepad1.y){
|
||||
//// num = 2;
|
||||
//// speed = "fast";
|
||||
//// }
|
||||
//// if(gamepad1.x){
|
||||
//// num = 1.5;
|
||||
//// speed = "Ludicrous";
|
||||
//// }
|
||||
//// if(gamepad1.x && gamepad1.y){
|
||||
//// num = 1;
|
||||
//// 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);
|
||||
|
@ -0,0 +1,283 @@
|
||||
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;
|
||||
DcMotor hang;
|
||||
DcMotor armThroughBoreEncoder;
|
||||
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");
|
||||
armThroughBoreEncoder = hardwareMap.dcMotor.get("arm raise");
|
||||
hang = hardwareMap.dcMotor.get("hang");
|
||||
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 = "medium";
|
||||
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(gamepad2.dpad_down){
|
||||
wrist.setPosition(1);
|
||||
|
||||
}
|
||||
if(gamepad2.dpad_up){
|
||||
|
||||
wrist.setPosition(0.5);
|
||||
}
|
||||
if(gamepad2.left_trigger > 0.35)
|
||||
{
|
||||
gripper.setPosition(0.25);
|
||||
}
|
||||
if(gamepad2.right_trigger > 0.35){
|
||||
gripper.setPosition(1);
|
||||
}
|
||||
|
||||
// int armState = 0;
|
||||
// int armPos = 0;
|
||||
// if(gamepad2.x)
|
||||
// {
|
||||
// armState = 1;
|
||||
// raisearm(0 - armPos);
|
||||
// armPos = 0;
|
||||
// }
|
||||
// if(gamepad2.y)
|
||||
// {
|
||||
// armState = 2;
|
||||
// raisearm(1000);
|
||||
// armPos = 30;
|
||||
// }
|
||||
// if(gamepad2.b)
|
||||
// {
|
||||
// armState = 2;
|
||||
// raisearm(200 - armPos);
|
||||
// armPos = 200;
|
||||
// }
|
||||
// if(gamepad2.right_bumper)
|
||||
// {
|
||||
// armState =+ 1;
|
||||
// }
|
||||
// if(gamepad2.left_bumper)
|
||||
// {
|
||||
// armState =- 1;
|
||||
// }
|
||||
// if(armState == 1)
|
||||
// {
|
||||
// raisearm(0 - armPos);
|
||||
// armPos = 0;
|
||||
// }
|
||||
// if(armState == 2)
|
||||
// {
|
||||
// raisearm(30 - armPos);
|
||||
// armPos = 30;
|
||||
// }
|
||||
// if(armState == 3)
|
||||
// {
|
||||
// raisearm(200 - armPos);
|
||||
// armPos = 200;
|
||||
//
|
||||
// }
|
||||
if(gamepad2.right_stick_y > 0)
|
||||
{
|
||||
arm.setPower(armPower);
|
||||
}
|
||||
if(gamepad2.left_stick_y < 0)
|
||||
{
|
||||
arm.setPower(armPower);
|
||||
}
|
||||
telemetry.addData("arm is at", " %7d", armThroughBoreEncoder.getCurrentPosition());
|
||||
telemetry.update();
|
||||
|
||||
|
||||
|
||||
|
||||
frontLeft.setPower(leftFrontPower);
|
||||
frontRight.setPower(rightFrontPower);
|
||||
backLeft.setPower(leftBackPower);
|
||||
backRight.setPower(rightBackPower);
|
||||
//arm.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)
|
||||
{
|
||||
arm.setDirection(DcMotor.Direction.REVERSE);
|
||||
armDriveWithEncoder(.2, degrees*TICKS_TO_DEGREES);
|
||||
|
||||
}
|
||||
public void armDriveWithEncoder(double speed,
|
||||
double Inches) {
|
||||
int newarmTarget;
|
||||
|
||||
|
||||
// Determine new target position, and pass to motor controller
|
||||
newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
|
||||
armThroughBoreEncoder.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
|
||||
armThroughBoreEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
}
|
||||
}
|
||||
|
@ -1,51 +0,0 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import java.util.concurrent.TimeUnit;
|
||||
|
||||
@Autonomous(name="Motor_test")
|
||||
public class Motor_Test extends OpMode {
|
||||
DcMotor hwMotorDriveFrontLeft;
|
||||
DcMotor hwMotorDriveFrontRight;
|
||||
|
||||
DcMotor hwMotorDriveBackLeft;
|
||||
DcMotor hwMotorDriveBackRight;
|
||||
|
||||
public void init() {
|
||||
hwMotorDriveFrontLeft = hardwareMap.dcMotor.get("Drive front lt");
|
||||
hwMotorDriveFrontRight = hardwareMap.dcMotor.get("Drive front rt");
|
||||
hwMotorDriveBackLeft = hardwareMap.dcMotor.get("Drive back lt");
|
||||
hwMotorDriveBackRight = hardwareMap.dcMotor.get("Drive back rt");
|
||||
}
|
||||
|
||||
public void sleepSec(int iSecs){
|
||||
try {
|
||||
Thread.sleep(iSecs*1000);
|
||||
} catch (InterruptedException e) {
|
||||
Thread.currentThread().interrupt();
|
||||
}
|
||||
}
|
||||
public void loop() {
|
||||
|
||||
hwMotorDriveFrontLeft.setPower(1);
|
||||
sleepSec(1);
|
||||
hwMotorDriveFrontRight.setPower(1);
|
||||
sleepSec(1);
|
||||
hwMotorDriveBackLeft.setPower(1);
|
||||
sleepSec(1);
|
||||
hwMotorDriveBackRight.setPower(1);
|
||||
sleepSec(1);
|
||||
hwMotorDriveFrontLeft.setPower(0);
|
||||
hwMotorDriveFrontRight.setPower(0);
|
||||
hwMotorDriveBackLeft.setPower(0);
|
||||
hwMotorDriveBackRight.setPower(0);
|
||||
sleepSec(10);
|
||||
|
||||
}
|
||||
|
||||
}
|
@ -0,0 +1,81 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import static java.lang.Math.round;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.AnalogSensor;
|
||||
import com.qualcomm.robotcore.hardware.ColorSensor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.DistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.TouchSensor;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
@Autonomous(name="Name")
|
||||
public class Name extends LinearOpMode {
|
||||
private DcMotor rightHandWheel;
|
||||
private DcMotor rightLegWheel;
|
||||
private DcMotor leftHandWheel;
|
||||
private DcMotor leftLegWheel;
|
||||
private TouchSensor iFeelYou;
|
||||
private DistanceSensor whereAreYou;
|
||||
private ;
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
rightHandWheel = hardwareMap.get(DcMotor.class,"right hand wheel");
|
||||
rightLegWheel = hardwareMap.get(DcMotor.class,"right leg wheel");
|
||||
leftHandWheel = hardwareMap.get(DcMotor.class, "left hand wheel");
|
||||
leftLegWheel = hardwareMap.get(DcMotor.class, "left leg wheel");
|
||||
iFeelYou = hardwareMap.get(TouchSensor.class, "i feel you");
|
||||
whereAreYou = hardwareMap.get(DistanceSensor.class , "where are you");
|
||||
|
||||
rightHandWheel.setDirection(DcMotor.Direction.REVERSE);
|
||||
rightLegWheel.setDirection(DcMotor.Direction.FORWARD);
|
||||
leftHandWheel.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
leftLegWheel.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
|
||||
// Wait for the game to start (driver presses PLAY)
|
||||
waitForStart();
|
||||
|
||||
while(opModeIsActive()){
|
||||
|
||||
/* telemetry.speak( "Oh see, you see" +
|
||||
"By the dusk's late light" +
|
||||
"What so proudly we rained" +
|
||||
"At the twilight's last gleaming?" +
|
||||
"Whose broad stripes and dark stars" +
|
||||
"Through the perilous fight" +
|
||||
"Under the ramparts we watched" +
|
||||
"Were so gallantly, no, streaming?" +
|
||||
"And the rockets' red glare" +
|
||||
"The bombs contracting in air" +
|
||||
"Gave proof through the night" +
|
||||
"That our flag was not there" +
|
||||
"O say, that star-spangled banner doesn't wave" +
|
||||
"Over the land of the enslaved and the home of the cowardly");*/
|
||||
|
||||
rightHandWheel.setPower(0.2);
|
||||
rightLegWheel.setPower(0.2);
|
||||
leftLegWheel.setPower(0.2);
|
||||
leftHandWheel.setPower(0.2);
|
||||
|
||||
if(iFeelYou.isPressed()) {
|
||||
telemetry.speak("Ouchie that hurt me and my feelings");
|
||||
telemetry.addData("was i triggered", iFeelYou.isPressed());
|
||||
}
|
||||
|
||||
double nicerValue = whereAreYou.getDistance(DistanceUnit.INCH);
|
||||
|
||||
telemetry.addData("you are this far (in inches) --> ", "%.2f", nicerValue);
|
||||
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
510
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java
Normal file
510
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java
Normal file
@ -0,0 +1,510 @@
|
||||
/* 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.DistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
/**
|
||||
* 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="Red", group="Robot")
|
||||
//@Disabled
|
||||
public class Red extends LinearOpMode {
|
||||
|
||||
/* Declare OpMode members. */
|
||||
private DcMotor leftDrive = null;
|
||||
private DcMotor rightDrive = null;
|
||||
private DcMotor backrightDrive = null;
|
||||
private DcMotor backleftDrive = null;
|
||||
private DistanceSensor distanceRight = null;
|
||||
private DistanceSensor distanceLeft = 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.3;
|
||||
|
||||
static final double DRIVE_SPEED_SLOW = 0.25;
|
||||
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();
|
||||
|
||||
}
|
||||
|
||||
// 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 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");
|
||||
distanceRight = hardwareMap.get(DistanceSensor.class, "color right");
|
||||
distanceLeft = hardwareMap.get(DistanceSensor.class, "color left");
|
||||
gripper = hardwareMap.get(Servo.class, "gripper");
|
||||
arm = hardwareMap.get(DcMotor.class, "arm raise");
|
||||
wrist = hardwareMap.get(Servo.class, "wrist");
|
||||
sleep(1000);
|
||||
// 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 straightRightOnPower(double speed) {
|
||||
speed *= -1;
|
||||
leftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backrightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backleftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
leftDrive.setPower(speed * 1.05);
|
||||
rightDrive.setPower(speed * 1.05);
|
||||
backrightDrive.setPower(speed);
|
||||
backleftDrive.setPower(speed);
|
||||
}
|
||||
|
||||
public void straighten(Double distance)
|
||||
|
||||
{
|
||||
driveForward(0);
|
||||
double D1 = distanceRight.getDistance(DistanceUnit.INCH);
|
||||
driveForward(distance);
|
||||
double D2 = distanceRight.getDistance(DistanceUnit.INCH);
|
||||
double rad = Math.atan2(D1 - D2, distance);
|
||||
double degrees = Math.toDegrees(rad);
|
||||
turnRight(-degrees);
|
||||
telemetry.addData("d1", D1);
|
||||
telemetry.addData("d2", D2);
|
||||
telemetry.addData("Calibration deg", degrees);
|
||||
telemetry.update();
|
||||
sleep(3000);
|
||||
}
|
||||
|
||||
public void centerRight()
|
||||
{
|
||||
double rightDistance = distanceRight.getDistance(DistanceUnit.INCH);
|
||||
straightRight(rightDistance - 3);
|
||||
telemetry.addData("rightDistance",rightDistance);
|
||||
telemetry.addData("moving left x inches",rightDistance - 3);
|
||||
telemetry.update();
|
||||
straightRight(0.0);
|
||||
sleep(3000);
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
// while (true)
|
||||
// {
|
||||
// int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
|
||||
// telemetry.addData("right", distanceright);
|
||||
// telemetry.update();
|
||||
// sleep(500);
|
||||
// }
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
driveForward(26);
|
||||
sleep(500);
|
||||
int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
|
||||
if (distanceleft < 7)
|
||||
{
|
||||
telemetry.addData("position", "left");
|
||||
telemetry.update();
|
||||
straightLeft(12);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
sleep(500);
|
||||
turnRight(90);
|
||||
driveForward(-10);
|
||||
do {
|
||||
straightRightOnPower(DRIVE_SPEED_SLOW);
|
||||
distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
|
||||
|
||||
} while (distanceright >= 4);
|
||||
straighten(12.0);
|
||||
centerRight();
|
||||
driveForward(88);
|
||||
sleep(1000);
|
||||
wrist.setPosition(.465);
|
||||
gripper.setPosition(1);
|
||||
sleep(1000);
|
||||
driveForward(-3);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
}
|
||||
if (distanceright < 7) //right
|
||||
{
|
||||
telemetry.addData("position","right");
|
||||
telemetry.update();
|
||||
turnRight(90);
|
||||
straightLeft(2);
|
||||
driveForward(6.5);
|
||||
raisearm(45);
|
||||
arm.setPower(0);
|
||||
sleep(500);
|
||||
driveForward(-6);
|
||||
do {
|
||||
straightRightOnPower(DRIVE_SPEED_SLOW);
|
||||
distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
|
||||
|
||||
} while (distanceright >= 6);
|
||||
straighten(12.0);
|
||||
centerRight();
|
||||
driveForward(88);
|
||||
sleep(1000);
|
||||
wrist.setPosition(.465);
|
||||
gripper.setPosition(1);
|
||||
sleep(1000);
|
||||
driveForward(-3);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
}
|
||||
else
|
||||
telemetry.addData("position","center");
|
||||
telemetry.update();
|
||||
driveForward(3.5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-8);
|
||||
turnRight(90);
|
||||
driveForward(-12);
|
||||
do {
|
||||
straightRightOnPower(DRIVE_SPEED_SLOW);
|
||||
distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
|
||||
|
||||
} while (distanceright >= 6);
|
||||
straighten(12.0);
|
||||
centerRight();
|
||||
//straightLeft(3);
|
||||
driveForward(88);
|
||||
sleep(1000);
|
||||
wrist.setPosition(.465);
|
||||
gripper.setPosition(1);
|
||||
sleep(1000);
|
||||
driveForward(-3);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//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,477 @@
|
||||
/* 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 android.annotation.SuppressLint;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
/**
|
||||
* 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="red (backstage)", group="Robot")
|
||||
//@Disabled
|
||||
public class RedBackStage extends LinearOpMode {
|
||||
|
||||
/* Declare OpMode members. */
|
||||
private DcMotor leftDrive = null;
|
||||
private DcMotor rightDrive = null;
|
||||
private DcMotor backrightDrive = null;
|
||||
private DcMotor backleftDrive = null;
|
||||
private DistanceSensor distanceRight = null;
|
||||
private DistanceSensor distanceLeft = null;
|
||||
private Servo wrist = null;
|
||||
private Servo gripper = null;
|
||||
private DcMotor arm = null;
|
||||
private DistanceSensor distance = 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);
|
||||
double DRIVE_SPEED = 0.5;
|
||||
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();
|
||||
|
||||
}
|
||||
|
||||
// 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 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");
|
||||
distanceRight = hardwareMap.get(DistanceSensor.class, "color right");
|
||||
distanceLeft = hardwareMap.get(DistanceSensor.class, "color left");
|
||||
gripper = hardwareMap.get(Servo.class, "gripper");
|
||||
arm = hardwareMap.get(DcMotor.class, "arm raise");
|
||||
wrist = hardwareMap.get(Servo.class, "wrist");
|
||||
distance = hardwareMap.get(DistanceSensor.class, "distance");
|
||||
sleep(1000);
|
||||
// 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.5);
|
||||
|
||||
}
|
||||
@SuppressLint("SuspiciousIndentation")
|
||||
public void executeAuto()
|
||||
{
|
||||
backrightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
backleftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
driveForward(26);
|
||||
sleep(500);
|
||||
|
||||
int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
|
||||
telemetry.addData("color left sensor",distanceleft);
|
||||
telemetry.addData("color right sensor",distanceright);
|
||||
telemetry.update();
|
||||
|
||||
if (distanceleft < 7)
|
||||
{
|
||||
telemetry.addData("postion","left");
|
||||
telemetry.update();
|
||||
turnLeft(90);
|
||||
straightLeft(2);
|
||||
driveForward(5.5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-21);
|
||||
straightLeft(34);
|
||||
driveForward(-10);
|
||||
DRIVE_SPEED = .3;
|
||||
straightRight(35);
|
||||
driveForward(-5);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(1);
|
||||
sleep(500);
|
||||
driveForward(4.5);
|
||||
sleep(500);
|
||||
driveForward(1.5);
|
||||
raisearm(-50);
|
||||
sleep(500);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
driveForward(-6);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
if (distanceright < 7)
|
||||
{
|
||||
telemetry.addData("postion", "right");
|
||||
telemetry.update();
|
||||
straightRight(12);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-15.5);
|
||||
turnLeft(90);
|
||||
straightLeft(15);
|
||||
driveForward(-20.5);
|
||||
DRIVE_SPEED = .3;
|
||||
straightRight(19);
|
||||
driveForward(-1.5);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(1);
|
||||
sleep(500);
|
||||
driveForward(8.5);
|
||||
raisearm(-50);
|
||||
sleep(500);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
driveForward(-6);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
}
|
||||
else
|
||||
telemetry.addData("postion","center");
|
||||
telemetry.update();
|
||||
driveForward(3.5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-8);
|
||||
straightRight(11.5);
|
||||
driveForward(-15);
|
||||
turnLeft(90);
|
||||
straightLeft(15);
|
||||
driveForward(-18);
|
||||
DRIVE_SPEED = .3;
|
||||
straightRight(29);
|
||||
turnRight(5);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(1);
|
||||
sleep(500);
|
||||
driveForward(5);
|
||||
raisearm(-50);
|
||||
sleep(500);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
driveForward(-6);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
|
||||
//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) (leftInches * 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,479 @@
|
||||
/* 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 android.annotation.SuppressLint;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
/**
|
||||
* 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="red (backstage test subject 716,980)", group="Robot")
|
||||
//@Disabled
|
||||
public class RedBackStageTest extends LinearOpMode {
|
||||
|
||||
/* Declare OpMode members. */
|
||||
private DcMotor leftDrive = null;
|
||||
private DcMotor rightDrive = null;
|
||||
private DcMotor backrightDrive = null;
|
||||
private DcMotor backleftDrive = null;
|
||||
private DistanceSensor distanceRight = null;
|
||||
private DistanceSensor distanceLeft = null;
|
||||
private Servo wrist = null;
|
||||
private Servo gripper = null;
|
||||
private DcMotor arm = null;
|
||||
private DistanceSensor distance = 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.35;
|
||||
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();
|
||||
|
||||
}
|
||||
|
||||
// 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 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");
|
||||
distanceRight = hardwareMap.get(DistanceSensor.class, "color right");
|
||||
distanceLeft = hardwareMap.get(DistanceSensor.class, "color left");
|
||||
gripper = hardwareMap.get(Servo.class, "gripper");
|
||||
arm = hardwareMap.get(DcMotor.class, "arm raise");
|
||||
wrist = hardwareMap.get(Servo.class, "wrist");
|
||||
distance = hardwareMap.get(DistanceSensor.class, "distance");
|
||||
sleep(1000);
|
||||
// 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.5);
|
||||
|
||||
}
|
||||
@SuppressLint("SuspiciousIndentation")
|
||||
public void executeAuto()
|
||||
{
|
||||
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
driveForward(26);
|
||||
sleep(500);
|
||||
|
||||
int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
|
||||
telemetry.addData("color left sensor",distanceleft);
|
||||
telemetry.addData("color right sensor",distanceright);
|
||||
telemetry.update();
|
||||
if (distanceleft < 7)
|
||||
{
|
||||
telemetry.addData("position","left");
|
||||
telemetry.update();
|
||||
turnLeft(90);
|
||||
straightLeft(2);
|
||||
driveForward(5.5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-21);
|
||||
straightLeft(34);
|
||||
driveForward(-10);
|
||||
straightRight(35);
|
||||
driveForward(-5);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(1);
|
||||
sleep(500);
|
||||
driveForward(4.5);
|
||||
raisearm(-50);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
|
||||
|
||||
driveForward(1.5);
|
||||
/* need to review */
|
||||
straightLeft(30);
|
||||
driveForward(-15);
|
||||
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
}
|
||||
if (distanceright < 7)
|
||||
{
|
||||
telemetry.addData("postion", "right");
|
||||
telemetry.update();
|
||||
straightRight(12);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-15.5);
|
||||
turnLeft(90);
|
||||
straightLeft(15);
|
||||
driveForward(-20.5);
|
||||
straightRight(19);
|
||||
driveForward(-1.5);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(1);
|
||||
driveForward(8.5);
|
||||
|
||||
raisearm(-50);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
|
||||
/* need to review */
|
||||
straightLeft(29);
|
||||
|
||||
driveForward(-10);
|
||||
|
||||
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
}
|
||||
else
|
||||
telemetry.addData("postion","center");
|
||||
telemetry.update();
|
||||
driveForward(3.5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-8);
|
||||
straightRight(11.5);
|
||||
driveForward(-15);
|
||||
turnLeft(90);
|
||||
straightLeft(15);
|
||||
driveForward(-18);
|
||||
straightRight(29);
|
||||
turnRight(10);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
|
||||
gripper.setPosition(1);
|
||||
driveForward(5);
|
||||
|
||||
/* added from bluebackstage */
|
||||
raisearm(-50);
|
||||
wrist.setPosition(1);
|
||||
raisearm(-70);
|
||||
straightLeft(29);
|
||||
driveForward(-10);
|
||||
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
|
||||
//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) (leftInches * 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,460 @@
|
||||
/* 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 android.annotation.SuppressLint;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
/**
|
||||
* 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="red (direct)", group="Robot")
|
||||
//@Disabled
|
||||
public class RedDirect extends LinearOpMode {
|
||||
|
||||
/* Declare OpMode members. */
|
||||
private DcMotor leftDrive = null;
|
||||
private DcMotor rightDrive = null;
|
||||
private DcMotor backrightDrive = null;
|
||||
private DcMotor backleftDrive = null;
|
||||
private DistanceSensor distanceRight = null;
|
||||
private DistanceSensor distanceLeft = null;
|
||||
private Servo wrist = null;
|
||||
private Servo gripper = null;
|
||||
private DcMotor arm = null;
|
||||
private DistanceSensor distance = 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.3;
|
||||
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();
|
||||
|
||||
}
|
||||
|
||||
// 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 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");
|
||||
distanceRight = hardwareMap.get(DistanceSensor.class, "color right");
|
||||
distanceLeft = hardwareMap.get(DistanceSensor.class, "color left");
|
||||
gripper = hardwareMap.get(Servo.class, "gripper");
|
||||
arm = hardwareMap.get(DcMotor.class, "arm raise");
|
||||
wrist = hardwareMap.get(Servo.class, "wrist");
|
||||
distance = hardwareMap.get(DistanceSensor.class, "distance");
|
||||
wrist.setPosition(1);
|
||||
sleep(1000);
|
||||
// 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.5);
|
||||
|
||||
}
|
||||
@SuppressLint("SuspiciousIndentation")
|
||||
public void executeAuto()
|
||||
{
|
||||
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
driveForward(26);
|
||||
sleep(500);
|
||||
|
||||
int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
|
||||
int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
|
||||
telemetry.addData("color left sensor",distanceleft);
|
||||
telemetry.addData("color right sensor",distanceright);
|
||||
telemetry.update();
|
||||
|
||||
if (distanceleft < 7)
|
||||
{
|
||||
telemetry.addData("postion","left");
|
||||
telemetry.update();
|
||||
turnLeft(90);
|
||||
straightLeft(2);
|
||||
driveForward(6.5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-21);
|
||||
straightLeft(32);
|
||||
driveForward(-10);
|
||||
straightRight(33);
|
||||
driveForward(-1.5);
|
||||
telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH));
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(0.25);
|
||||
sleep(500);
|
||||
driveForward(5);
|
||||
telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH));
|
||||
telemetry.update();
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
if (distanceright < 7)
|
||||
{
|
||||
telemetry.addData("postion", "right");
|
||||
telemetry.update();
|
||||
straightRight(12);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-10);
|
||||
turnLeft(90);
|
||||
driveForward(12);
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(0.25);
|
||||
sleep(500);
|
||||
driveForward(5);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
}
|
||||
else
|
||||
telemetry.addData("postion","center");
|
||||
telemetry.update();
|
||||
driveForward(3.5);
|
||||
raisearm(80);
|
||||
arm.setPower(0);
|
||||
driveForward(-8);
|
||||
straightRight(11.5);
|
||||
driveForward(-15);
|
||||
turnLeft(90);
|
||||
straightLeft(15);
|
||||
driveForward(-18);
|
||||
straightRight(29);
|
||||
driveForward(-1.5);
|
||||
telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH));
|
||||
raisearm(80);
|
||||
wrist.setPosition(0);
|
||||
raisearm(100);
|
||||
gripper.setPosition(0.25);
|
||||
telemetry.addData("distance back", distance.getDistance(DistanceUnit.INCH));
|
||||
telemetry.update();
|
||||
sleep(500);
|
||||
driveForward(5);
|
||||
terminateOpModeNow();
|
||||
|
||||
|
||||
|
||||
//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) (leftInches * 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,99 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
@TeleOp( name = "scds-chassis-manual")
|
||||
public class SCDSChassisDriverMode extends OpMode {
|
||||
|
||||
DcMotor frontRight;
|
||||
DcMotor backRight;
|
||||
DcMotor frontLeft;
|
||||
DcMotor backLeft;
|
||||
public double axial;
|
||||
public double lateral;
|
||||
public double yaw;
|
||||
|
||||
final static double MOTOR_LO_SPEED_RATIO = 3.5;
|
||||
final static double MOTOR_HI_SPEED_RATIO = 2.25;
|
||||
final static double ARM_POWER = 3.5;
|
||||
double RUNNING_MOTOR_SPEED_RATIO = MOTOR_LO_SPEED_RATIO;
|
||||
double CURRENT_SPEED_RATIO = MOTOR_HI_SPEED_RATIO;
|
||||
|
||||
@Override
|
||||
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");
|
||||
}
|
||||
|
||||
private void setForwardDirection() {
|
||||
telemetry.addData("Status","setForwardDirection()");
|
||||
telemetry.update();
|
||||
frontLeft.setDirection(DcMotor.Direction.REVERSE);
|
||||
backLeft.setDirection(DcMotor.Direction.FORWARD);
|
||||
frontRight.setDirection(DcMotor.Direction.REVERSE);
|
||||
backRight.setDirection(DcMotor.Direction.FORWARD);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void loop() {
|
||||
/*
|
||||
Initialize the wheels
|
||||
*/
|
||||
setForwardDirection();
|
||||
|
||||
/*
|
||||
Turn on high speed on the motors
|
||||
*/
|
||||
if(gamepad1.a) {
|
||||
RUNNING_MOTOR_SPEED_RATIO = MOTOR_HI_SPEED_RATIO;
|
||||
}
|
||||
|
||||
/*
|
||||
Turn on low speed on the motors
|
||||
*/
|
||||
if(gamepad1.b) {
|
||||
RUNNING_MOTOR_SPEED_RATIO = MOTOR_LO_SPEED_RATIO;
|
||||
}
|
||||
|
||||
axial = -gamepad1.left_stick_y/CURRENT_SPEED_RATIO; // Note: pushing stick forward gives negative value
|
||||
lateral = gamepad1.left_stick_x/CURRENT_SPEED_RATIO;
|
||||
yaw = gamepad1.right_stick_x/CURRENT_SPEED_RATIO;
|
||||
|
||||
// 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.
|
||||
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;
|
||||
}
|
||||
frontLeft.setPower(leftFrontPower);
|
||||
frontRight.setPower(rightFrontPower);
|
||||
backLeft.setPower(leftBackPower);
|
||||
backRight.setPower(rightBackPower);
|
||||
|
||||
// Show the elapsed game time and wheel power
|
||||
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,4 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
public class TestCode {
|
||||
}
|
@ -0,0 +1,215 @@
|
||||
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.Servo;
|
||||
|
||||
|
||||
@TeleOp( name = "manual control")
|
||||
public class manual extends OpMode {
|
||||
|
||||
DcMotor arm;
|
||||
Servo gripper;
|
||||
Servo wrist;
|
||||
public double axial;
|
||||
public double lateral;
|
||||
public double yaw;
|
||||
DcMotor frontRight;
|
||||
DcMotor backRight;
|
||||
DcMotor frontLeft;
|
||||
DcMotor backLeft;
|
||||
DcMotor hang;
|
||||
private Servo launch;
|
||||
|
||||
|
||||
/**
|
||||
* 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() {
|
||||
arm.setPower(0);
|
||||
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();
|
||||
arm = hardwareMap.dcMotor.get("arm raise");
|
||||
gripper = hardwareMap.servo.get("gripper");
|
||||
wrist = hardwareMap.servo.get("wrist");
|
||||
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");
|
||||
hang = hardwareMap.dcMotor.get("hang");
|
||||
launch = hardwareMap.servo.get("launch");
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(){
|
||||
|
||||
}
|
||||
|
||||
//double num = 2.25;
|
||||
final static double MOTOR_HI_SPEED_RATIO = 1.75;
|
||||
final static double MOTOR_MID_SPEED_RATIO = 2;
|
||||
final static double MOTOR_LO_SPEED_RATIO = 3.5;
|
||||
final static double ARM_POWER = 3;
|
||||
double num = MOTOR_MID_SPEED_RATIO;
|
||||
/**
|
||||
* User defined loop method.
|
||||
* This method will be called repeatedly in a loop while this op mode is running
|
||||
*/
|
||||
public void loop() {
|
||||
frontLeft.setDirection(DcMotor.Direction.REVERSE);
|
||||
backLeft.setDirection(DcMotor.Direction.REVERSE);
|
||||
frontRight.setDirection(DcMotor.Direction.FORWARD);
|
||||
backRight.setDirection(DcMotor.Direction.REVERSE);
|
||||
|
||||
|
||||
double armPower = gamepad2.right_stick_y/ARM_POWER;
|
||||
// Normalize the values so no wheel power exceeds 100%
|
||||
// This ensures that the robot maintains the desired motion.
|
||||
if(gamepad1.a)
|
||||
{
|
||||
num = MOTOR_HI_SPEED_RATIO;
|
||||
}
|
||||
if (gamepad1.x)
|
||||
{
|
||||
num = MOTOR_LO_SPEED_RATIO;
|
||||
}
|
||||
if (gamepad1.b)
|
||||
{
|
||||
num = MOTOR_MID_SPEED_RATIO;
|
||||
}
|
||||
if(gamepad2.right_stick_y != 0)
|
||||
{
|
||||
arm.setPower(armPower);
|
||||
telemetry.addData("joystick y value", gamepad2.right_stick_y);
|
||||
telemetry.update();
|
||||
}
|
||||
else
|
||||
{
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
arm.setPower(0);
|
||||
}
|
||||
if(gamepad2.left_bumper && gamepad2.right_bumper)
|
||||
{
|
||||
launch.setPosition(0);
|
||||
}
|
||||
if(gamepad2.left_trigger > 0.35)
|
||||
{
|
||||
gripper.setPosition(1);
|
||||
}
|
||||
if(gamepad2.right_trigger > 0.35){
|
||||
gripper.setPosition(0);
|
||||
}
|
||||
if(gamepad2.dpad_up)
|
||||
{
|
||||
wrist.setPosition(0.465);
|
||||
}
|
||||
if(gamepad2.dpad_down)
|
||||
{
|
||||
wrist.setPosition(1);
|
||||
}
|
||||
if(gamepad2.dpad_right)
|
||||
{
|
||||
wrist.setPosition(0);
|
||||
}
|
||||
if (gamepad1.dpad_up)
|
||||
{
|
||||
hang.setPower(1);
|
||||
}
|
||||
hang.setPower(0);
|
||||
if (gamepad1.dpad_down)
|
||||
{
|
||||
hang.setPower(-.5);
|
||||
}
|
||||
else {
|
||||
hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
hang.setPower(0);
|
||||
}
|
||||
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);
|
||||
// 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.
|
||||
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;
|
||||
}
|
||||
frontLeft.setPower(leftFrontPower);
|
||||
frontRight.setPower(rightFrontPower);
|
||||
backLeft.setPower(leftBackPower);
|
||||
backRight.setPower(rightBackPower);
|
||||
// Show the elapsed game time and wheel power
|
||||
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,205 @@
|
||||
package org.firstinspires.ftc.teamcode;
|
||||
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
|
||||
@TeleOp( name = "manual Chasis")
|
||||
public class manualChasis extends OpMode {
|
||||
|
||||
DcMotor arm;
|
||||
Servo gripper;
|
||||
Servo wrist;
|
||||
public double axial;
|
||||
public double lateral;
|
||||
public double yaw;
|
||||
DcMotor frontRight;
|
||||
DcMotor backRight;
|
||||
DcMotor frontLeft;
|
||||
DcMotor backLeft;
|
||||
DcMotor hang;
|
||||
|
||||
|
||||
/**
|
||||
* 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() {
|
||||
arm.setPower(0);
|
||||
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();
|
||||
arm = hardwareMap.dcMotor.get("arm raise");
|
||||
gripper = hardwareMap.servo.get("gripper");
|
||||
wrist = hardwareMap.servo.get("wrist");
|
||||
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");
|
||||
hang = hardwareMap.dcMotor.get("hang");
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(){
|
||||
|
||||
}
|
||||
|
||||
//double num = 2.25;
|
||||
final static double MOTOR_HI_SPEED_RATIO = 2.25;
|
||||
final static double MOTOR_LO_SPEED_RATIO = 3.5;
|
||||
final static double ARM_POWER = 3.5;
|
||||
double num = MOTOR_HI_SPEED_RATIO;
|
||||
/**
|
||||
* User defined loop method.
|
||||
* This method will be called repeatedly in a loop while this op mode is running
|
||||
*/
|
||||
public void loop() {
|
||||
frontLeft.setDirection(DcMotor.Direction.REVERSE);
|
||||
backLeft.setDirection(DcMotor.Direction.FORWARD);
|
||||
frontRight.setDirection(DcMotor.Direction.REVERSE);
|
||||
backRight.setDirection(DcMotor.Direction.FORWARD);
|
||||
|
||||
|
||||
double armPower = gamepad2.right_stick_y/ARM_POWER;
|
||||
// Normalize the values so no wheel power exceeds 100%
|
||||
// This ensures that the robot maintains the desired motion.
|
||||
if(gamepad1.a)
|
||||
{
|
||||
num = MOTOR_HI_SPEED_RATIO;
|
||||
}
|
||||
if (gamepad1.b)
|
||||
{
|
||||
num = MOTOR_LO_SPEED_RATIO;
|
||||
}
|
||||
if(gamepad2.right_stick_y != 0)
|
||||
{
|
||||
arm.setPower(armPower);
|
||||
telemetry.addData("joystick y value", gamepad2.right_stick_y);
|
||||
telemetry.update();
|
||||
}
|
||||
else
|
||||
{
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
arm.setPower(0);
|
||||
}
|
||||
if(gamepad2.left_trigger > 0.35)
|
||||
{
|
||||
gripper.setPosition(0);
|
||||
}
|
||||
if(gamepad2.right_trigger > 0.35){
|
||||
gripper.setPosition(1);
|
||||
}
|
||||
if(gamepad2.dpad_up)
|
||||
{
|
||||
wrist.setPosition(0.4);
|
||||
}
|
||||
if(gamepad2.dpad_down)
|
||||
{
|
||||
wrist.setPosition(1);
|
||||
}
|
||||
if(gamepad2.dpad_right)
|
||||
{
|
||||
wrist.setPosition(0);
|
||||
}
|
||||
if (gamepad1.dpad_up)
|
||||
{
|
||||
hang.setPower(1);
|
||||
}
|
||||
hang.setPower(0);
|
||||
if (gamepad1.dpad_down)
|
||||
{
|
||||
hang.setPower(-.5);
|
||||
}
|
||||
else {
|
||||
hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
hang.setPower(0);
|
||||
}
|
||||
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);
|
||||
// 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.
|
||||
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;
|
||||
}
|
||||
frontLeft.setPower(leftFrontPower);
|
||||
frontRight.setPower(rightFrontPower);
|
||||
backLeft.setPower(leftBackPower);
|
||||
backRight.setPower(rightBackPower);
|
||||
// Show the elapsed game time and wheel power
|
||||
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,7 @@
|
||||
package org.firstinspires.ftc.teamcode.reorg;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
|
||||
public interface AutonomousCommand {
|
||||
public void execute(boolean value, Telemetry telemetry);
|
||||
}
|
@ -0,0 +1,18 @@
|
||||
package org.firstinspires.ftc.teamcode.reorg;
|
||||
|
||||
public class AutonomousConstant {
|
||||
|
||||
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.3;
|
||||
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;
|
||||
|
||||
}
|
@ -0,0 +1,52 @@
|
||||
package org.firstinspires.ftc.teamcode.reorg;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
@Autonomous(name="auto-test-facade", group="Robot")
|
||||
public class AutonomousTestFacade extends LinearOpMode {
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
/*
|
||||
Class where all our initialization code lives
|
||||
To use it, we just instantiate a copy of it!
|
||||
*/
|
||||
AutonomousTestService service = new AutonomousTestService(hardwareMap);
|
||||
|
||||
/*
|
||||
Initialize the hardware stuff
|
||||
*/
|
||||
service.initializeHardware(); // good
|
||||
|
||||
/*
|
||||
There's a sleep here, need to determine if it's needed
|
||||
sleep(1000);
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
Initialize the direction stuff
|
||||
*/
|
||||
service.initializeDirection(); // good
|
||||
|
||||
/*
|
||||
Stop and reset all encoders
|
||||
*/
|
||||
service.initializeEncoders(); // good
|
||||
|
||||
/*
|
||||
Update telemetry
|
||||
*/
|
||||
telemetry.update();
|
||||
|
||||
/*
|
||||
From the JavaDoc: Pauses the Linear Op Mode until start has been pressed
|
||||
or until the current thread is interrupted.
|
||||
*/
|
||||
waitForStart();
|
||||
{
|
||||
service.execute(opModeIsActive(), telemetry);
|
||||
}
|
||||
}
|
||||
}
|
@ -0,0 +1,240 @@
|
||||
package org.firstinspires.ftc.teamcode.reorg;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.ColorSensor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
|
||||
public class AutonomousTestService implements AutonomousCommand {
|
||||
|
||||
private DcMotor leftDrive;
|
||||
private DcMotor rightDrive;
|
||||
private DcMotor backRightDrive;
|
||||
private DcMotor backLeftDrive;
|
||||
private ColorSensor colorRight;
|
||||
private ColorSensor colorLeft;
|
||||
private Servo wrist;
|
||||
private Servo gripper;
|
||||
private DcMotor arm;
|
||||
private ElapsedTime runTime;
|
||||
|
||||
public AutonomousTestService(HardwareMap hwMap) {
|
||||
this.runTime = new ElapsedTime();
|
||||
leftDrive = hwMap.get(DcMotor.class, "Drive front lt");
|
||||
rightDrive = hwMap.get(DcMotor.class, "Drive front rt");
|
||||
backLeftDrive = hwMap.get(DcMotor.class, "Drive back lt");
|
||||
backRightDrive = hwMap.get(DcMotor.class, "Drive back rt");
|
||||
colorRight = hwMap.get(ColorSensor.class, "color right");
|
||||
colorLeft = hwMap.get(ColorSensor.class, "color left");
|
||||
arm = hwMap.get(DcMotor.class, "arm raise");
|
||||
wrist = hwMap.get(Servo.class, "wrist");
|
||||
gripper = hwMap.get(Servo.class, "gripper");
|
||||
}
|
||||
|
||||
public void initializeHardware() {
|
||||
setWristPosition(1);
|
||||
setGripperPosition(1);
|
||||
arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
}
|
||||
|
||||
private void setWristPosition(int position) {
|
||||
wrist.setPosition(position);
|
||||
}
|
||||
|
||||
private void setGripperPosition(int position) {
|
||||
wrist.setPosition(position);
|
||||
}
|
||||
|
||||
public void initializeDirection() {
|
||||
setDriveDirectionForward();
|
||||
setArmDirection(DcMotor.Direction.REVERSE);
|
||||
}
|
||||
|
||||
public void initializeEncoders() {
|
||||
setDriveModeStopAndResetEncoder();
|
||||
setDriveModeRunUsingEncoder();
|
||||
setArmDriveModeStopAndResetEncoder();
|
||||
setArmDriveModeRunUsingEncoder();
|
||||
}
|
||||
|
||||
private void setDriveDirectionForward() {
|
||||
leftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
backRightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
}
|
||||
|
||||
private void setDriveDirectionLeft() {
|
||||
leftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backLeftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backRightDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
}
|
||||
|
||||
private void setDriveDirectionStraightLeft() {
|
||||
leftDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
|
||||
backRightDrive.setDirection(DcMotor.Direction.FORWARD);
|
||||
}
|
||||
|
||||
private void turn(double degrees, boolean isOpModeActive, Telemetry telemetry) {
|
||||
double turning_distance = degrees * AutonomousConstant.DEGREE_TOO_DISTANCE;
|
||||
move(turning_distance, isOpModeActive, telemetry);
|
||||
}
|
||||
|
||||
private void move(double degrees, boolean isOpModeActive, Telemetry telemetry) {
|
||||
encoderDrive(AutonomousConstant.DRIVE_SPEED, degrees, degrees, AutonomousConstant.LONG_TIMEOUT, isOpModeActive, telemetry);
|
||||
}
|
||||
|
||||
private void setDriveMode(DcMotor.RunMode runMode) {
|
||||
leftDrive.setMode(runMode);
|
||||
rightDrive.setMode(runMode);
|
||||
backLeftDrive.setMode(runMode);
|
||||
backRightDrive.setMode(runMode);
|
||||
}
|
||||
private void setArmMode(DcMotor.RunMode runMode) {
|
||||
arm.setMode(runMode);
|
||||
}
|
||||
|
||||
public void setArmDirection(DcMotor.Direction direction) {
|
||||
arm.setDirection(direction);
|
||||
}
|
||||
|
||||
public void setDriveModeStopAndResetEncoder() {
|
||||
setDriveMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
}
|
||||
|
||||
public void setDriveModeRunUsingEncoder() {
|
||||
setDriveMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
}
|
||||
|
||||
public void setArmDriveModeStopAndResetEncoder() {
|
||||
setArmMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
}
|
||||
|
||||
public void setArmDriveModeRunUsingEncoder() {
|
||||
setArmMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
}
|
||||
|
||||
private void setDriveTargetPosition(int nlt, int nrt, int nbrt, int nblt) {
|
||||
leftDrive.setTargetPosition(nlt);
|
||||
rightDrive.setTargetPosition(nrt);
|
||||
backRightDrive.setTargetPosition(nbrt);
|
||||
backLeftDrive.setTargetPosition(nblt);
|
||||
}
|
||||
|
||||
public void setDriveModeRunToPosition() {
|
||||
setDriveMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
|
||||
public void setDrivePower(double speed) {
|
||||
leftDrive.setPower(Math.abs(speed));
|
||||
rightDrive.setPower(Math.abs(speed));
|
||||
backRightDrive.setPower(Math.abs(speed));
|
||||
backLeftDrive.setPower(Math.abs(speed));
|
||||
}
|
||||
|
||||
public int getColorSensorReadingOnRightSide(Telemetry telemetry) {
|
||||
telemetry.addData("Clear Right", colorRight.alpha());
|
||||
telemetry.addData("Red Right", colorRight.red());
|
||||
telemetry.addData("Green Right", colorRight.green());
|
||||
telemetry.addData("Blue Right", colorRight.blue());
|
||||
telemetry.addData("Color Sensor","right");
|
||||
return colorRight.red();
|
||||
}
|
||||
|
||||
public int getColorSensorReadingOnLeftSide(Telemetry telemetry) {
|
||||
telemetry.addData("Clear Left", colorLeft.alpha());
|
||||
telemetry.addData("Red Left ", colorLeft.red());
|
||||
telemetry.addData("Green Left", colorLeft.green());
|
||||
telemetry.addData("Blue Left", colorLeft.blue());
|
||||
return colorLeft.red();
|
||||
}
|
||||
|
||||
private int calculateCurrentPosition(int position, double distance) {
|
||||
return position + (int) (distance * AutonomousConstant.COUNTS_PER_INCH);
|
||||
}
|
||||
|
||||
private void drive(double distance, boolean isOpModeActive, Telemetry telemetry) {
|
||||
encoderDrive(AutonomousConstant.DRIVE_SPEED, distance, distance, AutonomousConstant.LONG_TIMEOUT, isOpModeActive, telemetry);
|
||||
}
|
||||
|
||||
private void encoderDrive(double speed, double leftInches, double rightInches, double timeout, boolean isOpModeActive, Telemetry telemetry) {
|
||||
|
||||
if (isOpModeActive) {
|
||||
|
||||
int ldgcp = leftDrive.getCurrentPosition();
|
||||
int rdgcp = leftDrive.getCurrentPosition();
|
||||
int bldgcp = leftDrive.getCurrentPosition();
|
||||
int brdgcp = leftDrive.getCurrentPosition();
|
||||
|
||||
int nlt = calculateCurrentPosition(ldgcp, leftInches);
|
||||
int nrt = calculateCurrentPosition(rdgcp, rightInches);
|
||||
int nblt = calculateCurrentPosition(bldgcp, leftInches);
|
||||
int nbrt = calculateCurrentPosition(brdgcp, rightInches);
|
||||
|
||||
setDriveTargetPosition(nlt, nrt, nblt, nbrt);
|
||||
|
||||
setDriveModeRunToPosition();
|
||||
|
||||
runTime.reset();
|
||||
|
||||
setDrivePower(speed);
|
||||
|
||||
while (isOpModeActive && (runTime.seconds() < timeout) && (leftDrive.isBusy() && rightDrive.isBusy() && backLeftDrive.isBusy() && backRightDrive.isBusy())) {
|
||||
telemetry.addData("Running to", " %7d :%7d", nlt, nrt);
|
||||
telemetry.addData("Currently at", " at %7d :%7d", ldgcp, rdgcp, bldgcp, brdgcp);
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
setDrivePower(0);
|
||||
|
||||
setDriveModeRunUsingEncoder();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute(boolean isOpModeActive, Telemetry telemetry) {
|
||||
/*
|
||||
Do we need this? Can it be part of the initialization?
|
||||
*/
|
||||
// arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
/*
|
||||
Drive forward to left/right/center placement of item
|
||||
*/
|
||||
setDriveDirectionForward();
|
||||
drive(26, isOpModeActive, telemetry);
|
||||
|
||||
|
||||
|
||||
if (getColorSensorReadingOnLeftSide(telemetry) > 50) {
|
||||
|
||||
if(getColorSensorReadingOnLeftSide(telemetry) > getColorSensorReadingOnRightSide(telemetry))
|
||||
telemetry.addData("color sensor","left");
|
||||
|
||||
setDriveDirectionLeft();
|
||||
turn(90, isOpModeActive, telemetry);
|
||||
|
||||
setDriveDirectionStraightLeft();
|
||||
move(8.5, isOpModeActive, telemetry);
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (getColorSensorReadingOnRightSide(telemetry) > 50) {
|
||||
|
||||
if(getColorSensorReadingOnRightSide(telemetry) > getColorSensorReadingOnLeftSide(telemetry))
|
||||
telemetry.addData("color sensor","right");
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
BIN
doc/Image/autopath.png
Normal file
BIN
doc/Image/autopath.png
Normal file
Binary file not shown.
After Width: | Height: | Size: 229 KiB |
Reference in New Issue
Block a user