Camera Code help

[gixxy]

Our team is using the camera to track and auto aim a turret for us. However as it is, it is causing all kinds of delays in the code and many errors.

I do not know this code nearly as well as I should (we divided up the tasks and Vision systems wasn't one of mine). However I would like someone with more experience than either of us has on the WPIlib vision system.

Code: https://github.com/frc3946/ReboundRumble

Thanks for any and all help.


As it stands when we start up the code it boots through the subsystems and once the camera code starts running gets through the 1 2 and 3 print statements (look through the code and you should understand), but has the "robot drive.....output not updated often enough" error after 1 constantly. Then we put in some more sub statements (I think we used "alpha, beta, ect." for that section.) Not sure when it err'ed then.

[dvanvoorst]

I didn't have time to look through your code, but I'm guessing you're not starting a seperate thread for the image processing. I believe our image processing took about a half second to process an image - which is a very long time if the rest of the robot is waiting for that to get done. So if you start a seperate thread to get the image and process it - the rest of the robot code can continue on until the image processing is done.
Another option, since you probably don't have much time to resolve this, is to only grab one image, process it and do what you need to do. Don't continuiously grab and process images.
Hope that helps a bit.

Dale Van Voorst

[Ginto8]

This is actually a big reason I'm glad my team decided to ignore my early advice and go with IterativeRobot again instead of command-based. The command scheduler is... well, just that, a scheduler. It does a, then b, then c, then d. If any of these things takes too long, everything slows down. When a, b, c, and d are fairly small operations (like most operations on a robot), you don't have an issue. Camera tracking means processing at least 1 640x480 image, probably multiple times and with algorithms that are usually a bit more complex than O(n). When doing all of these operations, your cRio's 400 MHz processor isn't exactly a shining star. If you don't have the luxury of being able to change the processing over to the driver station end, your best bet will be to scrap the command structure (at least partially) and put the processing into a separate thread.

[gixxy]

Alright. So camera needs it's own thread, or make the camera work on command of a button. Simple enough. Just hope we have enough time to get this all implemented.

Thanks Guys.

[abrightwell]

You could also consider moving the camera/image processing OFF of the cRIO using a Smartdashboard extension. I agree with the creation of a new thread to run the processing in and if I remember correctly, the robot safety system expects that it will be updated ever 20 milliseconds. Therefore, if you are performing a lengthy operation, then the robot will effectively "timeout". Here is our smartdashboard extension code if you are interested. It has changed a bit over the past few days and we haven't taken the opportunity to push our changes to github, but you should see those tomorrow afternoon sometime. Good luck! Let us know if you need further help.

[gixxy]

Alright I will consider integrating that if we have time before Bayou Regional this weekend...

Currently, as for about 2 hours ago what we have is a case switch statement so every interval it runs one processor intensive piece of the code then the ton of little code at the end. Its certainly more stable than before.

[gixxy]

My end result has been to use a stepNumber variable and use a switch case statement to split up the parts over several iterations. This way we don't do tons of heavy processing all at once.

Thanks for the help!!!!!!!

[gixxy]

So now we are having problems with our distance and maybe angle (although it did seem to work...). Here is all code that has to do with the camera and our tracking/aiming systems: (in ProcessImage there is a findDistance method)

ProcessImage Command:

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.slidellrobotics.reboundrumble.commands;

import com.slidellrobotics.reboundrumble.RobotMap;
import com.slidellrobotics.reboundrumble.subsystems.TrackingCamera;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author Allister Wright
 */
public class ProcessImage extends CommandBase {
    static int stepNo =1;
    
    private static double lastTime = 0;
    private static double thisTime = 0;
    private static double timeLapse = 0;
    
    double targetHeight = TrackingCamera.targetHeight;   //  Create a few necesarry local variables
    double targetWidth = TrackingCamera.targetWidth;    //  for concise code and calcs.
    
    double targetHeightFt;   //  Target Height in Feet
    double targetWidthFt;   //  Target Width in Feet
    double imageHeight; //  Total Height in Pixels
    double imageWidth; //  Total Width imn Pixels
                      
    double verticalFOV; //  Vertical Field of View in Feet
    double horizontalFOV;  //  Horizontal Field of View in Feet
    double verticalViewingAngle;  //  Vertical Camera Viewing Angle
    double horizontalViewingAngle;   //  Horizontal Camera Viewing Angle
    
    double horizontalRattle;   //  Horizontal off-centerness of center of goal
    double verticalRattle;  //  Vertical off-centerness of center of goal
    
    double centerDistance = 0;   //  Distance Variable to be used in firing Calculation
    double offCenterPixels = 0;
    double offCenterFt = 0;
    double verticalDistanceResult = 0;
    double horizontalDistanceResult = 0;
    double trueDistance = 0;
    double d = 0;
    double pi = 3.14159262;
    
    public ProcessImage() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(camera);
        requires(lazySusan);
        requires(leftShootingMotors);
        requires(rightShootingMotors);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        stepNo = 1;
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        thisTime = Timer.getFPGATimestamp();
        timeLapse = thisTime - lastTime;
        
        if(timeLapse >= 1.0) {
            getImage();
            if(stepNo == 6) {
                if(TrackingCamera.reports != null) {
                    selectGoal();
                    findAngle();
                    findDistance();
                } else {    //  If no goals are found
                System.out.println("Goal Selection and Analysis Aborted");  //  Print a notifier
                }
                stepNo++;
            }
        }
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        //todo this can just be set to true
        if(stepNo == 7) {
            return true;
        } else if(stepNo > 7) {
            System.out.println("Error Exit");
            return true;
        } else {
            return false;
        }
    }

    // Called once after isFinished returns true
    protected void end() {
        try {
            if (TrackingCamera.pic != null) {
                TrackingCamera.pic.free();
            } 
            if (TrackingCamera.convexHullImage != null) {
                TrackingCamera.convexHullImage.free();
            } 
            if (TrackingCamera.thresholdHSL != null) {
                TrackingCamera.thresholdHSL.free();
            }
            if (TrackingCamera.boundImage != null) {
                TrackingCamera.boundImage.free();
            }
        } catch (Exception ex) {
            System.out.println("Memory: "+ex);
        }
        lastTime = thisTime;
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        end();
    }
    
    private void getImage() {
        System.out.println("getImage");
        try {
            switch(stepNo){
            case 1:
                System.out.println("Running Case: 1");
                TrackingCamera.pic = camera.getImageFromCamera();
                System.out.println("Ran Case: 1.1");
                if(TrackingCamera.pic != null) {
                    TrackingCamera.totalWidth = TrackingCamera.pic.getWidth();
                System.out.println("Ran Case: 1.2");
                TrackingCamera.totalHeight = TrackingCamera.pic.getHeight();
                System.out.println("Ran Case: 1.3");
                } else {
                    break;
                }
                stepNo++;
                break;
            case 2:
                System.out.println("Running Case: 2");
                TrackingCamera.thresholdHSL = TrackingCamera.pic.thresholdHSL(150, 185, 244, 255, 2, 20);      //Sets a Blue light threshold
                System.out.println("Ran Case: 2.1");
                stepNo++;
                break;
            case 3:
                System.out.println("Running Case: 3");
                TrackingCamera.convexHullImage = TrackingCamera.thresholdHSL.convexHull(false);        //Fills in the bounding boxes for the targets            
                System.out.println("Ran Case: 3.1");
                stepNo++;
                break;
            case 4:
                System.out.println("Running Case: 4");
                TrackingCamera.boundImage = TrackingCamera.convexHullImage.particleFilter(TrackingCamera.cc);
                System.out.println("Ran Case: 4.1");
                stepNo++;
                break;
            case 5:
                System.out.println("Running Case: 5");
                TrackingCamera.reports = TrackingCamera.boundImage.getOrderedParticleAnalysisReports();
                System.out.println("Ran Case: 5.1");
                System.out.println("Reports: "+TrackingCamera.reports.length);
                System.out.println("Ran Case: 5.2");
                stepNo++;
                break;
            }
        } catch (NIVisionException ex) {
            System.out.println(ex);
            stepNo = 60;
        } catch (Exception ex) {
            System.out.println(ex);
            stepNo = 60;
        }
    }
    
    private void selectGoal() {
        System.out.println("selectGoal");
        TrackingCamera.targetGoal = null;
        
        if (TrackingCamera.reports.length == 0) {
            return;
        } if (TrackingCamera.reports.length == 1) {
            System.out.println("Not enough goals");
            TrackingCamera.targetGoal = TrackingCamera.reports[0];
        } else {
            //Gus says: is this right. it doesn't look right to me...
            TrackingCamera.leftGoal = TrackingCamera.reports[0];     //Recognizes the
            TrackingCamera.rightGoal = TrackingCamera.reports[0];    //middle goals.
            int maxIndex = TrackingCamera.reports.length;
            if (maxIndex > 4) {
                maxIndex=4;
            }
            for(int i = 1; i <= maxIndex; i++) {
                if(TrackingCamera.reports[i].center_mass_x < TrackingCamera.leftGoal.center_mass_x) {
                    TrackingCamera.leftGoal = TrackingCamera.reports[i];
                } if(TrackingCamera.reports[i].center_mass_x > TrackingCamera.leftGoal.center_mass_x) {
                    TrackingCamera.rightGoal = TrackingCamera.reports[i];
                }
            }
            //We have four goals in view index 1 is the left and index 2 is right
            double leftWidth = TrackingCamera.leftGoal.boundingRectWidth;     //Finds the widths of
            double rightWidth = TrackingCamera.rightGoal.boundingRectWidth;   //both middle goals.
            if (leftWidth <= rightWidth) {
                TrackingCamera.targetGoal = TrackingCamera.rightGoal;   //Decides which goal we are
            } else {                                                    //closer to and targets it.
                TrackingCamera.targetGoal = TrackingCamera.leftGoal;    //
            }
            System.out.println("Target Selected");
        }
    }
    
    private void findAngle() {
        System.out.println("findAngle");
        if (TrackingCamera.targetGoal == null){
            return;
        }
        TrackingCamera.horCenter = (TrackingCamera.totalWidth / 2);     //Finds the pixel value of the horizontal center
        TrackingCamera.targetLocale = TrackingCamera.targetGoal.center_mass_x;        //Finds the center of our target
        TrackingCamera.targetDiff = Math.abs(TrackingCamera.targetLocale - TrackingCamera.horCenter); // see how far away we are

        //TODO: tune the 10 pixels to the right number
        //there is always going to be a little error, but we want some small window
        //where the lazy suzan stops moving to we can make an accurate shot.

        System.out.println("Targe Diff: "+TrackingCamera.targetDiff);
        if (TrackingCamera.targetDiff < 15) {
            //lazySusan.setRelay(RobotMap.susanOff);   //turn off
        } else if (TrackingCamera.targetLocale < TrackingCamera.horCenter) { //and if we are facing right
            //lazySusan.setRelay(RobotMap.susanLeft);   //turn left
            lazySusan.setSetpointRelative(-5);
        } else {                                        //if we face left
            //lazySusan.setRelay(RobotMap.susanRight);   //turn right
            lazySusan.setSetpointRelative(+5);
        }
        SmartDashboard.putDouble("Angle",TrackingCamera.targetDiff);
    }
    
    private void findDistance() {
        System.out.println("findDistance");
        if (TrackingCamera.targetGoal == null){ //  If no target is found
            //leftShootingMotors.setSetpoint(1000);   //  Set Left shooting Motors to about Half Speed
            //rightShootingMotors.setSetpoint(1000);  //  Set Right Shooting Motors to about Half Speed
            System.out.println("No target set");    //  Debug Print Statement
            
            System.out.println("Checkpoint 10a");
            
            return;
        }
        
        verticalViewingAngle = 47;      //  Defines the Viewing
        horizontalViewingAngle = 47;    //  Angles of our camera
        
        imageHeight = 480;  //  Image Height
        targetHeight = TrackingCamera.targetGoal.boundingRectHeight;    //  Sets the height of our target.
        targetHeightFt = 1.5;   //  Defines goal's constant ft height
        
        imageWidth = 640;   //  Image Width
        targetWidth = TrackingCamera.targetGoal.boundingRectWidth;  //  Sets the width of our target.
        targetWidthFt = 2.0;    //  Defines goal's constant ft width
        
        verticalFOV = imageHeight*(targetHeightFt/targetHeight);    //  Gets the Foot Value of our Vertical Field of View.
        horizontalFOV = imageWidth*(targetWidthFt/targetWidth); //  Gets the ft value of our horizontal Field of View.
        
        verticalRattle = Math.abs(TrackingCamera.targetGoal.center_mass_y - (imageHeight/2));   //  Finds the vertical off-ceneterness.
        horizontalRattle = Math.abs(TrackingCamera.targetGoal.center_mass_x - (imageWidth/2));  //  Finds the horizontal off-centerness.
        
        verticalDistanceResult = Math.sqrt(4/3)*(verticalFOV/2)/Math.tan(verticalViewingAngle/2);   //  Provides the Result of our Vertically-Based Calculation.
        horizontalDistanceResult = Math.sqrt(3/4)*(horizontalFOV/2)/Math.tan(horizontalViewingAngle/2); //  Provides the Result of our Horizontally-Based Calculation.
        
        centerDistance = (verticalDistanceResult + horizontalDistanceResult) / 2;   //  Take the average to try get a more accurate measurement.
        
        
        offCenterPixels = Math.sqrt((verticalRattle*verticalRattle) + (horizontalRattle*horizontalRattle)); //  Finds the Linear Distance from the Center of the Image to the Center of the Goal.
        offCenterFt = offCenterPixels*(Math.sqrt((verticalFOV*verticalFOV)+(horizontalFOV*horizontalFOV))); //  Converts the above Caluclated measurement into its proper Ft value.
        
        trueDistance = Math.sqrt((centerDistance*centerDistance)+(offCenterFt*offCenterFt));    //  Find the Linear Distance form the Lens of our Camera to the Center of our Goal.
        
        
        //if distance to target is invalid, just set it to some number
        if (TrackingCamera.distanceToTarget > 60 || TrackingCamera.distanceToTarget <= 0) {
            TrackingCamera.distanceToTarget = 60;
        }
        
        d = trueDistance;    //  See below Calculation for conciseness
        
        TrackingCamera.launchSpeed = 60 * (d / Math.sqrt((11 / 6 - d) / -16.1) / (2 / 3 * pi));  //Calcs the required rpms for firing
        leftShootingMotors.setSetpoint(TrackingCamera.launchSpeed);     //  Sets the shooting Left Shooting Motors
        rightShootingMotors.setSetpoint(TrackingCamera.launchSpeed);    //  Sets the Right Shooting Motors

        /* A String of Debug Print Statements */
        System.out.println();
        System.out.println("Vertcal Distance Result: "+verticalDistanceResult);
        System.out.println("Horizontal Distance Result: "+horizontalDistanceResult);
        System.out.println("Central Distance: "+centerDistance);
        System.out.println("True Distance: "+d);
        System.out.println("Camera Launch Speed: "+TrackingCamera.launchSpeed);
        System.out.println();
        
        //SmartDashboard.putDouble("Vertical Distance Result", verticalDistanceResult);
        //SmartDashboard.putDouble("Horizontal Distance Result", horizontalDistanceResult);
        //SmartDashboard.putDouble("Center Point Distance", centerDistance);
        SmartDashboard.putDouble("Distance", d);
        SmartDashboard.putDouble("Launch", TrackingCamera.launchSpeed);
    }
}

