SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/branch-rc-chassis-14493-subsystem' into branch-rc-chassis-14493-subsystem

Browse Source
This commit is contained in:
robotics1
2024-11-05 15:26:42 -08:00
parent 9b2a04013f 19fcec1fcc
commit a2fa3341b1
11 changed files with 402 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java Normal file
View File

@ -0,0 +1,97 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Dev Teleop Remix", group = "Debug")
@Disabled
public class DevTeleOpRemix extends OpMode {
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public LiftSubsystem lift;
public MotorsSubsystem motors;
public Gamepad currentGamepad1;
public Gamepad previousGamepad1;
public Gamepad currentGamepad2;
public Gamepad previousGamepad2;
public double power = .6;
@Override
public void init() {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
lift = new LiftSubsystem(hardwareMap);
motors = new MotorsSubsystem(hardwareMap, telemetry, power);
claw.init();
arm.init();
wrist.init();
lift.init();
motors.init();
currentGamepad1 = new Gamepad();
previousGamepad1 = new Gamepad();
currentGamepad2 = new Gamepad();
previousGamepad2 = new Gamepad();
}
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.a && !previousGamepad1.a) {
wrist.floorWrist();
arm.engageArm();
}
}
public void thePickup(ClawSubsystem claw) {
if (currentGamepad1.x && !previousGamepad1.x) {
claw.switchState();
}
}
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
}
}
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
if (currentGamepad1.y && !previousGamepad1.y) {
lift.toLowBucket();
wrist.bucketWrist();
}
}
@Override
public void loop() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
theDrop(arm, wrist);
thePickup(claw);
theLift(arm, wrist);
theLowBucketScore(lift, wrist, arm);
motors.calculateTrajectory(gamepad1);
}
}

107
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java Normal file
View File

@ -0,0 +1,107 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Dev Teleop Remix Deux", group = "Debug")
@Disabled
public class DevTeleOpRemixDeux extends OpMode {
private Follower follower;
public ClawSubsystem claw;
public ArmSubsystem arm;
public WristSubsystem wrist;
public LiftSubsystem lift;
public MotorsSubsystem motors;
public Gamepad currentGamepad1;
public Gamepad previousGamepad1;
public Gamepad currentGamepad2;
public Gamepad previousGamepad2;
public double power = .6;
@Override
public void init() {
follower = new Follower(hardwareMap);
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR);
motors = new MotorsSubsystem(hardwareMap, telemetry);
lift = new LiftSubsystem(hardwareMap);
claw.init();
arm.init();
wrist.init();
lift.init();
motors.init();
currentGamepad1 = new Gamepad();
previousGamepad1 = new Gamepad();
currentGamepad2 = new Gamepad();
previousGamepad2 = new Gamepad();
follower.setMaxPower(this.power);
follower.startTeleopDrive();
}
public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.a && !previousGamepad1.a) {
wrist.floorWrist();
arm.engageArm();
}
}
public void thePickup(ClawSubsystem claw) {
if (currentGamepad1.x && !previousGamepad1.x) {
claw.switchState();
}
}
public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
if (currentGamepad1.b && !previousGamepad1.b) {
arm.parkArm();
wrist.bucketWrist();
}
}
public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
if (currentGamepad1.y && !previousGamepad1.y) {
lift.toLowBucket();
wrist.bucketWrist();
}
}
@Override
public void loop() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
previousGamepad2.copy(currentGamepad2);
currentGamepad2.copy(gamepad2);
theDrop(arm, wrist);
thePickup(claw);
theLift(arm, wrist);
theLowBucketScore(lift, wrist, arm);
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java
View File

@ -27,7 +27,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClawTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java
View File

@ -27,7 +27,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftRawTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftRawTest.java
View File

@ -27,7 +27,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java
View File

