diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index eba05db..fe771bf 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -34,6 +34,7 @@ import frc4388.robot.subsystems.swerve.SwerveIO; import frc4388.robot.subsystems.swerve.SwerveReal; import frc4388.robot.subsystems.vision.VisionIO; import frc4388.robot.subsystems.vision.VisionReal; +import frc4388.utility.compute.JankCoder; import frc4388.utility.status.FaultCANCoder; import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; @@ -106,11 +107,10 @@ public class RobotMap { // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - - // shooterIO = new ShooterIO() {}; shooterIO = new ShooterReal(shooter1, shooter2, indexer); + JankCoder armEncoder = new JankCoder(0, IntakeConstants.ARM_ENCODER_OFFSET); - intakeIO = new IntakeReal(arm, roller); + intakeIO = new IntakeReal(arm, roller, armEncoder); // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 0246ddd..3c28ce0 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -44,7 +44,7 @@ public class Intake extends SubsystemBase { switch (mode) { case Bouncing: // When bounce is enabled: set the bounce timer - this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD; + this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get(); break; default: break; @@ -118,15 +118,15 @@ public class Intake extends SubsystemBase { state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_BOUNCE_CURRENT_LIMIT.get() && state.armMotorVelocity.in(RotationsPerSecond) < IntakeConstants.INTAKE_BOUNCE_VELOCITY_LIMIT.get() ) { - this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.BOUNCE_HALF_PERIOD; + this.state.currentBounceTime = Utils.getSystemTimeSeconds() + IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get(); } // Get the time delta from the last bounce time update double currentTime = Utils.getSystemTimeSeconds() - state.currentBounceTime; // Get the percentage through the bounce period (0 output means one half period has passed) - double percentOutput = (currentTime / IntakeConstants.BOUNCE_HALF_PERIOD) * IntakeConstants.INTAKE_BOUNCE_OUTPUT; + double percentOutput = (currentTime / IntakeConstants.INTAKE_BOUNCE_HALF_PERIOD.get()) * IntakeConstants.INTAKE_BOUNCE_OUTPUT.get(); // Clamp the output of the motor to some value - percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT); + percentOutput = Math.max(Math.min(percentOutput, IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()), -IntakeConstants.INTAKE_BOUNCE_MAX_OUTPUT.get()); io.armOutput(percentOutput); break; diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index e82ceea..fd689e7 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -13,11 +13,13 @@ public class IntakeConstants { public static final double ARM_MOTOR_GEAR_RATIO = 125; public static final double ROLLER_MOTOR_GEAR_RATIO = 3; + public static final double ARM_ENCODER_OFFSET = 0.; - public static final double BOUNCE_HALF_PERIOD = 5.; - public static final double INTAKE_BOUNCE_OUTPUT = 0.2; - public static final double INTAKE_BOUNCE_MAX_OUTPUT = 0.5; + + public static final ConfigurableDouble INTAKE_BOUNCE_HALF_PERIOD = new ConfigurableDouble("Bounce Half Period", 5.); + public static final ConfigurableDouble INTAKE_BOUNCE_OUTPUT = new ConfigurableDouble("Bounce Output", 0.2); + public static final ConfigurableDouble INTAKE_BOUNCE_MAX_OUTPUT = new ConfigurableDouble("Bounce Max Output", 0.5); public static final ConfigurableDouble INTAKE_BOUNCE_CURRENT_LIMIT = new ConfigurableDouble("Intake Bounce Current Limit", 20); public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 20); @@ -27,7 +29,7 @@ public class IntakeConstants { public static final CanDevice ARM_ID = new CanDevice("ARM", 20); public static final CanDevice ROLLER_ID = new CanDevice("INTAKE_ROLLER", 21); - // public static final int ARM_LIMIT_SWITCH_CHANNEL = 9; + public static final int ARM_ENCODER_ID = 0; // Limits @@ -40,7 +42,7 @@ public class IntakeConstants { // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.1); - public static final ConfigurableDouble ARM_ssLIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33); + public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.33); public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.4); public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.4); public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .70); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 4308c26..6066fce 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -17,11 +17,15 @@ public interface IntakeIO { public class IntakeState { double currentBounceTime = 0; + boolean extendedLimit = false; boolean retractedLimit = false; + Angle intakeEncoder = Rotations.of(0); + Angle armAngle = Rotations.of(0); Angle armTargetAngle = Rotations.of(0); Current armMotorCurrent = Amps.of(0); + AngularVelocity armMotorVelocity = RotationsPerSecond.of(0); // AngularAcceleration armMotorAcceleration = RotationsPerSecondPerSecond.of(0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index f170880..59fc5ba 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -19,22 +19,24 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc4388.utility.compute.JankCoder; public class IntakeReal implements IntakeIO { SparkMax m_armMotor; SparkMax m_rollerMotor; - DutyCycleEncoder encoder; + JankCoder m_encoder; public IntakeReal( - // DigitalInput armLimitSwitch, SparkMax armMotor, - SparkMax rollerMotor + SparkMax rollerMotor, + JankCoder jankCoder ) { // m_angleMotor = angleMotor; // m_pitchMotor = pitchMotor; m_armMotor = armMotor; m_rollerMotor = rollerMotor; + m_encoder = jankCoder; // m_armLimitSwitch = armLimitSwitch; } @@ -74,9 +76,24 @@ public class IntakeReal implements IntakeIO { // m_rollerMotor.set(0); } + private boolean retractedLimit() { + return m_encoder.get() >= IntakeConstants.ARM_LIMIT_EXTENDED.get(); + } + private boolean extendedLimit() { + return m_encoder.get() <= IntakeConstants.ARM_LIMIT_RETRACTED.get(); + } + @Override public void armOutput(double percentOutput){ + + if(retractedLimit()) { + percentOutput = Math.max(percentOutput, 0); + } else if (extendedLimit()) { + percentOutput = Math.min(percentOutput, 0); + } + m_armMotor.set(percentOutput); + } @Override @@ -89,6 +106,12 @@ public class IntakeReal implements IntakeIO { state.rollerOutput = m_rollerMotor.get(); state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent()); + state.retractedLimit = retractedLimit(); + state.extendedLimit = extendedLimit(); + state.armAngle = m_encoder.getRotations(); + + + // if(state.retractedLimit) { // // Set the arm motor to be zero if the limit switch is pressed diff --git a/src/main/java/frc4388/utility/compute/JankCoder.java b/src/main/java/frc4388/utility/compute/JankCoder.java new file mode 100644 index 0000000..801e69d --- /dev/null +++ b/src/main/java/frc4388/utility/compute/JankCoder.java @@ -0,0 +1,105 @@ +package frc4388.utility.compute; + +import static edu.wpi.first.units.Units.Rotations; + +import java.io.FileInputStream; +import java.io.FileOutputStream; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DutyCycleEncoder; + +public class JankCoder { + DutyCycleEncoder m_encoder; + + boolean loaded_rotations = false; + int rotations = 0; + double lastRotation = 0; + final double offset; + + public JankCoder(int dio_channel) { + this.offset = 0; + m_encoder = new DutyCycleEncoder(dio_channel); + loadRotations(); + } + public JankCoder(int dio_channel, double offset) { + this.offset = offset; + m_encoder = new DutyCycleEncoder(dio_channel); + loadRotations(); + } + + public void update() { + if(!m_encoder.isConnected()) { + return; + } + + if(!loaded_rotations) { + loadRotations(); + } else { + double curRot = m_encoder.get(); + if(lastRotation - curRot > 0.5) { + rotations += 1; + saveRotations(); + } else if (curRot - lastRotation > 0.5) { + rotations -= 1; + saveRotations(); + } + + lastRotation = curRot; + } + } + + public double get() { + return (double) rotations + m_encoder.get() + offset; + } + + public Angle getRotations() { + return Rotations.of(get()); + } + + public int getRotationCount() { + return rotations; + } + + public void resetRotations() { + setRotations(0); + } + + public void setRotations(int rotation) { + rotations = rotation; + saveRotations(); + } + + private void saveRotations() { + try (FileOutputStream stream = new FileOutputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { + stream.write(DataUtils.intToByteArray(rotations)); + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("ENCODER: Unable to write to trim file `" + m_encoder.getSourceChannel() + "`!?!"); + rotations = 0; + } + } + + private boolean loadRotations() { + lastRotation = m_encoder.get(); + this.loaded_rotations = true; + + try (FileInputStream stream = new FileInputStream("/home/lvuser/encoder" + m_encoder.getSourceChannel())) { + int fileValue = DataUtils.byteArrayToInt(stream.readNBytes(4)); + + rotations = fileValue; + // clampModify(); + // modified = false; + + // if (fileValue != currentValue) { + // // System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping..."); + // // modified = true; + // } + return true; + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("ENCODER: Unable to read encoder `" + m_encoder.getSourceChannel() + "`, using current value..."); + return false; + } + + } +} diff --git a/src/main/java/frc4388/utility/status/FaultSparkMax.java b/src/main/java/frc4388/utility/status/FaultSparkMax.java index f9035a9..0f52f21 100644 --- a/src/main/java/frc4388/utility/status/FaultSparkMax.java +++ b/src/main/java/frc4388/utility/status/FaultSparkMax.java @@ -63,7 +63,7 @@ public class FaultSparkMax implements Queryable { s.addReport(ReportLevel.ERROR, "Sensor fault"); } if(faults.temperature) { - s.addReport(ReportLevel.ERROR, "Tempreture fault"); + s.addReport(ReportLevel.ERROR, "Temperature fault"); } Warnings warnings = motor.getWarnings();