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

Merge remote-tracking branch 'origin/branch-swiss-cheese' into branch-swiss-cheese

Browse Source 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
This commit is contained in:
robotics3
2025-02-03 10:03:59 -08:00
parent b0db84a61c 3bcde94416
commit 15c561cd69
18 changed files with 212 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java
View File

@ -4,21 +4,27 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleOpDevelopment;
import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
@TeleOp(name = "CometBot Drive V2.137", group = "Competition")
public class CometBotTeleOpCompetition extends OpMode {
public DualMotorSliderSubsystem slide;
public CometBotTeleOpDevelopment runMode;
@Override
public void init() {
runMode = new CometBotTeleOpDevelopment(hardwareMap, gamepad1, gamepad2);
slide = new DualMotorSliderSubsystem(hardwareMap);
runMode.init();
}
@Override
public void loop() {
runMode.update();
telemetry.update();
}
public void stop(){

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java
View File

@ -1,21 +1,16 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.State;
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.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
@Autonomous(name = "Specimen Auto", group = "Development")

104
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java
View File

@ -1,17 +1,16 @@
package org.firstinspires.ftc.teamcode.cometbots;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
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.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
public class CometBotTeleOpDevelopment {
@ -32,9 +31,10 @@ public class CometBotTeleOpDevelopment {
public Gamepad currentGamepad2;
public Gamepad previousGamepad1;
public Gamepad previousGamepad2;
public boolean grabBlock;
public boolean wristPickup;
public boolean armParked;
public boolean goClaw;
private Follower follower;
public CometBotTeleOpDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
@ -43,7 +43,7 @@ public class CometBotTeleOpDevelopment {
arm = new ArmSubsystem(hardwareMap);
wrist = new WristSubsystem(hardwareMap);
skyhook = new HangMotorSubsystem(hardwareMap);
grabBlock = false;
this.gamepad1 = gamepad1;
this.gamepad2 = gamepad2;
currentGamepad1 = new Gamepad();
@ -51,6 +51,7 @@ public class CometBotTeleOpDevelopment {
previousGamepad1 = new Gamepad();
previousGamepad2 = new Gamepad();
follower = new Follower(hardwareMap);
}
public void init() {
@ -61,6 +62,9 @@ public class CometBotTeleOpDevelopment {
skyhook.init();
follower.setMaxPower(MAX_POWER);
follower.startTeleopDrive();
wristPickup = false;
armParked = true;
goClaw = false;
}
public void update() {
@ -82,15 +86,10 @@ public class CometBotTeleOpDevelopment {
openClaw();
openThumb();
//hang
hang();
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
}
private void openClaw() {
if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
claw.switchState();
@ -100,58 +99,68 @@ public class CometBotTeleOpDevelopment {
private void openThumb() {
if (currentGamepad2.left_bumper && !previousGamepad2.left_bumper) {
claw.switchTState();
}
}
private void armAndWristToFloor() {
if (currentGamepad2.a && !previousGamepad2.a && wristPickup) {
if (wrist.getState() != WristSubsystem.WristState.FLOOR) {
wrist.toFloorPositionTeleop();
} else if (wristPickup) {
claw.openClaw();
wrist.toPickupPosition();
}
}
if (currentGamepad2.a && !previousGamepad2.a) {
arm.toFloorPositionTeleOp();
if (arm.getState() == ArmSubsystem.ArmState.FLOOR) {
wrist.switchState();
} else if (arm.getState() != ArmSubsystem.ArmState.FLOOR) {
wrist.toPickupPosition();
} else {
wrist.toFloorPosition();
}
wristPickup = true;
armParked = false;
}
}
private void armToBucketPosition() {
if (currentGamepad2.dpad_up && !previousGamepad2.dpad_up) {
armParked = true;
arm.toBucketPosition();
wrist.toBucketPosition();
wristPickup = false;
}
}
private void armToParkPosition() {
if (currentGamepad2.x && !previousGamepad2.x) {
armParked = true;
arm.toParkPosition();
wrist.toFloorPosition();
wrist.toFloorPositionTeleop();
wristPickup = false;
}
}
private void dualSlidesToHighBucketPosition() {
if (currentGamepad2.y && !previousGamepad2.y) {
if (currentGamepad2.y && !previousGamepad2.y && armParked) {
dualSlides.toHighBucketPosition();
}
}
private void dualSlidesToFloorPosition() {
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down && armParked) {
dualSlides.toFloorPosition();
}
}
private void dualSlidesToLowBucketPosition() {
if (currentGamepad2.b && !previousGamepad2.b) {
if (currentGamepad2.b && !previousGamepad2.b && armParked) {
dualSlides.toLowBucketPosition();
}
}
private void hang(){
if (gamepad1.a) {
claw.grabBlueberry();
arm.setPosition(0.13);
arm.setPosition(0.15);
arm.grabBlueberrySkyhook();
//claw Open small amount
@ -161,51 +170,46 @@ public class CometBotTeleOpDevelopment {
if(gamepad1.b) {
//confirm grab
claw.closeClaw();
grabBlock = true;
}
if (gamepad1.x && grabBlock) {
if (gamepad1.x && claw.getState() == ClawSubsystem.ClawState.CLOSED) {
//now slap on bar, first wrist, then arm, then claw then driver must drive away
dualSlides.toFixedPosition(300);
dualSlides.toFixedPosition(500);
dualSlides.update();
}
if (gamepad1.y) {
arm.hangBlueberrySkyhook();
wrist.hangBlueberrySkyhook();
try {
Thread.sleep(1500);
Thread.sleep(500);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
dualSlides.toFixedPosition(2100);
goClaw = true;
dualSlides.toFixedPosition(2200);
dualSlides.update();
}
if (gamepad1.right_bumper) {
claw.openClaw();
try {
Thread.sleep(1000);
} catch (InterruptedException e) {
throw new RuntimeException(e);
if(goClaw == true) {
try {
Thread.sleep(500);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
dualSlides.toFloorPosition();
dualSlides.update();
arm.toParkPosition();
wrist.toPickupPosition();
}
dualSlides.toFloorPosition();
claw.closeClaw();
arm.toParkPosition();
wrist.toFloorPosition();
}
skyhook.setPower(0);
skyhook.setPowerForward(-gamepad2.right_stick_y);
/*if (gamepad1.dpad_down) {
skyhook.disableMotor();
skyhook.setPower(1);
}*/
if(gamepad1.right_trigger > 0){
skyhook.setPower(true, 1);
}
if(gamepad1.left_trigger > 0){
skyhook.setPower(false, 1);
}
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java
View File

@ -10,10 +10,10 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ElapsedTime;
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.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
@Autonomous(name = "Auto Test Competition V2", group = "Dev")
public class CometBotDriveV2 extends OpMode {

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java
View File

@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.cometbots.paths;
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -11,11 +10,11 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.states.FieldStates;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.MotorsSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
public class CometBotTeleopCompetition {

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

@ -34,7 +34,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
@TeleOp(name = "Arm Test v2", group = "Debug")
public class ArmTest extends LinearOpMode {

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

@ -34,7 +34,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
@TeleOp(name = "Claw Test v2", group = "Debug")

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

@ -29,13 +29,12 @@
package org.firstinspires.ftc.teamcode.cometbots.tests;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
@TeleOp(name = "Wrist Test v2", group = "Debug")
public class WristTest extends LinearOpMode {

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

@ -4,8 +4,10 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class RobotConstants {
public final static double clawClose = 1;
public final static double clawOpen = 0.05;
public final static double clawClose = 0.95;
public final static double clawOpen = 0.35;
public final static double armFloor = 0.7;
public final static double armSubmarine = 0.55;
@ -13,8 +15,8 @@ public class RobotConstants {
public final static double armPark = 0.33;
//value for grabbing the hook Specimen
public final static double grabBlueberry = 0.69;
public final static double armGrabBlueberrySkyhook = 0.05;
public final static double wristGrabBlueberrySkyhook = 0.1;
public final static double armGrabBlueberrySkyhook = 0.045;
public final static double wristGrabBlueberrySkyhook = 0.08;
public final static double armHangBlueberrySkyhook = 0.18;
public final static double wristHangBlueberrySkyhook = 0;
public final static int slideHangBlueberrySkyhook = 500;
@ -26,7 +28,7 @@ public class RobotConstants {
public final static double armBucket = 0.45;
public final static double armSpecimen = 0.155;
public final static double armInit = 0.13;
public final static double wristInit = 0.125;
public final static double wristInit = 0;
public final static double wristPickup = 0.425;
public final static double wristBucket = 0.56;
public final static double wristSpecimenPrep = 0.63;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ArmSubsystem.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.subsystem;
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.PedroConstants.ARM_SERVO;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ClawSubsystem.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.subsystem;
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.PedroConstants.CLAW_NAME;
import static org.firstinspires.ftc.teamcode.PedroConstants.THUMB_SERVO;

41
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DualMotorSliderSubsystem.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.subsystem;
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
@ -43,7 +43,10 @@ public class DualMotorSliderSubsystem {
a "getter" to get the target position value.
*/
public void setTargetPosition(int value) {
targetPosition = value;
if(value >= liftToHighBucketPos)
targetPosition = liftToHighBucketPos;
else if(targetPosition <= liftToHighBucketPos)
targetPosition = value;
}
private int getTargetPosition() { return targetPosition; }
@ -79,6 +82,9 @@ public class DualMotorSliderSubsystem {
public int getCurrentPosition(){
return liftSlideLeft.getCurrentPosition();
}
public double getCurrentPower(){
return liftSlideLeft.getPower();
}
public void init() {
/*
Initialize the motors with the following settings (assuming slide is at the very bottom position):
@ -91,7 +97,8 @@ public class DualMotorSliderSubsystem {
liftSlideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftSlideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
liftSlideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
liftSlideLeft.setDirection(DcMotorSimple.Direction.REVERSE);
//rotation is different because spools spin opposite directions now
liftSlideLeft.setDirection(DcMotorSimple.Direction.FORWARD);
liftSlideRight.setDirection(DcMotorSimple.Direction.REVERSE);
liftSlideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
liftSlideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@ -108,8 +115,12 @@ public class DualMotorSliderSubsystem {
*/
public void update() {
double power = calculatePower();
if(power < 0.1 && power > 0.05){
power = 0.12;
}
liftSlideLeft.setPower(power);
liftSlideRight.setPower(power);
}
/*
@ -124,6 +135,9 @@ public class DualMotorSliderSubsystem {
*/
private double calculatePower() {
double error = getTargetPosition() - liftSlideLeft.getCurrentPosition();
if(error < 20 && error > 0){
error = 20;
}else if(error > -20 && error < 0)
integralSum += error * timer.seconds();
double derivative = (error - lastError) / timer.seconds();
lastError = error;
@ -142,6 +156,27 @@ public class DualMotorSliderSubsystem {
public void toFixedPosition(int value) {
setTargetPosition(value);
}
/**
* go to a specified position (measure in CM)
* Measured from the top of the left Slide
*At the floor, measurement is 34 cm
* @param centimeters height you want to go to, lowest is 34
* UNFINISHED
*/
public void toCentimeterMeasurement(int centimeters){
int ticksPerCm = 60;
int HeightWhenAtFloor = 34;
if(centimeters > 0) {
liftSlideLeft.setTargetPosition((centimeters+ 34)* ticksPerCm);
liftSlideRight.setTargetPosition((centimeters+ 34) * ticksPerCm);
}
}
public int getCM(){
int ticksPerCm = 60;
int HeightWhenAtFloor = 34;
return 34 +(liftSlideLeft.getCurrentPosition()/ticksPerCm);
}
public void toHangHeight(){setTargetPosition(slideHangBlueberrySkyhook);}
public void toHangBelowFloor(){
setTargetPosition(slideBelowFloor);

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HangMotorSubsystem.java
View File

@ -1,8 +1,6 @@
package org.firstinspires.ftc.teamcode.subsystem;
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.PedroConstants.HOOK;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
@ -23,6 +21,7 @@ public class HangMotorSubsystem {
return Return;
}
public void init(){
//init hook, set runmodes
hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
hang.setDirection(DcMotorSimple.Direction.FORWARD);
hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@ -43,7 +42,12 @@ public class HangMotorSubsystem {
hang.setPower(0);
}
public void setPower(double power){
public void setPowerForward(double power){
hang.setDirection(DcMotorSimple.Direction.FORWARD);
hang.setPower(power);
}
public void setPowerReverse(double power){
hang.setDirection(DcMotorSimple.Direction.REVERSE);
hang.setPower(power);
}
public void stopMotor(){

25
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HookTest.java
View File

@ -1,15 +1,9 @@
package org.firstinspires.ftc.teamcode.subsystem;
package org.firstinspires.ftc.teamcode.subsystems;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
@TeleOp(name = "Hook Test")
@TeleOp(name = "Hook Test", group = "Debug")
public class HookTest extends OpMode {
private HangMotorSubsystem skyhook;
@ -81,10 +75,19 @@ public class HookTest extends OpMode {
if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) {
skyhook.hangRobot();
}
skyhook.setPower(0);
if (gamepad1.dpad_down) {
if (gamepad1.left_trigger > 0) {
skyhook.disableMotor();
skyhook.setPower(1);
skyhook.setPowerReverse(1.0);
}
else if(gamepad1.left_trigger == 0){
skyhook.setPowerReverse(0.0);
}
if (gamepad1.right_trigger > 0) {
skyhook.disableMotor();
skyhook.setPowerForward(1.0);
}
else if(gamepad1.right_trigger == 0){
skyhook.setPowerForward(0.0);
}

58
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Inspection.java Normal file
View File

@ -0,0 +1,58 @@
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.HangMotorSubsystem;
import org.firstinspires.ftc.teamcode.subsystems.WristSubsystem;
@TeleOp(name = "Inspection", group = "Debug")
public class Inspection extends OpMode {
private DualMotorSliderSubsystem dualSlides;
private ClawSubsystem claw;
private WristSubsystem wrist;
private ArmSubsystem arm;
private HangMotorSubsystem skyhook;
@Override
public void init() {
dualSlides = new DualMotorSliderSubsystem(hardwareMap);
claw = new ClawSubsystem(hardwareMap);
arm = new ArmSubsystem(hardwareMap);
wrist = new WristSubsystem(hardwareMap);
skyhook = new HangMotorSubsystem(hardwareMap);
dualSlides.init();
claw.init();
wrist.initTeleOp();
arm.initTeleOp();
skyhook.init();
}
@Override
public void loop() {
telemetry.addData("a: ", "init");
telemetry.addData("b: ", "armFloor/WristFloor");
telemetry.addData("y: ", "GrabSkyhook");
if(gamepad1.a){
wrist.InitAuto();
arm.initAuto();
}
if(gamepad1.b){
arm.toFloorPosition();
wrist.toFloorPositionTeleop();
}
if (gamepad1.y) {
claw.grabBlueberry();
arm.setPosition(0.15);
arm.grabBlueberrySkyhook();
//claw Open small amount
wrist.grabBlueberrySkyhook();
//wrist grab in strange way
}
}
}

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LiftArmWrist.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.subsystem;
package org.firstinspires.ftc.teamcode.subsystems;
/* Copyright (c) 2021 FIRST. All rights reserved.
*
@ -29,19 +29,11 @@ package org.firstinspires.ftc.teamcode.subsystem;
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.SleepAction;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
@TeleOp(name = "Lift Wrist Arm Test", group = "Debug")
public class LiftArmWrist extends LinearOpMode {
@ -118,12 +110,7 @@ public class LiftArmWrist extends LinearOpMode {
lift.setTargetPosition(lift.getCurrentPosition() - 250);
}
if(currentGamepad1.left_trigger > 0){
hang.setPower((double) currentGamepad1.left_trigger);
}
if(currentGamepad1.right_trigger > 0){
hang.setPower((double) currentGamepad1.right_trigger);
}
lift.update();

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MotorsSubsystem.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.subsystem;
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
@ -10,12 +10,7 @@ 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.MAX_POWER;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
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;

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/WristSubsystem.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.subsystem;
package org.firstinspires.ftc.teamcode.subsystems;
import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_SERVO;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBackwardBucket;
@ -8,7 +8,6 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristGrabBlu
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlueberrySkyhook;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenHang;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpecimenPrep;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristtravel;
@ -17,7 +16,6 @@ import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.ServoImplEx;
@ -68,7 +66,7 @@ public class WristSubsystem {
public void toFloorPositionTeleop() {
setState(WristState.FLOOR);
wrist.setPosition(.425);
wrist.setPosition(0.69);
}
public void toBucketPosition() {
@ -126,7 +124,7 @@ public class WristSubsystem {
public void initTeleOp() {
wrist.resetDeviceConfigurationForOpMode();
toPickupPosition();
toFloorPositionTeleop();
}
public void setPosition(double position) {