Elevator Motion Profiling / PID Ramping

[Jared]

Originally Posted by Tom Bottiglieri

Off topic a bit - I saw your team was using Java this year. Have you run into issues with the timing of the JVM's Timer? We have used this construct successfully over the past two years, but on the roborio the timing is complete garbage. We have ditched using derivatives on any control loops due to massive noise.

My best theory is the squawkVM was ported manually and used VXworks timers under the hood. The standard JRE we use now probably just sleeps or something.

Yes. Again and again, we run into weird timing issues with the roboRIO. The cRIO's timing jitter couldn't be measured with a millisecond timer, but the roboRIO's can be. The roboRIO occasionally gets suddenly very bad at timing for 10 or so seconds. I battled lots of weird intermittent timing bugs with the roboRIO when trying to get our autonomous down - we're driving, lifting, swatting, intaking all at the same time.

Our elevator control originally used PD control, but we had intermittent issues with oscillation.

Our PID controller is here: https://github.com/dicarlo236/2015-R...robot/PID.java
Our Updater that deals with PID is here: https://github.com/dicarlo236/2015-R...t/Updater.java

I assumed that I messed up something in my PID class for the derivative calculation, so I took it out, and we're only running P control now. However, we still experienced overshoot of 4" or more about 1 in 100 times. I've only had the bug happen once when I was analyzing loop timing, and it was caused by TimerTask waiting too long to call the run() method, and the P controller couldn't run and update the motor value. I was originally running all control loops (elevator PID, drive followers) in the same thread, and I split it up into separate threads for elevator and drive, but it did not solve the problem.


I have two videos from our competition that show the problem. In the first video, you can see that the robot drives smoothly and accurately in autonomous mode, and the elevator moves properly throughout the whole match. If you watch our robot load from the feeder station or place totes, you'll see that the elevator is tuned well to work well under any load. Randomly, this exact elevator code will have tiny issues/jerky movement, then go back to normal.

https://www.youtube.com/watch?v=7vw2...ature=youtu.be

In the second train wreck of an autonomous mode, our robot doesn't drive smoothly at the beginning, the swatter swats too early, the intake turns on too late, the turn begins too early, the drop happens too late, and the drive stops too late. In teleop, the elevator control has an issue and doesn't drop low enough. At that point, I switched to manual control.

https://www.youtube.com/watch?v=2pr8...ature=youtu.be

The second video was likely taken during our second finals match (the problem happened twice). The identical autonomous program ran in the first and third finals match, and worked correctly both times. This behavior was very close to costing us the event, and I'd really like to get a good idea of what's happening.

I am not 100% sure what caused the issues, but here is my current thought as to what went on.

The first time it happened, it was triggered by an e-stop of the field. The match was started while the announcer was in the middle of announcing the first alliance, and the field people e-stopped the robot. I went out, reset the robot, but when I got back to the driver station, the robot was in autonomous disabled, not teleop disabled like it normally is when it is first FMS connected. For some weird reason, this caused the FMS not to communicate the remaining match time to the robot. I detect whether the robot is connected to the FMS by checking to see if the time remaining returns a reasonable number, which decides if we should run the match version of autonomous mode vs. the practice mode. The two versions are exactly identical, except that the practice mode fails to run System.gc() at the beginning because I accidently commented out the wrong line in queue.

The second time it happened, I messed up a few changes when we dialed in our drop/back up for the carpet at the event, which was a little different from the old carpet back home. I removed only the line system.gc().

We don't have any mentors on our team who are familiar with Java or are programmers, so I've been unable to really figure out why System.gc() helps with loop timing.

There's also an issue of the roboRIO's internal clock. On powerup, it thinks it's 1970. If you start your timer task then, then connect to the driver station, which syncs the roboRIO's date/time, it thinks it has to make up for 45 years of not running your code, which causes other issues. Both times when our auto failed, this was NOT the problem, as the elevator won't run if this case happens, and the elevator ran both times.

[Spoam]

Originally Posted by AdamHeard

Gear slower. It will do wonders here.

+1 for this. As a team that also uses PID for elevator control (although we mostly use it to minimize the torsional stress caused by our braking mechanism) the first thing we did to get a manageable speed was lower our gearing by a lot. That has the added benefit of providing some resistance against gravity. Worked great, as Adam suggests. It sounds like right now you are unnecessarily fast and you'll get the most out of your motors if you gear slower.

