PIDController Rotate

[notmattlythgoe]

I am trying to use a PIDController to rotate the robot to a certain angle using a gyro, but I cannot figure out what to set the PIDOutput parameter to in the constructor. Any ideas?

[Ether]

The PID output is the voltage command to the motor being controlled.

Make sure create proper setpoint and processVariable inputs to the PID, for example like so:

angle_error = joystick_command - gyro_angle;
angle_error -= 360*floor(0.5+angle_error/360);
setpoint = gyro_angle + angle_error;
processVariable=0;

See discussion here (http://www.chiefdelphi.com/forums/showthread.php?p=1021821#post1021821).

[notmattlythgoe]

I know what the PIDOutput does, but I can't figure out what to use for it since I can't just use a variable, I have to put in a Controller of some sort. I'm thinking maybe I just need to write a class that implements PIDOutput and read the value out of it to use for the rotation value.

[Ether]

Isn't there any example code in the FRC Framework C++ WindRiver installation?

Have you looked at the C++ WPI Library source code (http://amhsrobotics.com/wpilib/html/class_p_i_d_controller.html) for help?

[notmattlythgoe]

I'm doing it in Java. Hence, the Java thread :-P

[Ether]

I'm doing it in Java. Hence, the Java thread :-P

My bad. Wasn't paying attention. :-(

[notmattlythgoe]

No problem :) Here is the code I have so far if anybody has any suggestions. The way PIDController works is it creates a thread and does the PID calculations and takes care of the input and output for you. But what do I set the output to to cause the robot to rotate? My guess is create a class that implements PIDOutput and do the rotation code myself. If anybody else knows a better way please inform me.

PIDController turn = new PIDController(DRIVE_P, DRIVE_I, DRIVE_D, gyro,?);
turn.setTolerance(0.05);
turn.setSetpoint(angle);
turn.setContinuous();
turn.enable();

[Ether]

But what do I set the output to to cause the robot to rotate? My guess is create a class that implements PIDOutput and do the rotation code myself. If anybody else knows a better way please inform me.

What kind of drivetrain do you have? Is it skid-steer, or omni, or mecanum, or perhaps swerve? And, what it your driver interface? 3-axis joystick, 2 2 axis joysticks, etc ?

[notmattlythgoe]

Mecanum with a 3-axis joystick.

[Ether]

Mecanum with a 3-axis joystick.

Did you write your own mecanum code to calculate the wheel speeds from the driver inputs, or are you using the provided WPI library function for that?

[notmattlythgoe]

public void turn(double speed, int angle)
{
PIDRotate rotate = new PIDRotate();
PIDController turn = new PIDController(DRIVE_P, DRIVE_I, DRIVE_D, gyro, rotate);
turn.setTolerance(0.05);
turn.setSetpoint(angle);
turn.setContinuous();
turn.enable();

while(!turn.onTarget())
{
drive(0.0, rotate.getValue());
}
turn.disable();
}


Here is what I have now. This is the method that completes a turn. Below is the PIDRotate class that gets the PID value written to it.

public class PIDRotate implements PIDOutput
{
private double value;

public PIDRotate()
{
value = 0.00;
}

public void pidWrite(double output)
{
value = output;
}

public double getValue()
{
return value;
}
}

[Ether]

The inverse kinematic calculations for mecanum require three input rate commands (forward/reverse, right/left, clockwise/counterclockwise) in order to compute the required 4 wheel speeds.


When the driver is controlling robot orientation open-loop, these three rate commands come from the driver.


If you want to use the gyro to go to and hold the robot orientation at some desired angle, then the rotation rate command for the inverse kinematic calculations must come from the output of the PID, rather than from the driver.


There are a few different ways to do this.


One way would be for the driver to open-loop rotate the vehicle to the desired orientation, and when it is oriented the way he wants it, he presses a joystick button to "lock in" that angle. By "lock in", I mean that the software reads and saves that angle from the gyro and uses that as the target value for the PID. The software then changes modes and uses the PID output, instead of the driver, to provide the rotation command (to be feed into the inverse kinematic calculation of wheel speeds). This holds the robot at that angular orientation. When the driver wants to change the robot's orientation, he presses another button to release it and give control back to him.


Another way is to program the buttons so that the driver has a small set of discrete robot orientations he can access with one button press. For example, one button for forward, one for 90 degrees to the right, one for 90 degrees to the left (and one to release control back to the driver). The target values for these buttons are hard-coded to the fixed discrete positions to which they correspond.


I suppose another way to do it would be to provide some sort of separate non-spring-loaded rotary device, like a potentiometer or encoder, for the driver to turn to the desired robot orientation angle. The angle from this device would become the target value for the PID. This could be the full-time mode of operation (replacing the joystick rotation z-axis) or a joystick button could be used to toggle between the two modes of control.


You could even just use the joystick z-axis as the target value for the PID (and use the PID output as the rotation rate input to the inverse kinematic calculations), but it seems to me this would be very awkward for the driver, because he would have to hold the twist at the desired angle.


Because of gyro drift, when using the gyro you might also want to provide a button that tells the software to re-zero the gyro at the present orientation. This allows the driver to manually orient the robot at the zero orientation and then press the button to re-zero the gyro.

[notmattlythgoe]

I appreciate all of the help Ether, but I don't think you are understanding what I am asking, and that is probably my fault for not explaining enough. What I am currently doing is working on rewriting our competition code that was done in Labview in Java. One of the programming sub-teams off-season projects this year is going to be learning to use the Java environment by reprogramming our robot from Logomotion. I am doing it myself so that I know how to do it when the students are working on it and can help them when they have questions. The problem that I ran into was that I needed an output that would go straight to a speed controller that would cause the robot to rotate. From what I can tell the PIDController in Java works differently than the ones in C++ and Labview. It works more like the CANJaguar where you give it the value and it does all of the calculations and outputs to the speed controllers for you. I think I have remedied that by writing a PIDOutput class that implements PIDOutput and tells the RobotDrive to rotate instad of telling one specific speed controller to move. I have attached the two code files, but don't quote me on anything because I have not tested it at all.

[notmattlythgoe]

The problem I really ran into was that there was no way from what I could tell that I could just put the output of the PID into the rotate portion of the drive command because the PIDController does the output automatically. And the output must be a instance of a PIDOutput which are Jaguars, Victors, and CANJaguars.

[Ether]

The problem I really ran into was that there was no way from what I could tell that I could just put the output of the PID into the rotate portion of the drive command because the PIDController does the output automatically. And the output must be a instance of a PIDOutput which are Jaguars, Victors, and CANJaguars.


Isn't there a generic PID in the WPI library for Java which lets you specify where the output goes? Like the LabVIEW PID (http://www.chiefdelphi.com/forums/attachment.php?attachmentid=10401&d=1300039530) ?

[notmattlythgoe]

Isn't there a generic PID in the WPI library for Java which lets you specify where the output goes? Like the LabVIEW PID (http://www.chiefdelphi.com/forums/attachment.php?attachmentid=10401&d=1300039530) ?





unfortunately not that I can find. Here is the link to the JavaDoc for the WPI Library. If you can find something let me know.

http://robotics.loyola.ca/javadoc2011/allclasses-noframe.html

[Jared Russell]

WPILib and WPILibJ PID controllers take PIDOutputs as constructor parameters - this is where they "put" their output command. Some classes in WPILib(J) already implement the PIDOutput interface (like Victor or Jaguar). But because you can apply PID to so many different types of problems, there are going to be lots of instances where you need to write some code to "glue" the PIDController to whatever it is you want to control.

There are a couple of ways you could do this:

1. Make a new class that implements PIDOutput. You must implement a "pidWrite" function for your new class - this will get called every time the PID controller has a new output available.

2. Don't use the PIDOutput feature; instead, simply call <yourPIDcontrollername>.get() inside of a periodic loop (like "teleopPeriodic()" in IterativeRobot) to grab the output of the controller at some regular frequency.

3. Write your own PID controller class that behaves exactly the way you want it to :)

