Neo Motor Code Manual and Encoder

src/main/java/frc/robot/commands/SetMotor.java

@@ -10,30 +10,52 @@
 public class SetMotor extends Command {
   Motor m_motor;
   double m_spd;
+  boolean m_usePosition = false;
+  double m_targetRotations = 0.0;
+  boolean m_resetOnInit = false;
 
   public SetMotor(Motor motor1, double spd) {
     m_motor = motor1;
     m_spd = spd;
     addRequirements(m_motor);
   }
 
+  // New: position-target constructor using the NEO integrated encoder
+  public SetMotor(Motor motor1, double targetRotations, boolean resetOnInit) {
+    m_motor = motor1;
+    m_usePosition = true;
+    m_targetRotations = targetRotations;
+    m_resetOnInit = resetOnInit;
+    addRequirements(m_motor);
+  }
+
   // The initialize method is called when the command is initially scheduled.
   @Override
   public void initialize() {
     // set init values.
+    if (m_usePosition) {
+      if (m_resetOnInit) {
+        m_motor.resetPosition();
+      }
+      m_motor.turnToRotations(m_targetRotations);
+    }
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_motor.setSpeed(m_spd);
+    if (!m_usePosition) {
+      m_motor.setSpeed(m_spd);
+    }
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    // Always return false so the command never ends on it's own. In this project we use the
-    // scheduler to end the command when the button is released.
+    // For position mode, finish when we're at the setpoint; otherwise never finish on its own.
+    if (m_usePosition) {
+      return m_motor.atSetpoint();
+    }
     return false;
   }
 

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

@@ -5,24 +5,60 @@
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+// import com.revrobotics.spark.SparkRelativeEncoder;
+import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+// update this to neo motor
 public class Motor extends SubsystemBase {
   // private final SparkMax intake;
   private final SparkMax intake;
+  private RelativeEncoder encoder;
+  private final PIDController positionPID = new PIDController(0.1, 0.0, 0.0);
+  private double targetRotations = 0.0;
 
   public Motor() {
-    intake = new SparkMax(CANConstants.kMotorID, MotorType.kBrushed);
+    intake = new SparkMax(CANConstants.kMotorID, MotorType.kBrushless);
 
     SparkMaxConfig inkCFG = new SparkMaxConfig();
 
-    inkCFG.inverted(true).idleMode(IdleMode.kCoast).smartCurrentLimit(80, 80);
+    inkCFG.inverted(true).idleMode(IdleMode.kBrake).smartCurrentLimit(60);
+    intake.configure(inkCFG, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
+
+    encoder = intake.getEncoder();
+    encoder.setPosition(0.0);
+
+    positionPID.setTolerance(0.02);
   }
 
   public void setSpeed(double spd) {
     intake.set(spd);
   }
 
+  public double getPositionRotations() {
+    return encoder.getPosition();
+  }
+
+  public void resetPosition() {
+    encoder.setPosition(0.0);
+  }
+
+  public void turnToRotations(double rotations) {
+    targetRotations = rotations;
+  }
+
+  public boolean atSetpoint() {
+    return positionPID.atSetpoint();
+  }
+
   @Override
-  public void periodic() {}
+  public void periodic() {
+    double output = positionPID.calculate(getPositionRotations(), targetRotations);
+    output = MathUtil.clamp(output, -1.0, 1.0);
+    intake.set(output);
+  }
 }