Skip to content
This repository was archived by the owner on Sep 12, 2023. It is now read-only.

Commit 712d939

Browse files
11981591198159
committed
codeeeee
1 parent 5231c4d commit 712d939

40 files changed

+510
-107
lines changed

‎.idea/misc.xml

+1-1
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎OspreyCode/src/main/java/org/firstinspires/ftc/teamcode/BaseControls.java

+21-9
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,11 @@
99

1010
import org.firstinspires.ftc.teamcode.commands.arm.ArmRaiseInCommand;
1111
import org.firstinspires.ftc.teamcode.commands.arm.ArmSharedCommand;
12+
import org.firstinspires.ftc.teamcode.commands.autonomous.AutoDepositAllianceCommand;
13+
import org.firstinspires.ftc.teamcode.commands.autonomous.TeleopDepositAllianceCommand;
14+
import org.firstinspires.ftc.teamcode.commands.autonomous.TeleopDepositSharedCommand;
15+
import org.firstinspires.ftc.teamcode.commands.autonomous.TeleopIntakeAllianceWarehouseCommand;
16+
import org.firstinspires.ftc.teamcode.commands.autonomous.TeleopIntakeSharedWarehouseCommand;
1217
import org.firstinspires.ftc.teamcode.commands.cap.CapDownCommand;
1318
import org.firstinspires.ftc.teamcode.commands.carousel.CarouselLeftCommand;
1419
import org.firstinspires.ftc.teamcode.commands.carousel.CarouselRightCommand;
@@ -100,6 +105,8 @@ public BaseControls(Robot r, CommandGamepad driver, CommandGamepad codriver, boo
100105

101106
RobotConstants.setStrategy(RobotConstants.AllianceHubStrategy.HIGH, RobotConstants.SharedHubStrategy.OWN);
102107

108+
109+
103110
if(bind) bindControls();
104111

105112

@@ -121,32 +128,37 @@ public void bindControls(){
121128
public void bindArmControls() {
122129
dumpAxis.whilePressedOnce(new BucketDumpVariableCommand(robot.armSubsystem, dumpAxis).asConditional(EXTENSION_CONNECTED ? robot.extensionSubsystem::isSlideOut : ()->true));
123130
toIntakeButton.whenPressed(new WaitCommand(0).andThen(new ArmRaiseInCommand(robot.armSubsystem).andThen(new ArmInCommand(robot.armSubsystem)).withTimeout(1.5)));
124-
allianceHubButton.whenPressed( new ArmAllianceCommand(robot.armSubsystem));
125-
sharedHubButton.whenPressed(new ArmSharedCommand(robot.armSubsystem));
131+
allianceHubButton.whileReleasedOnce( new ArmAllianceCommand(robot.armSubsystem).asConditional(RobotConstants::isDepositing));
132+
sharedHubButton.whileReleasedOnce(new ArmSharedCommand(robot.armSubsystem).asConditional(RobotConstants::isDepositing));
126133

127134
}
128135

129136
public void bindLiftControls() {
130-
sharedHubButton.whenPressed(new WaitCommand(0.3).andThen(new LiftSharedCommand(robot.liftSubsystem).withTimeout(0.5)));
131-
allianceHubButton.whenPressed(new WaitCommand(0.3).andThen(new LiftLevelCommand(robot.liftSubsystem).withTimeout(0.5)));
137+
sharedHubButton.whileReleasedOnce(new WaitCommand(0.3).andThen(new LiftSharedCommand(robot.liftSubsystem).withTimeout(0.5)).asConditional(RobotConstants::isDepositing));
138+
allianceHubButton.whileReleasedOnce(new WaitCommand(0.3).andThen(new LiftLevelCommand(robot.liftSubsystem).withTimeout(0.5)).asConditional(RobotConstants::isDepositing));
132139
toIntakeButton.whenPressed(new LiftLevel1Command(robot.liftSubsystem).withTimeout(0.8).andThen(new LiftCollectCommand(robot.liftSubsystem).withTimeout(0.4)));
133140
liftAdjustUpButton.whilePressed(new LiftTranslateCommand(robot.liftSubsystem, 50));
134141
liftAdjustDownButton.whilePressed(new LiftTranslateCommand(robot.liftSubsystem, -50));
135142
}
136143

137144
public void bindDriveControls() {
145+
if(EXTENSION_CONNECTED && DEPOSIT_CONNECTED && LIFT_CONNECTED && INTAKE_CONNECTED){
146+
allianceHubButton.whilePressed(new TeleopDepositAllianceCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem).andThen(new TeleopIntakeAllianceWarehouseCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem)));
147+
sharedHubButton.whilePressed(new TeleopDepositSharedCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem).andThen(new TeleopIntakeSharedWarehouseCommand(robot.drivebaseSubsystem, robot.intakeSubsystem, robot.liftSubsystem, robot.armSubsystem, robot.extensionSubsystem)));
148+
}
138149
robot.drivebaseSubsystem.setDefaultCommand(new DriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick));
150+
robot.drivebaseSubsystem.setExternalHeading(Math.toRadians(RobotConstants.getAlliance().selectOf(-90, 90)));
139151
allianceHubButton.whenPressed(new DriveSpeedCommand(robot.drivebaseSubsystem).cancelUpon(toIntakeButton));
140152
resetGyroButton.whenPressed(new DriveResetCommand(robot.drivebaseSubsystem));
141153
snailSpeedButton.whilePressedOnce(new DriveSpeedCommand(robot.drivebaseSubsystem));
142154
}
143155