TrackingCamera Subsystem:

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.slidellrobotics.reboundrumble.subsystems;

import com.slidellrobotics.reboundrumble.commands.ProcessImage;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.image.*;

/**
 *
 * @author Allister Wright
 */
public class TrackingCamera extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    
    public static AxisCamera camera;
    public static ColorImage pic;
    public static ParticleAnalysisReport[] reports;
    //public static int leftGoalIndex = 0, rightGoalIndex = 0;
    public static CriteriaCollection cc = new CriteriaCollection();
    public static BinaryImage thresholdHSL;
    public static BinaryImage convexHullImage;
    public static BinaryImage boundImage;
    //public static BinaryImage bigObjectsImage;
    public static double totalWidth = 640, totalHeight = 480;
    public static ParticleAnalysisReport targetGoal, leftGoal, rightGoal;    
    public static double targetLocale, horCenter, targetDiff, launchSpeed;
    public static double centerDistance;
    public static double distanceToTarget, targetHeight, targetHeightFeet = 1.5;
    public static double horFOV, vertFOV, cameraVertFOV = 47, cameraHorizFOV = 47;
    public static double d1, d2, d, targetWidth, targetWidthFeet = 2;
    //public static boolean getFinished = false, selectFinished = false;
    //public static boolean angleFinished = false, distanceFinished = false;
    
    public TrackingCamera() {
        if(camera == null) {
            TrackingCamera.camera = AxisCamera.getInstance("10.39.46.11");
        }
        if(camera != null) {
            System.out.println("Camera init");     
        } else {
            System.out.println("Camera Failure");  
        }
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new ProcessImage());
    }

    public ColorImage getImageFromCamera() throws NIVisionException {
        try {
            if(camera!=null) {
                if (camera.freshImage()) {
                    try {
                        System.out.println("sent image");
                        return camera.getImage();                      
                    } catch (AxisCameraException ex) {
                        System.out.println("getImageFromCamera"+ex);
                    } catch (NIVisionException ex) {
                        System.out.println(ex);
                    }
                }
            } else {
                System.out.println("CAMERA IS STILL NULL!!!!");
            }
        } catch (Exception ex) {
            System.out.println("Get Image From Camera: "+ex);
        }
        System.out.println("return null");
        return null;
    }
    
   private class CalibrationPoint {

        double distance;
        double rpms;

        public CalibrationPoint(double distance, double rpms) {
            this.distance = distance;
            this.rpms = rpms;
        }
    }

    //just incase the formula doesn't work out here is a test based
    //interpolation function
    public double distanceToRMP(double distance) {
        //load the test data when practing with the real launcher
        // points MUST be inorder of closest to furthest away
        CalibrationPoint calibrationPoints[] = {new CalibrationPoint(5, 100),
            new CalibrationPoint(10, 200),
            new CalibrationPoint(15, 500),
            new CalibrationPoint(40, 1500)};

        //find the two calibration points were the input distance is between them
        int upperIndex;
        
        
        upperIndex = 1;
        while(upperIndex < calibrationPoints.length) {
            if (distance <= calibrationPoints[upperIndex].distance &&
                distance > calibrationPoints[upperIndex-1].distance) {
                break;
            }
            upperIndex++;
        }
        
        //interpolate the point
      
        if (upperIndex >= calibrationPoints.length){
        upperIndex=calibrationPoints.length-1;
        System.out.println("Error calibration");
        }
        System.out.println("rpms "+calibrationPoints[upperIndex].rpms);
        double slope =  (calibrationPoints[upperIndex].rpms - calibrationPoints[upperIndex - 1].rpms)
                      / (calibrationPoints[upperIndex].distance - calibrationPoints[upperIndex - 1].distance);
        double intercept = calibrationPoints[upperIndex].rpms - slope * calibrationPoints[upperIndex].distance;
        return slope * distance + intercept;
    }
}

