diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/Chassis2023.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/Chassis2023.java index 0fc041b..544cd9e 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/Chassis2023.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/Chassis2023.java @@ -28,7 +28,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Chassis2023 extends WestCoastDrive { public static final double RAMP_START_ANGLE = 9.0; diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java index 05d33fa..af27ae1 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/Intake.java @@ -1,15 +1,16 @@ package org.usfirst.frc4904.robot.subsystems; +import org.usfirst.frc4904.standard.subsystems.RequirementsSubsystemBase; import org.usfirst.frc4904.standard.subsystems.motor.SparkMaxMotorSubsystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Intake extends SubsystemBase { +public class Intake extends RequirementsSubsystemBase { public static int DEFAULT_INTAKE_SPEED = -1; // TODO: set value public SparkMaxMotorSubsystem motors; public Intake (SparkMaxMotorSubsystem motors){ //motors has leftmotor and rightmotot + super(motors); this.motors = motors; } public Command setVoltage(double voltage) { diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java index f1957b2..b778a45 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmExtensionSubsystem.java @@ -8,6 +8,7 @@ import org.opencv.core.Mat.Tuple2; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezControl; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezMotion; +import org.usfirst.frc4904.standard.subsystems.RequirementsSubsystemBase; import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem; import edu.wpi.first.math.Pair; @@ -16,9 +17,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class ArmExtensionSubsystem extends SubsystemBase { +public class ArmExtensionSubsystem extends RequirementsSubsystemBase { private final TalonMotorSubsystem motor; private final static double SPOOL_DIAMETER = Units.inchesToMeters(0.75); @@ -44,6 +44,7 @@ public class ArmExtensionSubsystem extends SubsystemBase { * @param motor the motor controller used to extend the arm */ public ArmExtensionSubsystem(TalonMotorSubsystem motor, DoubleSupplier angleDegreesDealer) { + super(motor); this.motor = motor; this.feedforward = new ArmFeedforward(kS, kG, kV); this.angleDealer = angleDegreesDealer; diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java index d6d18fb..32f4b48 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmPivotSubsystem.java @@ -5,18 +5,17 @@ import org.opencv.core.Mat.Tuple2; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezControl; import org.usfirst.frc4904.standard.custom.motioncontrollers.ezMotion; +import org.usfirst.frc4904.standard.subsystems.RequirementsSubsystemBase; import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem; import org.usfirst.frc4904.standard.subsystems.motor.TelescopingArmPivotFeedForward; import edu.wpi.first.math.Pair; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class ArmPivotSubsystem extends SubsystemBase { +public class ArmPivotSubsystem extends RequirementsSubsystemBase { public static final double INITIAL_ARM_ANGLE = -38; public static final double GEARBOX_RATIO = 48; //48:1, 48 rotations of motor = 360 degrees public static final double GEARBOX_SLACK_DEGREES = 6; @@ -41,6 +40,7 @@ public class ArmPivotSubsystem extends SubsystemBase { private final EncoderWithSlack slackyEncoder; public ArmPivotSubsystem(TalonMotorSubsystem armMotorGroup, DoubleSupplier extensionDealer) { + super(armMotorGroup); this.armMotorGroup = armMotorGroup; this.extensionDealer = extensionDealer; this.feedforward = new TelescopingArmPivotFeedForward(kG_retracted, kG_extended, kS, kV, kA); diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java index 135411c..27e5b13 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/ArmSubsystem.java @@ -1,13 +1,14 @@ package org.usfirst.frc4904.robot.subsystems.arm; +import org.usfirst.frc4904.standard.subsystems.RequirementsSubsystemBase; + import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; -public class ArmSubsystem extends SubsystemBase { +public class ArmSubsystem extends RequirementsSubsystemBase { public final ArmPivotSubsystem armPivotSubsystem; public final ArmExtensionSubsystem armExtensionSubsystem; @@ -18,6 +19,7 @@ public class ArmSubsystem extends SubsystemBase { public static final double MAX_ACCEL_PIVOT = 0; public ArmSubsystem(ArmPivotSubsystem armPivotSubsystem, ArmExtensionSubsystem armExtensionSubsystem) { + super(armPivotSubsystem, armExtensionSubsystem); this.armPivotSubsystem = armPivotSubsystem; this.armExtensionSubsystem = armExtensionSubsystem; } diff --git a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/EncoderWithSlack.java b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/EncoderWithSlack.java index 4fa685c..d2f74cd 100644 --- a/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/EncoderWithSlack.java +++ b/src/main/java/org/usfirst/frc4904/robot/subsystems/arm/EncoderWithSlack.java @@ -2,10 +2,11 @@ import java.util.function.DoubleSupplier; +import org.usfirst.frc4904.standard.subsystems.RequirementsSubsystemBase; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class EncoderWithSlack extends SubsystemBase { // just extends SubsystemBase so we can have a periodic +public class EncoderWithSlack extends RequirementsSubsystemBase { // just extends SubsystemBase so we can have a periodic public final DoubleSupplier encoderRevsDealer; public final double slackWindow; public final double unitsPerRevolution; diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 738e9ae..7921ed2 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 738e9ae78c91679048bf9aa92a45df2386a1bd11 +Subproject commit 7921ed23cd118759c591da284c05034201f6149d