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?