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

Working Pre loaded auto! Can score 11 points consistently!

Browse Source
This commit is contained in:
robotics1
2024-11-16 14:02:45 -08:00
parent 707d7b0609
commit 283979afee
9 changed files with 198 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueNonBasketAuto.java Normal file
View File

@ -0,0 +1,155 @@
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 = "Blue Non Basket Auto", group = "Competition")
public class BlueNonBasketAuto extends OpMode {
private Telemetry telemetryA;
private Follower follower;
private PathChain path;
private final Pose startPose = new Pose(8.000, 55.000);
@Override
public void init() {
follower = new Follower(hardwareMap);
follower.setMaxPower(.6);
follower.setStartingPose(startPose);
path = follower.pathBuilder()
.addPath(
// Line 1
new BezierCurve(
new Point(8.000, 55.000, Point.CARTESIAN),
new Point(27.482, 33.750, Point.CARTESIAN),
new Point(62.357, 33.107, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 2
new BezierLine(
new Point(62.357, 33.107, Point.CARTESIAN),
new Point(62.000, 27.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 3
new BezierLine(
new Point(62.000, 27.000, Point.CARTESIAN),
new Point(10.000, 27.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 4
new BezierLine(
new Point(10.000, 27.000, Point.CARTESIAN),
new Point(61.875, 27.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 5
new BezierLine(
new Point(61.875, 27.000, Point.CARTESIAN),
new Point(61.714, 17.357, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 6
new BezierLine(
new Point(61.714, 17.357, Point.CARTESIAN),
new Point(14.464, 17.357, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 7
new BezierLine(
new Point(14.464, 17.357, Point.CARTESIAN),
new Point(61.714, 17.357, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 8
new BezierLine(
new Point(61.714, 17.357, Point.CARTESIAN),
new Point(61.554, 8.000, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 9
new BezierLine(
new Point(61.554, 8.000, Point.CARTESIAN),
new Point(12.536, 8.196, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 10
new BezierCurve(
new Point(12.536, 8.196, Point.CARTESIAN),
new Point(52.071, 19.929, Point.CARTESIAN),
new Point(50.786, 33.750, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 11
new BezierCurve(
new Point(50.786, 33.750, Point.CARTESIAN),
new Point(2.571, 39.375, Point.CARTESIAN),
new Point(20.732, 78.911, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 12
new BezierCurve(
new Point(20.732, 78.911, Point.CARTESIAN),
new Point(24.429, 111.054, Point.CARTESIAN),
new Point(46.929, 121.018, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 13
new BezierCurve(
new Point(46.929, 121.018, Point.CARTESIAN),
new Point(68.143, 116.357, Point.CARTESIAN),
new Point(63.000, 97.714, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).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/PedroConstants.java
View File

@ -26,7 +26,7 @@ public class PedroConstants {
/*
Centricity : true is robot-centric movement; false if field-centric movement
*/
public static final boolean CENTRICITY = false;
public static final boolean CENTRICITY = true;
/*
Motor Max Power

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

@ -35,7 +35,7 @@ public class PreLoadedBlueBasketAuto extends OpMode {
.addPath(
// Line 1
new BezierLine(
new Point(8.036, 89.196, Point.CARTESIAN),
new Point(8, 89, Point.CARTESIAN),
new Point(10.125, 126.804, Point.CARTESIAN)
)
)
@ -87,15 +87,15 @@ public class PreLoadedBlueBasketAuto extends OpMode {
// Line 7
new BezierLine(
new Point(59.625, 126.964, Point.CARTESIAN),
new Point(57.857, 133.071, Point.CARTESIAN)
new Point(57.857, 131.071, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(
// Line 8
new BezierLine(
new Point(57.857, 133.071, Point.CARTESIAN),
new Point(18.964, 134.679, Point.CARTESIAN)
new Point(57.857, 131.071, Point.CARTESIAN),
new Point(18.964, 131.679, Point.CARTESIAN)
)
)
.setConstantHeadingInterpolation(Math.toRadians(0))

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

@ -104,6 +104,8 @@ public class CometBotTeleopCompetition {
this.raiseSkyHook();
this.lowerSkyHook();
Actions.runBlocking(this.lift.toFloorPosition());
follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, CENTRICITY);
follower.update();
@ -244,7 +246,8 @@ public class CometBotTeleopCompetition {
if (this.currentGP2.square && !this.previousGP2.square) {
Actions.runBlocking(new SequentialAction(
this.wrist.toFloorPosition(),
this.arm.toParkPosition()
this.arm.toParkPosition(),
this.lift.toFloorPosition()
));
}
}
@ -272,37 +275,37 @@ public class CometBotTeleopCompetition {
this.wrist.toFloorPosition()
)
);
} else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE) {
} else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE && this.wrist.getState() == WristActionsSubsystem.WristState.FLOOR) {
Actions.runBlocking(
new SequentialAction(
this.arm.toFloorPosition(),
this.wrist.toFloorPosition()
// this.lift.toFloorPosition(),
this.arm.toSubmarinePosition(),
this.wrist.toPickupPosition()
)
);
} else if (this.arm.getState() == ArmActionsSubsystem.ArmState.FLOOR) {
} else if (this.wrist.getState() == WristActionsSubsystem.WristState.PICKUP) {
Actions.runBlocking(
new SequentialAction(
this.arm.toSubmarinePosition(),
// this.arm.toSubmarinePosition(),
this.wrist.toFloorPosition()
)
);
}
}
}
public void raiseSkyHook() {
if (this.currentGP2.dpad_right && !previousGP2.dpad_right) {
hook.raiseHook();
hook.raiseHook(currentGP2.left_trigger);
}
}
public void lowerSkyHook() {
if (this.currentGP2.dpad_left && !previousGP2.dpad_left) {
hook.lowerHook();
hook.lowerHook(currentGP2.right_trigger);
}
}
}

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

@ -10,13 +10,14 @@ public class RobotConstants {
public final static double armFloor = 0.7;
public final static double armSubmarine = 0.55;
public final static double armPark = 0.0;
public final static double armBucket = 0.15;
public final static double armBucket = 0.2;
public final static double wristFloor = 0.3;
public final static double wristBucket = 0;
public final static double wristFloor = 0.55;
public final static double wristBucket = 0.25;
public final static double wristPickup = 0.1;
public final static int liftToFloorPos = 0;
public final static int liftToSubmarinePos = 250;
public final static int liftToFloorPos = 350;
public final static int liftToSubmarinePos = 350;
public final static int liftToLowBucketPos = 2250;
public final static int liftToHighBucketPos = 3850;
public final static double liftPower = .625;

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

@ -68,11 +68,11 @@ public class ClawActionsSubsystem {
}
public void init() {
Actions.runBlocking(closeClaw());
Actions.runBlocking(openClaw());
}
public void start() {
Actions.runBlocking(closeClaw());
Actions.runBlocking(openClaw());
}
public double getPosition() {

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

@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos;
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.liftToSubmarinePos;
import androidx.annotation.NonNull;
@ -52,7 +53,7 @@ public class LiftActionsSubsystem {
}
public Action toFloorPosition() {
return new MoveToPosition(liftToFloorPos, LiftState.FLOOR);
return new MoveToPosition(liftToSubmarinePos, LiftState.FLOOR);
}
public Action toLowBucketPosition() {

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

@ -24,13 +24,13 @@ public class SkyHookSubsystem {
public void raiseHook(){
this.hook.setPower(1.00);
public void raiseHook(double power){
this.hook.setPower(power);
this.hook.setDirection(DcMotorSimple.Direction.FORWARD);
this.setState(SkyHookState.UP);
}
public void lowerHook(){
this.hook.setPower(1.00);
public void lowerHook(double power){
this.hook.setPower(power);
this.hook.setDirection(DcMotorSimple.Direction.REVERSE);
this.setState(SkyHookState.DOWN);
}

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

@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystem;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
import androidx.annotation.NonNull;
@ -14,7 +15,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
public class WristActionsSubsystem {
public enum WristState {
FLOOR, BUCKET
FLOOR, BUCKET, PICKUP
}
public ServoImplEx wrist;
@ -50,6 +51,11 @@ public class WristActionsSubsystem {
return new MoveToPosition(wristBucket, WristState.BUCKET);
}
public Action toPickupPosition() {
return new MoveToPosition(wristPickup, WristState.PICKUP);
}
public void setState(WristState wristState) {
this.state = wristState;
}