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

Specimen States written

Browse Source
This commit is contained in:
robotics1
2025-01-29 15:06:38 -08:00
parent f8f83a1228
commit edff580e84
8 changed files with 100 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PreLoadedBlueBasketAuto.java
View File

@ -13,7 +13,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@Autonomous(name = "Specimen Test", group = "Competition")
@Autonomous(name = "PreLoadedBlue", group = "Competition")
public class PreLoadedBlueBasketAuto extends OpMode {
private Telemetry telemetryA;
@ -27,7 +27,7 @@ public class PreLoadedBlueBasketAuto extends OpMode {
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.45);
follower.setMaxPower(.20);
follower.setStartingPose(startPose);

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

@ -1,44 +1,85 @@
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;
@Autonomous(name = "Specimen Auto", group = "Dev")
@Autonomous(name = "Specimen Auto", group = "Development")
public class SpecimenAuto extends OpMode {
private Follower follower;
private int state;
private ElapsedTime runtime;
private DualMotorSliderSubsystem lift;
private ClawSubsystem claw;
private WristSubsystem wrist;
private ArmSubsystem arm;
private HangMotorSubsystem hang;
@Override
public void init(){
state = 0;
follower = new Follower(hardwareMap);
follower.setMaxPower(.90);
runtime = new ElapsedTime();
lift = new DualMotorSliderSubsystem(hardwareMap);
claw = new ClawSubsystem(hardwareMap);
arm = new ArmSubsystem(hardwareMap);
wrist = new WristSubsystem(hardwareMap);
hang = new HangMotorSubsystem(hardwareMap);
follower = new Follower(hardwareMap);
runtime = new ElapsedTime();
state = 0;
lift.init();
claw.init();
wrist.InitAuto();
arm.initAuto();
follower.setMaxPower(.45);
}
@Override
public void loop() {
telemetry.addData("state", state);
switch(state) {
case 0:
//path 1
wrist.toSpecimenPrep();
arm.toSpecimenPrep();
state = 1;
new SleepAction(5);
break;
case 1:
//specimen drop
state = 2;
break;
case 2:
//path 2
lift.toSpecimanHangHeight();
new SleepAction(5);
state = 3;
break;
case 3:
//specimen pick up
wrist.toSpecimenHang();
new SleepAction(5);
state = 4;
break;
case 4:
//path 3
lift.toSpecimanReleaseHeight();
new SleepAction(5);
state = 5;
break;
case 5:
//specimen drop
@ -68,5 +109,7 @@ public class SpecimenAuto extends OpMode {
//specimen drop
break;
}
new SleepAction(5);
}
}

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

@ -21,7 +21,7 @@ public class SpecimenAutoLineTest extends OpMode {
private PathChain path;
private final Pose startPose = new Pose(8, 60);
private final Pose startPose = new Pose(8, 55);
@Override
public void init() {
@ -36,12 +36,12 @@ public class SpecimenAutoLineTest extends OpMode {
.addPath(
// Line 1
new BezierLine(
new Point(8.000, 60.000, Point.CARTESIAN),
new Point(8.000, 55.000, Point.CARTESIAN),
new Point(39.500, 60.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
/* .addPath(
// Line 2
new BezierLine(
new Point(39.500, 60.000, Point.CARTESIAN),
@ -155,7 +155,7 @@ public class SpecimenAutoLineTest extends OpMode {
new Point(37.000, 68.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180)).build();
.setConstantHeadingInterpolation(Math.toRadians(180))*/.build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());

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

@ -19,12 +19,18 @@ public class RobotConstants {
public final static double wristHangBlueberrySkyhook = 0.7;
public final static int slideHangBlueberrySkyhook = 500;
public final static int slideBelowFloor = -150;
public final static int slideSpecimanHang = 900;
public final static int slideSpecimanRelease = 200;
public final static int backwardBucketDrop = 4670;
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 wristPickup = 0.475;
public final static double wristBucket = 0.56;
public final static double wristSpecimenPrep = 0.63;
public final static double wristSpecimenHang = 0.53;
public final static double wristFloor = 0.75;
public final static double wristBackwardBucket = 0.725;

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java
View File

@ -8,6 +8,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueb
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armInit;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armReverseBucket;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armSpecimen;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.ServoImplEx;
@ -15,7 +16,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class ArmSubsystem {
public enum ArmState {
PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET
PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT, REVERE_BUCKET, SPECIMEN
}
private ServoImplEx arm;
@ -54,6 +55,10 @@ public class ArmSubsystem {
arm.setPosition(armBucket);
setState(ArmState.BUCKET);
}
public void toSpecimenPrep() {
arm.setPosition(armSpecimen);
setState(ArmState.SPECIMEN);
}
public void toInitPosition(){
arm.setPosition(armInit);
setState(ArmState.INIT);

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java
View File

@ -7,6 +7,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBu
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideBelowFloor;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideHangBlueberrySkyhook;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideSpecimanHang;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.slideSpecimanRelease;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
@ -149,6 +151,16 @@ public class DualMotorSliderSubsystem {
}
public void toSpecimanHangHeight(){
setTargetPosition(slideSpecimanHang);
}
public void toSpecimanReleaseHeight(){
setTargetPosition(slideSpecimanRelease);
}
public int getFixedPosition() {
return liftSlideLeft.getCurrentPosition();
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftArmWrist.java
View File

@ -94,11 +94,11 @@ public class LiftArmWrist extends LinearOpMode {
}
if (currentGamepad1.dpad_right && !previousGamepad1.dpad_right) {
lift.toHighBucketPosition();
lift.toSpecimanHangHeight();
}
if (currentGamepad1.dpad_left && !previousGamepad1.dpad_left) {
lift.toLowBucketPosition();
lift.toSpecimanReleaseHeight();
}
if (currentGamepad1.y && !previousGamepad1.y) {
@ -111,11 +111,11 @@ public class LiftArmWrist extends LinearOpMode {
if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
lift.toHighBucketPosition();
lift.setTargetPosition(lift.getCurrentPosition() + 250);
}
if (currentGamepad1.left_bumper && !previousGamepad1.left_bumper) {
lift.toLowBucketPosition();
lift.setTargetPosition(lift.getCurrentPosition() - 250);
}
if(currentGamepad1.left_trigger > 0){

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java
View File

@ -9,6 +9,8 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlu
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;
import androidx.annotation.NonNull;
@ -74,6 +76,16 @@ public class WristSubsystem {
wrist.setPosition(wristBucket);
}
public void toSpecimenPrep() {
setState(WristState.SPECIMEN);
wrist.setPosition(wristSpecimenPrep);
}
public void toSpecimenHang() {
setState(WristState.SPECIMEN);
wrist.setPosition(wristSpecimenHang);
}
public void toPickupPosition() {
setState(WristState.PICKUP);
wrist.setPosition(wristPickup);