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.hardware.DcMotor;
|
||||
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 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".
|
||||
@ -93,10 +87,6 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
||||
// Declare OpMode members for each of the 4 motors.
|
||||
private final ElapsedTime runtime = new ElapsedTime();
|
||||
|
||||
private ClawSubsystem.ClawState clawState;
|
||||
|
||||
// private WristSubsystem.WristState wristState;
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
|
||||
@ -107,25 +97,7 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
||||
DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_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
|
||||
Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER));
|
||||
@ -162,37 +134,8 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
|
||||
waitForStart();
|
||||
runtime.reset();
|
||||
|
||||
claw.setState(ClawSubsystem.ClawState.CLOSED);
|
||||
|
||||
|
||||
// run until the end of the match (driver presses STOP)
|
||||
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;
|
||||
|
||||
// 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("Right Encoder Value", rightEncoder.getDeltaPosition());
|
||||
telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
|
||||
telemetry.addData("Claw State", claw.getState());
|
||||
telemetry.addData("Wrist Servo Position", arm.getPosition());
|
||||
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 com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
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;
|
||||
|
||||
@TeleOp(name="Drive", group="A")
|
||||
@Disabled
|
||||
public class CometDrive extends OpMode {
|
||||
|
||||
private Teleop teleop;
|
||||
@ -19,9 +21,7 @@ public class CometDrive extends OpMode {
|
||||
telemetry,
|
||||
new Follower(hardwareMap),
|
||||
blueBucketStartPose,
|
||||
false,
|
||||
gamepad1,
|
||||
gamepad2);
|
||||
gamepad1);
|
||||
}
|
||||
|
||||
@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 Gamepad gamepad1;
|
||||
private Gamepad gamepad2;
|
||||
private Gamepad currentGamepad1 = new Gamepad();
|
||||
private Gamepad currentGamepad2 = new Gamepad();
|
||||
private Gamepad previousGamepad1 = new Gamepad();
|
||||
private Gamepad previousGamepad2 = new Gamepad();
|
||||
private Gamepad currentGamepad1;
|
||||
private Gamepad previousGamepad1;
|
||||
|
||||
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, boolean fieldCentric, Gamepad gamepad1, Gamepad gamepad2) {
|
||||
public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Pose startPose, Gamepad gamepad1) {
|
||||
claw = new ClawSubsystem(hardwareMap, clawState);
|
||||
|
||||
this.follower = follower;
|
||||
this.startPose = startPose;
|
||||
|
||||
this.fieldCentric = fieldCentric;
|
||||
this.telemetry = telemetry;
|
||||
this.gamepad1 = gamepad1;
|
||||
this.gamepad2 = gamepad2;
|
||||
|
||||
this.currentGamepad1 = new Gamepad();
|
||||
this.previousGamepad1 = new Gamepad();
|
||||
|
||||
stopDrive = new RunAction(this::stopDrive);
|
||||
startDrive = new RunAction(this::startDrive);
|
||||
@ -57,27 +53,18 @@ public class Teleop {
|
||||
follower.startTeleopDrive();
|
||||
}
|
||||
|
||||
private void stopDrive(){
|
||||
private void stopDrive() {
|
||||
follower.breakFollowing();
|
||||
}
|
||||
|
||||
public void update() {
|
||||
previousGamepad1.copy(currentGamepad1);
|
||||
previousGamepad2.copy(currentGamepad2);
|
||||
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)
|
||||
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();
|
||||
|
||||
telemetry.addData("X", follower.getPose().getX());
|
||||
|
||||
Reference in New Issue
Block a user