[Joe Ross]

seems like it would be easy to make a class that implements PIDOutput that only sets a variable.

[notmattlythgoe]

WPILib and WPILibJ PID controllers take PIDOutputs as constructor parameters - this is where they "put" their output command. Some classes in WPILib(J) already implement the PIDOutput interface (like Victor or Jaguar). But because you can apply PID to so many different types of problems, there are going to be lots of instances where you need to write some code to "glue" the PIDController to whatever it is you want to control.

There are a couple of ways you could do this:

1. Make a new class that implements PIDOutput. You must implement a "pidWrite" function for your new class - this will get called every time the PID controller has a new output available.

2. Don't use the PIDOutput feature; instead, simply call <yourPIDcontrollername>.get() inside of a periodic loop (like "teleopPeriodic()" in IterativeRobot) to grab the output of the controller at some regular frequency.

3. Write your own PID controller class that behaves exactly the way you want it to :)

So far I've taken the first step. I'm guessing this was the way it was intended to be done in Java. Currently my PIDOutput class takes a forward speed and a RobotDrive as parameters for its constructor and adjusts the right and left side motors as needed to correct the rotation.

[notmattlythgoe]

2. Don't use the PIDOutput feature; instead, simply call <yourPIDcontrollername>.get() inside of a periodic loop (like "teleopPeriodic()" in IterativeRobot) to grab the output of the controller at some regular frequency.



How would you accomplish this without passing the PIDController's constructor a PIDOutput object? Create an abstract instance of one?

[Jared Russell]

How would you accomplish this without passing the PIDController's constructor a PIDOutput object? Create an abstract instance of one?

Yeah, you would just create a minimalist implementation of a PIDOutput (it could even be an anonymous class).

Something like...

xxx = new PIDController(Kp, Ki, Kd, source, new PIDOutput() {void pidWrite(double output){ } }, period);

[notmattlythgoe]

Yeah, you would just create a minimalist implementation of a PIDOutput (it could even be an anonymous class).

Something like...

xxx = new PIDController(Kp, Ki, Kd, source, new PIDOutput() {void pidWrite(double output){ } }, period);

Makes sense. Having the thread controlled PID system is very nice though. Just takes that little bit of extra work. Once I get the code tested on the robot I will post it here for people to look at.

[Strants]

Just a couple nit-picks: you defined DRIVE_P, DRIVE_I, and DRIVE_D as zero, which means that the controller won't do any corrections. Also, what is the value variable in the PIDRotate class supposed to do?

[notmattlythgoe]

Just a couple nit-picks: you defined DRIVE_P, DRIVE_I, and DRIVE_D as zero, which means that the controller won't do any corrections. Also, what is the value variable in the PIDRotate class supposed to do?

I instantiated all of the PID values to 0 because I didn't have the correct values on me at the time. Here are some update files, I'm hoping to get the code tested this afternoon.