Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Programming (http://www.chiefdelphi.com/forums/forumdisplay.php?f=51)
-   -   Speed PID Function (http://www.chiefdelphi.com/forums/showthread.php?t=101244)

JohnFogarty 26-01-2012 22:03
Speed PID Function
 
As far as I know PID Control is normally applied to move a motor, ei. an arm or wheel to a set target/position/distance.

What I NEED to implement is a speed based PID method.
I input a rate.
In my java encoder.java class it lists a method getRate();
I suspect this works by returning......a rate which I could use to set my speed.

from the rate/speed I input... I need to implement a PID method so when voltage drops or a ball passes through the shooter I am using, the POWER % will increase to compensate to allow the shooter to hold a constant SPEED.

Tom Line 26-01-2012 22:17
Re: Speed PID Function
 
If you perform a search on "velocity speed control" you will find all the formulas you require derived for you by a mentor from team 341. He also does an excellent job explaining how a velocity pid differs from a positional pid.

JohnFogarty 26-01-2012 22:35
Re: Speed PID Function
 
Can you provide a link I can't find the exact page.
I saw something about Jaguar 2012 using PID.....I use Victors it shouldn't matter right.
:O
mostly because I am not a fan of the current flow fail-safe on the Jags.

Ether 26-01-2012 22:37
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1114302)
As far as I know PID Control is normally applied to move a motor, ei. an arm or wheel to a set target/position/distance.

What I NEED to implement is a speed based PID method.
I input a rate.
In my java encoder.java class it lists a method getRate();
I suspect this works by returning......a rate which I could use to set my speed.

from the rate/speed I input... I need to implement a PID method so when voltage drops or a ball passes through the shooter I am using, the POWER % will increase to compensate to allow the shooter to hold a constant SPEED.
Not a consensus, but teams have reported success using a controller using feedforward and the "I" term of a PID.

Here are some links which may be of interest:

http://www.chiefdelphi.com/forums/sh...80&postcount=7

http://www.chiefdelphi.com/forums/sh...5&postcount=15

http://www.chiefdelphi.com/forums/sh...28&postcount=6

http://www.chiefdelphi.com/forums/sh...04&postcount=3

http://www.chiefdelphi.com/forums/sh...7&postcount=34

http://www.chiefdelphi.com/forums/sh...7&postcount=29

davidthefat 27-01-2012 02:00
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1114302)
As far as I know PID Control is normally applied to move a motor, ei. an arm or wheel to a set target/position/distance.

What I NEED to implement is a speed based PID method.
I input a rate.
In my java encoder.java class it lists a method getRate();
I suspect this works by returning......a rate which I could use to set my speed.

from the rate/speed I input... I need to implement a PID method so when voltage drops or a ball passes through the shooter I am using, the POWER % will increase to compensate to allow the shooter to hold a constant SPEED.
Logically, it is no different than a positional PID. Just the input will be in FPS or RPM (RPM in my case). The only difference will be the constants. Instead of the target being an angle, it simply is the RPM. Same goes for any other unit.

Tom Line 27-01-2012 02:20
Re: Speed PID Function
 
Quote:
Originally Posted by davidthefat (Post 1114440)
Logically, it is no different than a positional PID. Just the input will be in FPS or RPM (RPM in my case). The only difference will be the constants. Instead of the target being an angle, it simply is the RPM. Same goes for any other unit.
That is incorrect.

When a PID derived for position passes the set point, it reverses the motor. Reversing a motor on a spinning wheel going at 4000 rpm can have negative consequences.

The derivations are different. Once you see the final formula, you will understand why position PID and velocity PID are different.

davidthefat 27-01-2012 02:44
Re: Speed PID Function
 
Quote:
Originally Posted by Tom Line (Post 1114444)
That is incorrect.

When a PID derived for position passes the set point, it reverses the motor. Reversing a motor on a spinning wheel going at 4000 rpm can have negative consequences.

The derivations are different. Once you see the final formula, you will understand why position PID and velocity PID are different.
:eek: Oh snap... I just ran a simulation, you are correct.

BornaE 27-01-2012 04:14
Re: Speed PID Function
 
You need to use and encoder to "measure" the Back EMF of the motor.

BackEMF = EncoderRPM * 12V/(12V Free RMP)


Motor output = Control Output + BackEMF

This essentially linearizes the motor to act as a torque source.

Now you can set up a standard PID with RPM as input. and Plug in the output to the equation : Motor output = Control Output + BackEMF.

JohnFogarty 27-01-2012 11:24
Re: Speed PID Function
 
I've been looking at the PID drive example in the FRC Java example projects.

SO. what I AM understanding is instead of putting distance in as the SETPOINT you would put the rate.

Now what I'm also understanding is that the Error HAS to be 0. because you never want to stop the motor once you've reached your target velocity.
I'm a client side coder once I have enough examples I can make my own functions and classes from that.

I'm confused as to what BACK EMF is?





Code:
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                            */
/* Open Source Software - may be modified and shared by FRC teams. The code  */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                              */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;



import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Victor; //If you use Victors, import them instead
import edu.wpi.first.wpilibj.Timer; //Make sure to use this version, not the java.util version of timer.

/**
 * This program initializes a PID Controller for the wheels and drives
 * 3 feet using the encoders.  PID works by changing the output of the
 * motors by PID constants, based on how far away you are from the target.
 * PID values need to be tuned for your robot.
 *
 * @author: Fredric Silberberg
 */

