PIDSubsystem works once then disables the entire RobotDrive

[fireXtract]

I can enable, drive, then use the pid to setpoint my drivetrain, drive somemore but if i reenable the pid it breaks the drivetrain.

/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package us.mn.k12.fairmont.robotics.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import us.mn.k12.fairmont.robotics.RobotMap;
import us.mn.k12.fairmont.robotics.commands.CommandBase;
import us.mn.k12.fairmont.robotics.commands.DriveWithJoysticks;

/**
 *
 * @author Teacher
 */
public class DriveTrain extends PIDSubsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    RobotDrive drive = new RobotDrive(RobotMap.frontLeftMotor, RobotMap.rearLeftMotor, RobotMap.frontRightMotor, RobotMap.rearRightMotor);
    Encoder enc = new Encoder(RobotMap.frontRightEncA, RobotMap.frontRightEncB);
//    Timer timer = new Timer();
    
    public DriveTrain() {
        super("Drivetrain", 6, 0, 0);
        setAbsoluteTolerance(10);
        enc.start();
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new DriveWithJoysticks());
    }

    public void driveMechanum_Cartesian(double rightJoyY, double rightJoyX, double rotation) {
        drive.mecanumDrive_Cartesian((rightJoyY * -1), rotation, rightJoyX * -1, 0); // Mechanam wheel rollers should form an x when looking directaly down from the top
    }

    
    public void putPID() {
        CommandBase.table.putNumber("p", 8.0); // change it to smartdashboard getnumber tomorrow when you arent bored of deploying 
        CommandBase.table.putNumber("i", 0.0); // HOLY shoot YOU FIXED THE MOMORY LIEEK
        CommandBase.table.putNumber("d", 0.0);
    }

    public double updateStatus() {
        SmartDashboard.putNumber("Enc", enc.getRaw());
        return enc.getRaw();
    }

    public void reset() {
        enc.reset();
        reset();
    }

    public double returnPIDInput() {
        return enc.getRaw();
    }

    public void setSetpoint(int setpoint) {
        setSetpoint(setpoint);
    }

    public void getSetpoint(int setpoint) {
        getSetpoint();
    }

    public void getPosition(int position) {
        getPosition();
    }

    public void updatePID() {
        RobotMap.p = CommandBase.table.getNumber("p", 8);
        RobotMap.i = CommandBase.table.getNumber("i", 0);
        RobotMap.d = CommandBase.table.getNumber("d", 0);
        getPIDController().setPID(RobotMap.p, RobotMap.i, RobotMap.d);
    }

    public void usePIDOutput(double output) {
        driveMechanum_Cartesian(output, output, 0);
    }

    
}

package us.mn.k12.fairmont.robotics.commands;

public class DriveToDistance extends CommandBase {

    boolean go;
    double setpoint;

    /**
     * Require the drive train and store the desired setpoint.
     *
     * @param setpoint The desired setpoint for the drive train.
     */
    public DriveToDistance(boolean go, double setpoint) {
        requires(drivetrain);
        this.go = go;
        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() {
        System.out.println(this);
        if (go) {
            drivetrain.enable();
            drivetrain.setSetpoint(setpoint);
        }
    }

    // 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 drivetrain.onTarget();
    }

    // Called once after isFinished returns true
    /**
     * When this command ends, disable the drivetrain's PID
     */
    protected void end() {
        System.out.println("done");
        go = false;
        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() {
        end();
    }
}

/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package us.mn.k12.fairmont.robotics.commands;

/**
 *
 * @author Teacher
 */
public class DriveWithJoysticks extends CommandBase {
    
