diff --git a/.run/Build.run.xml b/.run/Build.run.xml index 2bf3635..f38a594 100644 --- a/.run/Build.run.xml +++ b/.run/Build.run.xml @@ -18,6 +18,7 @@ true true false + false \ No newline at end of file diff --git a/.run/Robot Deploy Debug .run.xml b/.run/Robot Deploy Debug .run.xml index ce1e418..bc277f5 100644 --- a/.run/Robot Deploy Debug .run.xml +++ b/.run/Robot Deploy Debug .run.xml @@ -18,6 +18,7 @@ true true false + false \ No newline at end of file diff --git a/.run/Robot Deploy.run.xml b/.run/Robot Deploy.run.xml index 22d1679..559bd0f 100644 --- a/.run/Robot Deploy.run.xml +++ b/.run/Robot Deploy.run.xml @@ -18,6 +18,7 @@ true true false + false \ No newline at end of file diff --git a/.run/Simulate Debug.run.xml b/.run/Simulate Debug.run.xml index 7821ad5..ccae514 100644 --- a/.run/Simulate Debug.run.xml +++ b/.run/Simulate Debug.run.xml @@ -18,6 +18,7 @@ true true false + false \ No newline at end of file diff --git a/.run/Simulate.run.xml b/.run/Simulate.run.xml index a97fceb..b8f5ceb 100644 --- a/.run/Simulate.run.xml +++ b/.run/Simulate.run.xml @@ -18,6 +18,7 @@ true true false + false \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 327ee38..7c7ebbb 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -14,12 +14,13 @@ private RobotMap() {} public static final double ARM_CORAL_KI = 0; public static final double ARM_CORAL_KD = 0; public static final double ARM_CORAL_IZONE = 0; + public static final double ARM_CORAL_KF = 0; public static final double ARM_CORAL_TOLERANCE_POSITION_DEGREES = 0.5; public static final double ARM_CORAL_TOLERANCE_VELOCITY_RPM = 5; public static final int ARM_CORAL_START_PULSE_US = 1; public static final int ARM_CORAL_END_PULSE_US = 1024; public static final double ARM_CORAL_ZERO_OFFSET = 0; - public static final double ARM_CORAL_KF = 0; + public static final double ARM_CORAL_GEAR_RATIO = 0.005; public static final double ARM_CORAL_ANGLE_A = 0; public static final double ARM_CORAL_ANGLE_B = 0; diff --git a/src/main/java/frc/robot/commands/CoralArmCommand.java b/src/main/java/frc/robot/commands/CoralArmCommand.java index 96fb898..e173d72 100644 --- a/src/main/java/frc/robot/commands/CoralArmCommand.java +++ b/src/main/java/frc/robot/commands/CoralArmCommand.java @@ -1,38 +1,68 @@ package frc.robot.commands; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.CoralArm; public class CoralArmCommand extends Command { + private static final double MAX_VELOCITY_DEGREES_PER_SEC = 1000; + private static final double MAX_ACCELERATION_DEGREES_PER_SEC_PER_SEC = 200; + private final CoralArm arm; + private final TrapezoidProfile.Constraints constraints; + + private TrapezoidProfile motionProfile; + private TrapezoidProfile.State motionProfileGoal; + private TrapezoidProfile.State motionProfileSetPoint; private double targetPositionDegrees; private boolean hasNewTarget; private boolean didReachPosition; + private boolean isHolding; public CoralArmCommand(CoralArm arm) { this.arm = arm; + constraints = new TrapezoidProfile.Constraints(MAX_VELOCITY_DEGREES_PER_SEC, MAX_ACCELERATION_DEGREES_PER_SEC_PER_SEC); addRequirements(arm); } @Override public void initialize() { - + isHolding = false; + hasNewTarget = true; } - @Override public void execute() { if (hasNewTarget) { // reset to move to new angle - - didReachPosition = false; hasNewTarget = false; + didReachPosition = false; + + if (isHolding) { + motionProfile = new TrapezoidProfile(constraints); + motionProfileGoal = new TrapezoidProfile.State(targetPositionDegrees,0); + motionProfileSetPoint = new TrapezoidProfile.State(arm.getPositionDegrees(),0); + } else { + arm.stop(); + } } - arm.setMoveToPosition(targetPositionDegrees); - didReachPosition = arm.didReachPosition(targetPositionDegrees); + if (!isHolding) { + return; + } + + if (!didReachPosition && arm.didReachPosition(targetPositionDegrees)) { + didReachPosition = true; + } + + if (didReachPosition) { + arm.setMoveToPosition(targetPositionDegrees); + } else { + motionProfileSetPoint = motionProfile.calculate(0.02, motionProfileSetPoint, motionProfileGoal); + arm.setMoveToPosition(motionProfileSetPoint.position); + } } @Override @@ -48,6 +78,7 @@ public boolean isFinished() { public void setNewTargetPosition(double positionDegrees) { this.targetPositionDegrees = positionDegrees; this.hasNewTarget = true; + isHolding = true; } public boolean didReachTargetPosition() { @@ -57,4 +88,9 @@ public boolean didReachTargetPosition() { return didReachPosition; } + + public void stopHolding(){ + isHolding = false; + hasNewTarget = true; + } } diff --git a/src/main/java/frc/robot/subsystems/CoralArm.java b/src/main/java/frc/robot/subsystems/CoralArm.java index c38a370..6553482 100644 --- a/src/main/java/frc/robot/subsystems/CoralArm.java +++ b/src/main/java/frc/robot/subsystems/CoralArm.java @@ -2,8 +2,11 @@ import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.*; +import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.LimitSwitchConfig; +import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -13,11 +16,13 @@ public class CoralArm extends SubsystemBase { private final SparkMax motor; - private final AbsoluteEncoder encoder; + private final AbsoluteEncoder absEncoder; + private final RelativeEncoder relEncoder; private final SparkClosedLoopController controller; private final SparkLimitSwitch forwardLimitSwitch; private final SparkLimitSwitch reverseLimitSwitch; + public CoralArm() { motor = new SparkMax(RobotMap.ARM_CORAL_MOTOR, SparkLowLevel.MotorType.kBrushless); SparkMaxConfig config = new SparkMaxConfig(); @@ -38,23 +43,32 @@ public CoralArm() { config.closedLoop .pid(RobotMap.ARM_CORAL_KP, RobotMap.ARM_CORAL_KI, RobotMap.ARM_CORAL_KD) .iZone(RobotMap.ARM_CORAL_IZONE) - .outputRange(-1, 1); + .outputRange(-1, 1) + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); + config.idleMode(SparkBaseConfig.IdleMode.kBrake); + config.encoder + .positionConversionFactor(1 / RobotMap.ARM_CORAL_GEAR_RATIO) + .velocityConversionFactor(1 / RobotMap.ARM_CORAL_GEAR_RATIO); motor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters); + relEncoder = motor.getEncoder(); + absEncoder = motor.getAbsoluteEncoder(); + relEncoder.setPosition(absEncoder.getPosition()); + - encoder = motor.getAbsoluteEncoder(); controller = motor.getClosedLoopController(); forwardLimitSwitch = motor.getForwardLimitSwitch(); reverseLimitSwitch = motor.getReverseLimitSwitch(); + } public double getPositionDegrees() { - return encoder.getPosition() * 360; + return relEncoder.getPosition() * 360; } public boolean didReachPosition(double positionDegrees) { double currentPosition = getPositionDegrees(); return MathUtil.isNear(positionDegrees, currentPosition, RobotMap.ARM_CORAL_TOLERANCE_POSITION_DEGREES) && - Math.abs(encoder.getVelocity()) < RobotMap.ARM_CORAL_TOLERANCE_VELOCITY_RPM; + Math.abs(relEncoder.getVelocity()) < RobotMap.ARM_CORAL_TOLERANCE_VELOCITY_RPM; } public boolean isAtForwardLimit() { @@ -81,7 +95,8 @@ public void stop() { public void periodic() { SmartDashboard.putBoolean("CoralArmAtForwardLimit", isAtForwardLimit()); SmartDashboard.putBoolean("CoralArmAtReverseLimit", isAtReverseLimit()); - SmartDashboard.putNumber("CoralArmPosition", getPositionDegrees()); + SmartDashboard.putNumber("CoralArmPositionRelative", getPositionDegrees()); + SmartDashboard.putNumber("CoralArmPositionAbsolute",absEncoder.getPosition()); } }