public class PIDDrive extends SimpleRobot {

   
//    This class is required to invert the direction of the motor.
//    It negates the speed, so that the robot will drive forward.
   
    class MotorDrive extends Victor {

        public MotorDrive(int port) {
            super(port);
        }

        //Calls the super set method and gives it the negated speed.
        public void set(double speed) {
            super.set(-speed);
        }
    }
    //Initializes the motors.
    private final SpeedController left = new MotorDrive(2);
    private final SpeedController right = new Victor(1);
   
    //Initializes the Encoders.
    private final Encoder leftEncoder = new Encoder(1, 2);
    private final Encoder rightEncoder = new Encoder(4, 3);
   
    //Proportional, Integral, and Dervative constants.
    //These values will need to be tuned for your robot.
    private final double Kp = 0.3;
    private final double Ki = 0.0;
    private final double Kd = 0.0;
   
    //This must be fully initialized in the constructor, after the settings
    //for the encoders have been done.
    private final PIDController leftPID;
    private final PIDController rightPID;

    public PIDDrive() {
        //Sets the distance per pulse in inches.
        leftEncoder.setDistancePerPulse(.000623);
        rightEncoder.setDistancePerPulse(.000623);
       
        //Starts the encoders.
        leftEncoder.start();
        rightEncoder.start();
       
        //Sets the encoders to use distance for PID.
        //If this is not done, the robot may not go anywhere.
        //It is also possible to use rate, by changing kDistance to kRate.
        leftEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kDistance);
        rightEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kDistance);
       
        //Initializes the PID Controllers
        leftPID = new PIDController(Kp, Ki, Kd, leftEncoder, left);
        rightPID = new PIDController(Kp, Ki, Kd, rightEncoder, right);
       
        //Enables the PID Controllers.
        leftPID.enable();
        rightPID.enable();
       
        //Sets the input range of the PID Controller.
        //These will change, and you should change them based on how far
        //your robot will be driving.
        //For this example, we set them at 100 inches.
        leftPID.setInputRange(0, 100);
        rightPID.setInputRange(0, 100);
    }

    /**
    * This function is called once each time the robot enters operator control.
    * Teleop commands are put in here
    */
    public void operatorControl() {
        //Sets the left and the right motors to
        //drive forward 60 inches, or 5 feet
        leftPID.setSetpoint(60);
        rightPID.setSetpoint(60);
       
        //This will wait 10 seconds before the end of operator control
        //is reached, so that the robot has time to drive the full 5 feet.
        //You could have other tasks running here as well.
        Timer.delay(10);
    }
}

Ether 27-01-2012 12:57
Re: Speed PID Function
 
@BornaE:

The BackEMF term seems to react in the wrong direction to disturbances or changes in setpoint.

For example, if a change in load causes the motor speed to decrease, this causes the BackEMF term to decrease as well, instead of holding fast (as might be desired).

Or suppose there is a step decrease in setpoint. Instead of immediately decreasing (as might be desired), the BackEMF term will only decrease as the PID overcomes it and slows down the motor.

Also, at steady state, the BackEMF term provides only the voltage necessary to hold speed for an unloaded system. So the PID must contain an integrator to make up the difference.

Why not just replace the BackEMF term with the expected voltage* necessary to achieve the desired speed ?

Read the links provided in post #4. They provide further detail.


* As might be the case with a spinning wheel shooter for example where the load is predictable and the expected volts vs speed curve can be known. You can determine this voltage empirically by just reading the PWM from the dashboard.

JohnFogarty 28-01-2012 10:45
Re: Speed PID Function
 
I understand the theoretical stuff with the PID now.
I just need some psuedo-code or something to help me implement it.
I'm currently clueless.

Ether 28-01-2012 10:50
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1115173)
I understand the theoretical stuff with the PID now.
I just need some psuedo-code or something to help me implement it.
I'm currently clueless.
Is this at all helpful?
http://www.chiefdelphi.com/forums/sh...15&postcount=5

JohnFogarty 28-01-2012 11:07
Re: Speed PID Function
 
Code:

package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;
import edu.wpi.first.wpilibj.templates.commands.DriveWithJoysticks;

/**
 * <p>The drive train is PID subsystem, but unlike the {@link Wrist} and
 * {@link Elevator}, it is not always running PID. Instead, it can be run in a
 * manual tank drive or PID can be enabled and it will use a range finder to
 * drive a fixed distance away from the target.</p>
 *
 * <p>Recommended next step: {@link CommandBase}</p>
 *
 * @author Alex Henning
 */
public class DriveTrain extends PIDSubsystem {
    // The constants for the P, I and D portion of PID
    private static final double Kp = 3;
    private static final double Ki = .2;
    private static final double Kd = 0.0;
   
    RobotDrive drive;
    AnalogChannel rangefinder;


    // Initialize your subsystem here
    public DriveTrain() {
        super("DriveTrain", Kp, Ki, Kd);
        drive = new RobotDrive(RobotMap.leftMotor, RobotMap.rightMotor);
       
        rangefinder = new AnalogChannel(RobotMap.rangefinder);
    }
   
    /**
    * Set the default command to drive with joysticks.
    */
    public void initDefaultCommand() {
        setDefaultCommand(new DriveWithJoysticks());
    }

    /**
    * @return The value of the rangefinder used as the PID input device.
    *        These values correspond to your PID setpoint, in this case the
    *        value can be anywhere between 0v and 5v.
    */
    protected double returnPIDInput() {
        return rangefinder.getVoltage();
    }