    public DriveWithJoysticks() {
        // Use requires() here to declare subsystem dependencies
        requires(drivetrain);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        System.out.println(this);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        drivetrain.driveMechanum_Cartesian(oi.getRightJoyY(), oi.getRightJoyX(), oi.getRightJoyR());
        
//        drivetrain.drivetest(oi.getRightJoyY(), oi.getLeftJoyY());
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}

Does anybody see what is wrong with the code?

[Joe Ross]

A description of what happens when it breaks the drivetrain would be helpful.

[mwtidd]

A couple of things:

First thing, in the initialize function of drive to distance, you should reset the PID. Otherwise the second time you call it the PID isn't zero'd

driveTrain.getPIDController().reset();

Second thing,

public void usePIDOutput(double output) {
driveMechanum_Cartesian(output, output, 0);
}

This seems wrong. I would assume that you only want to use the output on a single axis of the mecanum drive. I would expect it to be

driveMechanum_Cartesian(output, 0, 0);

[fireXtract]

A description would be that it spams RobotDrive not updated enough and everything on the robot works except the drivetrain, and I deploying the suggestions of lineskier.
Thanks for the input
EDIT:

[cRIO] edu.wpi.first.wpilibj.networktables2.server.ServerConnectionAdapter@a entered connection state: CONNECTED_TO_CLIENT
[cRIO] 
[cRIO] Affirmitive. I read you dave.
[cRIO] 
[cRIO] Open the door HAL!
[cRIO] DriveWithJoysticks
[cRIO] DriveToDistance
[cRIO] done
[cRIO] DriveWithJoysticks
[cRIO] DriveToDistance
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.

And i changed 2 parts

package us.mn.k12.fairmont.robotics.commands;

public class DriveToDistance extends CommandBase {

    boolean go;
    double setpoint;

    /**
     * Require the drive train and store the desired setpoint.
     *
     * @param setpoint The desired setpoint for the drive train.
     */
    public DriveToDistance(boolean go, double setpoint) {
        requires(drivetrain);
        this.go = go;
        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() {
        System.out.println(this);
        if (go) {
            drivetrain.getPIDController().reset();
            drivetrain.getPIDController().enable();
            drivetrain.getPIDController().setSetpoint(setpoint);
        }
    }

    // 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 drivetrain.getPIDController().onTarget();
    }

    // Called once after isFinished returns true
    /**
     * When this command ends, disable the drivetrain's PID
     */
    protected void end() {
        System.out.println("done");
        go = false;
        drivetrain.getPIDController().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() {
        end();
    }
}

 /*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package us.mn.k12.fairmont.robotics.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import us.mn.k12.fairmont.robotics.RobotMap;
import us.mn.k12.fairmont.robotics.commands.CommandBase;
import us.mn.k12.fairmont.robotics.commands.DriveWithJoysticks;

/**
 *
 * @author Teacher
 */
public class DriveTrain extends PIDSubsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    RobotDrive drive = new RobotDrive(RobotMap.frontLeftMotor, RobotMap.rearLeftMotor, RobotMap.frontRightMotor, RobotMap.rearRightMotor);
    Encoder enc = new Encoder(RobotMap.frontRightEncA, RobotMap.frontRightEncB);
//    Timer timer = new Timer();
    
    public DriveTrain() {
        super("Drivetrain", 6, 0, 0);
        setAbsoluteTolerance(10);
        enc.start();
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new DriveWithJoysticks());
    }

    public void driveMechanum_Cartesian(double rightJoyY, double rightJoyX, double rotation) {
        drive.mecanumDrive_Cartesian((rightJoyY * -1), rotation, rightJoyX * -1, 0); // Mechanam wheel rollers should form an x when looking directaly down from the top
    }

    
    public void putPID() {
        CommandBase.table.putNumber("p", 8.0); // change it to smartdashboard getnumber tomorrow when you arent borend of deploying 
        CommandBase.table.putNumber("i", 0.0); // HOLY shoot YOU FIXED THE MOMORY LIEEK
        CommandBase.table.putNumber("d", 0.0);
    }

    public double updateStatus() {
        SmartDashboard.putNumber("Enc", enc.getRaw());
        return enc.getRaw();
    }

    public void reset() {
        enc.reset();
        reset();
    }

    public double returnPIDInput() {
        return enc.getRaw();
    }

    public void updatePID() {
        RobotMap.p = CommandBase.table.getNumber("p", 8);
        RobotMap.i = CommandBase.table.getNumber("i", 0);
        RobotMap.d = CommandBase.table.getNumber("d", 0);
        getPIDController().setPID(RobotMap.p, RobotMap.i, RobotMap.d);
    }

