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