LazySusan Subsystem:

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.slidellrobotics.reboundrumble.subsystems;

import com.slidellrobotics.reboundrumble.RobotMap;
import com.slidellrobotics.reboundrumble.commands.CommandBase;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Relay.Value;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author Gus Michel
 */
public class LazySusan extends PIDSubsystem {

    private static final double Kp = 1.0;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;
    
    private Relay susanWindow;
    private Gyro susanGyro;
    
    private final String dashName = "Lazy Susan";

    // Initialize your subsystem here
    public LazySusan() {
        super("LazySusanPID", Kp, Ki, Kd);
        System.out.println("[LazySusan] Starting");
        susanWindow = new Relay(RobotMap.susanSpike);
        System.out.println("[LazySusan] susanWindow initalized");
        susanGyro = new Gyro(RobotMap.turretGyro);
        System.out.println("[LazySusan] susanGyro initalized");
        System.out.println("[LazySusan] Started");
        setSetpoint(0);
        enable();
        getPIDController().setOutputRange(30, 30);
        getPIDController().setInputRange(-360, 360);
        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        // System.out.println("gyro: " + susanGyro.getAngle());
        SmartDashboard.putDouble("Gyro", susanGyro.getAngle());
        return susanGyro.getAngle();
    }
    
    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        output = getSetpoint()-susanGyro.getAngle();
       // System.out.println("lazy output: "+output);
        if(output > 5.0) {
          this.setRelay(RobotMap.susanRight);
          //System.out.println("Left");
        } else if(output < -5.0) {
          this.setRelay(RobotMap.susanLeft);
          //System.out.println("Right");
        } else {
           this.setRelay(RobotMap.susanOff);
        }
    }
    
    public void setRelay(Value value) {
        susanWindow.set(value);
        if(value.equals(RobotMap.susanRight)) {
            SmartDashboard.putString(dashName, "Right");
        } else if(value.equals(RobotMap.susanLeft)) {
            SmartDashboard.putString(dashName, "Left");
        } else {
            SmartDashboard.putString(dashName, "Off");
        }
    }
    
    public Relay getSpike() {
        return susanWindow;
    }
    
    public Gyro getGyro() {
        return susanGyro;
    }
}

