Cancel a command

[cpapplefamily]

We have a Command Group that parallels two subsystem movements. Our Scissor can not move to different heights if the wrist is not less then a max position. So what we did was in the initialize() of the scissor command we wrote this: [Lines 42 - 65]

 1 // RobotBuilder Version: 2.0 
2 // 
3 // This file was generated by RobotBuilder. It contains sections of 
4 // code that are automatically generated and assigned by robotbuilder. 
5 // These sections will be updated in the future when you export to 
6 // Java from RobotBuilder. Do not put any code or make any change in 
7 // the blocks indicating autogenerated code or it will be lost on an 
8 // update. Deleting the comments indicating the section will prevent 
9 // it from being updated in the future. 
10 
 
11 
 
12 package org.usfirst.frc3244.SirAntsABot.commands; 
13 
 
14 import edu.wpi.first.wpilibj.command.Command; 
15 import org.usfirst.frc3244.SirAntsABot.Robot; 
16 
 
17 /** 
18  * 
19  */ 
20 public class Scissor_To_Position extends Command { 
21 
 
22     // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS 
23     private double m_setpoint; 
24   
25     // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS 
26 
 
27     // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR 
28     public Scissor_To_Position(double setpoint) { 
29 
 
30     // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR 
31         // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING 
32         m_setpoint = setpoint; 
33 
 
34         // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING 
35         // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES 
36         requires(Robot.scissorPID); 
37 
 
38     // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES 
39     } 
40 
 
41     // Called just before this Command runs the first time 
42     protected void initialize() { 
43     	System.out.println(" Scissor_To_Position INIT()");
44     	// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 
45     	// Not Allow movement if Wrist Is up ! 
46     	// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 
47     	if(m_setpoint>70 && Robot.wristPID.getSetpoint()>30){ 
48     		System.out.println("Scissor Cancled Wrist > 30"); 
49     		end(); 
50     	}else{ 
51     		if(m_setpoint>30 && Robot.wristPID.getSetpoint()>55){ 
52     			System.out.println("Scissor Cancled Wrist > 55"); 
53         		end(); 
54         		 
55         	}else{ 
56         		// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INITIALIZE 
57         Robot.scissorPID.enable(); 
58         Robot.scissorPID.setSetpoint(m_setpoint); 
59 
 
60     // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INITIALIZE 
61         	} 
62     	} 
63     	 
64          
65     } 
66 
 
67     // Called repeatedly when this Command is scheduled to run 
68     protected void execute() { 
69     } 
70 
 
71     // Make this return true when this Command no longer needs to run execute() 
72     protected boolean isFinished() { 
73         // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=ISFINISHED 
74         return Robot.scissorPID.onTarget(); 
75 
 
76     // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=ISFINISHED 
77     } 
78 
 
79     // Called once after isFinished returns true 
80     protected void end() { 
81     	Robot.scissorPID.disable(); 
82     } 
83 
 
84     // Called when another command which requires one or more of the same 
85     // subsystems is scheduled to run 
86     protected void interrupted() { 
87     	 Robot.scissorPID.setSetpoint(Robot.scissorPID.getPosition()); 
88     	 end(); 
89     } 
90 }

It does protect the hardware and cancels the movement of the scissor as desired sort of. What we have notices is the command group will not re-initialize unless a different command that uses the scissor subsystem is requested witch we assume must be calling the interrupted(). We found this using line 43 and seen the print only happens the first button press and button presses only after a different command that requires(Robot.scissorPID)

Do you have any suggestions how should we cancel this Scissor_To_Position command if the condition in the initialize() is found? Do we need to force the isFinished?

[Joe Ross]

Yes, you need to make isFinished return true in that condition.

[cpapplefamily]

Thank you. We will give this a go.

private boolean m_CancelCommand = false;

 

protected void initialize() { 
43     	m_CancelCommand = false; 
44     	// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 
45     	// Not Allow movement if Wrist Is up ! 
46     	// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 
47     	if(m_setpoint>70 && Robot.wristPID.getSetpoint()>30){ 
48     		System.out.println("Scissor Cancled Wrist > 30"); 
49     		m_CancelCommand = true; 
50     	}else{ 
51     		if(m_setpoint>30 && Robot.wristPID.getSetpoint()>55){ 
52     			System.out.println("Scissor Cancled Wrist > 55"); 
53         		m_CancelCommand = true; 
54         		 
55         	}else{ 
56         		
57         Robot.scissorPID.enable(); 
58         Robot.scissorPID.setSetpoint(m_setpoint); 
59 
 
60    
61         	} 
62     	}

72     protected boolean isFinished() { 
73       
74         return Robot.scissorPID.onTarget() || m_CancelCommand ; 
75 
 
76     
77     } 
78

[euhlmann]

Does Command.cancel() work?

[cpapplefamily]

We will try

this.Cancel()

but the JavaDocs say:

void edu.wpi.first.wpilibj.command.Command.cancel()


This will cancel the current command.

This will cancel the current command eventually. It can be called multiple times. And it can be called when the command is not running. If the command is running though, then the command will be marked as canceled and eventually removed.

A command can not be canceled if it is a part of a command group, you must cancel the command group instead.

In bold we are at this time! Unless just this command is canceled with in the Command Group.

[cpapplefamily]

Originally Posted by euhlmann

Does Command.cancel() work?

NOPE! Tested and since its in a cammand Group it crashed the program and sent an error to the councle. I'm not going to type it out but in a nut shell it said

....IllegalUseOfCommandException: Can not manually cancel a command in a command group....

But storing the status in m_CancelCommand and monitoring it in isFinished did work as we want it to.

[euhlmann]

Whoops, didn't see that section of the docs

There's an easy way to do what you want in your situation though.

public class ScissorToPosition extends Command {
  // ...
  
  boolean isUnsafe = false;

  public void initialize() {
    if(m_setpoint > 70 && Robot.wristPID.getSetpoint() > 30) { 
      System.out.println("Scissor canceled since wrist > 30"); 
      
      isUnsafe = true;
    } else {
      isUnsafe = false;
      
      // init as usual
    }
  }

  // ...

  public void execute() {
    if(isUnsafe) {
      return;
      // IMPORTANT. execute() will be called once regardless of isFinished() status
    }
    
    // execute as usual
  }

  public void isFinished() {
    if(isUnsafe) {
      return true;
    }
    
    return Robot.scissorPID.onTarget();
  }
}

[cpapplefamily]

Good to know I'll have them add that part to the code as well.

public void execute() {
if(isUnsafe) {
return;
// IMPORTANT. execute() will be called once regardless of isFinished() status
}

// execute as usual
}

[euhlmann]

Originally Posted by cpapplefamily

Good to know I'll have them add that part to the code as well.

The key part is in isFinished(), which returns true if it's unsafe (so the command finishes)

I'm curious as to why a command can't be canceled in a commandgroup. Is there any particular reason for that?

[cpapplefamily]

Originally Posted by euhlmann

The key part is in isFinished(), which returns true if it's unsafe (so the command finishes)

I'm curious as to why a command can't be canceled in a commandgroup. Is there any particular reason for that?

Right! Seems like you should be able to cancel a running command anywhere. maybe when its in a Command group its not its own object anymore. I guess for now we will just have to play by the rules.