SingleJointedArmSim not working for arm

I already had it working for one so I just copied the code from that one and changed the names but now the new one doesn’t work. It seems that gravity isn’t effected. When I log the angle I get like -0.0274 degrees. Why is my elbow simulated ligament not dropping?

package frc.robot.subsystems;

import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.simulation.BatterySim;
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.commands.DriveArm;

public class ArmSubsystem extends SubsystemBase{
        
        private double m_armKp = 0.02;
        private double m_armKg = 0.109;
        //private double motorSpeed = 0;
        public CANSparkMax armMotor = new CANSparkMax(Constants.CAN.Arm.armMotor, MotorType.kBrushless); 
        public RelativeEncoder armEncoder = armMotor.getEncoder(); //in terms of revolutions
        SingleJointedArmSim armSimShoulder = new SingleJointedArmSim(DCMotor.getNEO(1), 
                 Constants.Arm.gearing, SingleJointedArmSim.estimateMOI(Constants.Arm.armLengthMeters, Constants.Arm.armMassKg), 
                 Constants.Arm.armLengthMeters, -628, 628, true);
        SingleJointedArmSim armSimElbow = new SingleJointedArmSim(DCMotor.getNEO(1), 
        Constants.Arm.gearing, SingleJointedArmSim.estimateMOI(Constants.Arm.armLengthMeters, Constants.Arm.armMassKg), 
        Constants.Arm.armLengthMeters, -628, 628, false);

        private final PIDController m_controller = new PIDController(m_armKp, 0, 0); //0.02 works well for kP in sim
        private ArmFeedforward feedforward = new ArmFeedforward(0.0, m_armKg, 0.0); //0.109 works well for kG in sim
        private double m_armSetpointDegrees = 0;

        private final Mechanism2d m_mech2d = new Mechanism2d(2, 2);
        
        private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 1, 1);
        private final MechanismLigament2d m_armTower =
                m_armPivot.append(new MechanismLigament2d("ArmTower", 1, -90));
        private final MechanismLigament2d m_armShoulder =
                m_armPivot.append(
                new MechanismLigament2d(
                        "Shoulder",
                        0.5,
                        Units.radiansToDegrees(armSimShoulder.getAngleRads()),
                        6,
                        new Color8Bit(Color.kGreen)));
        private final MechanismLigament2d m_armElbow =
        m_armPivot.append(
                new MechanismLigament2d(
                        "Elbow",
                        0.5,
                        Units.radiansToDegrees(armSimElbow.getAngleRads()),
                        6,
                        new Color8Bit(Color.kYellow)));
        double currentMotorEffort = 0.0;
        public ArmSubsystem(){
                this.setDefaultCommand(new DriveArm(this));
                Preferences.setDouble(Constants.Arm.kArmPositionKey, 0.0);
                SmartDashboard.putData("Arm Sim", m_mech2d);
                m_armTower.setColor(new Color8Bit(Color.kBlue));

                armMotor.clearFaults();
                armMotor.restoreFactoryDefaults();
                armMotor.disableVoltageCompensation();
                armMotor.setInverted(true);
                armMotor.setIdleMode(IdleMode.kBrake);
                armMotor.setSoftLimit(SoftLimitDirection.kForward, (float)(Constants.Arm.maxAngle * Constants.Arm.gearing));
                armMotor.setSoftLimit(SoftLimitDirection.kReverse, (float)(Constants.Arm.minAngle * Constants.Arm.gearing));
                armMotor.burnFlash();

                armEncoder.setPosition(0);

                Preferences.initDouble(Constants.Arm.kArmPositionKey, m_armSetpointDegrees);
                Preferences.setDouble(Constants.Arm.kArmPositionKey, m_armSetpointDegrees);
                Preferences.initDouble(Constants.Arm.kArmPKey, m_armKp);
                Preferences.initDouble(Constants.Arm.kArmGKey, m_armKg);
                Preferences.setDouble(Constants.Arm.kArmPKey, m_armKp);
                Preferences.setDouble(Constants.Arm.kArmGKey, m_armKg);
        }

        @Override
        public void periodic() {
                m_armSetpointDegrees = MathUtil.clamp(m_armSetpointDegrees, Constants.Arm.minAngle, Constants.Arm.maxAngle);
                double effort = feedforward.calculate(Units.degreesToRadians(m_armSetpointDegrees), 0) + m_controller.calculate(
                        (armEncoder.getPosition() / Constants.Arm.gearing) * 360.0,
                        m_armSetpointDegrees
                );
                //SmartDashboard.putNumber("current target", effort);
                //SmartDashboard.putNumber("encoder position", armEncoder.getPosition());
                effort = MathUtil.clamp(effort, -1, 1);
                currentMotorEffort = effort;
                armMotor.set(effort); //used to be effort
        }
        public void commandAngle(double angle) {
                angle %= 360.;
                m_armSetpointDegrees = angle;
                Preferences.setDouble(Constants.Arm.kArmPositionKey, angle);
        }

        @Override
        public void simulationPeriodic() {
                // In this method, we update our simulation of what our arm is doing
                // First, we set our "inputs" (voltages)
                armSimShoulder.setInput(currentMotorEffort * RobotController.getBatteryVoltage());
                armSimElbow.setInput(0.0);
            
                // Next, we update it. The standard loop time is 20ms.
                armSimShoulder.update(0.02);
                armSimElbow.update(0.02);
            
                // Finally, we set our simulated encoder's readings and simulated battery voltage
                armEncoder.setPosition((int) ((armSimShoulder.getAngleRads() / 6.28) * Constants.Arm.gearing));
                // SimBattery estimates loaded battery voltages
                RoboRioSim.setVInVoltage(
                    BatterySim.calculateDefaultBatteryLoadedVoltage(/*armSimShoulder.getCurrentDrawAmps() +*/ armSimElbow.getCurrentDrawAmps()));
            
                // Update the Mechanism Arm angle based on the simulated arm angle
                m_armShoulder.setAngle(Units.radiansToDegrees(armSimShoulder.getAngleRads()));
                System.out.println(Units.degreesToRadians(armSimElbow.getAngleRads()));
                m_armElbow.setAngle(Units.degreesToRadians(armSimElbow.getAngleRads()));
        }     

        public void loadPreferences() {
                m_armSetpointDegrees = Preferences.getDouble(Constants.Arm.kArmPositionKey, m_armSetpointDegrees);
                if (m_armKp != Preferences.getDouble(Constants.Arm.kArmPKey, m_armKp)) {
                  m_armKp = Preferences.getDouble(Constants.Arm.kArmPKey, m_armKp);
                  m_controller.setP(m_armKp);
                }
                if(m_armKg != Preferences.getDouble(Constants.Arm.kArmGKey, m_armKg)){
                        m_armKg = Preferences.getDouble(Constants.Arm.kArmGKey, m_armKg);
                        feedforward = new ArmFeedforward(0, m_armKg, 0);
                }
        }

        public double getAngle(){
                return m_armSetpointDegrees;
        }

	public void resetAngle(double angle) {
                Preferences.setDouble(Constants.Arm.kArmPositionKey, angle);
	}
}

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.