    /**
    * @param output The value to set the output to as determined by the PID
    *              algorithm. This gets called each time through the PID loop
    *              to update the output to the motor.
    */
    protected void usePIDOutput(double output) {
        tankDrive(output, output);
    }
   
    /**
    * Implements the tank drive capability of the drivetrain.
    *
    * @param left The speed for the left side of the drivetrain.
    * @param right The speed for the right side of the drivetrain.
    */
    public void tankDrive(double left, double right) {
        drive.tankDrive(left, right);
    }
}
This is his Drive Subsystem.
Code:

package edu.wpi.first.wpilibj.templates.commands;

/**
 * <p>Similar to SetWristSetpoint, but it also has to handle enabling and
 * disabling the PID loop</p>
 *
 * <p>Recommended next step: {@link PrepareToGrab}</p>
 *
 * @author Alex Henning
 */
public class DriveToDistance extends CommandBase {
    double setpoint;
   
    /**
    * Require the drive train and store the desired setpoint.
    *
    * @param setpoint The desired setpoint for the drive train.
    */
    public DriveToDistance(double setpoint) {
        requires(drivetrain);
        this.setpoint = setpoint;
    }

    // Called just before this Command runs the first time
    /**
    * Set the setpoint to the stored value and enable PID on the drivetrain.
    */
    protected void initialize() {
        drivetrain.setSetpoint(setpoint);
        drivetrain.enable();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
    * @return true when it's close enough to the setpoint
    */
    protected boolean isFinished() {
        return Math.abs(drivetrain.getPosition() - setpoint) < .02;
    }

    // Called once after isFinished returns true
    /**
    * When this command ends, disable the drivetrain's PID
    */
    protected void end() {
        drivetrain.disable();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    /**
    * When this command exits, disable the drivetrain's PID
    */
    protected void interrupted() {
        drivetrain.disable();
    }
}
This is the only PID Drive command I see...I'm not sure where he even uses the rangefinder.
because actually using a rangefinder is exactly what I'm going to be doing in the end.

davidthefat 28-01-2012 11:35
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1115173)
I understand the theoretical stuff with the PID now.
I just need some psuedo-code or something to help me implement it.
I'm currently clueless.
Code:
previous_error = setpoint - process_feedback
integral = 0
start:
  wait(dt)
  error = setpoint - process_feedback
  integral = integral + (error*dt)
  derivative = (error - previous_error)/dt
  output = (Kp*error) + (Ki*integral) + (Kd*derivative)
  previous_error = error
  goto start
Straight from: http://en.wikipedia.org/wiki/PID_controller#Pseudocode

JohnFogarty 28-01-2012 13:36
Re: Speed PID Function
 
I just ran a test with my encoder and I get a HUGE amount of oscillation when I'm just pulling the number from the getRate() method even when I'm running at a constant speed.
I can't get a RPM reading with this much oscillation.

davidthefat 28-01-2012 16:01
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1115254)
I just ran a test with my encoder and I get a HUGE amount of oscillation when I'm just pulling the number from the getRate() method even when I'm running at a constant speed.
I can't get a RPM reading with this much oscillation.
Filter it out. But to be honest, it is not even that bad. Our robot has less than a 1-2% margin of error, it just looks huge because the scalar values are huge. One of the former programming mentors told me that Kalman Filter is overkill with such small error for the purposes of FIRST.

Ether 28-01-2012 16:03
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1115254)
I just ran a test with my encoder and I get a HUGE amount of oscillation when I'm just pulling the number from the getRate() method even when I'm running at a constant speed.
I can't get a RPM reading with this much oscillation.
At what rpm are you getting this oscillation?

JohnFogarty 28-01-2012 22:52
Re: Speed PID Function
 
http://www.chiefdelphi.com/media/photos/37282?
I am running the Victor at 35%

Joe Ross 28-01-2012 23:00
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1115254)
I just ran a test with my encoder and I get a HUGE amount of oscillation when I'm just pulling the number from the getRate() method even when I'm running at a constant speed.
I can't get a RPM reading with this much oscillation.
When I ran experiments in 2009, I found there's a lot of noise with the FPGA's rate due to phase errors in the encoder. Changing the encoder to 1x decoding decreased this significantly, since it always used the same edge, however it still wasn't clean enough for us. This is equivalent to averaging over one cycle. We implemented a low pass filter and were able to get results that were good enough. Another option would be to calculate the rate from the position, which is equivalent to averaging for the sampling time.

I don't know if the rate implementation has changed since then. Your noise seems a little higher then what I saw before, but that could easily be explained by differences in setup.


Saying that you're running a Victor at 35% doesn't answer Ether's question without also defining what motor you're using. In your chart, what are the units for "data".

JohnFogarty 29-01-2012 00:04
Re: Speed PID Function
 
this is where my confusion lies.
I thought the getRate function was reading the RPM's
I guess not.
I looked at the method itself and it shows that the equation for getRate is...

setdistanceperpulse/getPeriod or right now for me.

1/the time between each pulse

What my software mentor told me is that what were going to do is probably sample from the getRaw data encoder function.

since we are getting 2.5 revolutions per second @ 35% power based on a test I did. since there are 1440 pulses per revolution.

davidthefat 29-01-2012 00:32
Re: Speed PID Function
 
