This is my first time implementing feed forward and PID together and I was wondering if this code for the arm is being correctly implemented. Basically what I’m trying to do is have the arm move to a specific point and holding the position.
public void moveToAngle(){
Rotation2d[] angles = new Rotation2d[2];
//get angles needed for each joint
angles = this.getAngles(goal[0], goal[1]);
double angle = angles[0].getRadians();
goToAngleShoulder.setSetpoint(angle);
goToAngleShoulder.setTolerance(0.05, 0.1);
SmartDashboard.putNumber("Angle Goal Shoulder", angle);
angle = angles[1].getRadians();
goToAngleJoint.setSetpoint(angle);
goToAngleJoint.setTolerance(0.05, 0.1);
SmartDashboard.putNumber("Angle Goal Shoulder", angle);
SmartDashboard.putNumberArray("GoalPoint: ", goal);
while (!goToAngleJoint.atSetpoint() && !goToAngleShoulder.atSetpoint()){
shoulderMotor.setVoltage((goToAngleShoulder.calculate(EncoderShoulder.getPosition() * Math.PI - arm.Shoulder.angleOffset.getRadians())
+ Shoulderff.calculate(EncoderShoulder.getPosition() * Math.PI - arm.Shoulder.angleOffset.getRadians()
, EncoderShoulder.getVelocity()))*.12);
jointMotor.setVoltage((goToAngleJoint.calculate(EncoderJoint.getPosition()*Math.PI - arm.Joint.angleOffset.getRadians())
+ Jointff.calculate(EncoderJoint.getPosition() * Math.PI - arm.Joint.angleOffset.getRadians()
, EncoderJoint.getVelocity()))*.12);
}
}