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

Commit e1259aa

Browse files
11981591198159
committed
stuff and instance hardware because mucho buggos
1 parent 030be2d commit e1259aa

36 files changed

+515
-122
lines changed

‎RobotLibrary/src/main/java/com/technototes/library/command/Command.java‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -130,12 +130,13 @@ default void run() {
130130
switch (getState()) {
131131
case RESET:
132132
getRuntime().reset();
133-
setState(CommandState.INITILAIZING);
133+
setState(CommandState.INITIALIZING);
134134
return;
135-
case INITILAIZING:
135+
case INITIALIZING:
136136
init();
137137
setState(CommandState.EXECUTING);
138138
//THERE IS NO RETURN HERE SO IT FALLS THROUGH TO POST-INITIALIZATION
139+
return;
139140
case EXECUTING:
140141
execute();
141142
if(isFinished()) setState(CommandState.FINISHED);
@@ -152,7 +153,7 @@ default void run() {
152153
*
153154
*/
154155
enum CommandState {
155-
RESET, INITILAIZING, EXECUTING, FINISHED, CANCELLED
156+
RESET, INITIALIZING, EXECUTING, FINISHED, CANCELLED
156157
}
157158

158159

@@ -195,7 +196,7 @@ default boolean justFinishedNoCancel(){
195196
return getState() == CommandState.FINISHED;
196197
}
197198
default boolean justStarted() {
198-
return getState() == CommandState.INITILAIZING;
199+
return getState() == CommandState.INITIALIZING;
199200
}
200201

201202

‎RobotLibrary/src/main/java/com/technototes/library/command/CommandScheduler.java‎

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -130,7 +130,6 @@ public void run() {
130130
for (Command c1 : commandMap.keySet()) {
131131
if (c1.justStarted()) {
132132
for (Subsystem s : c1.getRequirements()) {
133-
//ITS SAFE
134133
for (Command c2 : requirementMap.get(s)) {
135134
if (c1 != c2) c2.cancel();
136135
}
@@ -142,10 +141,7 @@ public void run() {
142141
requirementMap.keySet().forEach(Subsystem::periodic);
143142
}
144143

145-
public Command run(Command command, BooleanSupplier supplier) {
144+
public void run(Command command, BooleanSupplier supplier) {
146145
if (supplier.getAsBoolean() || command.isRunning()) command.run();
147-
return command;
148146
}
149-
150-
151147
}

‎RobotLibrary/src/main/java/com/technototes/library/hardware/HardwareDevice.java‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,8 +32,7 @@ protected HardwareDevice(String deviceName) {
3232
}
3333

3434
/** Get encapsulated device
35-
*
36-
* @return The device
35+
* @return The device
3736
*/
3837
public T getDevice() {
3938
return device;

‎RobotLibrary/src/main/java/com/technototes/library/structure/CommandOpMode.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@ public double getOpModeRuntime(){
5050

5151
@Override
5252
public final void runOpMode() {
53+
opModeState = OpModeState.INIT;
5354
driverGamepad = new CommandGamepad(gamepad1);
5455
codriverGamepad = new CommandGamepad(gamepad2);
5556
HardwareDevice.hardwareMap = hardwareMap;
@@ -64,6 +65,7 @@ public final void runOpMode() {
6465
logger.initUpdate();
6566
}
6667
opModeState = OpModeState.RUN;
68+
CommandScheduler.getInstance().run();
6769
uponStart();
6870
while (opModeIsActive() && !terminated && !isStopRequested()) {
6971
runLoop();

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/Controls.java‎

Lines changed: 3 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,10 @@
11
package org.firstinspires.ftc.teamcode;
22

3-
import com.technototes.library.command.CommandScheduler;
43
import com.technototes.library.command.WaitCommand;
54
import com.technototes.library.control.gamepad.CommandAxis;
65
import com.technototes.library.control.gamepad.CommandButton;
76
import com.technototes.library.control.gamepad.CommandGamepad;
87
import com.technototes.library.control.gamepad.Stick;
9-
import com.technototes.library.structure.CommandOpMode;
108

119
import org.firstinspires.ftc.teamcode.commands.carousel.CarouselLeftCommand;
1210
import org.firstinspires.ftc.teamcode.commands.carousel.CarouselRightCommand;
@@ -20,20 +18,17 @@
2018
import org.firstinspires.ftc.teamcode.commands.intake.IntakeInCommand;
2119
import org.firstinspires.ftc.teamcode.commands.intake.IntakeOutCommand;
2220
import org.firstinspires.ftc.teamcode.commands.intake.IntakeSafeCommand;
23-
import org.firstinspires.ftc.teamcode.commands.intake.IntakeStopCommand;
2421
import org.firstinspires.ftc.teamcode.commands.lift.LiftCollectCommand;
2522
import org.firstinspires.ftc.teamcode.commands.lift.LiftLevel1Command;
2623
import org.firstinspires.ftc.teamcode.commands.lift.LiftLevel3Command;
2724
import org.firstinspires.ftc.teamcode.commands.lift.LiftTranslateCommand;
28-
import org.firstinspires.ftc.teamcode.commands.vision.VisionBarcodeCommand;
2925

3026
import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.CAP_CONNECTED;
3127
import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.CAROUSEL_CONNECTED;
3228
import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.DEPOSIT_CONNECTED;
3329
import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.DRIVE_CONNECTED;
3430
import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.INTAKE_CONNECTED;
3531
import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.LIFT_CONNECTED;
36-
import static org.firstinspires.ftc.teamcode.Robot.RobotConstants.VISION_CONNECTED;
3732

3833
public class Controls {
3934

@@ -93,8 +88,8 @@ public Controls(CommandGamepad g, Robot r) {
9388
public void bindDepositControls() {
9489
dumpAxis.whilePressedOnce(new DumpVariableCommand(robot.depositSubsystem, dumpAxis));
9590
toIntakeButton.whilePressedContinuous(new ArmRetractCommand(robot.depositSubsystem));
96-
specificHubButton.whenPressed(new WaitCommand(0.1).andThen(new ArmExtendCommand(robot.depositSubsystem)));
97-
neutralHubButton.whenPressed(new WaitCommand(0.1).andThen(new ArmExtendCommand(robot.depositSubsystem)));
91+
specificHubButton.whenPressed(new ArmExtendCommand(robot.depositSubsystem));
92+
neutralHubButton.whenPressed(new ArmExtendCommand(robot.depositSubsystem));
9893
slideAdjustOutButton.whilePressed(new ArmTranslateCommand(robot.depositSubsystem, -0.01));
9994
slideAdjustInButton.whilePressed(new ArmTranslateCommand(robot.depositSubsystem, 0.01));
10095
}
@@ -115,7 +110,7 @@ public void bindDriveControls() {
115110
}
116111

117112
public void bindIntakeControls() {
118-
toIntakeButton.whilePressedContinuous(DEPOSIT_CONNECTED ? new IntakeSafeCommand(robot.intakeSubsystem, robot.depositSubsystem) : new IntakeInCommand(robot.intakeSubsystem));
113+
toIntakeButton.whilePressedContinuous(DEPOSIT_CONNECTED ? new WaitCommand(1).andThen(new IntakeSafeCommand(robot.intakeSubsystem, robot.depositSubsystem)) : new IntakeInCommand(robot.intakeSubsystem));
119114
intakeInButton.whilePressedContinuous(new IntakeInCommand(robot.intakeSubsystem));
120115
intakeOutButton.whilePressedOnce(new IntakeOutCommand(robot.intakeSubsystem));
121116
specificHubButton.whenPressed(new IntakeOutCommand(robot.intakeSubsystem).withTimeout(0.2));

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

Lines changed: 26 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -8,12 +8,21 @@
88
import com.technototes.library.hardware.sensor.RangeSensor;
99
import com.technototes.library.hardware.servo.Servo;
1010
import com.technototes.library.hardware.servo.ServoGroup;
11+
import com.technototes.library.subsystem.Subsystem;
1112
import com.technototes.vision.hardware.Webcam;
1213

14+
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
1315
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
1416
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
1517
import org.firstinspires.ftc.teamcode.subsystems.DepositSubsystem;
18+
import org.openftc.easyopencv.OpenCvCameraFactory;
19+
import org.openftc.easyopencv.OpenCvWebcam;
1620

21+
import java.lang.reflect.Field;
22+
import java.util.ArrayList;
23+
import java.util.List;
24+
25+
import static com.technototes.library.hardware.HardwareDevice.hardwareMap;
1726
import static org.firstinspires.ftc.teamcode.Hardware.HardwareConstants.ARM;
1827
import static org.firstinspires.ftc.teamcode.Hardware.HardwareConstants.CAMERA;
1928
import static org.firstinspires.ftc.teamcode.Hardware.HardwareConstants.CAP;
@@ -67,32 +76,32 @@ public static class HardwareConstants{
6776

6877
}
6978

70-
public static EncodedMotor<DcMotorEx> liftMotor;
79+
public EncodedMotor<DcMotorEx> liftMotor;
7180

72-
public static Servo dumpLeftServo, dumpRightServo;
73-
public static ServoGroup dumpServos;
74-
public static Servo armServo;
81+
public Servo dumpLeftServo, dumpRightServo;
82+
public ServoGroup dumpServos;
83+
public Servo armServo;
7584

76-
public static EncodedMotor<DcMotorEx> flDriveMotor;
77-
public static EncodedMotor<DcMotorEx> frDriveMotor;
78-
public static EncodedMotor<DcMotorEx> rlDriveMotor;
79-
public static EncodedMotor<DcMotorEx> rrDriveMotor;
80-
public static IMU imu;
81-
public static RangeSensor leftRangeSensor;
82-
public static RangeSensor rightRangeSensor;
85+
public EncodedMotor<DcMotorEx> flDriveMotor;
86+
public EncodedMotor<DcMotorEx> frDriveMotor;
87+
public EncodedMotor<DcMotorEx> rlDriveMotor;
88+
public EncodedMotor<DcMotorEx> rrDriveMotor;
89+
public IMU imu;
90+
public RangeSensor leftRangeSensor;
91+
public RangeSensor rightRangeSensor;
8392

8493

85-
public static Motor<DcMotorEx> intakeMotor;
94+
public Motor<DcMotorEx> intakeMotor;
8695

87-
public static Motor<DcMotorEx> carouselMotor;
96+
public Motor<DcMotorEx> carouselMotor;
8897

89-
public static Webcam camera;
98+
public RangeSensor intakeDistSensor;
9099

91-
public static RangeSensor intakeDistSensor;
100+
public Servo capServo;
92101

93-
public static Servo capServo;
102+
public Webcam camera;
94103

95-
static {
104+
public Hardware() {
96105
if(LIFT_CONNECTED) {
97106
liftMotor = new EncodedMotor<>(LIFT);
98107
}

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -49,19 +49,19 @@ public static class RobotConstants {
4949
@Log.NumberSlider(name = "Cap", color = Color.MAGENTA)
5050
public CapSubsystem capSubsystem;
5151

52-
public Robot(){
53-
if(LIFT_CONNECTED) liftSubsystem = new LiftSubsystem(Hardware.liftMotor);
52+
public Robot(Hardware hardware){
53+
if(LIFT_CONNECTED) liftSubsystem = new LiftSubsystem(hardware.liftMotor);
5454

55-
if(DEPOSIT_CONNECTED) depositSubsystem = new DepositSubsystem(Hardware.dumpServos, Hardware.armServo);
55+
if(DEPOSIT_CONNECTED) depositSubsystem = new DepositSubsystem(hardware.dumpServos, hardware.armServo);
5656

57-
if(DRIVE_CONNECTED) drivebaseSubsystem = new DrivebaseSubsystem(Hardware.flDriveMotor, Hardware.frDriveMotor, Hardware.rlDriveMotor, Hardware.rrDriveMotor, Hardware.imu, Hardware.leftRangeSensor, Hardware.rightRangeSensor);
57+
if(DRIVE_CONNECTED) drivebaseSubsystem = new DrivebaseSubsystem(hardware.flDriveMotor, hardware.frDriveMotor, hardware.rlDriveMotor, hardware.rrDriveMotor, hardware.imu, hardware.leftRangeSensor, hardware.rightRangeSensor);
5858

59-
if(CAROUSEL_CONNECTED) carouselSubsystem = new CarouselSubsystem(Hardware.carouselMotor);
59+
if(CAROUSEL_CONNECTED) carouselSubsystem = new CarouselSubsystem(hardware.carouselMotor);
6060

61-
if(INTAKE_CONNECTED) intakeSubsystem = new IntakeSubsystem(Hardware.intakeMotor, Hardware.intakeDistSensor);
61+
if(INTAKE_CONNECTED) intakeSubsystem = new IntakeSubsystem(hardware.intakeMotor, hardware.intakeDistSensor);
6262

63-
if(VISION_CONNECTED) visionSubsystem = new VisionSubsystem(Hardware.camera);
63+
if(VISION_CONNECTED) visionSubsystem = new VisionSubsystem(hardware.camera);
6464

65-
if(CAP_CONNECTED) capSubsystem = new CapSubsystem(Hardware.capServo);
65+
if(CAP_CONNECTED) capSubsystem = new CapSubsystem(hardware.capServo);
6666
}
6767
}

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/AutonomousConstants.java‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -14,23 +14,23 @@
1414
public class AutonomousConstants {
1515
public static class RedConstants {
1616
public static Pose2d CYCLE_START = new Pose2d(12, -63, toRadians(90));
17-
public static Pose2d CYCLE_DEPOSIT = new Pose2d(0, -43, toRadians(110));
17+
public static Pose2d CYCLE_DEPOSIT = new Pose2d(-4, -43, toRadians(110));
1818
public static Pose2d GAP = new Pose2d(30, -64, toRadians(0));
1919
public static Pose2d CYCLE_COLLECT = new Pose2d(45, -64, toRadians(180));
2020
public static Pose2d DUCK_START = new Pose2d(-36, -63, toRadians(90));
21-
public static Pose2d DUCK_DEPOSIT = new Pose2d(-24, -44, toRadians(60));
22-
public static Pose2d CAROUSEL = new Pose2d(-60, -60, toRadians(0));
21+
public static Pose2d DUCK_DEPOSIT = new Pose2d(-23, -44, toRadians(60));
22+
public static Pose2d CAROUSEL = new Pose2d(-59, -59, toRadians(0));
2323
public static Pose2d PARK = new Pose2d(-62, -36, toRadians(0));
2424

2525
}
2626

2727
public static class BlueConstants {
28-
public static Pose2d CYCLE_START = new Pose2d(0, 63, toRadians(-90));
29-
public static Pose2d CYCLE_DEPOSIT = new Pose2d(0, 44, toRadians(-120));
28+
public static Pose2d CYCLE_START = new Pose2d(12, 63, toRadians(-90));
29+
public static Pose2d CYCLE_DEPOSIT = new Pose2d(-4, 43, toRadians(-110));
3030
public static Pose2d GAP = new Pose2d(30, 64, toRadians(0));
31-
public static Pose2d CYCLE_COLLECT = new Pose2d(52, 64, toRadians(-180));
32-
public static Pose2d DUCK_START = new Pose2d(-24, 63, toRadians(-90));
33-
public static Pose2d DUCK_DEPOSIT = new Pose2d(-24, 44, toRadians(-60));
31+
public static Pose2d CYCLE_COLLECT = new Pose2d(45, 64, toRadians(-180));
32+
public static Pose2d DUCK_START = new Pose2d(-36, 63, toRadians(-90));
33+
public static Pose2d DUCK_DEPOSIT = new Pose2d(-23, 44, toRadians(-60));
3434
public static Pose2d CAROUSEL = new Pose2d(-62, 60, toRadians(-90));
3535
public static Pose2d PARK = new Pose2d(-62, 36, toRadians(0));
3636

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/DepositFreightCommand.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
import org.firstinspires.ftc.teamcode.commands.deposit.ArmExtendCommand;
77
import org.firstinspires.ftc.teamcode.commands.deposit.ArmRetractCommand;
88
import org.firstinspires.ftc.teamcode.commands.deposit.DumpCommand;
9+
import org.firstinspires.ftc.teamcode.commands.intake.IntakeOutCommand;
910
import org.firstinspires.ftc.teamcode.commands.intake.IntakeSafeCommand;
1011
import org.firstinspires.ftc.teamcode.commands.intake.IntakeStopCommand;
1112
import org.firstinspires.ftc.teamcode.commands.lift.LiftCollectCommand;
@@ -18,7 +19,7 @@
1819
public class DepositFreightCommand extends SequentialCommandGroup {
1920
public DepositFreightCommand(DrivebaseSubsystem drive, IntakeSubsystem intake, LiftSubsystem lift, DepositSubsystem deposit){
2021
super(new TrajectorySequenceCommand(drive, AutonomousConstants.CYCLE_COLLECT_TO_DEPOSIT)
21-
.alongWith(new ArmExtendCommand(deposit), new LiftLevel3Command(lift), new IntakeStopCommand(intake)),
22+
.alongWith(new ArmExtendCommand(deposit), new LiftLevel3Command(lift), new IntakeOutCommand(intake).withTimeout(1)),
2223
new DumpCommand(deposit));
2324
}
2425
}

‎TechnoOneCode/src/main/java/org/firstinspires/ftc/teamcode/commands/autonomous/IntakeDepotCommand.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
public class IntakeDepotCommand extends SequentialCommandGroup {
1717
public IntakeDepotCommand(DrivebaseSubsystem drive, IntakeSubsystem intake, LiftSubsystem lift, DepositSubsystem deposit){
1818
super(new TrajectorySequenceCommand(drive, AutonomousConstants.CYCLE_DEPOSIT_TO_COLLECT)
19-
.alongWith(new ArmRetractCommand(deposit), new LiftCollectCommand(lift))
20-
.deadline(new WaitCommand(1).andThen(new IntakeSafeCommand(intake, deposit))));
19+
.alongWith(new ArmRetractCommand(deposit), new WaitCommand(0.5).andThen(new LiftCollectCommand(lift)))
20+
.raceWith(new WaitCommand(1.5).andThen(new IntakeSafeCommand(intake, deposit))));
2121
}
2222
}

0 commit comments

Comments
 (0)