Well, from a test I have done today, the encoder that came with the KOPs from a previous year outputs 250 ± 5 for every revolution. So it is not directly 360 degrees. Keep that in mind. What I am doing is just using the raw output and getting the RPM from that.

JohnFogarty 29-01-2012 01:10
Re: Speed PID Function
 
Yeah I'm going to be using the RAW from now on as well.
I forget...is there is a way of subtracting consecutive samples and dividing them by 2 to find the rate.

Ether 29-01-2012 01:11
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1115671)
this is where my confusion lies.
I thought the getRate function was reading the RPM's
I guess not.
I looked at the method itself and it shows that the equation for getRate is...

setdistanceperpulse/getPeriod or right now for me.

1/the time between each pulse

What my software mentor told me is that what were going to do is probably sample from the getRaw data encoder function.

since we are getting 2.5 revolutions per second @ 35% power based on a test I did. since there are 1440 pulses per revolution.
If your encoder is giving you 1440 pulses per second:

1 RPM = 1/60 rev/sec = 1440/60 pulses/sec = 24 pulses/sec

So RPM = (delta_pulses/dt)/24

...where delta_pulses is the change in the raw count from the previous cycle, and dt is the cycle time in seconds.

Don't forget: you must scale your setpoint to the same units as your process_variable.

Ether 29-01-2012 01:15
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1115708)
Yeah I'm going to be using the RAW from now on as well.
I forget...is there is a way of subtracting consecutive samples and dividing them by 2 to find the rate.
Why would you divide by 2?

If your cycle time is 20 milliseconds (TeleOp) and you have 1440 pulses/rev, then to get RPM you would take the difference in pulses and divide by 24*0.02 = 0.48

JohnFogarty 29-01-2012 02:04
Re: Speed PID Function
 
The encoder returns 1440 per REV.
I am getting about 2.5-2.7 REVS a second.

Ether 29-01-2012 03:37
Re: Speed PID Function
 
In post #24, "cycle time" is not the time it takes for your encoder to rotate once.

it is the delta time between the two GetRaw() readings you are subtracting to get "delta_pulses" (the change in pulse count).

if you are running the PID in TeleOp, that would be approximately 20 milliseconds.

JohnFogarty 31-01-2012 22:01
Re: Speed PID Function
 
I just got a VERY constant rate from using this function I wrote.
Code:
    public void getSpeed(){ 
        samples[counter] = Shooter_En.getRaw();
        counter++;
        Shooter_En.reset();
        if(samples[9] > 0){
            counter = 0;
            for(int i = 0; i <= 9; i++){
                sum = sum + samples[i];
              }   
            AVG = sum / 10;
            Rate =  AVG / 0.48;
            sum = 0;
        }
    }

Jared Russell 31-01-2012 22:05
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1117459)
I just got a VERY constant rate from using this function I wrote.
Code:
    public void getSpeed(){ 
        samples[counter] = Shooter_En.getRaw();
        counter++;
        Shooter_En.reset();
        if(samples[9] > 0){
            counter = 0;
            for(int i = 0; i <= 9; i++){
                sum = sum + samples[i];
              }   
            AVG = sum / 10;
            Rate =  AVG / 0.48;
            sum = 0;
        }
    }
Do you notice that the speed you get using this method is slow to respond to changes?

Ether 31-01-2012 22:23
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1117459)
I just got a VERY constant rate from using this function I wrote.
Code:
    public void getSpeed(){ 
        samples[counter] = Shooter_En.getRaw();
        counter++;
        Shooter_En.reset();
        if(samples[9] > 0){
            counter = 0;
            for(int i = 0; i <= 9; i++){
                sum = sum + samples[i];
              }   
            AVG = sum / 10;
            Rate =  AVG / 0.48;
            sum = 0;
        }
    }
When does samples[9] get reset to zero?

If it's getting reset, you're only getting a new speed every 10 cycles.

If it's not, then once it becomes non-zero it stays that way, and you re-zero the counter every cycle so the filter does nothing (except skew the calculation).

What you want is a circular buffer (ring buffer) that you populate each cycle and use each cycle to get a new speed, like this:
Code:
public void getSpeed(){ 
  samples[counter++] = Shooter_En.getRaw();
  Shooter_En.reset();
  if(counter>9) counter=0;
  sum=0;
  for(int i = 0; i <= 9; i++) sum += samples[i];
  Rate =  sum / (10* 0.48);
}

JohnFogarty 01-02-2012 13:57
My mentor explained to me this way.
Your first 9 values are going to be very skewed so wait until you have at least 9 values in your array before you start averaging. Now counter is a pointer variable it ONLY points to a position in the array. I add all the values and get an average every time after 9 samples. I haven't tested changing speeds yet. Only constants.

Ether 01-02-2012 14:17
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1117895)
My mentor explained to me this way.
Your first 9 values are going to be very skewed so wait until you have at least 9 values in your array before you start averaging. Now counter is a pointer variable it ONLY points to a position in the array. I add all the values and get an average every time after 9 samples. I haven't tested changing speeds yet. Only constants.
Your code as written doesn't do what you described above. That was the point of the two responses (post#28 & post#29) to your post#27.

Show your code to your mentor and ask him.

JohnFogarty 01-02-2012 14:24
I just saw what you meant you are right. My bad.