Originally Posted by Tom Bottiglieri

Off topic a bit - I saw your team was using Java this year. Have you run into issues with the timing of the JVM's Timer? We have used this construct successfully over the past two years, but on the roborio the timing is complete garbage. We have ditched using derivatives on any control loops due to massive noise.

My best theory is the squawkVM was ported manually and used VXworks timers under the hood. The standard JRE we use now probably just sleeps or something.

Hmm, I'm curious about this. We also use java, but all our mechanisms are currently running PI control. Thank you for pointing this out.

[Tom Bottiglieri]

Originally Posted by Jared

We don't have any mentors on our team who are familiar with Java or are programmers, so I've been unable to really figure out why System.gc() helps with loop timing.

1) Any object allocations (using the 'new' operator) cannot be assumed to be constant time, as it is basically just mallocing. System.gc is probably just clearing out a portion of memory large enough to not cause the memory allocator to have to look very far to find a spot. We allocate very infrequently and never in control loops to help with this issue.
2) The garbage collector probably won't need to run much within 15 seconds after you explicitly call it. This could be eating CPU.

The real answer is probably to use C/C++.

:-/

[Jared]

Originally Posted by Tom Bottiglieri

1) Any object allocations (using the 'new' operator) cannot be assumed to be constant time, as it is basically just mallocing. System.gc is probably just clearing out a portion of memory large enough to not cause the memory allocator to have to look very far to find a spot. We allocate very infrequently and never in control loops to help with this issue.
2) The garbage collector probably won't need to run much within 15 seconds after you explicitly call it. This could be eating CPU.

I do not believe we are allocating anything in our control loops. All our variables are declared and our objects instantiated outside of the loops to avoid memory leaks.

Our CPU usage is around 50% during normal use and 60-65% during auto.

It may just be a coincidence that System.gc() didn't run when we had issues.

[Merfoo]

Originally Posted by Jared

Our PID controller is here: https://github.com/dicarlo236/2015-R...robot/PID.java
Our Updater that deals with PID is here: https://github.com/dicarlo236/2015-R...t/Updater.java

I assumed that I messed up something in my PID class for the derivative calculation, so I took it out, and we're only running P control now.

I believe the mistake you made for the derivative calculation is that you never update the variable lastError in your update function. You also don't appear to initialize the variables i and lastError so if you try to run this code you'll get variable uninitialized errors, but I think you fixed this at competition and just didn't push that code to github else I don't see how this could work.

@Override
	public void update() {
		// PID calculations
		double dt = .02; // length of time between each calculation
		// calculate how far from our target we are
		double error = setPoint - source.pidGet();
		// p is proportional to error and our kP gain
		double p = error;
		// integrate error
		i += error * dt;
		// make sure it isn't too big
		coerce(i, -max_I, max_I);
		// differentiate error
		double dError_dt = (error - lastError) / dt;
		double d = dError_dt;
		//output is sum of all terms multiplied by gains
		double result = kP * p + kI * i + kD * d;

		// check to see if we're at our target
		if ((Math.abs(dError_dt) < max_derror_dt)
				&& (Math.abs(error) < max_error)) {
			// reset I if we want to
			//possibly add decay?
			i = (reset_I_whenOnTarget) ? 0 : i;
		}
		
		//set output
		if(enabled){
			output.pidWrite(result);
		}
		
		
		
	}

[Ether]

Originally Posted by Aero

our elevator is really fast. It's about 0.75 seconds from bottom to top.

First, take Adam's advice about gearing (post#13). Then...

Originally Posted by Aero

motion profiling really is what we want. How inefficient would it be to generate a new motion profile every time we change setpoints?

Well it depends on your definition of inefficient I guess.

If you need to change the setpoint only when the speed is zero, it's pretty straightforward to generate a sinusoidal profile for smooth acceleration and deceleration.

Given the max desired acceleration M and the distance D to the new position, compute the constants T, K₁, K₂, and K₃ as shown in the equations. Then you can use the functions a(t), v(t), and x(t) to generate a nice smooth trajectory to the new target. "t" is elapsed time from start of profile.

Note that T will be the time-to-destination and K₂ will be the maximum speed.

See the example profile for M=3.5 ft/s² and D=5 ft.

[Gdeaver]

