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

Fixed floor position issue

Browse Source
This commit is contained in:
carlos
2025-01-14 15:39:17 -08:00
parent 8602dd878b
commit a39332954e
7 changed files with 193 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -78,8 +78,8 @@ public class BlueBasketAuto extends OpMode {
if (runtime.seconds() > 15) {
telemetry.addData("case2", "case2");
new SleepAction(.5);
path2.moveToPath1(follower);
// new SleepAsction(.5);
//path2.moveToPath1(follower);
//For next time, add encoder control to skyhook and extend here
//comp.moveSkyHook();

102
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java
View File

@ -4,6 +4,9 @@ import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
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.util.ElapsedTime;
@ -13,6 +16,8 @@ import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.subsystem.AutoPark;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3;
import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4;
import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
@ -24,6 +29,10 @@ public class ComeBotDriveDevV2 extends OpMode {
private HighBasketAutoPath1 path1;
private HighBasketAutoPath2 path2;
private HighBasketAutoPath3 path3;
private HighBasketAutoPath4 path4;
private AutoPark pathPark;
private SkyHookSubsystem hook;
private CometBotTeleopCompetition comp;
@ -60,6 +69,33 @@ public class ComeBotDriveDevV2 extends OpMode {
case 2:
moveToPathTwoAndPickSampleUp();
break;
case 3:
doPickUpThing();
break;
case 4:
moveToBasketPath3();
break;
case 5:
theArmThing();
break;
case 6:
moveToPickupAgainPath4();
break;
case 7:
doPickUpThingAgain();
break;
case 8:
//line five
break;
case 9:
theArmThingAgain();
break;
case 10:
//line six
break;
case 11:
}
telemetry.update();
follower.update();
@ -74,7 +110,7 @@ public class ComeBotDriveDevV2 extends OpMode {
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 6.0) {
state = 1;
state = 3;
followingPath = false;
}
}
@ -104,6 +140,16 @@ public class ComeBotDriveDevV2 extends OpMode {
state = 2;
}
private void theArmThing(){
comp.highBucketDrop();
state = 6;
}
private void theArmThingAgain(){
comp.highBucketDrop();
state = 10;
}
private void moveToPathTwoAndPickSampleUp() {
if (!followingPath) {
path2.moveToPath2(follower);
@ -111,12 +157,64 @@ public class ComeBotDriveDevV2 extends OpMode {
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 12.0) {
if (follower.atParametricEnd() || runtime.seconds() > 22.0) {
state = 3;
followingPath = false;
}
}
}
private void moveToPickupAgainPath4() {
if (!followingPath) {
path4.moveToPickupAgainPath4(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 12.0) {
state = 7;
followingPath = false;
}
}
}
private void moveToBasketPath3() {
if (!followingPath) {
path3.moveToBasketPath3(follower);
followingPath = true;
}
if (runtime != null) {
telemetry.addData("Runtime (seconds)", runtime.seconds());
if (follower.atParametricEnd() || runtime.seconds() > 12.0) {
state = 5;
followingPath = false;
}
}
}
private void thePickUp(){
Actions.runBlocking(new SequentialAction(
comp.arm.toFloorPosition(),
new SleepAction(.5),
comp.wrist.toPickupPosition(),
new SleepAction(1),
comp.claw.closeClaw(),
new SleepAction(1),
comp.wrist.toFloorPosition(),
new SleepAction(0.5),
comp.arm.toParkPosition()
));
}
private void doPickUpThing(){
thePickUp();
if(!follower.isBusy()) {
state = 4;
}
}
private void doPickUpThingAgain(){
thePickUp();
state = 8;
}
}

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

@ -105,7 +105,7 @@ public class CometBotTeleopCompetition {
this.currentGP1.copy(this.GP1);
this.previousGP2.copy(currentGP2);
this.currentGP2.copy(this.GP2);
this.moveSkyHook();
this.toHighBucketScore();
this.toLowBucketScore();
this.toArmParkPosition();
@ -157,6 +157,18 @@ public class CometBotTeleopCompetition {
this.follower.setMaxPower(this.currentPower);
}
}
public void moveSkyHook() {
if (this.currentGP2.dpad_down) {
hook.moveSkyHook(-1.00);
}
else if (this.currentGP2.dpad_up) {
hook.moveSkyHook(1.00);
}
else{
hook.moveSkyHook(0.00);
}
}
/*
Type: PS4 / Logitech

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

@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class RobotConstants {
public final static double clawClose = 0.8;
public final static double clawOpen = 0.05;
public final static double clawOpen = 0.15;
public final static double armFloor = 0.7;
public final static double armSubmarine = 0.55;
@ -20,7 +20,7 @@ public class RobotConstants {
public final static double wristSpeciemen = 0.1;
public final static int liftToFloorPos = 550;
public final static int liftToFloorPos = 500;
public final static int liftToLowBucketPos = 2650;
public final static int liftToHighRung = 2100;
public final static int dropToHighRung = 1675;

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

@ -4,6 +4,7 @@ package org.firstinspires.ftc.teamcode.subsystem;
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.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@ -15,19 +16,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
*/
public class HighBasketAutoPath2 {
public void moveToPath1(Follower robot) {
public void moveToPath2(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 2
new BezierCurve(
// Line 1
new BezierLine(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(85.000, 132.750, Point.CARTESIAN),
new Point(84.000, 97.000, Point.CARTESIAN)
new Point(29.000, 120.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(135));
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
pathChain = builder.build();
robot.followPath(pathChain);
}

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java Normal file
View File

@ -0,0 +1,36 @@
package org.firstinspires.ftc.teamcode.subsystem;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class HighBasketAutoPath3 {
public void moveToBasketPath3(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(29.000, 120.000, Point.CARTESIAN),
new Point(18.000, 126.000, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
pathChain = builder.build();
robot.followPath(pathChain);
}
}

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath4.java Normal file
View File

@ -0,0 +1,34 @@
package org.firstinspires.ftc.teamcode.subsystem;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
/*
AutoLine# - This file does something of a path......
*/
public class HighBasketAutoPath4 {
public void moveToPickupAgainPath4(Follower robot) {
PathChain pathChain;
PathBuilder builder = new PathBuilder();
builder
.addPath(
// Line 1
new BezierLine(
new Point(18.000, 126.000, Point.CARTESIAN),
new Point(29.000, 131.400, Point.CARTESIAN)
)
)
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));
pathChain = builder.build();
robot.followPath(pathChain);
}
}