JohnFogarty 01-02-2012 19:22
My mentor sent me this.
Quote:
That should work just fine. I was trying to reduce the number of arithmetic calculations, which would be necessary if the array/table was larger than it is. If you take the sum and subtract the oldest value and add the newest value it take a lot less time. Can you imagine adding up 100 or more entries each time?

In the final example:
What you want is a circular buffer (ring buffer) that you populate each cycle and use each cycle to get a new speed, like this:
Code:
public void getSpeed(){
samples[counter++] = Shooter_En.getRaw();
Shooter_En.reset();
if(counter>9) counter=0;
sum=0;
for(int i = 0; i <= 9; i++) sum += samples[i];
Rate = sum / (10* 0.48);
I have to assume that the first encoder value is way out of line and will skew the results since you have not reset the encoder until you read it the first time. As a matter of fact, until you have 11 samples the Rate is not really valid since you have some amount of samples that are zero since the array has not been filled yet. Would you consider this? Once you have 10 samples, a valid flag says to compute the Rate and could tell the user that it is now valid. See This;

public void getSpeed(){
samples[counter++] = Shooter_En.getRaw();
Shooter_En.reset();
if (counter >9){ counter = 0; ValidFlag = true;}

If (ValidFlag)
{
sum = 0;
for(int i = 0; i < counter; i++) sum += samples[i];
Rate = sum / (10* 0.48);
}





I first questioned the last line: Rate = sum / (10* 0.48); I thought is should be: Rate = (sum /10) / 0.48; Then realized it is the same thing. You should make it Rate = sum/4.80; to eliminate the multiplication.

Dennis

Ether 01-02-2012 20:34
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1118091)
My mentor sent me this.
Quote:
I have to assume that the first encoder value is way out of line and will skew the results since you have not reset the encoder until you read it the first time. As a matter of fact, until you have 11 samples the Rate is not really valid since you have some amount of samples that are zero since the array has not been filled yet. Would you consider this? Once you have 10 samples, a valid flag says to compute the Rate and could tell the user that it is now valid.

Code:
public void getSpeed(){
samples[counter++] = Shooter_En.getRaw();
Shooter_En.reset();
if (counter >9){ counter = 0; ValidFlag = true;}

If (ValidFlag)
{
sum = 0;
for(int i = 0; i < counter; i++) sum += samples[i];
Rate = sum / (10* 0.48);
}
The added code suppresses updating the rate calculation while the buffer is initially filling. If you are sampling at 20ms it will take only 1/5 of a second to fill the ring buffer at startup. Whether or not that is a problem depends on what this code is being used for. For a shooter flywheel speed signal my guess is you won't be able to tell the difference.

Another option is to use an IIR filter instead of FIR. It's simpler and it's tunable:

Code:
public void getSpeed(){
  filtered_count = a*filtered_count + (1-a)*Shooter_En.getRaw();
  Shooter_En.reset();
  Rate =  filtered_count /0.48;
}
... set a=0 and you've got no filtering. As you increase a from zero to 1, the filtering becomes stronger.

Quote:
I first questioned the last line: Rate = sum / (10* 0.48); I thought is should be: Rate = (sum /10) / 0.48; Then realized it is the same thing. You should make it Rate = sum/4.80; to eliminate the multiplication.
Changing 10*0.48 to 4.80 does not eliminate a runtime multiplication. The multiplication is done at compile time. Leaving it 10*0.48 makes it clearer where the numbers came from.

JohnFogarty 02-02-2012 21:41
Re: Speed PID Function
 
Am I stupid.
Or is the CAN Jaguar functions built to do all of this for me.

Ether 02-02-2012 23:03
Re: Speed PID Function
 
Quote:
Originally Posted by John_1102 (Post 1118870)
is the CAN Jaguar functions built to do all of this for me.
If you use CAN to communicate with the Jag, then you can plug the encoder directly in to the Jag and access the Jag's built-in PID (built-in to the Jag's firmware) to control speed. There are several threads on CD discussing the pros and cons of doing this.

I don't know what kind of noise filtering, if any, the Jag does on the encoder signal.

Others on this forum have studied the Jag firmware source code (freely available) and may be able to answer.

vamfun 09-02-2012 17:44
Re: Speed PID Function
 
Quote:
Originally Posted by Ether (Post 1118130)
Another option is to use an IIR filter instead of FIR. It's simpler and it's tunable:

Code:
public void getSpeed(){
  filtered_count = a*filtered_count + (1-a)*Shooter_En.getRaw();
  Shooter_En.reset();
  Rate =  filtered_count /0.48;
}
... set a=0 and you've got no filtering. As you increase a from zero to 1, the filtering becomes stronger.
I agree with you here, but if the FIR version is desired, they should implement it in the recursive form. It then approaches the timing of the IIR filter. The ring is still required but rather than summing over the whole ring each time the output is updated with the difference between the fresh sample and the oldest sample.

avg(n) = avg(n) + (sample(n) -sample(n-L))/L where L is the length of the average.

Essentially before you overwrite the oldest sample, you calculate the difference and then overwrite and compute the new average.

Ether 09-02-2012 17:55
Re: Speed PID Function
 
Quote:
Originally Posted by vamfun (Post 1123274)
The ring is still required but rather than summing over the whole ring each time the output is updated with the difference between the fresh sample and the oldest sample.
Yup.

Although the bigger question here is, do they really need a filter at all? If they're reading the encoder delta counts every execution cycle and dividing by the cycle period, there's already a whole lotta averagin' goin' on (except at very low speeds).

vamfun 09-02-2012 18:15
Re: Speed PID Function
 
Quote:
Originally Posted by Ether (Post 1123281)
Yup.

Although the bigger question here is, do they really need a filter at all? If they're reading the encoder delta counts every execution cycle and dividing by the cycle period, there's already a whole lotta averagin' goin' on (except at very low speeds).
Yup:)
For our team, we plan to bypass getRate() and derive encoder rate by (encoder – encoder_last)/T where T is the loop update time. Our typical wheel speed is 2000 rpm or 33.3 rps . The encoder has 250cnts per rev so there are about 8333 cnts/sec. With a 20 ms loop, there are 167 cnts per cycle. If 1 pulse is missed, it is less than .6% error. The code wheel slit phase errors are being averaged over half of the wheel so much of the high frequency phase variations that would affect getRate() are averaged out.

