How to use WPILib TrapezoidalMotionSubsystem when starting at a non zero position?

Say my motor is starting with an initial angle of 1 radian, and I want it to follow a trapezoidal motion profile to end with a final angle of 2 radians. I can call trapProfileSubsyst_.SetGoalState({units::radian_t{1}, units::radians_per_second_t{0}}); //desired goal distance is 1 away in my code to set a goal state of being 1 radian away, but the profiled positions start at zero. So, when I do PID.Calculate(motor_rads, goal_profile_rads), my motor will turn the opposite way because motor_rads starts at 1 and goal_profile_rads (which I’m getting through the UseState function) starts at 0. I’ve seen that potentially one option may be to use the ProfiledPIDController class, but if possible I like being able to have my motion profiling separate from my PID and feedforward objects. What should I do to solve this issue?

1 Like

I believe you may need to call reset() on your PID controller. We experienced a similar issue. Can you post your entire repo?

1 Like

I’m currently working in a test repo, which is here: GitHub - SusanC3/MotionProfiling
My class extending TrapezoidProfileSubsystem is ProfileSubsystem, and I’m using the MotorSim class to test basic motor movements in the WPILib simulation.

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