    public void usePIDOutput(double output) {
        driveMechanum_Cartesian(output,0, 0);
    }

    
}

[fireXtract]

I have more detail after playing around with what triggers it.

1. Deploy code
2. enable robot
3. drive robot
4. press button 11 to activate the DriveToDistance command
5. robot uses pid to drive robot to the setpoint
6. command ends and i resume driving and shooting
7. press button 11 again to reactivate the DriveToDistance command
8. [cRIO] Robot Drive... Output not updated often enough *100
9. robot wont listen to any commands related to drivetrain but shooting still works
10. Disable robot
11. enable robot, and [cRIO] Robot Drive... Output not updated often enough
12. Redeploy to fix robot temporarily; until i activate drive to distance twice

[mwtidd]

https://www.google.com/search?q=recu...sm=93&ie=UTF-8


Try the "Did you mean".

Now look at this line of code:

public void reset() {
enc.reset();
reset();
}

I'm not going to give you the full answer, but here's a little mod to help you figure it out.

private int i = 0;
public void reset() {
i++;
if(i > 1000) System.out.println("Please, please, please, STOP RESETTING ME!");
enc.reset();
reset();
}

[fireXtract]

Originally Posted by lineskier

https://www.google.com/search?q=recu...sm=93&ie=UTF-8


Try the "Did you mean".

Now look at this line of code:



I'm not going to give you the full answer, but here's a little mod to help you figure it out.

I fixed my recursion error i believe, but its still yielding the same problem.
Output with my comments:

[cRIO] [Squawk VM] Version: 2011 FRC, Nov  5 2011, 14:34:13
[cRIO] FPGA Hardware GUID: 0x1394f6dc1feb42ec6910e5767ed1d22c
[cRIO] FPGA Software GUID: 0xa14c11bde4bb64aef6a86fc52a294cd9
[cRIO] 
[cRIO] Affirmitive. I read you dave.
[cRIO] 
[cRIO] edu.wpi.first.wpilibj.networktables2.server.ServerConnectionAdapter@1d entered connection state: GOT_CONNECTION_FROM_CLIENT
[cRIO] edu.wpi.first.wpilibj.networktables2.server.ServerConnectionAdapter@1d entered connection state: CONNECTED_TO_CLIENT
[cRIO] Open the door HAL!
[cRIO] Robot Drive... Output not updated often enough. // right when i enable, what is that all about?
[cRIO] DriveWithJoysticks // i move around the floor and read output from the enc on my smartboard
[cRIO] DriveToDistance //press button 11 and it goes to setpoint 100
[cRIO] done
[cRIO] DriveWithJoysticks //drive around some more and then disable, if i dont disable and reenable same thing happens so it doesnt matter
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Open the door HAL! //enable and press button 11
[cRIO] DriveToDistance
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough.
[cRIO] Robot Drive... Output not updated often enough. // here stuff works but not any drivetrain stuff

//////// Here i turn off(physical switch) and turn back on the robot and now it works again!

[cRIO] Open the door HAL!
[cRIO] DriveWithJoysticks
[cRIO] DriveToDistance
[cRIO] done
[cRIO] DriveWithJoysticks

Drivetrain sub

/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package us.mn.k12.fairmont.robotics.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import us.mn.k12.fairmont.robotics.RobotMap;
import us.mn.k12.fairmont.robotics.commands.CommandBase;
import us.mn.k12.fairmont.robotics.commands.DriveWithJoysticks;

/**
 *
 * @author Teacher
 */
public class DriveTrain extends PIDSubsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    RobotDrive drive = new RobotDrive(RobotMap.frontLeftMotor, RobotMap.rearLeftMotor, RobotMap.frontRightMotor, RobotMap.rearRightMotor);
    Encoder enc = new Encoder(RobotMap.frontRightEncA, RobotMap.frontRightEncB);
//    Timer timer = new Timer();
    
    public DriveTrain() {
        super("Drivetrain", 6, 0, 0);
        setAbsoluteTolerance(10);
        enc.start();
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new DriveWithJoysticks());
    }

    public void driveMechanum_Cartesian(double rightJoyY, double rightJoyX, double rotation) {
        drive.mecanumDrive_Cartesian((rightJoyY * -1), rotation, rightJoyX * -1, 0); // Mechanam wheel rollers should form an x when looking directaly down from the top
    }

    
    public void putPID() {
        CommandBase.table.putNumber("p", 8.0); // change it to smartdashboard getnumber tomorrow when you arent borend of deploying 
        CommandBase.table.putNumber("i", 0.0); // HOLY shoot YOU FIXED THE MOMORY LIEEK
        CommandBase.table.putNumber("d", 0.0);
    }