144156
public void bindIntakeControls() {
145-
toIntakeButton.whenPressed(new WaitCommand(1.3).andThen(new IntakeInCommand(robot.intakeSubsystem)));
157+
toIntakeButton.whenPressed(new WaitCommand(1.5).andThen(new IntakeInCommand(robot.intakeSubsystem)));
146158
intakeInButton.whilePressedContinuous(new IntakeInCommand(robot.intakeSubsystem));
147159
intakeOutButton.whilePressedOnce(new IntakeOutCommand(robot.intakeSubsystem));
148-
allianceHubButton.whenPressed(new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.2));
149-
sharedHubButton.whenPressed(new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.2));
160+
allianceHubButton.whenReleased(new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.2));
161+
sharedHubButton.whenReleased(new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.2));
150162

151163
}
152164

@@ -164,8 +176,8 @@ public void bindCapControls() {
164176
}
165177

166178
public void bindExtensionControls() {
167-
allianceHubButton.whenPressed(new ExtensionCommand(robot.extensionSubsystem, ExtensionSubsystem.ExtensionConstants.TELEOP_ALLIANCE, ExtensionSubsystem.ExtensionConstants.CENTER));
168-
sharedHubButton.whenPressed(new ExtensionSideCommand(robot.extensionSubsystem));
179+
allianceHubButton.whileReleasedOnce(new ExtensionCommand(robot.extensionSubsystem, ExtensionSubsystem.ExtensionConstants.TELEOP_ALLIANCE, ExtensionSubsystem.ExtensionConstants.CENTER).asConditional(RobotConstants::isDepositing));
180+
sharedHubButton.whileReleasedOnce(new ExtensionSideCommand(robot.extensionSubsystem).asConditional(RobotConstants::isDepositing));
169181
toIntakeButton.whenPressed(new ExtensionCollectCommand(robot.extensionSubsystem));
170182
turretAdjustLeftButton.whilePressed(new TurretTranslateCommand(robot.extensionSubsystem, -0.05, ()-> DRIVE_CONNECTED && robot.drivebaseSubsystem.getExternalHeading() > ExtensionSubsystem.ExtensionConstants.SNAP_1 && robot.drivebaseSubsystem.getExternalHeading() < ExtensionSubsystem.ExtensionConstants.SNAP_2));
171183
turretAdjustRightButton.whilePressed(new TurretTranslateCommand(robot.extensionSubsystem, 0.05, ()-> DRIVE_CONNECTED && robot.drivebaseSubsystem.getExternalHeading() > ExtensionSubsystem.ExtensionConstants.SNAP_1 && robot.drivebaseSubsystem.getExternalHeading() < ExtensionSubsystem.ExtensionConstants.SNAP_2));

‎OspreyCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware.java

+7-7
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
import com.technototes.library.hardware.motor.Motor;
77
import com.technototes.library.hardware.sensor.IMU;
88
import com.technototes.library.hardware.sensor.IMU.AxesSigns;
9-
import com.technototes.library.hardware.sensor.RangeSensor;
9+
import com.technototes.library.hardware.sensor.Rev2MDistanceSensor;
1010
import com.technototes.library.hardware.servo.Servo;
1111
import com.technototes.library.logger.Loggable;
1212
import com.technototes.vision.hardware.Webcam;
@@ -65,9 +65,9 @@ public static class HardwareConstants{
6565
public EncodedMotor<DcMotorEx> rlDriveMotor;
6666
public EncodedMotor<DcMotorEx> rrDriveMotor;
6767
public IMU imu;
68-
public RangeSensor leftRangeSensor;
69-
public RangeSensor rightRangeSensor;
70-
public RangeSensor frontRangeSensor;
68+
public Rev2MDistanceSensor leftRangeSensor;
69+
public Rev2MDistanceSensor rightRangeSensor;
70+
public Rev2MDistanceSensor frontRangeSensor;
7171
public Motor<DcMotorEx> intakeMotor;
7272

7373
public Motor<DcMotorEx> carouselMotor;
@@ -94,9 +94,9 @@ public Hardware() {
9494
rlDriveMotor = new EncodedMotor<>(RL_MOTOR);
9595
rrDriveMotor = new EncodedMotor<>(RR_MOTOR);
9696
imu = new IMU(HardwareConstants.IMU).remapAxes(AxesOrder.YXZ, AxesSigns.NPP);
97-
leftRangeSensor = new RangeSensor(L_RANGE).setDistanceUnit(DistanceUnit.INCH);
98-
rightRangeSensor = new RangeSensor(R_RANGE).setDistanceUnit(DistanceUnit.INCH);
99-
frontRangeSensor = new RangeSensor(F_RANGE).setDistanceUnit(DistanceUnit.INCH);
97+
leftRangeSensor = new Rev2MDistanceSensor(L_RANGE).setDistanceUnit(DistanceUnit.INCH);
98+
rightRangeSensor = new Rev2MDistanceSensor(R_RANGE).setDistanceUnit(DistanceUnit.INCH);
99+
frontRangeSensor = new Rev2MDistanceSensor(F_RANGE).setDistanceUnit(DistanceUnit.INCH);
100100
}
101101
if(CAROUSEL_CONNECTED){
102102
carouselMotor = new Motor<>(CAROUSEL);

‎OspreyCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java

+35-5
Original file line numberDiff line numberDiff line change
@@ -17,8 +17,8 @@ public class RobotConstants {
1717
@Config
1818
public static class AutoRedConstants {
1919
public static ConfigurablePose CYCLE_START = new ConfigurablePose(12, -63, toRadians(90));
20-
public static ConfigurablePose ALLIANCE_HUB = new ConfigurablePose(8, -52, toRadians(125));
21-
public static ConfigurablePose CYCLE_TRENCH = new ConfigurablePose(24, -63.5, toRadians(180));
20+
public static ConfigurablePose ALLIANCE_HUB = new ConfigurablePose(7, -50, toRadians(125));
21+
public static ConfigurablePose CYCLE_TRENCH = new ConfigurablePose(26, -63.5, toRadians(180));
2222
public static ConfigurablePose CYCLE_INTERMEDIATE = new ConfigurablePose(34, -63.5, toRadians(180));
2323
public static ConfigurablePose[] AUTO_WAREHOUSE = new ConfigurablePose[]{
2424
new ConfigurablePose(44, -63.5, toRadians(190)),
@@ -38,14 +38,15 @@ public static class AutoRedConstants {
3838
public static ConfigurablePose BARRIER_PARK = new ConfigurablePose(60, -40, toRadians(180));
3939

4040

41-
public static ConfigurablePose SHARED_TRENCH = new ConfigurablePose(63.5, -30, toRadians(90));
42-
public static ConfigurablePose SHARED_HUB = new ConfigurablePose(63.5, -20, toRadians(90));
41+
public static ConfigurablePose SHARED_TRENCH = new ConfigurablePose(64, -23, toRadians(90));
42+
public static ConfigurablePose SHARED_HUB = new ConfigurablePose(62.5, -17, toRadians(100));
43+
public static ConfigurablePose SHARED_INTAKE = new ConfigurablePose(64, -50, toRadians(85));
4344
}
4445
@Config
4546
public static class AutoBlueConstants {
4647
public static ConfigurablePose CYCLE_START = new ConfigurablePose(12, 63, toRadians(-90));
4748
public static ConfigurablePose ALLIANCE_HUB = new ConfigurablePose(8, 52, toRadians(-125));
48-
public static ConfigurablePose CYCLE_TRENCH = new ConfigurablePose(24, 63.5, toRadians(-180));
49+
public static ConfigurablePose CYCLE_TRENCH = new ConfigurablePose(26, 63.5, toRadians(-180));
4950
public static ConfigurablePose CYCLE_INTERMEDIATE = new ConfigurablePose(34, 63.5, toRadians(-180));
5051
public static ConfigurablePose[] AUTO_WAREHOUSE = new ConfigurablePose[]{
5152
new ConfigurablePose(44, 63.5, toRadians(-190)),
@@ -66,6 +67,8 @@ public static class AutoBlueConstants {
6667

6768
public static ConfigurablePose SHARED_TRENCH = new ConfigurablePose(63.5, 30, toRadians(-90));
6869
public static ConfigurablePose SHARED_HUB = new ConfigurablePose(63.5, 20, toRadians(-90));
70+
public static ConfigurablePose SHARED_INTAKE = new ConfigurablePose(63.5, 50, toRadians(-90));
71+
6972
}
7073

7174
private static Alliance alliance = Alliance.BLUE;
@@ -81,6 +84,7 @@ public static Alliance getAlliance(){
8184
CYCLE_START_SELECT = ()->alliance.selectOf(AutoRedConstants.CYCLE_START, AutoBlueConstants.CYCLE_START).toPose(),
8285
ALLIANCE_HUB_SELECT = ()->alliance.selectOf(AutoRedConstants.ALLIANCE_HUB, AutoBlueConstants.ALLIANCE_HUB).toPose(),
8386
SHARED_HUB_SELECT = ()->alliance.selectOf(AutoRedConstants.SHARED_HUB, AutoBlueConstants.SHARED_HUB).toPose(),
87+
SHARED_INTAKE_SELECT = ()->alliance.selectOf(AutoRedConstants.SHARED_INTAKE, AutoBlueConstants.SHARED_INTAKE).toPose(),
8488
ALLIANCE_TRENCH_SELECT = ()->alliance.selectOf(AutoRedConstants.CYCLE_TRENCH, AutoBlueConstants.CYCLE_TRENCH).toPose(),
8589
CYCLE_INTERMEDIATE_SELECT = ()->alliance.selectOf(AutoRedConstants.CYCLE_INTERMEDIATE, AutoBlueConstants.CYCLE_INTERMEDIATE).toPose(),
8690
SHARED_TRENCH_SELECT = ()->alliance.selectOf(AutoRedConstants.SHARED_TRENCH, AutoBlueConstants.SHARED_TRENCH).toPose(),
@@ -123,9 +127,25 @@ public static Alliance getAlliance(){
123127
.turn(DUCK_INTAKE_END_SELECT.get().getHeading()-DUCK_INTAKE_START_SELECT.get().getHeading())
124128
.lineToLinearHeading(DUCK_INTAKE_END_SELECT.get())
125129
.build(),
130+
HUB_TO_PARK = b->b.apply(ALLIANCE_HUB_SELECT.get())
131+
.setReversed(true)
132+
.setAccelConstraint((a, e, c, d) -> 30)
133+
.splineTo(ALLIANCE_TRENCH_SELECT.get().vec(), 0)
134+
.setAccelConstraint((a, e, c, d) -> 60)
135+
.setVelConstraint((a, e, c, d)->70)
136+
.lineToSplineHeading(CYCLE_INTERMEDIATE_SELECT.get())
137+
.build(),
126138
DUCK_INTAKE_TO_HUB = b -> b.apply(DUCK_INTAKE_END_SELECT.get())
127139
.setAccelConstraint((a, e, c, d) -> 30)
128140
.lineToLinearHeading(DUCK_ALLIANCE_HUB_SELECT.get())
141+
.build(),
142+
SHARED_HUB_TO_WAREHOUSE = b -> b.apply(SHARED_HUB_SELECT.get())
143+
.setReversed(true)
144+
.setAccelConstraint((a, e, c, d) -> 30)
145+
.splineTo(SHARED_TRENCH_SELECT.get().vec(), toRadians(RobotConstants.getAlliance().selectOf(-90, 90)))
146+
.setAccelConstraint((a, e, c, d) -> 60)
147+
.setVelConstraint((a, e, c, d)->70)
148+
.lineToSplineHeading(SHARED_INTAKE_SELECT.get())
129149
.build();
130150

131151

@@ -189,5 +209,15 @@ public enum AllianceHubStrategy {
189209
public enum SharedHubStrategy {
190210
OWN, STEAL;
191211
}
212+
private static boolean depositTarget = false;
213+
public static void startDeposit(){
214+
depositTarget = true;
215+
}
216+
public static void stopDeposit(){
217+
depositTarget = false;
218+
}
219+
public static boolean isDepositing(){
220+
return depositTarget;
221+
}
192222
}
193223

‎OspreyCode/src/main/java/org/firstinspires/ftc/teamcode/commands/arm/ArmInCommand.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ public void execute() {
1717

1818
@Override
1919
public boolean isFinished() {
20-
return getRuntime().seconds()>0.3;
20+
return getRuntime().seconds()>0.6;
2121
}
2222
}
2323

‎OspreyCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/AutoDepositAllianceCommand.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ public AutoDepositAllianceCommand(DrivebaseSubsystem drive, IntakeSubsystem inta
2020
super(new DriveRelocalizeCycleCommand(drive),
2121
new RegenerativeTrajectorySequenceCommand(drive, RobotConstants.WAREHOUSE_TO_HUB, drive)
2222
.alongWith(new IntakeOutCommand(intake).withTimeout(0.5),
23-
new WaitCommand(0.3).andThen(new DriveRelocalizeCycleCommand(drive)),
23+
//new WaitCommand(0.1).andThen(new DriveRelocalizeCycleCommand(drive)),
2424
new WaitCommand(0.8).andThen(new DepositAllianceCommand(deposit, extension, lift))),
2525
new BucketDumpCommand(deposit));
2626
}

‎OspreyCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/AutoParkWarehouseCommand.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212

1313
public class AutoParkWarehouseCommand extends SequentialCommandGroup {
1414
public AutoParkWarehouseCommand(DrivebaseSubsystem drive, LiftSubsystem lift, ArmSubsystem deposit, ExtensionSubsystem extension){
15-
super(new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_WAREHOUSE, 0)
15+
super(new TrajectorySequenceCommand(drive, RobotConstants.HUB_TO_PARK)
1616
.alongWith(new DepositCollectCommand(deposit, extension, lift)));
1717
}
1818
}

‎OspreyCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/TeleopDepositAllianceCommand.java

+44
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
package org.firstinspires.ftc.teamcode.commands.autonomous;
2+
3+
import com.acmerobotics.roadrunner.geometry.Pose2d;
4+
import com.technototes.library.command.SequentialCommandGroup;
5+
import com.technototes.library.command.WaitCommand;
6+
import com.technototes.path.command.RegenerativeTrajectorySequenceCommand;
7+
8+
import org.firstinspires.ftc.teamcode.RobotConstants;
9+
import org.firstinspires.ftc.teamcode.commands.arm.BucketDumpCommand;
10+
import org.firstinspires.ftc.teamcode.commands.deposit.DepositAllianceCommand;
11+
import org.firstinspires.ftc.teamcode.commands.drivebase.DriveRelocalizeCycleCommand;
12+
import org.firstinspires.ftc.teamcode.commands.intake.IntakeOutCommand;
13+
import org.firstinspires.ftc.teamcode.subsystems.ArmSubsystem;
14+
import org.firstinspires.ftc.teamcode.subsystems.DrivebaseSubsystem;
15+
import org.firstinspires.ftc.teamcode.subsystems.ExtensionSubsystem;
16+
import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem;
17+
import org.firstinspires.ftc.teamcode.subsystems.LiftSubsystem;
18+
19+
public class TeleopDepositAllianceCommand extends SequentialCommandGroup {
20+
public DrivebaseSubsystem drivebaseSubsystem;
21+
public TeleopDepositAllianceCommand(DrivebaseSubsystem drive, IntakeSubsystem intake, LiftSubsystem lift, ArmSubsystem deposit, ExtensionSubsystem extension) {
22+
super(new DriveRelocalizeCycleCommand(drive),
23+
new RegenerativeTrajectorySequenceCommand(drive, RobotConstants.WAREHOUSE_TO_HUB, drive)
24+
.alongWith(new IntakeOutCommand(intake).withTimeout(0.5),
25+
//new WaitCommand(0.3).andThen(new DriveRelocalizeCycleCommand(drive)),
26+
new WaitCommand(0.8).andThen(new DepositAllianceCommand(deposit, extension, lift))),
27+
new BucketDumpCommand(deposit));
28+
drivebaseSubsystem = drive;
29+
}
30+
31+
@Override
32+
public void initialize() {
33+
super.initialize();
34+
RobotConstants.startDeposit();
35+
drivebaseSubsystem.setExternalHeading(drivebaseSubsystem.getExternalHeading()+Math.toRadians(RobotConstants.getAlliance().selectOf(-90, 90)));
36+
}
37+
38+
@Override
39+
public void end(boolean cancel) {
40+
super.end(cancel);
41+
drivebaseSubsystem.setExternalHeading(drivebaseSubsystem.getExternalHeading()+ Math.toRadians(RobotConstants.getAlliance().selectOf(90, -90)));
42+
43+
}
44+
}

0 commit comments

Comments
 (0)