buncha changes

Author: laelHI

src/main/java/frc/robot/subsystems/dropper/DropperConstants.java

@@ -5,11 +5,13 @@
 public class DropperConstants {
 
     public static Constraints kDropperConstraints;
+    
+    public static final int LEFT_DROPPER_MOTOR_ID = 0;
+    public static final int RIGHT_DROPPER_MOTOR_ID = 0;
+    
     public static final double UPPER_LIMIT = 1;
     public static final double LOWER_LIMIT = -1;
-
-    public static final int LEFT_MOTOR_ID = 0;
-    public static final int RIGHT_MOTOR_ID = 0;
+    public static final double PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679; // 100 digits of pi just cuz
 
 
 }

src/main/java/frc/robot/subsystems/dropper/DropperIOSparkMax.java

@@ -2,70 +2,72 @@
 
 import static frc.robot.Constants.NEO_CURRENT_LIMIT;
 
+import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Default;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.servohub.ServoHub.ResetMode;
 import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-
+import com.revrobotics.spark.SparkMax;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 
 public class DropperIOSparkMax implements DropperIO {
 
     private ProfiledPIDController controller = new ProfiledPIDController(0, 0, 0, new TrapezoidProfile(-12, 12));
-    private SparkFlex rightMotor;
+    private SparkMax rightMotor; //changed from spark flex to can spark max cuz 
     private RelativeEncoder rightEncoder;    
-    private SparkFlex leftMotor;
+    private SparkMax leftMotor;
     private RelativeEncoder leftEncoder;
+    private final ProfiledPIDController pidController;
+    private double pi = DropperConstants.PI;
 
     public DropperIOSparkMax() {
         //motors
-        leftMotor = new SparkFlex(DropperConstants.blank, MotorType.kBrushless);//make a constant for motor id and ask the type of motor
-        rightMotor = new SparkFlex(DropperConstants.blankmotorid, MotorType.kBrushless);
+        leftMotor = new SparkMax(DropperConstants.LEFT_DROPPER_MOTOR_ID, MotorType.kBrushless);//ask the type of motor
+        rightMotor = new SparkMax(DropperConstants.RIGHT_DROPPER_MOTOR_ID, MotorType.kBrushless);
+        
+        rightMotor.follow(leftMotor, false);
 
         leftEncoder = leftMotor.getEncoder();
         rightEncoder = rightMotor.getEncoder();
 
-        SparkFlexConfig configLeft = new SparkFlexConfig();
+        SparkMaxConfig configLeft = new SparkMaxConfig();
         configLeft
             .idleMode(IdleMode.kBrake)
             .voltageCompensation(12)
             .smartCurrentLimit(NEO_CURRENT_LIMIT)
             .inverted(true);
 
-        leftMotor.configure(configLeft, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-
-        SparkFlexConfig configRight = new SparkFlexConfig();
-        configRight
-            .idleMode(IdleMode.kBrake)
-            .voltageCompensation(12)
-            .smartCurrentLimit(NEO_CURRENT_LIMIT)
-            .inverted(false);
-        rightMotor.configure(configLeft, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);   
+        leftMotor.configure(configLeft, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);   
     }
     //methods from IO
 
     @Override
 
-    public void updateInputs(DropperIOInputs inputs){
-        //do later im lazy
+    public void updateInputs(DropperIOInputs inputs){ //update the imputs form io
+        inputs.appliedVoltage = leftMotor.getAppliedOutput() * leftMotor.getBusVoltage();
+        inputs.angVelocityRadsPerSec = leftEncoder.getVelocity() * 2 * pi / 60; // in rotations so radians per sec converted
+        inputs.angleRads = leftEncoder.getPosition() * 2 * pi;
+        inputs.angle = leftEncoder.getPosition() * 360;
+        inputs.velocity = leftEncoder.getVelocity();
     }
 
     @Override //not sure if there is one of these for each motor or not
 
     public double getAngle(){
-        return leftEncoder.getPosition() * 6.28; //its just two pi make a pi constant later
+        return leftEncoder.getPosition() * 2 * pi; //its just gets the rotations and convers to angle using two pi make a pi constant later
     }
 
     @Override
 
     public double getAngVelocity(){
         double rpm = leftEncoder.getVelocity();//in rotations per min i think so convert
-        double rps = rpm * 6.28 / 60; //radians per second (its just 2pi divide by 60)
+        double rps = rpm * 2 * pi / 60; //radians per second (its just 2pi divide by 60)
         return rps;
     }
 
@@ -77,7 +79,19 @@ public void setVoltage(double voltage) {
 
     @Override
 
-    public void setPosition(){
+    public void setPosition(int position){
+        leftEncoder.setPosition(position);
+    }
+    
+    @Override
+    public void setSetpoint(double setpoint){
+        pidController.setGoal(setpoint);
+        pidController.reset(getAngle(), getAngVelocity());
+
+    }
+
+    @Override
+    public void goToSetpoint(){
 
     }
 
@@ -89,15 +103,26 @@ public void stop(){
     }
 
     //PID
-
+    
+    @Override
     public void setPIDGains(double kp, ki, kd){
         velocityPID.setP(kp);//idek
         velocityPID.setI(ki);
-        velocityPID.setD(kd);
+        velocityPID.setD(kd); 
     }
 
-
-
+    @Override
+    public default double getP(){
+        return pidController.getP();
+    }
+    @Override
+    public default double getI(){
+        return pidController.getI();
+    }
+    @Override
+    public default double getD(){
+        return pidController.getD();
+    }
 }
 
 /*
@@ -112,4 +137,6 @@ public default void setPIDGains(double kp, ki, kd) {}
     public default double getP() {}
     public default double getI() {}
     public default double getD() {}
+
+
  */