khatamidk 11-02-2012 12:01
Re: Speed PID Function
 
Our team was also getting wild fluctuations with velocity, with an error range of upwards of 300 RPM and spikes that extend well past 1000 RPM beyond our setpoint. This was unacceptable for our PID control loop, so our mentor suggested we implement a moving average/median filter to smooth out the graph and get rid of those nasty rate spikes. We also changed the encoder rate to k1X instead of k4X (default).

We are working with the command-subsystem in Java, so this should might help those that are working with PIDSubsystems (which our Shooter class is). We only changed the returnPIDInput() method.

The result was an incredibly smooth graph, fluctuating within an acceptable range that can be tuned with the P constant (and maybe D depending on how close to the value we get).

Here is a picture of repeated tests.

In the above picture, there were 5 tests separated by the deep drops to zero (First 4 were set to 1000RPM, the last set to 2000RPM0. You can see how much smoother the fluctuation of RPM is (the fluctuation that still persists is a result of a much-needed PID constant tuning). As stated earlier, we went from a 300-1000 RPM spike/fluctuation to one less than 100 RPM. We also played around with the sampling size, and we chose 20. It did not affect cRIO CPU usage very much at all. 100 sampling size provided no advantage from what we saw compared to 20.


Here is our implementation of the returnPIDInput()
Code:
private double[] samplingValues = new double[HW.SAMPLING_SIZE];

    //MOVING MEDIAN/AVERAGE FILTER BASED ON A SAMPLING SIZE OF "N" VALUES (ex. 20)
    protected double returnPIDInput() {
       
        //LOOP THROUGH AND SHIFT ALL VALUES TO THE RIGHT BY ONE, REMOVING THE LAST VALUE
        for(int i = samplingValues.length-1; i > 0; i--){
            samplingValues[i] = samplingValues[i-1];
        }
        //PUSH IN A NEW VALUE BASED ON RATE (we are using RPS*60 = RPM)
        samplingValues[0] = shooter_encoder.getRate()*60;
       
        //TEMP ARRAY FOR PERFORMING MEDIAN OPERATION
        double[] median = Arrays.copy(samplingValues, 0, HW.SAMPLING_SIZE, 0, HW.SAMPLING_SIZE);
        Arrays.sort(median);
       
        //RETURNS THE MIDDLE OF THE SORTED MEDIAN ARRAY
        return median[median.length/2];
    }

Hope this helps,
David K.
Team Spectrum
FRC#3847

Ether 11-02-2012 13:38
Re: Speed PID Function
 
Quote:
Originally Posted by khatamidk (Post 1124240)
...
David,

If I am reading this correctly, you are doing the following:
1) Using GetRate() to grab the "instantaneous" (consecutive-count) rate from the FPGA, and then

2) shifting all the values in an array to make room for this new datum (and dropping the oldest datum from the arrary, and then

3) sorting the whole array, and then

4) picking the median value from the middle of the array.

Have you tried any of the suggestions in earlier posts in this thread, such as:
a) Instead of using GetRate(), read the encoder counts instead, and divide by the sampling time (as explained in post39), or

b) Use a simple IIR filter (as shown in post37), or

c) Use an efficient FIR moving-window average filter (post37)?

vamfun 22-02-2012 15:42
Re: Speed PID Function
 
Here is a note on precalculating PID gains without a simulation. The note has the full derivation.Excerpt:
Note: Precomputing PID gains for a velocity shooter using pole/zero placement


I wanted to post some formula’s for deriving P and I gains for a velocity PID control loop such as a shooter used in the 2012 Rebound Rumble game.

Assumptions:
a) motor L/R time constant is small relative to motor control frequencies.
b) The motor first order time constant tau_m is known.
c) The input speeds are normalized to the maximum motor speed, w_max. w_max = 12v/ke*60/2pi rpm which is approx w_free of the motor.
d) The desired motor response time constant, tau_d, is smaller than the motor tau_m by a factor of r.

Then the integral gain KI is given by

KI = 1/tau_d

and the proportional gain KP is given by

KP = r = tau_m/tau_d ;

If the integral computation of the PID loop doesn’t contain a dt factor (i.e it is just an error accumulation) as is typical of the WPILIB PIDcontroller then the KI must be multiplied by the PID update period.

KI_WPILIB = T/tau_d where T is the PID period.

If you are feeding back RPM then the gains must be divided by w_max.

Example:

The 599 shooter wheel has an open loop time constant tau_m = .68 sec

We want to have a tau_d = .33 sec which means the error should be around 1% of the target by 1 second (three time constants). During autonomous this would be the delay before we can shoot.

