Created a rough version of funnel and funnel map

src/main/java/frc/robot/maps/subsystems/FunnelMap.java

@@ -1,9 +1,37 @@
 package frc.robot.maps.subsystems;
 
+import com.chopshop166.chopshoplib.ValueRange;
 import com.chopshop166.chopshoplib.motors.SmartMotorController;
+import com.chopshop166.chopshoplib.logging.DataWrapper;
+import com.chopshop166.chopshoplib.logging.LoggableMap;
+import com.chopshop166.chopshoplib.logging.data.MotorControllerData;
+import com.chopshop166.chopshoplib.sensors.IEncoder;
 
-public class FunnelMap {
+public class FunnelMap implements LoggableMap<FunnelMap.Data> {
 
     public SmartMotorController motor;
+    public final IEncoder encoder;
+    public final ValueRange hardLimits;
+    public final ValueRange softLimits;
+
+    public FunnelMap(SmartMotorController motor, IEncoder encoder, ValueRange hardLimits, ValueRange softLimits) {
+        this.motor = motor;
+        this.encoder = encoder;
+        this.softLimits = softLimits;
+        this.hardLimits = hardLimits;
+    }
+
+    @Override
+    public void updateData(Data data) {
+        data.motor.updateData(motor);
+        data.rotationAbsAngleDegrees = encoder.getAbsolutePosition();
+        data.rotatingAngleVelocity = encoder.getRate();
+    }
+
+    public static class Data extends DataWrapper {
+        public MotorControllerData motor = new MotorControllerData();
+        public double rotationAbsAngleDegrees;
+        public double rotatingAngleVelocity;
+    }
 
 }

src/main/java/frc/robot/subsystems/Data.java

@@ -0,0 +1,5 @@
+package frc.robot.subsystems;
+
+public class Data {
+
+}

src/main/java/frc/robot/subsystems/Funnel.java

@@ -1,5 +1,59 @@
 package frc.robot.subsystems;
 
-public class Funnel {
+import java.util.function.DoubleSupplier;
 
+import com.chopshop166.chopshoplib.logging.LoggedSubsystem;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.maps.subsystems.FunnelMap;
+import frc.robot.maps.subsystems.FunnelMap.Data;
+
+public class Funnel extends LoggedSubsystem<Data, FunnelMap> {
+    private final double RAISE_SPEED = 0.5;
+    private final double LOWER_SPEED = 0.3;
+    private final double MANUAL_LOWER_SPEED_COEF = 0.3;
+    private final double SLOW_DOWN_COEF = 0.3;
+
+    public Funnel(FunnelMap funnelMap) {
+        super(new Data(), funnelMap);
+    }
+
+    Command move(DoubleSupplier rotateSpeed) {
+
+        return run(() -> {
+            double speed = rotateSpeed.getAsDouble();
+            double speedCoef = RAISE_SPEED;
+            if (speed < 0) {
+                speedCoef = MANUAL_LOWER_SPEED_COEF;
+            }
+            if (Math.abs(speed) > 0) {
+                getData().motor.setpoint = (limits(speed * speedCoef));
+
+            } else {
+                getData().motor.setpoint = 0.0;
+            }
+
+        });
+    }
+
+    private double limits(double speed) {
+        double height = getFunnelAngle();
+        speed = getMap().hardLimits.filterSpeed(height, speed);
+        speed = getMap().softLimits.scaleSpeed(height, speed, SLOW_DOWN_COEF);
+        return speed;
+    }
+
+    private double getFunnelAngle() {
+        return getData().rotationAbsAngleDegrees;
+    }
+
+    @Override
+    public void reset() {
+        getMap().encoder.reset();
+    }
+
+    @Override
+    public void safeState() {
+        getData().motor.setpoint = 0;
+    }
 }