FireMotors Subsystem

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.slidellrobotics.reboundrumble.subsystems;

import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;


/**
 *
 * @author Gus Michel
 * 
 */
public class FireMotors extends PIDSubsystem {

    private static double Kp = 0.00001;
    private static  double Ki = 0.0;
    private static  double Kd = 0.0;
    
    private Counter counter;
    private Jaguar victor;
    double lastTime;
    double newtime;
    
    static final int rpmsFilterMax = 5;
    Double[] rpmsFilter= new Double[rpmsFilterMax];
    int rpmsFilterIndex = 0;
    
    double rpms=0;
    double victorSetting=0;
    
    String Name;
    
    // Initialize your subsystem here
    public FireMotors(String Name,int counterChannel, int victorChannel) {
        super(Name, Kp, Ki, Kd);
        counter = new Counter(counterChannel);
        victor = new Jaguar(victorChannel); 
        enable();
        setSetpoint(1000); //rpms
        lastTime = Timer.getFPGATimestamp();
        counter.start();
        getPIDController().setOutputRange(-.1, .1);
        setSetpointRange(0,2000);
        this.Name = Name; //set class scope name to the Name passed in the constructor
        System.out.println(Name +  "Firemotor init");
        for (int index =0;index < rpmsFilterMax;index++){
             rpmsFilter[index]=Double.valueOf(0);
            
        }
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
      
    }
    
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        
        double timespan;
        int counts;
        
