Chief Delphi - What's your favorite programming/control system magic this year?

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- Programming (http://www.chiefdelphi.com/forums/forumdisplay.php?f=51)

- - What's your favorite programming/control system magic this year? (http://www.chiefdelphi.com/forums/showthread.php?t=129511)

Re: What's your favorite programming/control system magic this year?

One of our team's members developed an emulator as part of our software framework that let us thoroughly test our code without a real robot:

This let us have our robot code mostly ready before we even had a prototype robot, and then let us make sure our changes would work before deploying.

Re: What's your favorite programming/control system magic this year?

Quote:

Originally Posted by Colby Skeggs (Post 1386303)

One of our team's members developed an emulator as part of our software framework that let us thoroughly test our code without a real robot:
This let us have our robot code mostly ready before we even had a prototype robot, and then let us make sure our changes would work before deploying.

The robot emulator can be very useful for training new programmers too. No need to share a physical robot to test their code (and reduce safety risks too.)

I would very much look forward to seeing this running on the new roboRIO platform. Very well thought-out.

Thanks for sharing it here!

Re: What's your favorite programming/control system magic this year?

Quote:

Originally Posted by Thad House (Post 1386012)

That is absolutely incredible. Hopefully they actually release that code, because that would be so cool to read through and learn how it works.

Yes, we will be releasing the code for this (both path generation and follower controller) later this year. Right now we are turning over student leadership and most mentors are busy barbecuing :)

I would like to elaborate on this system a bit though.

From the start of the season (Or rather, once we were set on a robot that carried 3 balls to a closer shooting position) we wanted the ability to drive smooth S curves. The reasoning was we knew we were going to need to get over to the one point goal to avoid defensive goalie robots by the time Einstein came around. In place of this system we could have used the typical "Drive, turn X degrees, Drive" auto driving architecture, but decided this was not going to work based on our troubles with translating and spinning in 2012 and 2011.

The implementation of this system was a tool [1] that allowed the programmer to specify a list of waypoints for the robot to drive. Each waypoint consisted of a X coordinate, a Y coordinate, and heading (Assume the robot starts at 0,0,0). From there, we found a set of splines that intersect each waypoint (at the desired heading) and fit within a maximum radius which was something that we knew we could drive (aka no abrupt 180* turns).

Next, we figured out what it would take to get the robot to drive that path.Given a maximum velocity, acceleration, and jerk (derivative of acceleration) for the robot to drive with, we walked the spline-y path generated above in steps of 0.01 seconds. For each step, we generated (Position, Velocity, Acceleration, Jerk, Heading, X, Y, and current time step) for the geometric center of the robot. These values corresponded to how the robot /should/ be moving at that point in time. From there, given the wheel base of the robot, we were able to build two sets of steps, one for each wheel. These lists of steps were built into arrays which we serialized to a text file.[2]

On the robot side we built a controller that would drive the robot on the path (after deserializing). The controller that made the robot move consisted of 3 sub controllers: 2 wheel controllers for the left and right wheel, and a heading controller that would correct the angle of the robot on the field.

The wheel controller which followed the trajectory basically looked like this (one running for each wheel, using the given wheel's path data:

Code:

double update() {

  Segment step = follower.getNextSegment();

  double motor = 0;

  motor = (step.vel * K_vel) + (step.acc * K_acc) + (step.jerk * K_jerk);

  motor += ((step.pos - getWheelPos()) * K_p);

  return motor;

}

The heading controller looked something like this

Code:

double updateHeadingController() {

  Segment step = leftWheelFollower.getCurrentSegment()

   return (step.heading - gyro.getAngle()) * K_heading;

}

And the overall system looked something like this

Code:

updateDrive() {

  double l = leftWheelController.update();

  double r = rightWheelController.update();

  double turn = headingController.update();

  leftMotor.set(l+t);

  rightMotor.set(r-t);

}

So basically, even without a gyro or encoders we could drive something like the path just by pressing 'play'. Once the closed loop position and heading controllers were turned on, it was much better.[3]

We will release this code over the summer. Hopefully it will make more sense then.

[1] This tool was run on a laptop, as the math to build all the paths was pretty slow on the cRIO.
[2] We originally wanted to render java class files with static arrays defined for each path, but we ran into an issue with the tool that packages the final jar file for deploying. It could not handle more than about 1KB of static data in any Java class.
[3] We did no work to validate that our paths did not cause motor saturation. Whenever the driving looked weird we would down the velocity or move the waypoints to make bigger curves.

Re: What's your favorite programming/control system magic this year?

Quote:

Originally Posted by Tom Bottiglieri (Post 1386359)

The wheel controller which followed the trajectory basically looked like this (one running for each wheel, using the given wheel's path data:

Code:

double update() {

  Segment step = follower.getNextSegment();

  double motor = 0;

  motor = (step.vel * K_vel) + (step.acc * K_acc) + (step.jerk * K_jerk);

  motor += ((step.pos - getWheelPos()) * K_p);

  return motor;

}

Very cool! Your last second change in the Einstein finals was exciting to watch, and the controlled s-curve was very impressive.

What I can gather from this code is that the motor speed for each side is first calculated with a feedforward term from the velocity, acceleration, and jerk, and then additionally a proportional feedback term is added using feedback from the actual encoder position. Is this correct?

If I'm reading it correctly, is there a reason why the feedforward velocity (the velocity, acceleration, and jerk sum) is calculated on the robot using the curve values rather than calculated beforehand and output as motor velocity values?

Re: What's your favorite programming/control system magic this year?

Quote:

Originally Posted by randantor (Post 1386385)

What I can gather from this code is that the motor speed for each side is first calculated with a feedforward term from the velocity, acceleration, and jerk, and then additionally a proportional feedback term is added using feedback from the actual encoder position. Is this correct?

If I'm reading it correctly, is there a reason why the feedforward velocity (the velocity, acceleration, and jerk sum) is calculated on the robot using the curve values rather than calculated beforehand and output as motor velocity values?

Yes. Most of the real work is done by the feed forward terms of vel, acc, and jerk. Since we know what position we want to be in at that particular point in time, we can close the loop on the error in the case that we are trailing or leading our moving carrot.

Yes, you could also back out the motor values on the front end since it is all open loop. We did not because we wanted everything in the path to be in same units, and it made it a bit easier to tune the gains on the robot without re-generating a path.

The neat part is it's crazy easy to find K_acc and K_vel. Put your robot on the ground, give it full joystick forward, and log encoder values and time. Plot velocity and acceleration. Find max velocity and max acceleration. K_vel = 1.0/Max_vel and K_acc = 1.0/max_acc. Think about it, when you are commanding half your max velocity, your feedforward term should be about half max voltage. The acceleration term will add a bit more power in and the loop closed around position should help fix any error you get (from chain tension or battery voltage differences).

Re: What's your favorite programming/control system magic this year?

Quote:

Originally Posted by Tom Bottiglieri (Post 1386396)

So how do you guys tune the p and the jerk gains? I've always wondered if there is a good way to tune loops without just guessing and checking. And even then I don't really have a good idea on where to even start with the guesses.

Re: What's your favorite programming/control system magic this year?

Quote:

Originally Posted by Thad House (Post 1386400)

Oops, I lied. We don't apply jerk to the control output.

We find the position controller gains (we have I and D terms coded in, but we ended using 0 as a gain for those) by plotting the position setpoint and measured position over time. The setpoint moves and you can watch how closely the sensor is catching up to it. If it's slow, up the gain until it starts to overshoot. Then back it off a little. For this application, close enough is fine.