22
33import static frc .robot .Constants .NEO_CURRENT_LIMIT ;
44
5+ import com .revrobotics .RelativeEncoder ;
6+ import com .revrobotics .spark .SparkLowLevel .MotorType ;
57import com .revrobotics .spark .SparkMax ;
68
9+ import edu .wpi .first .math .controller .PIDController ;
710import edu .wpi .first .math .controller .ProfiledPIDController ;
811import edu .wpi .first .math .system .plant .DCMotor ;
912
1013public class DropperIOSim implements DropperIO {
1114 private SparkMax leftMotor = new SparkMax (DropperConstants .LEFT_DROPPER_MOTOR_ID , MotorType .kBrushless );
15+ private RelativeEncoder leftEncoder = leftMotor .getEncoder ();
16+ private final double pi = DropperConstants .PI ;
17+ private ProfiledPIDController pidControllerSim = new ProfiledPIDController (0 , 0 , 0 , DropperConstants .kDropperConstraints );
1218
1319 @ Override
14- public default void updateInputs (DropperIOInputs inputs ) {
20+ public void updateInputs (DropperIOInputs inputs ) {
1521 sim .update (0.02 ); //updates
1622 inputs .appliedVoltage = leftMotor .getAppliedOutput () * leftMotor .getBusVoltage (); //from spark max
1723 inputs .angVelocityRadsPerSec = leftEncoder .getVelocity () * 2 * pi / 60 ;
@@ -21,47 +27,46 @@ public default void updateInputs(DropperIOInputs inputs) {
2127 }
2228
2329 @ Override
24- public default double getAngle () {
30+ public double getAngle () {
2531 return DropperIOInputs .angle ;
2632 }
2733
2834 @ Override
29- public default double getAngVelocity () {
35+ public double getAngVelocity () {
3036 return DropperIOInputs .velocity ;
3137 }
3238
3339 @ Override
34- public default void setVoltage (double voltage ) {
40+ public void setVoltage (double voltage ) {
3541
3642 }
3743
3844 @ Override
39- public default void setPosition (int position ) {
45+ public void setPosition (int position ) {
4046
4147 }
4248
4349 @ Override
44- public default void stop () {}
50+ public void stop () {}
4551
4652 // PID
4753
4854 @ Override
49- public default void setPIDGains (double kp , ki , kd ) {
55+ public void setPIDGains (double kp , double ki , double kd ) {
5056
5157 }
5258
59+
5360 @ Override
54- public default double getP () {
55-
61+ public double getP (){
62+ return pidControllerSim . getP ();
5663 }
57-
5864 @ Override
59- public default double getI () {
60-
65+ public double getI (){
66+ return pidControllerSim . getI ();
6167 }
62-
6368 @ Override
64- public default double getD () {
65-
69+ public double getD (){
70+ return pidControllerSim . getD ();
6671 }
6772}
0 commit comments