diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6c75e81..35d529d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,13 +53,13 @@ public static final class CANConstants { // Analog Ports /// Ultrasonic Sensors and ports. public static final int ULTRASONICSHOOTERPORT = 0; - public static final int ARMLIMITSWITCHBACK = 1; - public static final int ARMLIMITSWITCHFRONT = 2; // Digital Ports + public static final int ARMLIMITSWITCHBACK = 0; + public static final int ARMLIMITSWITCHFRONT = 1; // Shooter Angles - public static final double ARMENCODEROFFSET = 0.8457221; + public static final double ARMENCODEROFFSET = 2.8378526; public static final double ARMSTARTINGANGLE = 22.5; // WHY MaTH HURT public static final double ARMMINRELATVESTART = 0.0; public static final double ARMLOADANGLE = 65 - ARMSTARTINGANGLE; diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 4a37c92..95817fe 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -55,7 +55,9 @@ public class ArmSubsystem extends SubsystemBase { // gear ratio by dividing by the gear ratio. // remember that 2pi radians in 360 degrees. private final double kRadiansConversionRatio = (Math.PI * 2) / kGearRatio; + private final double kVelocityConversionRatio = kRadiansConversionRatio / 60; private final double kAbsoluteRadiansConversionRatio = (Math.PI * 2); + private final double kAbsoluteVelocityConversionRatio = kAbsoluteRadiansConversionRatio / 60; private final ArmFeedforward m_armFeedforward = new ArmFeedforward( ksArmVolts, kgArmGravityGain, kvArmVoltSecondsPerMeter, kaArmVoltSecondsSquaredPerMeter); @@ -85,9 +87,11 @@ public ArmSubsystem(ShooterState shooterState) { m_AbsoluteEncoder = m_armMotorMain.getAbsoluteEncoder(); m_AbsoluteEncoder.setInverted(false); m_AbsoluteEncoder.setPositionConversionFactor(kAbsoluteRadiansConversionRatio); + m_AbsoluteEncoder.setVelocityConversionFactor(kAbsoluteVelocityConversionRatio); // m_AbsoluteEncoder.setZeroOffset(Constants.ARMENCODEROFFSET); // setup the encoders m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); + m_MainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); m_MainEncoder.setPosition(Units.degreesToRadians(Constants.ARMMINRELATVESTART)); // PID coefficients kP = 1.7496; @@ -104,6 +108,8 @@ public ArmSubsystem(ShooterState shooterState) { m_armMainPIDController.setIZone(kIz); m_armMainPIDController.setOutputRange(kMinOutput, kMaxOutput); m_armMainPIDController.setFeedbackDevice(m_AbsoluteEncoder); + // m_armMotorMain.setSoftLimit(SoftLimitDirection.kReverse, kMinArmAngleRadians)); + // m_armMotorMain.setSoftLimit(SoftLimitDirection.kForward, kMaxArmAngleRadians); m_armMotorMain.burnFlash(); // setup SysID for auto profiling diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 9ab6d2d..97bd541 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -30,8 +30,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.CANConstants; import frc.robot.Constants; +import frc.robot.Constants.CANConstants; import frc.robot.DriveConstants; /** This Subsystem is what allows the code to interact with the drivetrain of the robot. */ diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 2bdacfe..8dc2c65 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -32,7 +32,8 @@ public class ShooterSubsystem extends SubsystemBase { private final double kGearRatio = 4; // TBD // basically converted from rotations to to radians to then meters using the wheel diameter. // the diameter is already *2 so we don't need to multiply by 2 again. - private final double kVelocityConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio / 60; + private final double kPositionConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio; + private final double kVelocityConversionRatio = kPositionConversionRatio / 60; // setup feedforward private final double ksShooterVolts = 0.17875; @@ -63,6 +64,7 @@ public ShooterSubsystem() { // allow us to read the encoder m_ShooterMainEncoder = m_ShooterMotorMain.getEncoder(); + m_ShooterMainEncoder.setPositionConversionFactor(kPositionConversionRatio); m_ShooterMainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); // PID coefficients kP = 0.00032423; diff --git a/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java b/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java index e27792c..ef7566f 100644 --- a/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java +++ b/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; /** This Subsystem is what translates the output of the ultrasonic sensor to standard units. */ @@ -34,6 +35,7 @@ public void periodic() { // SmartDashboard.putNumber("Ultrasonic Sensor Distance", getRangeInches(ultrasonic1)); // Calculate what percentage of 5 Volts we are actually at voltageScaleFactor = 5 / RobotController.getVoltage5V(); + SmartDashboard.putNumber("Ring Distance", DistanceIn()); } @Override