Trouble with Basic Command-Based Programming

I’m the lead programmer for Team 2145 trying to write command-based autonomous code using the PIDController in the WPILib, and I’m having trouble… with everything. Would someone be able to point me to a tutorial series or lead me through the process of coding with Commands and the PIDController? It would be a huge help for me.

Can you show us what you have tried?

Have you seen this info?

There are multiple teams that have posted code online have you looked at any of them?

1 Like

This is all in our “Drive” command, which extends CommandBase

private final Drivetrain driveSubsystem;
private final PID pid = Robot.pid;

double target;
double speed;

public Drive(Drivetrain dSub, double targetPos, double speed) {

    driveSubsystem = dSub;
    target = targetPos;
    this.speed = speed;

    addRequirements(dSub);
}

@Override
public void initialize() {
    pid.setGoal(target);
    pid.setTolerance(0.01);
    System.out.print("hello, I work");
}

@Override

public void execute() {
    driveSubsystem.set(pid.getOutput(driveSubsystem.getLeftEncoderValue() * speed), pid.getOutput(driveSubsystem.getRightEncoderValue()) * speed); // driveSubsystem.set sets the motor values for the left and right sides of the robot
}

@Override
public void end(boolean interrupted) {
    driveSubsystem.set(0d, 0d);
}

@Override
public boolean isFinished() {
return pid.atGoal();
}

We’re using a PID Controller written by our previous programmer because I’m not sure how to use the PIDController provided by the WPILib. I had this set up to drive forward for 10 feet as the Default selection for Autonomous and it didn’t budge whatsoever.

public void autonomousPeriodic {
    switch(m_autoSelected) {
        case kDefault : 
        deafult :
            new Drive(drive, 10, 0.5);
        break;

        // More code...
    }
}

We tried looking at the Screensteps website and I couldn’t wrap my head around everything. It’s like they’re talking about the updated PIDController with the impression that I already knew how to code in the previous version, which I’ve never used.

I don’t see anything in your code that is actually telling the drivetrain (or drivesubsystem) to do anything.

If all you want to do is drive forward you can probably do this with a simple command to your drive subsystem much easier

You are creating a new Drive command instance inside autonomousPeriodic, which isn’t going to work even if your command works. You need to instantiate and schedule your command in autonomousInit:

public void autonomousInit {
    switch(m_autoSelected) {
        case kDefault : 
        default :
            (new Drive(drive, 10, 0.5)).schedule();
        break;
    }
} 

If you post a link to your actual code in github I’m happy to look at the command itself in more detail. Or feel free to look at our equivalent in github.

1 Like

Here is the GitHub

Your code seems to be an odd mix of non-command based and command based implementation. I would recommend picking one and starting over, preferably with one of the wpilib template projects.

You have some “subsystems” (Drivetrain, Wheel, Climb and Conveyor) that extentd SubsystemBase and others that do not (Camera, Door and Lift). You have one, ColorSensor, that extends TimedRobot, which is very bad.

The only time you appear to be trying to use commands is during auto and the observation from Ben that you should create the command in the auto init method rather than the auto periodic is quite correct. In addition, that command calls a method on Drivetrain that appears to just update variables that hold the next motor values but do not actually update the motors. That appears to be left to the very non-command base Drivetrain.run() method that appears to only be called during teleop and not auto. So, no motion.

Finally, if all you are trying to do in AutoLine is move off the initiation line for the 5 points, then there is really no need for PID control.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.