DId I implement this PID + Feed Forward correctly?

Technical Programming
1

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);
        }
    }
1 Like
2

You almost certainly don’t want that while loop in there… Can you link to the rest of your code? You seem to have several variables in a larger scope, so we have no way of knowing what they are or if you’re using them correctly.

3
1 Like
4

What’s wrong with the while loop, don’t I have to keep updating the encoder value for the Feedback loop?

5

Read through the comments in Robot.java. Basically, your project exists within a framework that already runs a main loop. This loop is what handles the transition between modes (auto/teleop/disabled) and calls the *Init and *Periodic methods. Since you’re using command-based, these methods call the CommandScheduler::run() method, which is what determines what commands should be running and runs them. Each command has an execute() method that is run on each loop iteration.

Now with that description out of the way, what happens when you add a loop in your code? Well, it’ll sit there and block anything else from running. Best-case scenario, your arm does work, but you’re unable to drive or whatever while it’s changing position. Worst-case scenario, your code doesn’t actually get updated sensor values or write values to the motors and the robot visually does nothing. I think we can agree neither of these is ideal.

4 Likes
6

Ah, i see why our drivetrain stops moving when I trigger the arm now. So all I need is a if statement checking if the encoder value is at the set point?

7

As far as getting rid of the while loop, yup that’s all you need to do!

8

Ok thanks! Also where should my encoder be updated? Would it be in the Periodic method in the ArmSubsystem class?

9

Ok, so I tested the code today with an if loop replacing the while loop but for some reason the arm starts spinning once the robot is started even if I don’t press any buttons. I already put an if statement that is supposed to stop the arm if no button is pressed but it isn’t working