Speed PID Function

[JohnFogarty]

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]

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]

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]

Originally Posted by John_1102

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]

Originally Posted by John_1102

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]

Originally Posted by davidthefat

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]

Originally Posted by Tom Line

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.

Oh snap... I just ran a simulation, you are correct.

[BornaE]

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]

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?

/*----------------------------------------------------------------------------*/
/* 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]

@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]

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]

Originally Posted by John_1102

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]

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.

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]

Originally Posted by John_1102

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.

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]

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.