diff --git a/swerve100/src/main/java/org/team100/frc2023/commands/Arm/LowerArmToGoal.java b/swerve100/src/main/java/org/team100/frc2023/commands/Arm/LowerArmToGoal.java index fc8a60f6..12678300 100644 --- a/swerve100/src/main/java/org/team100/frc2023/commands/Arm/LowerArmToGoal.java +++ b/swerve100/src/main/java/org/team100/frc2023/commands/Arm/LowerArmToGoal.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand; +// TODO: remove use of ProfiledPIDCommand public class LowerArmToGoal extends ProfiledPIDCommand { private final ArmController arm; diff --git a/swerve100/src/main/java/org/team100/frc2023/commands/Arm/UpperArmToGoal.java b/swerve100/src/main/java/org/team100/frc2023/commands/Arm/UpperArmToGoal.java index d3b854ad..bb6f5aa5 100644 --- a/swerve100/src/main/java/org/team100/frc2023/commands/Arm/UpperArmToGoal.java +++ b/swerve100/src/main/java/org/team100/frc2023/commands/Arm/UpperArmToGoal.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand; +// TODO: remove use of ProfiledPIDCommand public class UpperArmToGoal extends ProfiledPIDCommand { private final ArmController m_arm; diff --git a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/Arm.java b/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/Arm.java deleted file mode 100644 index b28f8f0a..00000000 --- a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/Arm.java +++ /dev/null @@ -1,126 +0,0 @@ -package org.team100.frc2023.subsystems.arm; - -import java.text.DecimalFormat; - -import org.team100.lib.motors.FRCNEO; - -import com.revrobotics.CANSparkMax.IdleMode; - -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.AnalogEncoder; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/** Add your docs here. */ -public class Arm extends SubsystemBase { - - private final AnalogEncoder encoder0 = new AnalogEncoder(5); - private final AnalogEncoder encoder1 = new AnalogEncoder(4); - - public FRCNEO upperArm; - public FRCNEO lowerArm; - - public UpperArm upperArmSubsytem; - public LowerArm lowerArmSubsytem; - - DecimalFormat df; - - public Arm() { - - df = new DecimalFormat("000.00"); - - upperArm = new FRCNEO.FRCNEOBuilder(42) - .withInverted(false) - // .withFeedbackPort(Constants.IndexerConstants.IndexerMotors.IndexerStageOne.FEEDBACK_PORT) - .withSensorPhase(false) - .withTimeout(10) - .withCurrentLimitEnabled(true) - .withCurrentLimit(30) - // .withOpenLoopRampRate(Constants.IndexerConstants.IndexerMotors.IndexerStageOne.OPEN_LOOP_RAMP) - .withPeakOutputForward(0.3) - .withPeakOutputReverse(-0.3) - .withNeutralMode(IdleMode.kBrake) - .withForwardSoftLimitEnabled(false) - .build(); - - lowerArm = new FRCNEO.FRCNEOBuilder(43) - .withInverted(true) - // .withFeedbackPort(Constants.IndexerConstants.IndexerMotors.IndexerStageOne.FEEDBACK_PORT) - .withSensorPhase(false) - .withTimeout(10) - .withCurrentLimitEnabled(true) - .withCurrentLimit(40) - // .withOpenLoopRampRate(Constants.IndexerConstants.IndexerMotors.IndexerStageOne.OPEN_LOOP_RAMP) - .withPeakOutputForward(40) - .withPeakOutputReverse(-0.3) - .withNeutralMode(IdleMode.kBrake) - .build(); - - // encoder1.setDistancePerRotation( 360); - encoder0.setDistancePerRotation(360); - - // encoder0.setPositionOffset(0.15); - - upperArmSubsytem = new UpperArm(() -> getUpperArm(), upperArm); - lowerArmSubsytem = new LowerArm(() -> getLowerArm(), lowerArm); - - SmartDashboard.putData("Arm", this); - - } - - /** - * The angle the upper arm is at (in degrees) - * 90 degrees is pointing directly forward, and 180 degrees is straight up. - * @return upper arm angle - */ - public double getUpperArm() { - double x = (1 - encoder0.getAbsolutePosition()) * 350 + 13; - double formatted = Math.round(x); - - return formatted; - } - - // public double get(){ - // return Double.parseDouble(df.format()); - - // return encoder0.get(); - // } - - public void reset() { - encoder0.reset(); - } - - /** - * The angle the lower arm is at (in degrees) - * 0 degrees is pointing straight up, increasing when the arm is pointing forward. - * Will be negative when the arm is pointing backwards. - * @return lower arm angle - */ - public double getLowerArm() { - double x = (1 - encoder1.getAbsolutePosition()) * 360 - 101; - // double formatted = Math.round(x); - - return x; - } - - public void drive(double x, double y) { - upperArm.drivePercentOutput(x); - lowerArm.drivePercentOutput(y); - } - - @Override - public void periodic() { - - } - - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty("Upper Arm", () -> getUpperArm(), null); - builder.addDoubleProperty("Lower Arm", () -> getLowerArm(), null); - builder.addDoubleProperty("Upper Arm Get", () -> encoder0.getAbsolutePosition(), null); - builder.addDoubleProperty("Lower Arm Get", () -> encoder1.get(), null); - builder.addDoubleProperty("OUptut", () -> upperArm.motor.get(), null); - - } - -} diff --git a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/ArmController.java b/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/ArmController.java index bb2bbb99..7527921a 100644 --- a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/ArmController.java +++ b/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/ArmController.java @@ -64,8 +64,8 @@ public ArmController() { .withForwardSoftLimitEnabled(false) .build(); - lowerArmSegment = new ArmSegment(this::getLowerArm, lowerArmMotor, "Lower Motor"); - upperArmSegment = new ArmSegment(this::getUpperArm, upperArmMotor, "Upper Motor"); + lowerArmSegment = new ArmSegment(lowerArmMotor, "Lower Motor"); + upperArmSegment = new ArmSegment(upperArmMotor, "Upper Motor"); Translation2d initial = ArmKinematics.getArmPosition(getLowerArm(), getUpperArm()); diff --git a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/ArmSegment.java b/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/ArmSegment.java index dd2bb1f1..f5ab230b 100644 --- a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/ArmSegment.java +++ b/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/ArmSegment.java @@ -1,6 +1,5 @@ package org.team100.frc2023.subsystems.arm; -import java.util.function.Supplier; import org.team100.lib.motors.FRCNEO; @@ -8,60 +7,31 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class ArmSegment extends SubsystemBase{ - +// TODO: remove this class +public class ArmSegment extends SubsystemBase { private FRCNEO motor; - private Supplier positionSupplier; - - /** Creates a new UpperArm. */ - public ArmSegment(Supplier positionSupplier, FRCNEO motor, String name) { + public ArmSegment(FRCNEO motor, String name) { this.motor = motor; - this.positionSupplier = positionSupplier; - SmartDashboard.putData(name, this); - - } - /** - * Sets the motor output, accounting for soft limits. - *

