building

Gears-and-Buccaneers · Jan 4, 2024 · cecb5d0 · cecb5d0
1 parent 55bf812
commit cecb5d0
Show file tree

Hide file tree

Showing 7 changed files with 133 additions and 97 deletions.
diff --git a/src/main/java/frc/lib/hardware/HardwareRequirements.java b/src/main/java/frc/lib/hardware/HardwareRequirements.java
@@ -7,12 +7,9 @@ public interface HardwareRequirements extends AutoCloseable, LoggableInputs {
 
   boolean connected();
 
-  default void toLog(LogTable table, String path) {
-  }
+  default void toLog(LogTable table, String path) {}
 
-  default void fromLog(LogTable table) {
-  }
+  default void fromLog(LogTable table) {}
 
-  default void toLog(LogTable table) {
-  }
+  default void toLog(LogTable table) {}
 }
diff --git a/src/main/java/frc/lib/hardware/Motors/Motor.java b/src/main/java/frc/lib/hardware/Motors/Motor.java
@@ -174,10 +174,8 @@ public double getPosition() {
     if (RobotBase.isReal()) {
       return mEncoder.getPosition();
     } else {
-      if (simVelocity)
-        return mSimPositionMeters;
-      else
-        return mSimPositionRad;
+      if (simVelocity) return mSimPositionMeters;
+      else return mSimPositionRad;
     }
   }
 
@@ -225,8 +223,10 @@ private void logSim(LogTable table) {
 
     mSim.update(Robot.defaultPeriodSecs);
 
-    mSimPositionRad = mSimPositionRad + (mSim.getAngularVelocityRadPerSec() * Robot.defaultPeriodSecs);
-    mSimPositionMeters += (mSim.getAngularVelocityRadPerSec() * Robot.defaultPeriodSecs) * Units.inchesToMeters(2);
+    mSimPositionRad =
+        mSimPositionRad + (mSim.getAngularVelocityRadPerSec() * Robot.defaultPeriodSecs);
+    mSimPositionMeters +=
+        (mSim.getAngularVelocityRadPerSec() * Robot.defaultPeriodSecs) * Units.inchesToMeters(2);
     mSimVelocityDeg = mSim.getAngularVelocityRadPerSec();
     mSimVelocityMeters = mSim.getAngularVelocityRadPerSec() * Units.inchesToMeters(2);
     // --------------------------------------------------------
@@ -245,10 +245,8 @@ private void logSim(LogTable table) {
 
   @Override
   public boolean connected() {
-    if (!mController.connected())
-      return false;
-    if (hasEncoder)
-      return mEncoder.connected();
+    if (!mController.connected()) return false;
+    if (hasEncoder) return mEncoder.connected();
     return true;
   }
 }
diff --git a/src/main/java/frc/lib/hardware/Motors/Motor2.java b/src/main/java/frc/lib/hardware/Motors/Motor2.java
@@ -5,15 +5,12 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+//
 import frc.lib.hardware.HardwareRequirements;
-import frc.lib.hardware.Motors.MotorControlers.MotorController;
-import frc.lib.hardware.Motors.MotorControlers.Talon_SRX;
-import frc.lib.hardware.Motors.PID.EncoderConfigs;
-import frc.lib.hardware.Motors.PID.PIDConfigs;
-import frc.lib.hardware.Motors.PID.SimConfig;
+import frc.lib.hardware.Motors.MotorControllers.*;
+import frc.lib.hardware.Motors.PID.*;
 import frc.lib.hardware.sensor.encoders.Encoder;
-import frc.robot.Robot;
+//
 import org.littletonrobotics.junction.LogTable;
 
 public class Motor2 implements HardwareRequirements {
@@ -34,7 +31,19 @@ protected DCMotor config() {
     }
   }
 
+  public enum ControllerType {
+    TalonSRX(new Talon_SRX());
 
+    MotorController mController;
+
+    private ControllerType(MotorController mController) {
+      this.mController = mController;
+    }
+
+    public MotorController config(int canID) {
+      return mController.build(canID);
+    }
+  }
 
   // -----------------------------------------
   private final MotorController mController;
@@ -54,38 +63,38 @@ public Motor2(ControllerType motorControllerType, int canID, Type motor) {
   }
 
   // ----------------- COnfigs ---------------------
-  public Motor addEncoder(Encoder encoder) {
+  public Motor2 addEncoder(Encoder encoder) {
     this.mEncoder = encoder;
     hasEncoder = true;
     return this;
   }
 
-  public Motor inverted(boolean enable) {
+  public Motor2 inverted(boolean enable) {
     mController.setInverted(enable);
 
     return this;
   }
 
-  public Motor brakeMode(boolean enable) {
+  public Motor2 brakeMode(boolean enable) {
     mController.brakeMode(enable);
 
     return this;
   }
 
   private PIDController mFeedback;
 
-  public Motor pidConfigs(PIDConfigs PIDConfigs) {
+  public Motor2 pidConfigs(PIDConfigs PIDConfigs) {
     mFeedback = new PIDController(1, 0, 0);
 
     return this;
   }
 
-  public Motor EncoderConfigs(EncoderConfigs encoderConfigs) {
+  public Motor2 EncoderConfigs(EncoderConfigs encoderConfigs) {
 
     return this;
   }
 
-  public Motor setName(String name) {
+  public Motor2 setName(String name) {
     this.name = name;
     return this;
   }
@@ -99,33 +108,28 @@ public void setPositoin(double Positoin) {
     if (!hasEncoder) {
       throw new Error("you dumb. This motor has no encoder");
     }
+    mFeedback.setSetpoint(Positoin);
 
-    mSimFeedback.setSetpoint(Positoin);
-    // mFeedback.setSetpoint(Positoin);
-
-    // mAppliedVolts = mFeedback.calculate(getPositoin());
-    // mAppliedVolts = MathUtil.clamp(mAppliedVolts, -12.0, 12.0);
-
-    setVolts(mAppliedVolts);
+    setVolts(clamp(mFeedback.calculate(getPositoin())));
   }
 
   public void setVelocity(double velocity) {
     if (!hasEncoder) {
       throw new Error("you dumb. This motor has no encoder");
     }
-    mSimFeedback.setSetpoint(velocity);
-    // mFeedback.setSetpoint(velocity);
-
-    // mAppliedVolts = mFeedback.calculate(getVelocity());
-    // mAppliedVolts = MathUtil.clamp(mAppliedVolts, -12.0, 12.0);
+    mFeedback.setSetpoint(velocity);
 
-    setVolts(mAppliedVolts);
+    setVolts(clamp(mFeedback.calculate(getVelocity())));
   }
 
   public void disable() {
     mController.disable();
   }
 
+  public double clamp(double setPoint) {
+    return MathUtil.clamp(setPoint, -12.0, 12.0);
+  }
+
   // get ---------------------------------
   public int getCanID() {
     return mController.getCanID();
@@ -142,12 +146,9 @@ public double getPositoin() {
       throw new Error("you dumb. This motor has no encoder");
     }
     if (RobotBase.isReal()) {
-      return mEncoder.getPositoin();
+      return mEncoder.getPosition();
     } else {
-      if (simVelocity)
-        return mSimPositionMeters;
-      else
-        return mSimPositionRad;
+      return 1;
     }
   }
 
@@ -158,7 +159,7 @@ public double getVelocity() {
     if (RobotBase.isReal()) {
       return mEncoder.getVelocity();
     } else {
-      return mSimVelocityDeg;
+      return 1;
     }
   }
 
@@ -179,10 +180,8 @@ public void toLog(LogTable table) {
 
   @Override
   public boolean connected() {
-    if (!mController.connected())
-      return false;
-    if (hasEncoder)
-      return mEncoder.connected();
+    if (!mController.connected()) return false;
+    if (hasEncoder) return mEncoder.connected();
     return true;
   }
 }
diff --git a/src/main/java/frc/lib/hardware/Motors/MotorControllers/Talon_SRX.java b/src/main/java/frc/lib/hardware/Motors/MotorControllers/Talon_SRX.java
@@ -25,19 +25,15 @@ public void runVolts(double num) {
 
   @Override
   public void brakeMode(boolean enable) {
-    if (enable)
-      motor.setNeutralMode(NeutralMode.Brake);
-    else
-      motor.setNeutralMode(NeutralMode.Coast);
+    if (enable) motor.setNeutralMode(NeutralMode.Brake);
+    else motor.setNeutralMode(NeutralMode.Coast);
   }
 
-
   public void disable() {
     motor.set(ControlMode.Disabled, 0);
   }
 
-  public void currentLimit() {
-  }
+  public void currentLimit() {}
 
   // ----------------------------------------------------------
   @Override
@@ -57,8 +53,7 @@ public boolean connected() {
 
   // Unit Testing
   @Override
-  public void close() throws Exception {
-  }
+  public void close() throws Exception {}
 
   @Override
   public double getAppliedVolts() {
@@ -95,5 +90,4 @@ public MotorController setSimulated(double simulated) {
     // TODO Auto-generated method stub
     throw new UnsupportedOperationException("Unimplemented method 'setSimulated'");
   }
-
 }
diff --git a/src/main/java/frc/lib/hardware/Motors/MotorControllers/simMC.java b/src/main/java/frc/lib/hardware/Motors/MotorControllers/simMC.java
@@ -0,0 +1,82 @@
+package frc.lib.hardware.Motors.MotorControllers;
+
+public class simMC implements MotorController {
+
+  @Override
+  public boolean connected() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'connected'");
+  }
+
+  @Override
+  public void close() throws Exception {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'close'");
+  }
+
+  @Override
+  public MotorController build(int canID) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'build'");
+  }
+
+  @Override
+  public void runVolts(double num) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'runVolts'");
+  }
+
+  @Override
+  public void brakeMode(boolean enable) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'brakeMode'");
+  }
+
+  @Override
+  public void disable() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'disable'");
+  }
+
+  @Override
+  public int getCanID() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'getCanID'");
+  }
+
+  @Override
+  public double getAppliedVolts() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'getAppliedVolts'");
+  }
+
+  @Override
+  public double getCurentAmps() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'getCurentAmps'");
+  }
+
+  @Override
+  public double getBusVoltage() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'getBusVoltage'");
+  }
+
+  @Override
+  public MotorController setInverted(boolean enable) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setInverted'");
+  }
+
+  @Override
+  public MotorController setCurrentLimit(double CurrentLimit) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setCurrentLimit'");
+  }
+
+  @Override
+  public MotorController setSimulated(double simulated) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setSimulated'");
+  }
+}
diff --git a/src/main/java/frc/lib/hardware/sensor/imu/IMU.java b/src/main/java/frc/lib/hardware/sensor/imu/IMU.java
@@ -41,6 +41,5 @@ default void toLog(LogTable table) {
   }
 
   @Override
-  default void fromLog(LogTable table) {
-  }
+  default void fromLog(LogTable table) {}
 }
diff --git a/src/test/java/IntakeUTest.java b/src/test/java/IntakeUTest.java