        // these three lines are time critial
        newtime = Timer.getFPGATimestamp();
         timespan = newtime- lastTime; //number of seconds during counting 
         if (timespan <= 0.130) {
             //System.out.println("Firemotor return PID timspan 0");
            return rpms; //don't recalculate, just give what ever it was
        }
         
        counts = counter.get(); //number of counts since last update
       counter.reset();        //reset counts ASAP so we don't miss any
       //  System.out.println("1");
              
        lastTime = newtime;
       
        //System.out.println("2");
        rpms = counts/ 8.0 / timespan*60.0;
        //8 counts per revolution, 60 seconds per minute  
          //System.out.println(Name + ": " +  rpms);
          //System.out.println("     Counts: "+counts);
          //System.out.println("          Timespan: "+timespan);
          //System.out.println();
        //sometimes the counter goes nuts
        //this deboundes it
        if (rpms > 1750)
            rpms = 1750;
        
        //    System.out.println("3");
        rpmsFilter[rpmsFilterIndex]=Double.valueOf(rpms);
        rpmsFilterIndex++;
        if (rpmsFilterIndex >= rpmsFilterMax){
            rpmsFilterIndex=0;
        }
        //System.out.println("4");
       rpms =0;
        for (int index =0;index < rpmsFilterMax;index++){
             rpms = rpms + rpmsFilter[index].doubleValue();
             //System.out.println(index);
        }
        rpms = rpms / rpmsFilterMax;
      
        return rpms;
        
    }
    
    protected void usePIDOutput(double output) {
        //When the PID system thinks there is no error then
        //set point is equal to rpms and output = 0        
        //soft start if we not moving, only set power to 10%
       
         victorSetting=victor.get()-output;
        
        
        if (Math.abs(victorSetting) < .3){
           
                victorSetting = -.3;
            
        }
          //System.out.println("Use PID "+victorSetting);
        //victor.set(victorSetting);
        victor.set(getSetpoint());

        
    }
    
    public void updateStatus(){
        SmartDashboard.putDouble(Name + " RPMS",rpms);   
        SmartDashboard.putDouble(Name + " victor",victor.get()); 
        SmartDashboard.putDouble(Name + " victor Setting",victorSetting); 
        
    }
}

FireMotor Instances

public static FireMotors leftShootingMotors = new FireMotors("Left",RobotMap.leftFireEncoder, RobotMap.leftShootingMotor);
    public static FireMotors rightShootingMotors = new FireMotors("Right",RobotMap.rightFireEncoder, RobotMap.rightShootingMotor);