DoubleSolenoid Programming Debugging

This is the first year my team has wired a pneumatics system that uses a double solenoid. I have programmed a Subsystem and Command to allow for a button to extend the piston and another to retract it. The compressor is not activating at all though. Does my code look correct? I’m not sure if this is a code error, or if it’s a potential wiring problem. We have used the Phoenix tuner to verify that the Pneumatic Control Module is, in fact, set to CAN ID 0 – so I think the DoubleSolenoid object instantiation is correct. Below is the code:

The Subsystem:

package frc.robot.subsystems;

import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class PneumaticsSubsystem extends SubsystemBase {

  /*
  The Pneumatics Control Module (PCM) we will be using has
  been given the default CAN id of 0. This means we do not
  have to specify an id in the DoubleSolenoid constructor.

  In the DoubleSolenoid constructor, we only need to specify
  the port numbers on which the double solenoid is connected
  to the PCM (in our case, 1 and 2).
  */
  private DoubleSolenoid doubleSolenoid = new DoubleSolenoid(1, 2);

  /**
   * Creates a new PneumaticsSubsystem.
   */
  public PneumaticsSubsystem() {
    super(); //add this
  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }

  public void extendPiston() {
    //Should extend the piston
    doubleSolenoid.set(DoubleSolenoid.Value.kForward);
  }

  public void retractPiston() {
    //Should retract the piston
    doubleSolenoid.set(DoubleSolenoid.Value.kReverse);
  }
}

Below are the Command classes that trigger the extend and retract functionalities:

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.PneumaticsSubsystem;

public class PneumaticsRetractPistonCommand extends CommandBase {

  private PneumaticsSubsystem pneumaticsSubsystem = new PneumaticsSubsystem();

  /**
   * Creates a new PneumaticsRetractPistonCommand.
   */
  public PneumaticsRetractPistonCommand(PneumaticsSubsystem pneumaticsSubsystem) {
    // Use addRequirements() here to declare subsystem dependencies.
    this.pneumaticsSubsystem = pneumaticsSubsystem;
    addRequirements(this.pneumaticsSubsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    pneumaticsSubsystem.retractPiston();
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return false;
  }
}

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.PneumaticsSubsystem;

public class PneumaticsExtendPistonCommand extends CommandBase {

  //Member variables will be subsystems that the command will be executed on:
  PneumaticsSubsystem pneumaticsSubsystem;

  //Pass in an instance of all the required subsystems in the constructor.
  //and set them to the member variables.
  public PneumaticsExtendPistonCommand(PneumaticsSubsystem pneumatics) {
    // Use addRequirements() here to declare subsystem dependencies.
    pneumaticsSubsystem = pneumatics;
    addRequirements(pneumaticsSubsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    pneumaticsSubsystem.extendPiston(); //This is the command to execute -- extend the piston
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return false;
  }
}

Below is the RobotContainer class:

public class RobotContainer {

  /*
  Specify what port (as labeled on the FRC Driver's Station)
  the physical controller is connected to. This is listed in
  the Driver Station's "USB order list". If the controller is
  the only thing attached to the computer, then the port is
  almost always 0.  You should double check, though.
  */
  public static Joystick controller = new Joystick(0); //This variable represents the physical game controller

  //Pneumatics Subsystems and Commands
  PneumaticsSubsystem pneumaticsSubsystem = new PneumaticsSubsystem();
  PneumaticsExtendPistonCommand pneumaticsExtendPistonCommand = new PneumaticsExtendPistonCommand(pneumaticsSubsystem);
  PneumaticsRetractPistonCommand pneumaticsRetractPistonCommand = new PneumaticsRetractPistonCommand(pneumaticsSubsystem);


  // The robot's subsystems and commands are defined here...
  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
  private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);



  /**
   * The container for the robot.  Contains subsystems, OI devices, and commands.
   */
  public RobotContainer() {
    // Configure the button bindings
    configureButtonBindings();
  }

  /**
   * Use this method to define your button->command mappings.  Buttons can be created by
   * instantiating a {@link GenericHID} or one of its subclasses ({@link
   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
   */
  private void configureButtonBindings() {

    //Map buttons 3 and 4 on the controller to extend and retract the piston, respectively:
    final JoystickButton extendPistonButton = new JoystickButton(controller, 3);
    final JoystickButton retractPistonButton = new JoystickButton(controller, 4);

    //Define each button's behaviour:
    extendPistonButton.whenHeld(pneumaticsExtendPistonCommand);
    retractPistonButton.whenHeld(pneumaticsRetractPistonCommand);
  }


  /**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
   */
  public Command getAutonomousCommand() {
    // An ExampleCommand will run in autonomous
    return m_autoCommand;
  }
}

Any help is tremendously appreciated!

The code for extending and retracting the cylinder looks great, the compressor isn’t turning on because it seems like you never instantiate a Compressor object and call it with the .start(); method. A way i do this is by creating it in my Robot.java class, and then calling .start() in the init() methods to guarantee that the compressor will stay on in all modes (though I believe you only need it in robotInit())

Here’s my teams code for reference:

(refer to lines 28 and 40)

This is not required. If you have a solenoid in your code, the compressor will automatically run on that PCM.

OP, the issue likely lies in wiring, but it’s possible the pressure switch is bad. You can use Phoenix Tuner to look at the PCM faults and status of the pressure switch and compressor output from the PCM’s perspective.

2 Likes

^^ What Peter said. Assuming the PCM thinks its pressurized and it isn’t, you can try briefly shorting across your pressure switch. If it comes on, then it’s your switch. Next, I would try removing the switch wires from the PCM and (with your vent plug open to prevent boom) insert a short piece of wire with stripped ends into the pressure switch ports on the PCM. If it doesn’t come on then, either your code is trying to use the compressor on a different PCM, or you have a bad PCM. (Or compressor, but not likely if you did Peter’s check first.)

Also, you are creating two instances of the PneumaticsSubsystem. Once in RobotContainer, which is the right place and it is passed to the two command constructors, which is good.

PneumaticsSubsystem pneumaticsSubsystem = new PneumaticsSubsystem();

And again at your declaration of the variable to hold it in PneumaticsRetractPistonCommand:

private PneumaticsSubsystem pneumaticsSubsystem = new PneumaticsSubsystem();

This is bad. It is immediately replaced with the one passed on the constructor, but you now have two of these trying to use the same PCM ports.

Get rid of the one in the command and see if that helps.

Are your wires actually in 1 and 2 for the channels? That seems really odd to me. IIRC the numbers on the PCM are from 0 -7.

If you were plugging them in to the first and second areas you should be using

  private DoubleSolenoid doubleSolenoid = new DoubleSolenoid(0, 1);

It’s entirely possible that 1,2 would work, I actually have no idea. It just seems really odd to start at 1, rather than 0 in the PCM.

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