@ -27,12 +27,10 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.cometbots.tests;
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;
@ -76,7 +74,7 @@ public class LiftTest extends LinearOpMode {
currentGamepad1.copy(gamepad1);
if (currentGamepad1.square && !previousGamepad1.square) {
lift.toZero();
lift.toFloor();
}
if (currentGamepad1.triangle && !previousGamepad1.triangle) {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java
View File

@ -27,7 +27,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
View File

@ -13,7 +13,8 @@ public class RobotConstants {
public static double wristFloor = 0.625;
public static double wristBucket = 0.215;
public static int liftZeroPos = 0;
public static int liftToFloorPos = 0;
public static int liftToPoolPos = 500;
public static int liftToLowBucketPos = 2250;
public static int liftToHighBucketPos = 3700;
public static double liftPower = .45;

65
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java
View File

@ -33,6 +33,8 @@ public class Auto {
public Follower follower;
public Telemetry telemetry;
public int caseState = 1;
public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) {
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK);
@ -61,23 +63,52 @@ public class Auto {
}
public void update() {
//follower.update();
if (clawTimer.getElapsedTimeSeconds() > 2) {
claw.openClaw();
}
if (armTimer.getElapsedTimeSeconds() > 4) {
arm.engageArm();
}
if (clawTimer.getElapsedTimeSeconds() > 6) {
claw.closeClaw();
}
if (armTimer.getElapsedTimeSeconds() > 8) {
arm.bucketArm();
wrist.bucketWrist();
}
if (clawTimer.getElapsedTimeSeconds() > 10) {
claw.openClaw();
this.telemetry.addData("Current State", caseState);
this.telemetry.addData("Claw Timer", clawTimer.getElapsedTimeSeconds());
this.telemetry.addData("Arm Timer", armTimer.getElapsedTimeSeconds());
this.telemetry.addData("Wrist Timer", wristTimer.getElapsedTimeSeconds());
this.telemetry.update();
switch(caseState) {
case 1:
claw.openClaw();
caseState = 2;
break;
case 2:
if (clawTimer.getElapsedTimeSeconds() > 2) {
arm.engageArm();
caseState = 3;
}
break;
case 3:
if (armTimer.getElapsedTimeSeconds() > 4) {
wrist.floorWrist();
caseState = 4;
}
break;
case 4:
if (clawTimer.getElapsedTimeSeconds() > 6) {
claw.closeClaw();
caseState = 5;
}
break;
case 5:
if (armTimer.getElapsedTimeSeconds() > 8) {
arm.bucketArm();
wrist.bucketWrist();
caseState = 6;
}
break;
case 6:
if (clawTimer.getElapsedTimeSeconds() > 10) {
claw.openClaw();
caseState = 7;
}
break;
case 7:
this.init();
break;
}
}
}

42
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java
View File

@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftZeroPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToPoolPos;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@ -14,16 +14,17 @@ import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class LiftSubsystem {
public DcMotor lift;
public RunAction toZero, toLowBucket, toHighBucket;
public RunAction toFloor, toLowBucket, toHighBucket;
public enum LiftState {
FLOOR, LOW_BUCKET, HIGH_BUCKET
FLOOR, LOW_BUCKET, HIGH_BUCKET, POOL
}
private LiftState liftState;
public LiftSubsystem(HardwareMap hardwareMap) {
lift = hardwareMap.get(DcMotor.class, "lift-motor");
toZero = new RunAction(this::toZero);
toFloor = new RunAction(this::toFloor);
toLowBucket = new RunAction(this::toLowBucket);
toHighBucket = new RunAction(this::toHighBucket);
}
@ -33,29 +34,36 @@ public class LiftSubsystem {
lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
public void toZero() {
lift.setTargetPosition(liftZeroPos);
lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
public void switchState() {
if(this.liftState == LiftState.FLOOR) {
if (this.liftState == LiftState.FLOOR) {
this.toPool();
} else if (this.liftState == LiftState.POOL) {
this.toLowBucket();
} else if (this.liftState == LiftState.LOW_BUCKET) {
this.toHighBucket();
} else if (this.liftState == LiftState.HIGH_BUCKET) {
this.toZero();
this.toFloor();
}
}
public void toFloor() {
this.setTarget(liftToPoolPos);
this.setState(LiftState.FLOOR);
}
public void toPool() {
this.setTarget(liftToPoolPos);
this.setState(LiftState.POOL);
}
public void toLowBucket() {
lift.setTargetPosition(liftToLowBucketPos);
lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.setTarget(liftToLowBucketPos);
this.setState(LiftState.LOW_BUCKET);
}
public void toHighBucket() {
lift.setTargetPosition(liftToHighBucketPos);
lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.setTarget(liftToHighBucketPos);
this.setState(LiftState.HIGH_BUCKET);
}
public void init() {
@ -73,7 +81,7 @@ public class LiftSubsystem {
}
public void start() {
setTarget(10);
this.toFloor();
}
}

117
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java Normal file
View File

@ -0,0 +1,117 @@
package org.firstinspires.ftc.teamcode.subsystem;
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 com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
public class MotorsSubsystem {
public HardwareMap hardwareMap;
public Telemetry telemetry;
public DcMotor frontLeftMotor;
public DcMotor backLeftMotor;
public DcMotor frontRightMotor;
public DcMotor backRightMotor;
public double power;
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
this.hardwareMap = hardwareMap;
this.telemetry = telemetry;
this.power = 1.0;
}
public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) {
this.hardwareMap = hardwareMap;
this.telemetry = telemetry;
this.power = power;
}
public void init() {
frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION);
frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
public void setFrontLeftMotorPower(double power) {
frontLeftMotor.setPower(power);
}
public void setBackLeftMotorPower(double power) {
backLeftMotor.setPower(power);
}
public void setFrontRightMotorPower(double power) {
frontRightMotor.setPower(power);
}
public void setBackRightMotorPower(double power) {
backRightMotor.setPower(power);
}
public void calculateTrajectory(Gamepad gamepad1) {
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 leftBackPower = axial - lateral + yaw;
double rightFrontPower = 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;
}
// Send calculated power to wheels
this.setFrontLeftMotorPower(leftFrontPower * this.power);
this.setFrontRightMotorPower(rightFrontPower * this.power);
this.setBackLeftMotorPower(leftBackPower * this.power);
this.setBackRightMotorPower(rightBackPower * this.power);
// Show the elapsed game time and wheel power.
this.telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
this.telemetry.update();
}
}