KI = 1/tau_d = 3.

KP = tau_m/tau_d = 3*.68 = 2.04 ;

For WPILIB we want KI_WPILIB = T*KI = .05*3 = .15 ; KP remains the same.

The shooter has a w_max = 5614 rpm so if the feedbacks are not normalized then KP and KI must be divided by w_max. or

KP = 2.04/5614 = 3.63 e-4 ; KI_WPILIB = .15/5614 = 2.49e-5 ;

Thats it. The main factor limiting the value of KP is the magnitude of the noise on the RPM feedback signal. Typically after filtering this can get down to 1% of the RPM. KP multiplies the noise so to keep the noise below 5% then the max KP would be 5.

Tom Line 22-02-2012 16:03
Re: Speed PID Function
 
I wanted to sum this information up for some of the team members (since a couple of the kids on my team didn't understand a lot of things in this post).

First - the getrate implementation (at least in labview) oscillates because it has a very short time period. To obtain a more stable rate, you canput the calculation function in periodic tasks and run it at its own loop speed. This function should be:

(current encoder count - past encoder count) / loop time = count rate

To smooth this rate further, you can average the samples over several loops, or you can extend the time (calculate the count over 2 or three loops and the time over 2 or three loops). Both may give you somewhat delayed results, depending on your loop rate.

Next, a speed PID varies mainly in that you need to send the setpoint from the last speed loop into the new loop and modify THAT value. This prevents wheel reversal. The derivation from Jared on team 341 can be found here:

http://www.chiefdelphi.com/forums/sh...7&postcount=13

It shows the derivation and implementation of a velocity PID loop.

Ether 22-02-2012 17:15
Re: Speed PID Function
 
Quote:
Originally Posted by vamfun (Post 1132752)
b) The motor first order time constant tau_m is known
Can you address this in a bit more detail?

Are there some fairly simple tests that a team could do to obtain this number?

Or do you recommend calculating, and if so it might be helpful to do a blog entry on that or post some example calculations here.

Ether 22-02-2012 17:36
Re: Speed PID Function
 