//
//    public boolean autoDrive(int dist, double speed) {
//        System.out.println("going fishes");
//        enc.reset();
//        while (enc.get() <= dist - 100) {
////          tal.set(speed*(.1*(dist-enc.get())));
//            if (enc.get() <= dist - 100) {
//                driveMechanum_Cartesian(speed, speed, 0);
//            } else if (enc.get() >= dist + 100) {
//                driveMechanum_Cartesian(-speed, -speed, 0);
//            }
//
//        }
//        return true;
//    }
//
//    // credit to team 1160's github for pieces of this
//    public void autoTimeDrive(double time, boolean run) {
//        timer.start();
//        timer.reset();
//        while (run) {
//            driveMechanum_Cartesian(.3, .3, 0);
//            System.out.println(timer.get());
//            if (timer.get() >= time) {
//                driveMechanum_Cartesian(0, 0, 0);
//                run = false;
//            }
//        }
//    }

    public double updateStatus() {
        SmartDashboard.putNumber("Enc", enc.getRaw());
        return enc.getRaw();
    }

    public void resetthing() {
        enc.reset();
        getPIDController().reset();
    }

    public double returnPIDInput() {
        return enc.getRaw();
    }

    public void updatePID() {
        RobotMap.p = CommandBase.table.getNumber("p", 8);
        RobotMap.i = CommandBase.table.getNumber("i", 0);
        RobotMap.d = CommandBase.table.getNumber("d", 0);
        getPIDController().setPID(RobotMap.p, RobotMap.i, RobotMap.d);
    }

    public void usePIDOutput(double output) {
        driveMechanum_Cartesian(output/4,0, 0);
    }

    
}

drive to distance

package us.mn.k12.fairmont.robotics.commands;

public class DriveToDistance extends CommandBase {

    boolean go;
    double setpoint;

    /**
     * Require the drive train and store the desired setpoint.
     *
     * @param setpoint The desired setpoint for the drive train.
     */
    public DriveToDistance(boolean go, double setpoint) {
        requires(drivetrain);
        this.go = go;
        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() {
        System.out.println(this);
        if (go) {
            drivetrain.getPIDController().reset();
            drivetrain.getPIDController().enable();
            drivetrain.getPIDController().setSetpoint(setpoint);
        }
    }

    // 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 drivetrain.getPIDController().onTarget();
    }

    // Called once after isFinished returns true
    /**
     * When this command ends, disable the drivetrain's PID
     */
    protected void end() {
        System.out.println("done");
        go = false;
        drivetrain.getPIDController().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() {
        end();
    }
}

[pblankenbaker]

It looks to me like your DriveToDistance command will only work one time the way you have it coded. The "go" flag was being set to "false" in the original end() method in the original implementation. Once the flag is set to false, then your drive train PID was never enabled on any subsequent invocation (until you power cycle).

Can you try refactoring it without the "go" flag to something like:

package us.mn.k12.fairmont.robotics.commands;

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() {
        System.out.println(this);
        drivetrain.enable();
        drivetrain.setSetpoint(setpoint);
    }

    // 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 drivetrain.onTarget();
    }

    // Called once after isFinished returns true
    /**
     * When this command ends, disable the drivetrain's PID
     */
    protected void end() {
        System.out.println("done");
        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() {
        end();
    }
}

You may also want to add a time out parameter to your command. For example, if you know it should always take less than 5 seconds for the command to complete, set the timeout to 5 seconds to make sure you get back manual control should something go wrong in reaching the set point (like a broken encoder).

[mwtidd]

Originally Posted by pblankenbaker

It looks to me like your DriveToDistance command will only work one time the way you have it coded. The "go" flag was being set to "false" in the original end() method in the original implementation. Once the flag is set to false, then your drive train PID was never enabled on any subsequent invocation (until you power cycle).

You may also want to add a time out parameter to your command. For example, if you know it should always take less than 5 seconds for the command to complete, set the timeout to 5 seconds to make sure you get back manual control should something go wrong in reaching the set point (like a broken encoder).

Good catch!

Both of these suggestions look solid to me.

[fireXtract]

Thank you both, I'm testing this now. I was looking at the documentation on how commands are called and i wasn't sure if the go thing was necessary.
EDIT: IT WORK ZOMGLOLOLOL IM SO HAPPY I LOVE YOU ALL I CAN SETPOINT AS MUCH AS I WANT AND NOMORE ROBOTDRIVE TIMING OUT!!>!>!>!>!>>!!
Thanks lineskier and pblankenbaker! It ended up being the go bool and I'm looking into the timeout thing. I'm so happy because we go to competition today, and i can make code for the main robot now!