I haven't seen much discussion about this on CD but, There is a fuzy logic controller in labview. If implemented well can solve control problems very well. However, This is not a good time to try this path. Off season project.

[Jared]

Originally Posted by Tom Bottiglieri

The real answer is probably to use C/C++.

:-/

Switching would likely fix the problem, but it's not a good solution.

The motion code I had is copy pasted from a project that ran perfectly on the cRIO - except I used Vectors instead of ArrayLists. There were no timing issues ever.

It's ironic that the fancy dual-core ARM controller running realtime linux and a newer version of Java has less accurate timing than a very outdated version of Java running on a system an order of magnitude less powerful.

[Tom Bottiglieri]

Originally Posted by Jared

Switching would likely fix the problem, but it's not a good solution.

The motion code I had is copy pasted from a project that ran perfectly on the cRIO - except I used Vectors instead of ArrayLists. There were no timing issues ever.

It's ironic that the fancy dual-core ARM controller running realtime linux and a newer version of Java has less accurate timing than a very outdated version of Java running on a system an order of magnitude less powerful.

The robot code does not run as a real time process. Nor does the standard JRE have real time support.

(That being said, I don't think C/C++ programs run as real time processes by default either.)

[virtuald]

Originally Posted by Tom Bottiglieri

The robot code does not run as a real time process.

That's a good point. Wonder why not?

[MrRoboSteve]

The JVM has changed a lot since the 1.3 version that was used on the cRIO.

Do we know what the startup command line looks like for the Java VM? I am curious to see if there are special settings regarding garbage collection.

You want to do GC tuning using the command line parameters, not by calling System.GC(). System.GC() can be undesirable as it can trigger a full GC when you shouldn't need one. http://stackoverflow.com/questions/6...gc-do-anything has some discussion.

This article discusses the tradeoffs in GC tuning. If I was having issues like this, I'd want to be looking at the output of -verbose:gc to see if I had big pauses. Then I'd experiment with the parallel collector, using in particular -XX:MaxGCPauseMillis to set a cap to the amount of time spent in GC. You want lots of very short GC runs in the younger generations.

I'd also set -Xmx and -Xms to the same value, large enough that -verbose:gc never shows a full GC occurring during a run. If you're getting full GC runs occurring, you probably need to look at changes in your program to reduce allocation as there's too much memory pressure.

[chloe]

Originally Posted by Jared

Randomly, this exact elevator code will have tiny issues/jerky movement, then go back to normal.

Usually jerky movement is a sign that your dampening / D factor is too high. Are you maybe having a brownout that causes your elevator motors to stop briefly?

Also you could try using your PID to control velocity rather than position.

[Tom Bottiglieri]

We have alleviated some of the issues with timing by measuring dt for every iteration of the control loop. This still isn't good enough to use any derivative in the controller, but it makes the motion a bit smoother. (You will probably over/undershoot the expected position part of your profile and get a bunch of error out of nowhere, causing the jerky motion.

[Tom Bottiglieri]

A bit of a follow up:

I talked this weekend at CVR with Joe Hershberger, who develops the RoboRIO side of the FRC stack. There is a construct in the HAL called a Notifier which uses the FPGA to generate interrupts at specific period. The CPP version of WPILib has support for this, but the java version is missing a JNI wrapper for this. Currently all timing based stuff (including the bundled PIDController) is being doing using Thread.sleep.

So, it seems possible to get decent timing in Java, but someone is going to have to get the Notifier built into the JNI and working. Has anyone managed to get the allwpilib repo java stuff built? The documentation isn't incredibly helpful.

[rrossbach]

Originally Posted by Tom Bottiglieri

So, it seems possible to get decent timing in Java, but someone is going to have to get the Notifier built into the JNI and working. Has anyone managed to get the allwpilib repo java stuff built? The documentation isn't incredibly helpful.

I was able to successfully build on Ubuntu back in January when I had to add some missing JNI bindings for the DIO's which were needed to get ultrasonic sensors working in wpilibj.

Taking a quick glance through the source, it looks like Java already has the FPGA interrupt infrastructure available so hopefully adding support for Notifier to wpilibj isn't too bad....should require adding 1) a NotifierJNI class provide the HAL bridge; and 2) a Notifier class for robot programs to use. I'll see if I can poke around with this over the weekend.

- Ron
Team #2607 controls mentor