last changes

Author: laelHI

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

@@ -2,7 +2,7 @@
 
 public interface DropperIO {
 
-    public class DropperIOInputs {
+    public static class DropperIOInputs {
         public double angle;
         public double velocity;
         public double appliedVoltage;
@@ -14,11 +14,11 @@ public class DropperIOInputs {
     public default void updateInputs(DropperIOInputs inputs) {}
 
     public default double getAngle() {
-        return DropperIOInputs.angle;
+        return 0.0;
     }
 
     public default double getAngVelocity() {
-        return DropperIOInputs.velocity;
+        return 0.0;
     }
 
     public default void setVoltage(double voltage) {}

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

@@ -2,14 +2,23 @@
 
 import static frc.robot.Constants.NEO_CURRENT_LIMIT;
 
+import com.revrobotics.spark.SparkMax;
+
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
 
 public class DropperIOSim implements DropperIO {
-    
+    private  SparkMax leftMotor = new SparkMax(DropperConstants.LEFT_DROPPER_MOTOR_ID, MotorType.kBrushless);
+
     @Override
     public default void updateInputs(DropperIOInputs inputs) {
-
-    }
+        sim.update(0.02); //updates
+        inputs.appliedVoltage = leftMotor.getAppliedOutput() * leftMotor.getBusVoltage(); //from spark max
+        inputs.angVelocityRadsPerSec = leftEncoder.getVelocity() * 2 * pi / 60;
+        inputs.angleRads = leftEncoder.getPosition() * 2 * pi;
+        inputs.angle = leftEncoder.getPosition() * 360;
+        inputs.velocity = leftEncoder.getVelocity();
+      }
 
     @Override
     public default double getAngle() {

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

@@ -10,22 +10,30 @@
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.SparkMax;
+
+import edu.wpi.first.math.MathUtil;
 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;
+import edu.wpi.first.wpilibj.Timer;
 
 public class DropperIOSparkMax implements DropperIO {
 
     private ProfiledPIDController controller = new ProfiledPIDController(0, 0, 0, new TrapezoidProfile(-12, 12));
-    private SparkMax rightMotor; //changed from spark flex to can spark max cuz 
+    private SparkMax rightMotor; 
     private RelativeEncoder rightEncoder;    
     private SparkMax leftMotor;
     private RelativeEncoder leftEncoder;
     private final ProfiledPIDController pidController;
     private double pi = DropperConstants.PI;
 
+    //some variables for the methods like go to setpoint
+    
+    double lastSpeed = 0; //just blanl
+    double lastTime = Timer.getFPGATimestamp();
+
     public DropperIOSparkMax() {
         //motors
         leftMotor = new SparkMax(DropperConstants.LEFT_DROPPER_MOTOR_ID, MotorType.kBrushless);//ask the type of motor
@@ -89,10 +97,16 @@ public void setSetpoint(double setpoint){
         pidController.reset(getAngle(), getAngVelocity());
 
     }
-
     @Override
     public void goToSetpoint(){
+        double pidOutput = pidController.calculate(getAngle()); //feyrgfasrrkguilsrhfdljf;ksdjfakfjieowj;owedsjkllfjweo;ifjoasi;djlkjfwoiej;dksjf;sldjfoaejriofj;orjlkdjsoiifejrkldjsiojrkfjslkfjo;ei;rjdjkljfois;irrje;lsjkajfsoijrofajas
+        double acceleration = (pidController.getSetpoint().velocity - lastSpeed) / (Timer.getFPGATimestamp() - lastTime); //change in velocity over change in time... physics..
+    //acceleration of the PID setpoint
+
+        setVoltage(MathUtil.clamp(pidOutput, -12, 12));
 
+        lastSpeed = pidController.getSetpoint().velocity;
+        lastTime = Timer.getFPGATimestamp();
     }
 
     @Override
@@ -105,7 +119,7 @@ public void stop(){
     //PID
 
     @Override
-    public void setPIDGains(double kp, ki, kd){
+    public default void setPIDGains(double kp, ki, kd){
         velocityPID.setP(kp);//idek
         velocityPID.setI(ki);
         velocityPID.setD(kd);