Quote:
Originally Posted by Tom Line (Post 1132769)
I wanted to sum this information up for some of the team members (since a couple of the kids on my team didn't understand a lot of things in this post).

First - the getrate implementation (at least in labview) oscillates because it has a very short time period. To obtain a more stable rate, you canput the calculation function in periodic tasks and run it at its own loop speed.
...or you could set the encoder to 1x, if you haven't done so already


Quote:
This function should be:

(current encoder count - past encoder count) / loop time = count rate

To smooth this rate further, you can average the samples over several loops, or you can extend the time (calculate the count over 2 or three loops and the time over 2 or three loops). Both may give you somewhat delayed results, depending on your loop rate.
I think taking readings every execution cycle and filtering them would be a better solution than taking readings only every nth cycle. Can anyone comment on that?

Quote:
a speed PID varies mainly in that you need to send the setpoint from the last speed loop into the new loop and modify THAT value...The derivation from Jared on team 341 can be found here:

http://www.chiefdelphi.com/forums/sh...7&postcount=13
I believe what Jared showed was that you need to accumulate the output of the PID.

davidthefat 22-02-2012 19:01
Re: Speed PID Function
 
May I suggest using a different unit than RPM? The input is in the range of hundreds and thousands, but the output is from -1 to 1. If you were to use FPS instead, which would be 0 to 14 FPS or so at maximum, you will get more stable outputs since you do not have to use such tiny constants.

Ether 22-02-2012 19:24
Re: Speed PID Function
 
Quote:
Originally Posted by davidthefat (Post 1132892)
you will get more stable outputs since you do not have to use such tiny constants.
Are you suggesting that round-off errors using double precision floats cause instability if using RPM?

davidthefat 22-02-2012 20:39
Re: Speed PID Function
 
Quote:
Originally Posted by Ether (Post 1132911)
Are you suggesting that round-off errors using double precision floats cause instability if using RPM?
Perhaps, wrong choice of words. Consider this: if the system can, in fact, go up to 1000 RPM, the initial error when starting up from rest is 1000 RPM. In order for you to get the appropriate output from -1 to 1, your P constant will need to be about 0.0001. Now, that is not accurate at all, but trying to demonstrate that the constant will need to be tiny. But if that same system was measured in FPS (You are using RPM to derive this, so it will be the same accuracy as measurement), you will only be getting FPS up to 14 FPS at most. You have less of a range to work with, so the P can be about 0.1 instead. I was just pointing that out because his picture showed the P to be 0.3, which is way too big for a system measured in RPM. Just tuning it would be easier and I doubt the round off will have much errors using doubles.

Of course, always check your inputs and outputs to be in range.

vamfun 23-02-2012 16:53
Re: Speed PID Function
 
Quote:
Originally Posted by Ether (Post 1132816)
Can you address this in a bit more detail?

Are there some fairly simple tests that a team could do to obtain this number?

Or do you recommend calculating, and if so it might be helpful to do a blog entry on that or post some example calculations here.
note-velocity-pid-loop


This note on Velocity PID controller addresses your questions on how to obtain the open loop motor time constant tau_m. If testing is available always use that source since there is really no such thing as a spec motor. However, I use spec data to precompute gains since the gains will be adjusted anyway when we optimize the loop gains during integration testing. Often , very little adjustment is needed.

The note also comments on various ways to implement a PID loop in software.

Ether 23-02-2012 17:02
Re: Speed PID Function
 
Quote:
Originally Posted by vamfun (Post 1133592)
Thanks.

A couple of suggestions:

The figure is barely readable. Can you upsize the resolution, or provide a vector graphic option?

Also, you might want to clarify that what you are calling the "motor time constant tau_m" is not the time constant of the motor itself, but the time constant of the plant (motor plus whatever it is driving). It's slightly confusing on first reading.

vamfun 23-02-2012 20:28
Re: Speed PID Function
 
Quote:
Originally Posted by Ether (Post 1133606)
Thanks.

A couple of suggestions:

The figure is barely readable. Can you upsize the resolution, or provide a vector graphic option?

Also, you might want to clarify that what you are calling the "motor time constant tau_m" is not the time constant of the motor itself, but the time constant of the plant (motor plus whatever it is driving). It's slightly confusing on first reading.
Ok, added the plant stuff. BTW, you just click on figure to see it full size.

Ether 24-02-2012 12:58
Re: Speed PID Function
 
Quote:
http://vamfun.wordpress.com/2012/02/...city-pid-loop/

"Apply a normal PID loop with speed as the input, set the D term to zero and have a high integration gain , KI, that responds quickly to the error and holds the speed reference when the error goes to zero. The integrator must have full authority in this case. KP is adjusted to add damping and minimize the overshoot. This is the method I recommend and was the basis for the last post on precomputing PID gains."
Caveat:
The following is an excerpt from a post dated 01-14-2012 in the thread titled 2012: Tuning the Jaguar's Speed Control PID Loop :

Quote:
Originally Posted by Mr. Lim (Post 1105777)
The I-term appears to be capped. Theoretically, even a small I-term should cause the speed control loop to reach its setpoint eventually. It might take a long time, but it should reach the setpoint, given enough time.

It does not.

What we're seeing is the maximum speed we can achieve is directly proportional to the I constant.

This is unfortunate, because we have been able to find some I-constants which appear to work beautifully, however since the I-term is capped, we can never quite achieve our top speed.

Capping the I-term is usually a great way to ensure our PIDs don't wind up too much, however in this case, it's inhibiting us from ever getting to our top speed.

vamfun 28-02-2012 16:04
Re: Speed PID Function
 
It has been a while since I looked at the JAG cpp ... what is the actual limit they impose? The WPILIB PID integrator limits are ok.

Also, since I have been nursing a cold over the weekend, I decided to expand a little more on the PID loop driving a torque controller. This is a subcase of the PID with feedforward but it has a nice topology that allows direct control of torque. This is basically the back emf approach discussed earlier in this thread. I wanted to show a method for precalculating PID gains in this case.

Note: Velocity PID loop driving a torque input


I also cleaned up the velocity control loop figure and added a few comments to my last notes. The addiditons are mostly highlighted in color.

Tom Bottiglieri 28-02-2012 17:59
Re: Speed PID Function
 
Quote:
Originally Posted by vamfun (Post 1133592)
note-velocity-pid-loop


This note on Velocity PID controller addresses your questions on how to obtain the open loop motor time constant tau_m. If testing is available always use that source since there is really no such thing as a spec motor. However, I use spec data to precompute gains since the gains will be adjusted anyway when we optimize the loop gains during integration testing. Often , very little adjustment is needed.

The note also comments on various ways to implement a PID loop in software.
You note that tau_d = tau_m/r. How does one find R? Do we just choose this? If so, what is a reasonable choice?

vamfun 28-02-2012 20:13
Re: Speed PID Function
 
Quote:
Originally Posted by Tom Bottiglieri (Post 1136333)
You note that tau_d = tau_m/r. How does one find R? Do we just choose this? If so, what is a reasonable choice?
Tom, tau_d is the time constant of your closed loop system.... and that is your choice... usually it is driven by the timing constraints of the shooter design. As in the example at the end of my note we wanted the shooter to be within 5% (I had a typo of 1%) of the target speed in one second. Since this is a first order exponential response the output will have a step response of

v_out = v_in*(1 - exp(- t/tau_d) .

The % error as a function of time is err% = 100*exp(-t/tau_d);
t= tau_d , err%= 37
t=2*tau_d, err% = 14
t=3*tau_d, err% = 5
t=4*tau_d, err% = 2
t=5*tau_d, err% = .7

So to meet the 5% in 1 second we needed 3 time constants to elapse which means 1 sec = 3*tau_d. So tau_d = .333 seconds and that in turn drives r the ratio of tau_m/tau_d.

It is important to note that you must have excess torque to take advantage of the gain increase caused by the PID loop. Our nominal target speed for the key is around 2000 rpm. So we designed the the max speed to be 2.5 times the nominal.

In general, you have a feeling for how much you want to speed up the response of your system. It is this ratio that determines r. Of course r has bounds like any amplified system. I like to keep it under 5 and nominally about 3.

Edit:
The pole/zero placement method I described gives you a lot of gain margin so usually the limiting factor is not stability but rather system saturation caused by large command changes or noise in the command signal. Saturation makes the system nonlinear and the response can no longer be predicted by tau_d. High frequency noise is amplified by KP and can be unevenly rectified by saturation causing a bias offset the error signal and unwanted current transients. So it is difficult to generalize about what gains are tolerable. I know 254 has lots of experience with PID loops and you might share what your experiences are relative to tolerable gains in a velocity loop.


All times are GMT -5. The time now is 08:53.

Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi