My friend malamar

src/main/java/frc/robot/subsystems/grabber/GrabberConstants.java

@@ -0,0 +1,85 @@
+package frc.robot.subsystems.grabber;
+
+import static edu.wpi.first.units.Units.Milliseconds;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.units.measure.Voltage;
+
+public class GrabberConstants {
+
+    public static final int kDistanceSensorInput = 1 ;
+    public static final double kDistanceSensorSlope = 1.0 ;
+    public static final double kDistanceSensorIntercept = 0.0 ;
+
+    public static final Voltage kCollectVoltage = Volts.of(3.0) ;
+
+    public class Grabber {
+
+        public static final int kMotorCANID = 1;
+        public static final double kGearRatio = 1.0;
+        public static final boolean kInverted = true; 
+        public static final double kHoldingVoltage = -3.0 ;
+        public static final double kDepositVoltage = 3.0 ;
+
+        public class PID {
+            public static final double kP = 5.0; 
+            public static final double kI = 0.0 ;
+            public static final double kD = 0.0 ;
+            public static final double kV = 0.14 ;
+            public static final double kA = 0.0 ;
+            public static final double kG = 0.0 ;
+            public static final double kS = 0.36102 ;
+        }
+
+        public class MotionMagic {
+            public static final double kMaxVelocity = 60.0 ;
+            public static final double kMaxAcceleration = 300.0 ;
+            public static final double kJerk = 0.0 ;
+        }
+
+        public class CoralSensor {
+            public static final int kChannel = 0;
+        }
+
+        public class AlgaeSensor {
+            public static final int kChannel1 = 1;
+            public static final int kChannel2 = 5;
+        }
+
+        public class DepositCoral {
+            public static final AngularVelocity l1velocity = RotationsPerSecond.of(40.0) ;
+            public static final AngularVelocity velocity = RotationsPerSecond.of(100.0) ;
+            public static final Time delay = Milliseconds.of(400.0) ;
+            public static final Time l1delay = Milliseconds.of(1000.0) ;
+        }
+
+        public class CollectCoral {
+            public static final AngularVelocity kVelocity = RotationsPerSecond.of(15.0) ;
+            public static final AngularVelocity kBackupVelocity = RotationsPerSecond.of(-20.0) ;
+        }
+
+        public class CollectAlgae {
+            public static final AngularVelocity velocity = RotationsPerSecond.of(-20.0) ;
+        }
+
+        public class DepositAlgae {
+            public static final AngularVelocity velocity = RotationsPerSecond.of(-120.0) ;
+            public static final Time delay = Milliseconds.of(500.0) ;
+        }
+
+        public class Positions {
+            public static final double CoralPositionVelocity = 0.0;
+
+            public static final double ejectCoralVelocty = 1.0;
+            public static final double ejectCoralWait = 0.0;
+
+            public static final double collectAlgaeVelocity = 0.0;
+            public static final double ejectAlgaeVelocity = 0.0;
+            public static final double ejectAlgaeWait = 0.0;
+        }
+
+    }
+}

src/main/java/frc/robot/subsystems/grabber/GrabberIO.java

@@ -1,7 +1,43 @@
 package frc.robot.subsystems.grabber;
 
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
+import static edu.wpi.first.units.Units.Volts;
 
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularAcceleration;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
 
 public interface GrabberIO {
 
+    @AutoLog
+    public class GrabberIOInputs {
+
+        public Angle grabberPosition = Radians.zero();
+        public AngularVelocity grabberVelocity = RadiansPerSecond.zero();
+        public AngularAcceleration grabberAccel = RadiansPerSecondPerSecond.zero();
+        public Voltage grabberVoltage = Volts.zero();
+        public Current grabberCurrent = Amps.zero();
+
+        public boolean hasAlgae = false;
+        public boolean algaeSensor1 = false;
+        public boolean algaeSensor2 = false;
+
+        public boolean hasCoral = false;
+        public boolean coralSensor = false;
+    }
+
+    public default void updateInputs(GrabberIOInputs inputs){};
+
+    public default void setGrabberVoltage(Voltage voltage){};
+
+    public default void setGrabberTarget(Angle angle){};
+
+
 }

src/main/java/frc/robot/subsystems/grabber/GrabberIOHardware.java

@@ -1,5 +1,13 @@
 package frc.robot.subsystems.grabber;
 
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import frc.robot.subsystems.grabber.GrabberConstants.Grabber;
+
 public class GrabberIOHardware {
-
+    private TalonFX grabber_motor_;
+
+    public GrabberIOHardware() {
+        grabber_motor_ = new TalonFX(GrabberConstants.Grabber.kMotorCANID);
+    }
 }

src/main/java/frc/robot/subsystems/grabber/GrabberSubsystem.java

@@ -1,5 +1,20 @@
 package frc.robot.subsystems.grabber;
 
-public class GrabberSubsystem {
+import static edu.wpi.first.units.Units.Milliseconds;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class GrabberSubsystem extends SubsystemBase{
 
 }