View Single Post
  #13   Spotlight this post!  
Unread 28-01-2012, 11:07
JohnFogarty's Avatar
JohnFogarty JohnFogarty is offline
Trapped under a pile of Mechanums
AKA: @doctorfogarty
FTC #11444 (Garnet Squadron) & FRC#1102 (M'Aiken Magic)
Team Role: Mentor
 
Join Date: Aug 2009
Rookie Year: 2006
Location: SC
Posts: 1,570
JohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond repute
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.
__________________
John Fogarty
2010 FTC World Championship Winner & 2013-2014 FRC Orlando Regional Winner
Mentor FRC Team 1102 M'Aiken Magic
"Head Bot Coach" FTC Team 11444 Garnet Squadron
Former Student & Mentor FLL 1102, FTC 1102 & FTC 3864, FRC 1772, FRC 5632
2013 FTC World Championship Guest Speaker