Merge branch 'development' of https://github.com/awtybots/FRC-2024 in…

…to development Merge
awtybots · Feb 23, 2024 · c076bf1 · c076bf1
2 parents 73fa609 + 3f2c222
commit c076bf1
Show file tree

Hide file tree

Showing 4 changed files with 26 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -57,7 +57,7 @@ public static final class FlywheelConstants {
 
     // CAN ID's
     public static final int kTopFlywheelSparkMaxCanId = 3;
-    public static final int kBottomFlywheelSparkMaxCanId = 14;
+    public static final int kBottomFlywheelSparkMaxCanId = 6;
 
     // TODO implement the below
     public static final int kCurrentLimit = 30;
@@ -78,6 +78,7 @@ public static final class FlywheelConstants {
 
   public static final class IntakeConstants {
     public static final int kIntakeSparkMaxCanId = 7;
+    public static final int kFollowerIntakeSparkMaxCanId = 14;
 
     public static final int kCurrentLimit = 30;
 
@@ -104,6 +105,8 @@ public static final class ArmConstants {
 
     public static final double armConversion = 0.05; // TODO TUNE
 
+    public static final double kMaxOutput = 1.0; 
+
     // Arm PID constants
     public static final double kP = 0.1; // TODO TUNE
     public static final double kI = 0.0; // TODO TUNE
@@ -113,8 +116,8 @@ public static final class ArmConstants {
     public static final double ks = 0.10; // TODO TUNE
     public static final double kv = 0.05; // TODO TUNE
 
-    public static final double minimumAngle = -1.441;
-    public static final double maximumAngle = 1.441;
+    public static final double minimumAngle = -2 * 1.441;
+    public static final double maximumAngle = 2 * 1.441;
 
     // do not use
     // public static final double kFF = 0.0;
@@ -149,7 +152,7 @@ public static final class Presets {
   }
 
   public static final class WristConstants {
-    public static final int kWristMotorId = 6; // ! Change before testing
+    public static final int kWristMotorId = 60; // ! Change before testing
 
     public static final int kCurrentLimit = 30;
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -86,7 +86,7 @@ public class RobotContainer {
   private final ArmElevator sArmElevator;
   private final Wrist sWrist;
   private final Climber sClimber;
-//   private final Sticks sSticks;
+  //   private final Sticks sSticks;
 
   // Controllers
   private final CommandXboxController driverController = new CommandXboxController(0);

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java
@@ -28,7 +28,7 @@
  * "CANSparkFlex".
  */
 public class ArmIOSparkMax implements ArmIO {
-  private static final double GEAR_RATIO = 40.0 * (60.0 / 12.0); // May be reciprocal
+  private static final double GEAR_RATIO = 1; // May be reciprocal
 
   private final CANSparkMax leftMotor =
       new CANSparkMax(ArmConstants.kLeftArmMotorId, MotorType.kBrushless);
@@ -63,16 +63,17 @@ public ArmIOSparkMax() {
     rightMotor.setSmartCurrentLimit(ArmConstants.kCurrentLimit);
 
     rightMotor.follow(leftMotor, true);
+    pid.setFeedbackDevice(leftMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle));
 
     // leftMotor.burnFlash();
     // rightMotor.burnFlash();
   }
 
   @Override
   public void updateInputs(ArmIOInputs inputs) {
-    inputs.positionRad = Units.rotationsToRadians(leftRelativeEncoder.getPosition() / GEAR_RATIO);
+    inputs.positionRad = Units.rotationsToRadians(leftAbsoluteEncoder.getPosition() / GEAR_RATIO);
     inputs.velocityRadPerSec =
-        Units.rotationsPerMinuteToRadiansPerSecond(leftRelativeEncoder.getVelocity() / GEAR_RATIO);
+        Units.rotationsPerMinuteToRadiansPerSecond(leftAbsoluteEncoder.getVelocity() / GEAR_RATIO);
     inputs.appliedVolts = leftMotor.getAppliedOutput() * leftMotor.getBusVoltage();
     inputs.currentAmps = new double[] {leftMotor.getOutputCurrent()};
     inputs.targetPositionRad = targetAngle;
@@ -117,5 +118,6 @@ public void configurePID(double kP, double kI, double kD) {
     pid.setI(kI, 0);
     pid.setD(kD, 0);
     pid.setFF(0, 0);
+    pid.setOutputRange(-ArmConstants.kMaxOutput, ArmConstants.kMaxOutput);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java
@@ -31,21 +31,32 @@ public class IntakeIOSparkMax implements IntakeIO {
 
   private final CANSparkMax intakeMotor =
       new CANSparkMax(IntakeConstants.kIntakeSparkMaxCanId, MotorType.kBrushless);
+  private final CANSparkMax followerIntakeMotor =
+      new CANSparkMax(IntakeConstants.kFollowerIntakeSparkMaxCanId, MotorType.kBrushless);
 
   private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder();
+  private final RelativeEncoder followerIntakeEncoder = followerIntakeMotor.getEncoder();
+
   private final SparkPIDController intakePID = intakeMotor.getPIDController();
 
   public IntakeIOSparkMax() {
     intakeMotor.restoreFactoryDefaults();
+    followerIntakeMotor.restoreFactoryDefaults();
 
     intakeMotor.setCANTimeout(250);
+    followerIntakeMotor.setCANTimeout(250);
 
-    intakeMotor.setInverted(false);
+    intakeMotor.setInverted(true);
 
     intakeMotor.enableVoltageCompensation(12.0);
     intakeMotor.setSmartCurrentLimit(30);
+    followerIntakeMotor.enableVoltageCompensation(12.0);
+    followerIntakeMotor.setSmartCurrentLimit(30);
+
+    followerIntakeMotor.follow(intakeMotor, false);
 
     intakeMotor.burnFlash();
+    followerIntakeMotor.burnFlash();
   }
 
   @Override
@@ -65,7 +76,7 @@ public void setVoltage(double volts) {
   @Override
   public void setVelocity(double velocityRadPerSec, double ffVolts) {
     intakePID.setReference(
-        (6.0 / 458.0) * Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO,
+        (1.5 / 458.0) * Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO,
         ControlType.kVoltage,
         0,
         ffVolts,