Help with feedForward

NKRY_FRC · February 16, 2024, 1:42am

I’m trying to implement FeedForward in our arm, but I’m having difficulties. Every time he goes to the desired point with the setPoint he simply doesn’t stop at the point and hits the shooter. I have no idea what this could be as this is my first time using it.
Here is the code

github.com

KryptonFRC9487/Swerve_wek_6/blob/master/src/main/java/frc/robot/subsystems/Intake2.java

package frc.robot.subsystems;

import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.MotorPorts;

public class Intake2 extends SubsystemBase {

    private static CANSparkMax PIVOT_MOTOR, INTAKE_MOTOR;
    private final ProfiledPIDController pid;
    private final DutyCycleEncoder encoder;

This file has been truncated. show original

SLAB-Mr.Thomas · February 16, 2024, 1:59pm

If this was my first use of this method I’d follow more closely the WPILib documentation example. You appear to have significant differences with that example in the area of feed forward that you mention as your problem.

The docs say

Since the ProfiledPIDController goal differs from the setpoint, is if often desirable to poll the current setpoint of the controller (for instance, to get values to use with feedforward). This can be done with the getSetpoint() method.

The returned setpoint might then be used as in the following example:

Your feed forward code doesn’t appear to be working with the controller setpoint but with the difference between your position setpoint (goal, I think) and the measured position.

spearson23 · February 16, 2024, 2:58pm

A few things:

Your feed forward should use the set point in its calculations, not the difference between the set point and the current position.
Because you are using a TrapezoidProfile, the set point isn’t static. It changes with the TrapezoidProfile. So you need to update the feed forward term in periodic(). Not just set it once like with a normal PID.
You are doing a clamp on your output. This won’t hurt anything, but it is redundant since you are setting the TrapezoidalProfile max velocity to 0.5 already.

system · February 15, 2025, 2:58pm

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