Working tests and reverting basic omni back to default
This commit is contained in:
@ -0,0 +1,148 @@
|
|||||||
|
/* 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 static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
|
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||||
|
import com.qualcomm.robotcore.hardware.ServoImplEx;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
||||||
|
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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="Arm Test", group="Debug")
|
||||||
|
public class ArmTest extends LinearOpMode {
|
||||||
|
|
||||||
|
// Declare OpMode members for each of the 4 motors.
|
||||||
|
private final ElapsedTime runtime = new ElapsedTime();
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() {
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Instantiate Arm
|
||||||
|
*/
|
||||||
|
ServoImplEx arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
|
||||||
|
arm.resetDeviceConfigurationForOpMode();
|
||||||
|
arm.setPosition(.15);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Instantiate gamepad state holders
|
||||||
|
*/
|
||||||
|
Gamepad currentGamepad1 = new Gamepad();
|
||||||
|
Gamepad previousGamepad1 = new Gamepad();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
runtime.reset();
|
||||||
|
|
||||||
|
// run until the end of the match (driver presses STOP)
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
|
||||||
|
previousGamepad1.copy(currentGamepad1);
|
||||||
|
currentGamepad1.copy(gamepad1);
|
||||||
|
|
||||||
|
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
||||||
|
arm.setPosition(0.3);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.square && !previousGamepad1.square) {
|
||||||
|
arm.setPosition(.45);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
|
||||||
|
double newPosition = arm.getPosition() - .05;
|
||||||
|
if (newPosition < 0.15) {
|
||||||
|
arm.setPosition(0.15);
|
||||||
|
} else {
|
||||||
|
arm.setPosition(newPosition);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
|
||||||
|
double newPosition = arm.getPosition() + .05;
|
||||||
|
if (newPosition > .6) {
|
||||||
|
arm.setPosition(.6);
|
||||||
|
} else {
|
||||||
|
arm.setPosition(newPosition);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Show the elapsed game time and wheel power.
|
||||||
|
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||||
|
telemetry.addData("Arm Servo Position", arm.getPosition());
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}}
|
||||||
@ -49,15 +49,9 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
|||||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
|
||||||
import com.qualcomm.robotcore.hardware.PwmControl;
|
|
||||||
import com.qualcomm.robotcore.hardware.Servo;
|
|
||||||
import com.qualcomm.robotcore.hardware.ServoImplEx;
|
|
||||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
||||||
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
|
||||||
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* This file contains an example of a Linear "OpMode".
|
* This file contains an example of a Linear "OpMode".
|
||||||
@ -93,10 +87,6 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
// Declare OpMode members for each of the 4 motors.
|
// Declare OpMode members for each of the 4 motors.
|
||||||
private final ElapsedTime runtime = new ElapsedTime();
|
private final ElapsedTime runtime = new ElapsedTime();
|
||||||
|
|
||||||
private ClawSubsystem.ClawState clawState;
|
|
||||||
|
|
||||||
// private WristSubsystem.WristState wristState;
|
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void runOpMode() {
|
public void runOpMode() {
|
||||||
|
|
||||||
@ -107,25 +97,7 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
|
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
|
||||||
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
|
DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
|
||||||
|
|
||||||
/*
|
|
||||||
* Instantiate Claw
|
|
||||||
*/
|
|
||||||
ClawSubsystem claw = new ClawSubsystem(hardwareMap, clawState);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Instantiate Arm
|
|
||||||
*/
|
|
||||||
ServoImplEx arm = hardwareMap.get(ServoImplEx.class, "arm-servo");
|
|
||||||
//arm.setPwmRange(PwmControl.PwmRange.defaultRange);
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Instantiate gamepad state holders
|
|
||||||
*/
|
|
||||||
Gamepad currentGamepad1 = new Gamepad();
|
|
||||||
Gamepad previousGamepad1 = new Gamepad();
|
|
||||||
|
|
||||||
|
|
||||||
arm.resetDeviceConfigurationForOpMode();
|
|
||||||
|
|
||||||
// TODO: replace these with your encoder ports
|
// TODO: replace these with your encoder ports
|
||||||
Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
||||||
@ -162,37 +134,8 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
waitForStart();
|
waitForStart();
|
||||||
runtime.reset();
|
runtime.reset();
|
||||||
|
|
||||||
claw.setState(ClawSubsystem.ClawState.CLOSED);
|
|
||||||
|
|
||||||
|
|
||||||
// run until the end of the match (driver presses STOP)
|
// run until the end of the match (driver presses STOP)
|
||||||
while (opModeIsActive()) {
|
while (opModeIsActive()) {
|
||||||
|
|
||||||
previousGamepad1.copy(currentGamepad1);
|
|
||||||
currentGamepad1.copy(gamepad1);
|
|
||||||
//
|
|
||||||
// if (gamepad1.cross) {
|
|
||||||
// claw.switchState();
|
|
||||||
// }
|
|
||||||
|
|
||||||
if (currentGamepad1.cross && !previousGamepad1.cross) {
|
|
||||||
// This will set intakeToggle to true if it was previously false
|
|
||||||
// and intakeToggle to false if it was previously true,
|
|
||||||
// providing a toggling behavior.
|
|
||||||
claw.switchState();
|
|
||||||
}
|
|
||||||
|
|
||||||
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
|
||||||
// wrist.switchState();
|
|
||||||
arm.setPosition(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (currentGamepad1.square && !previousGamepad1.square) {
|
|
||||||
// wrist.switchState();
|
|
||||||
arm.setPosition(.5);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
double max;
|
double max;
|
||||||
|
|
||||||
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
|
// POV Mode uses left joystick to go forward & strafe, and right joystick to rotate.
|
||||||
@ -250,8 +193,6 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
|||||||
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
|
telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
|
||||||
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
|
telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
|
||||||
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
|
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
|
||||||
telemetry.addData("Claw State", claw.getState());
|
|
||||||
telemetry.addData("Wrist Servo Position", arm.getPosition());
|
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
}}
|
}}
|
||||||
|
|||||||
@ -0,0 +1,125 @@
|
|||||||
|
/* 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 static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
|
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||||
|
import com.qualcomm.robotcore.hardware.ServoImplEx;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
||||||
|
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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="Claw Test", group="Debug")
|
||||||
|
public class ClawTest extends LinearOpMode {
|
||||||
|
|
||||||
|
// Declare OpMode members for each of the 4 motors.
|
||||||
|
private final ElapsedTime runtime = new ElapsedTime();
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() {
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Instantiate Claw
|
||||||
|
*/
|
||||||
|
ClawSubsystem claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.OPEN);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Instantiate gamepad state holders
|
||||||
|
*/
|
||||||
|
Gamepad currentGamepad1 = new Gamepad();
|
||||||
|
Gamepad previousGamepad1 = new Gamepad();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
runtime.reset();
|
||||||
|
|
||||||
|
// run until the end of the match (driver presses STOP)
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
|
||||||
|
previousGamepad1.copy(currentGamepad1);
|
||||||
|
currentGamepad1.copy(gamepad1);
|
||||||
|
|
||||||
|
if (currentGamepad1.cross && !previousGamepad1.cross) {
|
||||||
|
claw.switchState();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Show the elapsed game time and wheel power.
|
||||||
|
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||||
|
telemetry.addData("Claw State", claw.getState());
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode;
|
|||||||
|
|
||||||
import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.blueBucketStartPose;
|
import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.blueBucketStartPose;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
|
||||||
@ -9,6 +10,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
|
|||||||
import org.firstinspires.ftc.teamcode.runmodes.Teleop;
|
import org.firstinspires.ftc.teamcode.runmodes.Teleop;
|
||||||
|
|
||||||
@TeleOp(name="Drive", group="A")
|
@TeleOp(name="Drive", group="A")
|
||||||
|
@Disabled
|
||||||
public class CometDrive extends OpMode {
|
public class CometDrive extends OpMode {
|
||||||
|
|
||||||
private Teleop teleop;
|
private Teleop teleop;
|
||||||
@ -19,9 +21,7 @@ public class CometDrive extends OpMode {
|
|||||||
telemetry,
|
telemetry,
|
||||||
new Follower(hardwareMap),
|
new Follower(hardwareMap),
|
||||||
blueBucketStartPose,
|
blueBucketStartPose,
|
||||||
false,
|
gamepad1);
|
||||||
gamepad1,
|
|
||||||
gamepad2);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
@ -0,0 +1,159 @@
|
|||||||
|
/* 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.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||||
|
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||||
|
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 = "Lift Test", group = "Debug")
|
||||||
|
public class LiftTest extends LinearOpMode {
|
||||||
|
|
||||||
|
// Declare OpMode members for each of the 4 motors.
|
||||||
|
private final ElapsedTime runtime = new ElapsedTime();
|
||||||
|
|
||||||
|
private final int MIN_POINT = 0;
|
||||||
|
private final int MAX_POINT = 6500;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() {
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Instantiate Lift
|
||||||
|
*/
|
||||||
|
DcMotor liftDrive = hardwareMap.get(DcMotor.class, "lift-motor");
|
||||||
|
liftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
liftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Instantiate gamepad state holders
|
||||||
|
*/
|
||||||
|
Gamepad currentGamepad1 = new Gamepad();
|
||||||
|
Gamepad previousGamepad1 = new Gamepad();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
runtime.reset();
|
||||||
|
|
||||||
|
|
||||||
|
// run until the end of the match (driver presses STOP)
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
|
||||||
|
previousGamepad1.copy(currentGamepad1);
|
||||||
|
currentGamepad1.copy(gamepad1);
|
||||||
|
|
||||||
|
liftDrive.setPower(.5);
|
||||||
|
|
||||||
|
// Max position is 6800, safely setting to 6500
|
||||||
|
|
||||||
|
if (currentGamepad1.square && !previousGamepad1.square) {
|
||||||
|
liftDrive.setTargetPosition(MIN_POINT);
|
||||||
|
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.triangle && !previousGamepad1.triangle) {
|
||||||
|
liftDrive.setTargetPosition(2000);
|
||||||
|
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
||||||
|
liftDrive.setTargetPosition(4000);
|
||||||
|
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.cross && !previousGamepad1.cross) {
|
||||||
|
liftDrive.setTargetPosition(MAX_POINT);
|
||||||
|
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
|
||||||
|
int newPosition = liftDrive.getCurrentPosition() - 250;
|
||||||
|
if (newPosition < MIN_POINT) {
|
||||||
|
liftDrive.setTargetPosition(MIN_POINT);
|
||||||
|
} else {
|
||||||
|
liftDrive.setTargetPosition(newPosition);
|
||||||
|
}
|
||||||
|
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
|
||||||
|
int newPosition = liftDrive.getCurrentPosition() + 250;
|
||||||
|
if (newPosition > MAX_POINT) {
|
||||||
|
liftDrive.setTargetPosition(MAX_POINT);
|
||||||
|
} else {
|
||||||
|
liftDrive.setTargetPosition(newPosition);
|
||||||
|
}
|
||||||
|
liftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// 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
|
||||||
|
|
||||||
|
// Send calculated power to wheels
|
||||||
|
|
||||||
|
|
||||||
|
// Show the elapsed game time and wheel power.
|
||||||
|
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||||
|
telemetry.addData("Lift Drive Position", liftDrive.getCurrentPosition());
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,150 @@
|
|||||||
|
/* 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 static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
|
||||||
|
import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||||
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||||
|
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||||
|
import com.qualcomm.robotcore.hardware.Servo;
|
||||||
|
import com.qualcomm.robotcore.hardware.ServoImplEx;
|
||||||
|
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
|
||||||
|
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* 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="Wrist Test", group="Debug")
|
||||||
|
public class WristTest extends LinearOpMode {
|
||||||
|
|
||||||
|
// Declare OpMode members for each of the 4 motors.
|
||||||
|
private final ElapsedTime runtime = new ElapsedTime();
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void runOpMode() {
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Instantiate Wrist
|
||||||
|
*/
|
||||||
|
ServoImplEx wrist = hardwareMap.get(ServoImplEx.class, "wrist-servo");
|
||||||
|
// wrist.resetDeviceConfigurationForOpMode();
|
||||||
|
wrist.setDirection(Servo.Direction.FORWARD);
|
||||||
|
wrist.setPosition(0);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Instantiate gamepad state holders
|
||||||
|
*/
|
||||||
|
Gamepad currentGamepad1 = new Gamepad();
|
||||||
|
Gamepad previousGamepad1 = new Gamepad();
|
||||||
|
|
||||||
|
waitForStart();
|
||||||
|
runtime.reset();
|
||||||
|
|
||||||
|
// run until the end of the match (driver presses STOP)
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
|
||||||
|
previousGamepad1.copy(currentGamepad1);
|
||||||
|
currentGamepad1.copy(gamepad1);
|
||||||
|
|
||||||
|
if (currentGamepad1.square && !previousGamepad1.square) {
|
||||||
|
wrist.setPosition(0.25);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.circle && !previousGamepad1.circle) {
|
||||||
|
wrist.setPosition(0.75);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
|
||||||
|
double newPosition = wrist.getPosition() - .05;
|
||||||
|
if (newPosition < 0.1) {
|
||||||
|
wrist.setPosition(0.1);
|
||||||
|
} else {
|
||||||
|
wrist.setPosition(newPosition);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
|
||||||
|
double newPosition = wrist.getPosition() + .05;
|
||||||
|
if (newPosition > .9) {
|
||||||
|
wrist.setPosition(.9);
|
||||||
|
} else {
|
||||||
|
wrist.setPosition(newPosition);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Show the elapsed game time and wheel power.
|
||||||
|
telemetry.addData("Status", "Run Time: " + runtime.toString());
|
||||||
|
telemetry.addData("Wrist Servo Position", wrist.getPosition());
|
||||||
|
telemetry.update();
|
||||||
|
}
|
||||||
|
}}
|
||||||
@ -20,28 +20,24 @@ public class Teleop {
|
|||||||
private Telemetry telemetry;
|
private Telemetry telemetry;
|
||||||
|
|
||||||
private Gamepad gamepad1;
|
private Gamepad gamepad1;
|
||||||
private Gamepad gamepad2;
|
private Gamepad currentGamepad1;
|
||||||
private Gamepad currentGamepad1 = new Gamepad();
|
private Gamepad previousGamepad1;
|
||||||
private Gamepad currentGamepad2 = new Gamepad();
|
|
||||||
private Gamepad previousGamepad1 = new Gamepad();
|
|
||||||
private Gamepad previousGamepad2 = new Gamepad();
|
|
||||||
|
|
||||||
public RunAction stopDrive, startDrive;
|
public RunAction stopDrive, startDrive;
|
||||||
|
|
||||||
private boolean fieldCentric = true;
|
public double speed = 0.5;
|
||||||
|
|
||||||
public double speed = 0.75;
|
public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Pose startPose, Gamepad gamepad1) {
|
||||||
|
|
||||||
public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Pose startPose, boolean fieldCentric, Gamepad gamepad1, Gamepad gamepad2) {
|
|
||||||
claw = new ClawSubsystem(hardwareMap, clawState);
|
claw = new ClawSubsystem(hardwareMap, clawState);
|
||||||
|
|
||||||
this.follower = follower;
|
this.follower = follower;
|
||||||
this.startPose = startPose;
|
this.startPose = startPose;
|
||||||
|
|
||||||
this.fieldCentric = fieldCentric;
|
|
||||||
this.telemetry = telemetry;
|
this.telemetry = telemetry;
|
||||||
this.gamepad1 = gamepad1;
|
this.gamepad1 = gamepad1;
|
||||||
this.gamepad2 = gamepad2;
|
|
||||||
|
this.currentGamepad1 = new Gamepad();
|
||||||
|
this.previousGamepad1 = new Gamepad();
|
||||||
|
|
||||||
stopDrive = new RunAction(this::stopDrive);
|
stopDrive = new RunAction(this::stopDrive);
|
||||||
startDrive = new RunAction(this::startDrive);
|
startDrive = new RunAction(this::startDrive);
|
||||||
@ -57,27 +53,18 @@ public class Teleop {
|
|||||||
follower.startTeleopDrive();
|
follower.startTeleopDrive();
|
||||||
}
|
}
|
||||||
|
|
||||||
private void stopDrive(){
|
private void stopDrive() {
|
||||||
follower.breakFollowing();
|
follower.breakFollowing();
|
||||||
}
|
}
|
||||||
|
|
||||||
public void update() {
|
public void update() {
|
||||||
previousGamepad1.copy(currentGamepad1);
|
previousGamepad1.copy(currentGamepad1);
|
||||||
previousGamepad2.copy(currentGamepad2);
|
|
||||||
currentGamepad1.copy(gamepad1);
|
currentGamepad1.copy(gamepad1);
|
||||||
currentGamepad2.copy(gamepad2);
|
|
||||||
|
|
||||||
if (gamepad1.right_bumper)
|
|
||||||
speed = 1;
|
|
||||||
else if (gamepad1.left_bumper)
|
|
||||||
speed = 0.25;
|
|
||||||
else
|
|
||||||
speed = 0.75;
|
|
||||||
|
|
||||||
if (currentGamepad1.a && !previousGamepad1.a)
|
if (currentGamepad1.a && !previousGamepad1.a)
|
||||||
claw.switchState();
|
claw.switchState();
|
||||||
|
|
||||||
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y * speed, -gamepad1.left_stick_x * speed, -gamepad1.right_stick_x * speed, !fieldCentric);
|
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y * speed, -gamepad1.left_stick_x * speed, -gamepad1.right_stick_x * speed);
|
||||||
follower.update();
|
follower.update();
|
||||||
|
|
||||||
telemetry.addData("X", follower.getPose().getX());
|
telemetry.addData("X", follower.getPose().getX());
|
||||||
|
|||||||
Reference in New Issue
Block a user