Code Help with PID Loops

Technical
, ,
1

Beginning coder here! We are trying to figure out how to use PID control to control an arm. Our arm uses a REV Through Bore encoder in Absolute encoder mode, with 2 SparkMAX driven NEOs (facing opposite directions), with one in follower mode (inverted true). The lead motor has the sensor connected through a breakout board. We do have the physical chains for those motors removed, until we can test it for direction and speed (don’t want to tear things up).

We basically copied the REV Smart Motion example into our Arm subsystem, but whenever we try and change the setPoint in SmartDashboard (or ShuffleBoard) with the robot enabled, the code crashes and disables the robot.

As long as we don’t try to change those values, everything else on the robot works (drive base, intake and shooter motors). Using simple motor.set() methods, we did get the specified motors moving (verified that they moved the same way), and that doesn’t crash the code. We also aren’t sure of the best way to code setPoints to specific buttons on our second controller. We have tried looking at the various examples (WPILib and REV) but can’t seem to localize where our mistakes are. Code below:

Worst case scenario, we will code some simple if-thens to move our arm to certain encoder points (or do a manual joystick controlled arm, with encoder stop values, ew), but we were hoping to learn some basic PID control this year for smoother and more exact control.

2

What’s the error message.

3

Are you changing your P value on the smart dash? Or is it still 0 when you run setPostion()?

We have a similar Arm subsystem. Still a few bits that are work-in-progress, but we were able move to positions on an absolute encoder. Here’s the code.

4

The set of three messages from the log file are:

Loop time of 0.02s overrun
CommandScheduler loop overrun Warning at edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:412): Loop time of 0.02s overrun
Scoring.periodic(): 0.000056s
Arm.periodic(): 0.001410s
buttons.run(): 0.011111s
DriveSubsystem.periodic(): 0.083790s
disabledPeriodic(): 0.000761s Shuffleboard.update(): 0.000056s LiveWindow.updateValues(): 0.000074s robotPeriodic(): 0.316667s disabledInit(): 0.000296s Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): SmartDashboard.updateValues(): 0.001725s SmartDashboard.updateValues(): 0.001725s
disabledInit(): 0.000296s
robotPeriodic(): 0.316667s
LiveWindow.updateValues(): 0.000074s
Shuffleboard.update(): 0.000056s
disabledPeriodic(): 0.000761s
DriveSubsystem.periodic(): 0.083790s buttons.run(): 0.011111s Arm.periodic(): 0.001410s Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): Scoring.periodic(): 0.000056s

5

We were leaving the P value at 0 initially; just trying to get movement on the motors (so that we could reconnect the arm) before we started tuning the PID values. Should we put it at an arbitrarily small value initially? What I’ve read seems to say you should start with it at 0 when tuning.

6

Do not use SmartMotion for position control. It does not actually use position feedback.

7

We tried using the example code from REV for SparkMAX closedloop position control, and were having similar issues (code immediately crashing/disabling the robot) whenever we tried to edit the setPoint in the SmartDashboard (or even when we pass a value using setReference()). Which is why we tried the SmartMotion, just to see if we could get the motors to spin at all. It isn’t a hardware issue (we think), because a simple motor.set() worked to spin the motors.

Will keep trying to rework it (another complete rewrite!), probably using the code from @DrewTheRat, but that will have to wait until tomorrow.

8

Is that the only thing printing? That doesn’t cause a crash.

9

Those are the only Warnings. No errors or faults. The robot will be enabled, then we change the value on the Smart Dashboard, hit enter, and the light stops, the robot is disabled. I have 4-5 logs with the same (or very similar) warnings, because we would clear the Driver Station log, and try it again, with the same result.

I think one test we ran was commenting out all of the Smart Dashboard parts, and it didn’t crash, but we also didn’t have the ability to change the SetPoint (or PID and other values), since that is how the example REV code has it set to adjust the setpoint, I’m assuming for tuning purposes.

10

Thank you @DrewTheRat ! Your code made much more sense to me than the SpakMAX example codes. Still not sure what wasn’t working on my previous subsystem, I just created a new one modeled on yours, and I got it working. Now to just set the controller buttons and tune the PID values, and find our specific setpoints.

2 Likes
11

REV’s software examples are not generally of extremely high quality, and are pretty old to boot.

Working with everything on the RIO before jumping to using the build-in motor controller features is a good idea.

12

If you are running SmartDashboard on same laptop as driver station, pressing enter will disable the robot. Change a value in SmartDashboard and then click outside the text box. That should cause the value to be read/updated.

13

Generally I agree, and FWIW, CTRE’s aren’t much better. That said, REV’s MAXSwerve-Java-Template is a very good sample project. Their MAXSwerveModule class is a more useful example of both position and velocity closed loop control. We patterned our Arm code off the steering motor PID controller.

14

If you want to have functionality similar to Smart Motion is supposed to do (but doesn’t), and still take advantage of doing PID onboard the Spark, you could calculate a trapezoid motion profile on the RIO, then pipe the output of that to the Spark Max in regular position PID mode.

We’ve done the work of implementing this in our wrapper library PurpleLib.

15

I’d agree re: the Phoenix 5 examples, but the Phoenix 6 examples are quite good.