THIS SHOULD BE THE ONLY PLACE WHERE MOTOR OUTPUT IS SET.

- * @param x - */ public void setMotor(double x) { - // Account for soft limits - // kGravityGain *= Math.sin(positionSupplier.get()); - // if (getAngle() <= lowerSoftLimit && x < 0) - // this.motor.motor.set(0); - // else if (getAngle() >= upperSoftLimit && x > 0) - // this.motor.motor.set(0); - // else - this.motor.motor.set(x) ; - } - - @Override - public void periodic() { - - } - - - public double getDegrees() { - return positionSupplier.get() * 180/Math.PI; + this.motor.motor.set(x); } public void initSendable(SendableBuilder builder) { super.initSendable(builder); - // builder.addDoubleProperty("Error", () -> m_controller.getPositionError(), null); + // builder.addDoubleProperty("Error", () -> m_controller.getPositionError(), + // null); // builder.addDoubleProperty("Measurement", () -> getMeasurement(), null); - // builder.addDoubleProperty("Setpoint", () -> m_controller.getSetpoint().position, null); - // builder.addDoubleProperty("Goal", () -> m_controller.getGoal().position, null); + // builder.addDoubleProperty("Setpoint", () -> + // m_controller.getSetpoint().position, null); + // builder.addDoubleProperty("Goal", () -> m_controller.getGoal().position, + // null); // builder.addDoubleProperty("Output", () -> output, null); // builder.addDoubleProperty("Degrees", () -> getDegrees(), null); - - - } } \ No newline at end of file diff --git a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/LowerArm.java b/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/LowerArm.java deleted file mode 100644 index 84b0cff2..00000000 --- a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/LowerArm.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.team100.frc2023.subsystems.arm; - -import java.util.function.Supplier; - -import org.team100.lib.motors.FRCNEO; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class LowerArm extends SubsystemBase { - /** Creates a new UpperArm. */ - FRCNEO lowerArmMotor; - Supplier lowerArmAngleSupplier; - - public LowerArm(Supplier lowerArmAngleSupplier, FRCNEO lowerArmMotor) { - this.lowerArmMotor = lowerArmMotor; - this.lowerArmAngleSupplier = lowerArmAngleSupplier; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - public void set(double x) { - this.lowerArmMotor.motor.set(x); - } - - public double getAngle() { - return lowerArmAngleSupplier.get(); - } -} diff --git a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/UpperArm.java b/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/UpperArm.java deleted file mode 100644 index 28757f4e..00000000 --- a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/UpperArm.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.team100.frc2023.subsystems.arm; - -import java.util.function.Supplier; - -import org.team100.lib.motors.FRCNEO; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class UpperArm extends SubsystemBase { - /** Creates a new UpperArm. */ - FRCNEO upperArmMotor; - Supplier upperArmPositionSupplier; - - public UpperArm(Supplier upperArmPositionSupplier, FRCNEO upperArmMotor) { - this.upperArmMotor = upperArmMotor; - this.upperArmPositionSupplier = upperArmPositionSupplier; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - public void set(double x) { - this.upperArmMotor.motor.set(x); - } - - public double getAngle() { - return upperArmPositionSupplier.get(); - } -}