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.

[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 for help?

[notmattlythgoe]

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

[Ether]

Originally Posted by notmattlythgoe

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]

Originally Posted by notmattlythgoe

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]

Originally Posted by notmattlythgoe

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.