Skip to content

Commit 7280e6a

Browse files
TechnototesLaptopkevinfrei
TechnototesLaptop
authored and
kevinfrei
committed
harshini 12/12/24
made a isfinished command thingy for arm as well, it works!! auto is not finished at all :(( intake compound command is not working for some reason, i need to figure this out still a huge work in progress :DD
1 parent fa5e6ac commit 7280e6a

File tree

9 files changed

+294
-210
lines changed

9 files changed

+294
-210
lines changed

‎Twenty403/src/main/java/org/firstinspires/ftc/twenty403/AutoConstants.java‎

Lines changed: 173 additions & 202 deletions
Large diffs are not rendered by default.

‎Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/ArmSubCmds.java‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
package org.firstinspires.ftc.twenty403.commands;
22

33
import com.technototes.library.command.Command;
4+
import org.firstinspires.ftc.twenty403.commands.driving.MoveArmCommand;
45
import org.firstinspires.ftc.twenty403.subsystems.ArmSubsystem;
56

67
public class ArmSubCmds {
@@ -23,12 +24,20 @@ public static Command highbasketArm(ArmSubsystem AS) {
2324
return Command.create(AS::highBasket);
2425
}
2526

27+
public static Command highbasketArmWaitTillDone(ArmSubsystem AS) {
28+
return new MoveArmCommand(AS, AS::highBasket);
29+
}
30+
2631
public static Command lowbasketArm(ArmSubsystem AS) {
2732
return Command.create(AS::lowBasket);
2833
}
2934

3035
public static Command intakePos(ArmSubsystem AS) {
3136
return Command.create(AS::setArmToIntake);
3237
}
38+
39+
public static Command intakePosSlides(ArmSubsystem AS) {
40+
return Command.create(AS::setSlidesToIntake);
41+
}
3342
}
3443
}

‎Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/HighBasketCommand.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -21,8 +21,8 @@ public static SequentialCommandGroup HighBasket(Robot r) {
2121
KidShampooCmds.cmds.StopIntake(r.kidShampooSubsystem),
2222
KidShampooCmds.cmds.ScoopWrist(r.kidShampooSubsystem),
2323
ArmSubCmds.cmds.slideZero(r.armSubsystem),
24-
ArmSubCmds.cmds.highbasketArm(r.armSubsystem),
25-
new WaitCommand(1),
24+
ArmSubCmds.cmds.highbasketArmWaitTillDone(r.armSubsystem),
25+
//new WaitCommand(1),
2626
ArmSubCmds.cmds.highbasketSlide(r.armSubsystem),
2727
new WaitCommand(1.2),
2828
HighBasketPreArm(r)

‎Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/IntakePositionCommand.java‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,25 @@
11
package org.firstinspires.ftc.twenty403.commands;
22

33
import com.technototes.library.command.SequentialCommandGroup;
4+
import com.technototes.library.command.WaitCommand;
45
import org.firstinspires.ftc.twenty403.Robot;
56

67
public class IntakePositionCommand {
78

8-
public static SequentialCommandGroup Intakepos(Robot r) {
9+
public static SequentialCommandGroup IntakePos(Robot r) {
910
return new SequentialCommandGroup(
1011
KidShampooCmds.cmds.OpenRetainer(r.kidShampooSubsystem),
1112
KidShampooCmds.cmds.StopIntake(r.kidShampooSubsystem),
1213
KidShampooCmds.cmds.DumpWrist(r.kidShampooSubsystem),
1314
ArmSubCmds.cmds.slideZero(r.armSubsystem),
1415
ArmSubCmds.cmds.intakePos(r.armSubsystem),
15-
KidShampooCmds.cmds.CloseRetainer(r.kidShampooSubsystem)
16+
new WaitCommand(1),
17+
ArmSubCmds.cmds.intakePosSlides(r.armSubsystem),
18+
new WaitCommand(1),
19+
KidShampooCmds.cmds.CloseRetainer(r.kidShampooSubsystem),
20+
KidShampooCmds.cmds.SlurpIntake(r.kidShampooSubsystem),
21+
new WaitCommand(1),
22+
KidShampooCmds.cmds.StopIntake(r.kidShampooSubsystem)
1623
);
1724
}
1825
}

‎Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/auto/Paths.java‎

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,15 @@
11
package org.firstinspires.ftc.twenty403.commands.auto;
22

3+
import static org.firstinspires.ftc.twenty403.commands.IntakePositionCommand.IntakePos;
4+
35
import com.technototes.library.command.Command;
46
import com.technototes.library.command.SequentialCommandGroup;
57
import com.technototes.library.command.WaitCommand;
68
import com.technototes.path.command.TrajectorySequenceCommand;
79
import org.firstinspires.ftc.twenty403.AutoConstants;
810
import org.firstinspires.ftc.twenty403.Robot;
11+
import org.firstinspires.ftc.twenty403.commands.HighBasketCommand;
12+
import org.firstinspires.ftc.twenty403.commands.IntakePositionCommand;
913

1014
public class Paths {
1115

@@ -75,18 +79,31 @@ public static Command SampleScoring(Robot r) {
7579
r.drivebaseSubsystem,
7680
AutoConstants.START_TO_NETSCORING
7781
)
82+
.andThen(HighBasketCommand.HighBasket(r))
83+
.andThen(new WaitCommand(1))
84+
.andThen(Command.create(r.armSubsystem::setSlideToZero, r.armSubsystem))
85+
.andThen(Command.create(r.armSubsystem::horizontal, r.armSubsystem))
86+
//.andThen(new WaitCommand(1))
7887
.andThen(
7988
new TrajectorySequenceCommand(
8089
r.drivebaseSubsystem,
8190
AutoConstants.NETSCORING_TO_INTAKE1
8291
)
8392
)
93+
.andThen(IntakePositionCommand.IntakePos(r))
94+
.andThen(Command.create(r.armSubsystem::setSlideToZero, r.armSubsystem))
95+
.andThen(new WaitCommand(0.5))
8496
.andThen(
8597
new TrajectorySequenceCommand(
8698
r.drivebaseSubsystem,
8799
AutoConstants.INTAKE1_TO_NETSCORING
88100
)
89101
)
102+
.andThen(new WaitCommand(1))
103+
.alongWith(Command.create(r.armSubsystem::horizontal, r.armSubsystem))
104+
.andThen(
105+
HighBasketCommand.HighBasket(r).andThen(new WaitCommand(1))
106+
/*
90107
.andThen(
91108
new TrajectorySequenceCommand(
92109
r.drivebaseSubsystem,
@@ -109,7 +126,7 @@ public static Command SampleScoring(Robot r) {
109126
new TrajectorySequenceCommand(
110127
r.drivebaseSubsystem,
111128
AutoConstants.INTAKE3_TO_NETSCORING
112-
)
129+
) */
113130
);
114131
}
115132

‎Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/driving/MoveArmCommand.java‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
package org.firstinspires.ftc.twenty403.commands.driving;
2+
3+
import com.technototes.library.command.Command;
4+
import org.firstinspires.ftc.twenty403.subsystems.ArmSubsystem;
5+
6+
public class MoveArmCommand implements Command {
7+
8+
ArmSubsystem sub;
9+
Runnable cmd;
10+
11+
public MoveArmCommand(ArmSubsystem arm, Runnable armCommand) {
12+
addRequirements(arm);
13+
sub = arm;
14+
cmd = armCommand;
15+
}
16+
17+
@Override
18+
public void execute() {
19+
cmd.run();
20+
}
21+
22+
@Override
23+
public boolean isFinished() {
24+
return sub.isArmAtTarget();
25+
}
26+
}

‎Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -145,7 +145,7 @@ public void bindArmControls() {
145145
HighBasket.whenPressed(HighBasketCommand.HighBasket(robot));
146146
//IntakeSample.whenPressed(IntakeSampleCommand.IntakeSample(robot));
147147
// IntakeSpecimen.whenPressed(IntakeSpecimenCommand.IntakeSpecimen(robot));*/
148-
IntakePos.whenPressed(IntakePositionCommand.Intakepos(robot));
148+
IntakePos.whenPressed(IntakePositionCommand.IntakePos(robot));
149149
/*armHorizontal.whenPressed(
150150
Command.create(robot.armSubsystem::horizontal, robot.armSubsystem)
151151
);*/

‎Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/auto/NetScoring.java‎

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
package org.firstinspires.ftc.twenty403.opmodes.auto;
2+
3+
import com.acmerobotics.dashboard.FtcDashboard;
4+
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
5+
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
6+
import com.technototes.library.command.CommandScheduler;
7+
import com.technototes.library.command.SequentialCommandGroup;
8+
import com.technototes.library.structure.CommandOpMode;
9+
import com.technototes.library.util.Alliance;
10+
import org.firstinspires.ftc.twenty403.AutoConstants;
11+
import org.firstinspires.ftc.twenty403.Hardware;
12+
import org.firstinspires.ftc.twenty403.Robot;
13+
import org.firstinspires.ftc.twenty403.commands.auto.Paths;
14+
import org.firstinspires.ftc.twenty403.helpers.StartingPosition;
15+
16+
@Autonomous(name = "NetScoring")
17+
@SuppressWarnings("unused")
18+
public class NetScoring extends CommandOpMode {
19+
20+
public Robot robot;
21+
public Hardware hardware;
22+
23+
@Override
24+
public void uponInit() {
25+
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
26+
hardware = new Hardware(hardwareMap);
27+
robot = new Robot(hardware, Alliance.RED, StartingPosition.Net.Net);
28+
robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.NET_START.toPose());
29+
//CommandScheduler.register(robot.verticalSlidesSubsystem);
30+
CommandScheduler.scheduleForState(
31+
new SequentialCommandGroup(
32+
Paths.SampleScoring(robot),
33+
CommandScheduler::terminateOpMode
34+
),
35+
OpModeState.RUN
36+
);
37+
}
38+
39+
public void uponStart() {
40+
robot.atStart();
41+
}
42+
// public void end() {
43+
// HeadingHelper.savePose(robot.drivebaseSubsystem.getPoseEstimate());
44+
// }
45+
}

‎Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java‎

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,8 @@ public class ArmSubsystem implements Subsystem, Loggable {
3737
public static int ARM_VERTICAL = 3100;
3838
public static int ARM_HORIZONTAL = 1000;
3939
public static int INITIAL_POSITION = 150;
40-
public static int INTAKE_POS;
41-
40+
public static int INTAKE_POS = 301;
41+
public static int INTAKE_POS_SLIDES = 950;
4242
public static int INCREMENT_DECREMENT = 230;
4343
public static int SLIDE_INC_DEC = 250;
4444
public static int SLIDE_MAX_POS_HORIZONTAL = 950; //new tick position
@@ -51,6 +51,7 @@ public class ArmSubsystem implements Subsystem, Loggable {
5151
public static double SLIDE_FEEDFORWARD_GRAVITY_VALUE = 0.3;
5252
public static double SLIDE_FEEDFORWARD_INTAKE_POS = 0;
5353
public static int SLIDE_TARGET_IN_RANGE = 100;
54+
public static int ARM_TARGET_IN_RANGE = 50;
5455
// This is "5 degrees" if our numbers are correct:
5556
public static int ARM_POS_CLOSE_ENOUGH = Math.abs(ARM_HORIZONTAL - ARM_VERTICAL) / 18;
5657

@@ -225,6 +226,10 @@ public boolean isSlidesAtTarget() {
225226
return Math.abs(getCurrentSlidePos() - slideTargetPos) < SLIDE_TARGET_IN_RANGE;
226227
}
227228

229+
public boolean isArmAtTarget() {
230+
return Math.abs(getArmCurrentPos() - armTargetPos) < ARM_TARGET_IN_RANGE;
231+
}
232+
228233
public void slideincrement(double v) {
229234
int newSlidePos = (int) (slideTargetPos + v * SLIDE_INC_DEC);
230235
if (newSlidePos > SLIDE_MAX_POS) {
@@ -306,6 +311,10 @@ public void setArmToIntake() {
306311
setArmPos(INTAKE_POS);
307312
}
308313

314+
public void setSlidesToIntake() {
315+
setArmPos(INTAKE_POS_SLIDES);
316+
}
317+
309318
@Override
310319
public void periodic() {
311320
armPos = getArmCurrentPos();

0 commit comments

Comments
 (0)