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
This commit is contained in:
robotics1
2025-01-29 12:40:29 -08:00
parent 44448d642d f70213af27
commit f8f83a1228
5 changed files with 305 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAuto.java Normal file
View File

@ -0,0 +1,72 @@
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
@Autonomous(name = "Specimen Auto", group = "Dev")
public class SpecimenAuto extends OpMode {
private Follower follower;
private int state;
private ElapsedTime runtime;
public void init(){
state = 0;
follower = new Follower(hardwareMap);
follower.setMaxPower(.90);
runtime = new ElapsedTime();
}
@Override
public void loop() {
switch(state) {
case 0:
//path 1
break;
case 1:
//specimen drop
break;
case 2:
//path 2
break;
case 3:
//specimen pick up
break;
case 4:
//path 3
break;
case 5:
//specimen drop
break;
case 6:
//path 4
break;
case 7:
//specimen pick up
break;
case 8:
//path 5
break;
case 9:
//specimen drop
break;
case 10:
//path 6
break;
case 11:
//specimen pickup
break;
case 12:
//path 7
break;
case 13:
//specimen drop
break;
}
}
}

170
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SpecimenAutoLineTest.java Normal file
View File

@ -0,0 +1,170 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
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")
public class SpecimenAutoLineTest extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(8, 60);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.35);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierLine(
new Point(8.000, 60.000, Point.CARTESIAN),
new Point(39.500, 60.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))
.addPath(
// Line 2
new BezierLine(
new Point(39.500, 60.000, Point.CARTESIAN),
new Point(37.000, 60.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 3
new BezierCurve(
new Point(37.000, 60.000, Point.CARTESIAN),
new Point(16.000, 12.000, Point.CARTESIAN),
new Point(80.000, 54.000, Point.CARTESIAN),
new Point(58.000, 23.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 4
new BezierLine(
new Point(58.000, 23.500, Point.CARTESIAN),
new Point(14.000, 23.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 5
new BezierLine(
new Point(14.000, 23.500, Point.CARTESIAN),
new Point(58.000, 23.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 6
new BezierCurve(
new Point(58.000, 23.500, Point.CARTESIAN),
new Point(64.000, 6.000, Point.CARTESIAN),
new Point(14.000, 12.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 7
new BezierCurve(
new Point(14.000, 12.000, Point.CARTESIAN),
new Point(60.000, 14.000, Point.CARTESIAN),
new Point(60.000, 7.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 8
new BezierLine(
new Point(60.000, 7.500, Point.CARTESIAN),
new Point(14.000, 7.500, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 9
new BezierCurve(
new Point(14.000, 7.500, Point.CARTESIAN),
new Point(34.000, 14.250, Point.CARTESIAN),
new Point(19.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 10
new BezierCurve(
new Point(19.000, 24.000, Point.CARTESIAN),
new Point(39.500, 24.000, Point.CARTESIAN),
new Point(19.000, 64.000, Point.CARTESIAN),
new Point(39.500, 64.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 11
new BezierLine(
new Point(39.500, 64.000, Point.CARTESIAN),
new Point(37.000, 64.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 12
new BezierCurve(
new Point(37.000, 64.000, Point.CARTESIAN),
new Point(19.000, 64.000, Point.CARTESIAN),
new Point(37.000, 24.000, Point.CARTESIAN),
new Point(19.000, 24.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 13
new BezierCurve(
new Point(19.000, 24.000, Point.CARTESIAN),
new Point(39.500, 24.000, Point.CARTESIAN),
new Point(19.000, 68.000, Point.CARTESIAN),
new Point(39.500, 68.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(
// Line 14
new BezierLine(
new Point(39.500, 68.000, Point.CARTESIAN),
new Point(37.000, 68.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(180)).build();
follower.followPath(path);
telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
telemetryA.update();
}
@Override
public void loop() {
follower.update();
follower.telemetryDebug(telemetryA);
}
}

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

@ -26,7 +26,7 @@ public class ArmSubsystem {
} }
public void grabBlueberrySkyhook(){ public void grabBlueberrySkyhook(){
setState(ArmState.GRAB_BLUEBERRY_SKYHOOK); setState(ArmState.GRAB_BLUEBERRY_SKYHOOK);
arm.setPosition(armGrabBlueberrySkyhook); arm.setPosition(armGrabBlueberrySkyhook+ 0.3);
} }
public void hangBlueberrySkyhook(){ public void hangBlueberrySkyhook(){
setState(ArmState.HANG_BLUEBERRY_SKYHOOK); setState(ArmState.HANG_BLUEBERRY_SKYHOOK);

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java
View File

@ -15,10 +15,7 @@ public class HangMotorSubsystem {
private DcMotorEx hang; private DcMotorEx hang;
public HangMotorSubsystem(HardwareMap hardwareMap) { public HangMotorSubsystem(HardwareMap hardwareMap) {
hang = hardwareMap.get(DcMotorEx.class, HOOK); hang = hardwareMap.get(DcMotorEx.class, HOOK);
} }
public int getCurrentPosition(){ public int getCurrentPosition(){

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java Normal file
View File

@ -0,0 +1,62 @@
package org.firstinspires.ftc.teamcode.subsystem;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
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;
public class HookTest extends OpMode {
private HangMotorSubsystem skyhook;
private ArmSubsystem arm;
private WristSubsystem wrist;
private ClawSubsystem claw;
private DualMotorSliderSubsystem slides;
@Override
public void init() {
skyhook = new HangMotorSubsystem(hardwareMap);
arm = new ArmSubsystem(hardwareMap);
wrist = new WristSubsystem(hardwareMap);
claw = new ClawSubsystem(hardwareMap);
slides = new DualMotorSliderSubsystem(hardwareMap);
claw.init();
arm.initTeleOp();
wrist.initTeleOp();
slides.init();
}
@Override
public void loop() {
telemetry.addData("arm", arm.getState());
telemetry.addData("wrist", wrist.getState());
telemetry.update();
if(gamepad1.a){
arm.grabBlueberrySkyhook();
telemetry.update();
try {
Thread.sleep(500);
} catch (InterruptedException e) {
throw new RuntimeException(e);
}
//claw Open small amount
wrist.grabBlueberrySkyhook();
//wrist grab in strange way
telemetry.update();
}
if(gamepad1.b){
//after confirmed grab, close claw
claw.closeClaw();
}
if(gamepad1.x){
//now slap on bar, first wrist, then arm, then claw then driver must drive away
}
if(gamepad1.y){
//go up
}
}
}