diff --git a/swerve100/src/main/java/org/team100/frc2023/commands/Arm/ArmTrajectories.java b/swerve100/src/main/java/org/team100/frc2023/commands/Arm/ArmTrajectories.java new file mode 100644 index 00000000..03e2a6e2 --- /dev/null +++ b/swerve100/src/main/java/org/team100/frc2023/commands/Arm/ArmTrajectories.java @@ -0,0 +1,104 @@ +package org.team100.frc2023.commands.Arm; + +import java.util.List; + +import org.team100.frc2023.subsystems.arm.ArmController; +import org.team100.frc2023.subsystems.arm.ArmPosition; +import org.team100.frc2023.subsystems.arm.ArmAngles; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGenerationException; + +public class ArmTrajectories { + // Cone + private static final ArmAngles highGoalCone = new ArmAngles(1.178, 0.494); + private static final ArmAngles midGoalCone = new ArmAngles(1.609977, 0.138339); + private static final ArmAngles lowGoalCone = new ArmAngles(2.21, 0); + private static final ArmAngles subCone = new ArmAngles(ArmController.coneSubVal, -0.338940); + + // Cube + private static final ArmAngles highGoalCube = new ArmAngles(1.147321, 0.316365); + private static final ArmAngles midGoalCube = new ArmAngles(1.681915, 0.089803); + private static final ArmAngles lowGoalCube = new ArmAngles(2.271662, -0.049849); + private static final ArmAngles subCube = new ArmAngles(1.361939, -0.341841); + private static final ArmAngles subToCube = new ArmAngles(1.361939, -0.341841); + + private static final ArmAngles safeBack = new ArmAngles(1.97, -0.55); + private static final ArmAngles safeGoalCone = new ArmAngles(1.838205, -0.639248); + private static final ArmAngles safeGoalCube = new ArmAngles(1.838205, -0.639248); + private static final ArmAngles safeWaypoint = new ArmAngles(1.226285, -0.394089); + + private final TrajectoryConfig trajecConfig; + + public ArmTrajectories(TrajectoryConfig config) { + trajecConfig = config; + } + + public Trajectory makeTrajectory(ArmAngles start, ArmPosition goal, boolean cubeMode) { + switch (goal) { + case SAFEBACK: + return twoPoint(start, safeWaypoint, safeBack, -180); + case SAFE: + if (cubeMode) + return twoPoint(start, safeWaypoint, safeGoalCube, -180); + return twoPoint(start, safeWaypoint, safeGoalCone, -180); + case SAFEWAYPOINT: + return onePoint(start, safeWaypoint, -180); + case HIGH: + if (cubeMode) + return onePoint(start, highGoalCube, 90); + return onePoint(start, highGoalCone, 90); + case MID: + if (cubeMode) + return onePoint(start, midGoalCube, 90); + return onePoint(start, midGoalCone, 90); + case LOW: + if (cubeMode) + return onePoint(start, lowGoalCube, 90); + return onePoint(start, lowGoalCone, 90); + case SUB: + if (cubeMode) + return onePoint(start, subCube, 90); + return onePoint(start, subCone, 90); + case SUBTOCUBE: + if (cubeMode) + return onePoint(start, subToCube, 90); + return onePoint(start, subToCube, 90); + } + + return null; + } + + /** from current location to an endpoint */ + private Trajectory onePoint(ArmAngles start, ArmAngles end, double degrees) { + return withList(start, List.of(), end, degrees); + } + + /** from current location, through a waypoint, to an endpoint */ + private Trajectory twoPoint(ArmAngles start, ArmAngles mid, ArmAngles end, double degrees) { + return withList(start, List.of(new Translation2d(mid.upperTheta, mid.lowerTheta)), end, degrees); + } + + private Trajectory withList(ArmAngles start, List list, ArmAngles end, double degrees) { + try { + return TrajectoryGenerator.generateTrajectory(startPose(start, degrees), list, endPose(end, degrees), + trajecConfig); + } catch (TrajectoryGenerationException e) { + return null; + } + } + + private Pose2d startPose(ArmAngles start, double degrees) { + return new Pose2d(start.upperTheta, start.lowerTheta, Rotation2d.fromDegrees(degrees)); + } + + private Pose2d endPose(ArmAngles angles, double degrees) { + return new Pose2d(angles.upperTheta, angles.lowerTheta, Rotation2d.fromDegrees(degrees)); + } + +} diff --git a/swerve100/src/main/java/org/team100/frc2023/commands/Arm/ArmTrajectory.java b/swerve100/src/main/java/org/team100/frc2023/commands/Arm/ArmTrajectory.java index 6fdd84d8..5eef00a5 100644 --- a/swerve100/src/main/java/org/team100/frc2023/commands/Arm/ArmTrajectory.java +++ b/swerve100/src/main/java/org/team100/frc2023/commands/Arm/ArmTrajectory.java @@ -1,20 +1,12 @@ package org.team100.frc2023.commands.Arm; -import java.util.List; - import org.team100.frc2023.subsystems.arm.ArmController; import org.team100.frc2023.subsystems.arm.ArmPosition; -import org.team100.frc2023.subsystems.arm.InverseKinematicsAngle; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.Trajectory.State; -import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGenerationException; +import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Timer; @@ -22,32 +14,15 @@ public class ArmTrajectory extends CommandBase { - // Cone - private static final InverseKinematicsAngle highGoalCone = new InverseKinematicsAngle(1.178, 0.494); - private static final InverseKinematicsAngle midGoalCone = new InverseKinematicsAngle(1.609977, 0.138339); - private static final InverseKinematicsAngle lowGoalCone = new InverseKinematicsAngle(2.21, 0); - private static final InverseKinematicsAngle subCone = new InverseKinematicsAngle(ArmController.coneSubVal, - -0.338940); - - // Cube - private static final InverseKinematicsAngle highGoalCube = new InverseKinematicsAngle(1.147321, 0.316365); - private static final InverseKinematicsAngle midGoalCube = new InverseKinematicsAngle(1.681915, 0.089803); - private static final InverseKinematicsAngle lowGoalCube = new InverseKinematicsAngle(2.271662, -0.049849); - private static final InverseKinematicsAngle subCube = new InverseKinematicsAngle(1.361939, -0.341841); - private static final InverseKinematicsAngle subToCube = new InverseKinematicsAngle(1.361939, -0.341841); - - private static final InverseKinematicsAngle safeBack = new InverseKinematicsAngle(1.97, -0.55); - private static final InverseKinematicsAngle safeGoalCone = new InverseKinematicsAngle(1.838205, -0.639248); - private static final InverseKinematicsAngle safeGoalCube = new InverseKinematicsAngle(1.838205, -0.639248); - private static final InverseKinematicsAngle safeWaypoint = new InverseKinematicsAngle(1.226285, -0.394089); - private final ArmController m_arm; private final ArmPosition m_position; + private final ArmTrajectories m_trajectories; + private final Timer m_timer; + private final PIDController upperController; private final PIDController lowerController; private final PIDController upperDownController; private final PIDController lowerDownController; - private final TrajectoryConfig trajecConfig; private final NetworkTableInstance inst; private final DoublePublisher measurmentX; @@ -55,16 +30,20 @@ public class ArmTrajectory extends CommandBase { private final DoublePublisher setpointUpper; private final DoublePublisher setpointLower; - private final Timer m_timer; - private Trajectory m_trajectory; - private boolean isSafeWaypoint = false; - public ArmTrajectory(ArmPosition position, ArmController arm) { m_arm = arm; m_position = position; + + if (m_position != ArmPosition.SAFE) { + m_trajectories = new ArmTrajectories(new TrajectoryConfig(12, 2)); + } else { + m_trajectories = new ArmTrajectories(new TrajectoryConfig(9, 1.5)); + } + m_timer = new Timer(); + upperController = new PIDController(4, 0.2, 0.05); upperController.setTolerance(0.001); lowerController = new PIDController(3, 0, 0.1); @@ -72,12 +51,6 @@ public ArmTrajectory(ArmPosition position, ArmController arm) { upperDownController = new PIDController(2.5, 0, 0); lowerDownController = new PIDController(2.5, 0, 0); - if (m_position != ArmPosition.SAFE) { - trajecConfig = new TrajectoryConfig(12, 2); - } else { - trajecConfig = new TrajectoryConfig(9, 1.5); - } - inst = NetworkTableInstance.getDefault(); measurmentX = inst.getTable("Arm Trajec").getDoubleTopic("measurmentX").publish(); measurmentY = inst.getTable("Arm Trajec").getDoubleTopic("measurmentY").publish(); @@ -87,78 +60,13 @@ public ArmTrajectory(ArmPosition position, ArmController arm) { addRequirements(m_arm); } - private Pose2d startPose(double degrees) { - return new Pose2d(m_arm.getUpperArm(), m_arm.getLowerArm(), Rotation2d.fromDegrees(degrees)); - } - - private Pose2d endPose(InverseKinematicsAngle angles, double degrees) { - return new Pose2d(angles.upperTheta, angles.lowerTheta, Rotation2d.fromDegrees(degrees)); - } - - /** from current location to an endpoint */ - private Trajectory onePoint(InverseKinematicsAngle end, double degrees) { - return withList(List.of(), end, degrees); - } - - /** from current location, through a waypoint, to an endpoint */ - private Trajectory twoPoint(InverseKinematicsAngle mid, InverseKinematicsAngle end, double degrees) { - return withList(List.of(new Translation2d(mid.upperTheta, mid.lowerTheta)), end, degrees); - } - - private Trajectory withList(List list, InverseKinematicsAngle end, double degrees) { - try { - return TrajectoryGenerator.generateTrajectory(startPose(degrees), list, endPose(end, degrees), - trajecConfig); - } catch (TrajectoryGenerationException e) { - return null; - } - } - - private Trajectory makeTrajectory() { - isSafeWaypoint = false; - switch (m_position) { - case SAFEBACK: - return twoPoint(safeWaypoint, safeBack, -180); - case SAFE: - if (m_arm.cubeMode) - return twoPoint(safeWaypoint, safeGoalCube, -180); - return twoPoint(safeWaypoint, safeGoalCone, -180); - case SAFEWAYPOINT: - isSafeWaypoint = true; - return onePoint(safeWaypoint, -180); - case HIGH: - if (m_arm.cubeMode) - return onePoint(highGoalCube, 90); - return onePoint(highGoalCone, 90); - case MID: - if (m_arm.cubeMode) - return onePoint(midGoalCube, 90); - return onePoint(midGoalCone, 90); - case LOW: - if (m_arm.cubeMode) - return onePoint(lowGoalCube, 90); - return onePoint(lowGoalCone, 90); - case SUB: - if (m_arm.cubeMode) - return onePoint(subCube, 90); - return onePoint(subCone, 90); - case SUBTOCUBE: - if (m_arm.cubeMode) - return onePoint(subToCube, 90); - return onePoint(subToCube, 90); - } - - return null; - } - @Override public void initialize() { m_timer.restart(); - m_trajectory = makeTrajectory(); + m_trajectory = m_trajectories.makeTrajectory(m_arm.getArmAngles(), m_position, m_arm.cubeMode); } public void execute() { - if (m_trajectory == null) { return; } @@ -169,31 +77,21 @@ public void execute() { double desiredUpper = desiredState.poseMeters.getX(); double desiredLower = desiredState.poseMeters.getY(); - double upperSpeed = 0; - double lowerSpeed = 0; - - double lowerFeed = 0; + double currentUpper = m_arm.getUpperArm(); + double currentLower = m_arm.getLowerArm(); if (m_position == ArmPosition.SAFE) { - upperSpeed = upperDownController.calculate(m_arm.getUpperArm(), desiredUpper); - lowerSpeed = lowerDownController.calculate(m_arm.getLowerArm(), desiredLower); - + m_arm.setUpperArm(upperDownController.calculate(currentUpper, desiredUpper)); + m_arm.setLowerArm(lowerDownController.calculate(currentLower, desiredLower)); } else { - upperSpeed = upperController.calculate(m_arm.getUpperArm(), desiredUpper); - lowerSpeed = lowerController.calculate(m_arm.getLowerArm(), desiredLower); - - // lowerFeed = 0.01 * Math.signum(lowerController.getPositionError()); + m_arm.setUpperArm(upperController.calculate(currentUpper, desiredUpper)); + m_arm.setLowerArm(lowerController.calculate(currentLower, desiredLower)); } - m_arm.setUpperArm(upperSpeed); - m_arm.setLowerArm(lowerSpeed + lowerFeed); - - measurmentX.set(m_arm.getUpperArm()); - measurmentY.set(m_arm.getLowerArm()); - + measurmentX.set(currentUpper); + measurmentY.set(currentLower); setpointUpper.set(desiredUpper); setpointLower.set(desiredLower); - } @Override @@ -204,7 +102,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - if (isSafeWaypoint) { + if (m_position == ArmPosition.SAFEWAYPOINT) { return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds()); } else { return false; 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/AutonGamePiece.java b/swerve100/src/main/java/org/team100/frc2023/subsystems/AutonGamePiece.java index 1f1530bb..cff81c39 100644 --- a/swerve100/src/main/java/org/team100/frc2023/subsystems/AutonGamePiece.java +++ b/swerve100/src/main/java/org/team100/frc2023/subsystems/AutonGamePiece.java @@ -1,6 +1,6 @@ package org.team100.frc2023.subsystems; -/** Add your docs here. */ public enum AutonGamePiece { - CUBE, CONE + CUBE, + CONE } 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/ArmAngles.java b/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/ArmAngles.java new file mode 100644 index 00000000..2667aea8 --- /dev/null +++ b/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/ArmAngles.java @@ -0,0 +1,17 @@ +package org.team100.frc2023.subsystems.arm; + +public class ArmAngles { + public final double lowerTheta; + public final double upperTheta; + + public ArmAngles(double upperTheta, double lowerTheta) { + this.lowerTheta = lowerTheta; + this.upperTheta = upperTheta; + } + + public ArmAngles() { + this.lowerTheta = 100; + this.upperTheta = 100; + } + +} 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 c7041257..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 @@ -36,7 +36,6 @@ public class ArmController extends SubsystemBase { private final FRCNEO upperArmMotor; private final AnalogEncoder upperArmEncoder = new AnalogEncoder(5); - public ArmController() { // TrapezoidProfile.Const raints constraints = new TrapezoidProfile.Constraints( // 0.3, // velocity rad/s @@ -65,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()); @@ -88,7 +87,7 @@ public ArmController() { * @param x vertical axis coordinate * @param y horizontal axis coordinate */ - public InverseKinematicsAngle manualSetpoint(XboxController m_driverController) { + public ArmAngles manualSetpoint(XboxController m_driverController) { double dx = -m_driverController.getLeftY(); double dy = m_driverController.getRightX(); @@ -118,7 +117,7 @@ public InverseKinematicsAngle manualSetpoint(XboxController m_driverController) double[] angles = ArmKinematics.algorithm2RIKS(coords[0], coords[1]); if (angles == null) { - return new InverseKinematicsAngle(); + return new ArmAngles(); } upperAngleSetpoint = angles[0]; @@ -126,7 +125,7 @@ public InverseKinematicsAngle manualSetpoint(XboxController m_driverController) xSetpoint = coords[0]; ySetpoint = coords[1]; - return new InverseKinematicsAngle(angles[0], angles[1]); // upper theta, lower theta + return new ArmAngles(angles[0], angles[1]); // upper theta, lower theta } public void resetSetpoint() { @@ -151,12 +150,11 @@ public Translation2d getPose() { return ArmKinematics.getArmPosition(getLowerArm(), getUpperArm()); } - public InverseKinematicsAngle calculate(double x, double y) { + public ArmAngles calculate(double x, double y) { double[] angles = ArmKinematics.algorithm2RIKS(x, y); - return new InverseKinematicsAngle(angles[0], angles[1]); // upper theta, lower theta + return new ArmAngles(angles[0], angles[1]); // upper theta, lower theta } - /** * The angle the lower arm is at (in radians) * 0 is pointing straight up, increasing when the arm is pointing forward. @@ -185,6 +183,10 @@ public double getUpperArm() { return formatted * Math.PI / 180; } + public ArmAngles getArmAngles() { + return new ArmAngles(getUpperArm(), getLowerArm()); + } + public double getLowerArmDegrees() { return getLowerArm() * 180 / Math.PI; } 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/InverseKinematicsAngle.java b/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/InverseKinematicsAngle.java deleted file mode 100644 index edef4878..00000000 --- a/swerve100/src/main/java/org/team100/frc2023/subsystems/arm/InverseKinematicsAngle.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team100.frc2023.subsystems.arm; - -/** Add your docs here. */ -public class InverseKinematicsAngle { - public double lowerTheta; - public double upperTheta; - - public InverseKinematicsAngle(double upperTheta, double lowerTheta) { - this.lowerTheta = lowerTheta; - this.upperTheta = upperTheta; - } - - - public InverseKinematicsAngle() { - this.lowerTheta = 100; - this.upperTheta = 100; - } - -} 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(); - } -}