Run a command with conditions

[cpapplefamily]

I have 2 sendableChoosers for our Autonomous. One picks the obstacle we are going to attempt. (works well) The other picks the wrist angle of our shooter for High goal or low goal. We came up with the idea after milling over the code shared in the thread More than 1 Sendablechooser (http://www.chiefdelphi.com/forums/showthread.php?t=145930) where we can run a line of code in a command like


/* The goalChooser construction
*goalChooser.addDefault("0: No Shoot",0);
*goalChooser.addOption("1: Low goal",1);
*goalChooser.addOption("2: High Goal",2);
*/


protected void initialize() {
Integer goalChoice = (Integer) Robot.goalChooser.getSelected();
angle = findAngle(goalChoice);
Robot.wristSubsystem.setAngle(angle);
}

private double findAngle(integer Choice){
if(Choice == 1){
return 15;
}else if(Choice == 2{
return 75;
}else{
return 90;
}
}


The syntax may be off here but the real code worked every time. If goalChooser was 1 wrist set to 15deg, 2 then 75deg.

Now comes the trouble I'm trying to resolve. Our AutonomousChooser selects a command group that looks like


addSequential(new drive());
addSequential(new setWristAngle()); //Above code

//************* Code Focus ************

addSequential(new Auto_ShootDecide()); //New shoot or Not
//addSequential(new shootBall()); //Old always shoots ball

//***********************************

In Auto_ShootDecide I have

protected void initialize() {
Integer goalChoice = (Integer) Robot.goalChooser.getSelected();
if (goalChoice == 0){//don't shoot
end();
}else{
new shootBall(); This is where the code does not run
}
}

new shootBall(); This is where the code does not run.

We use shootBall() tied to a button and the code is proven and I don't think I what to add conditions to it to check the goalChoice like "Auto_ShootDecide()".

Maybe Auto_ShootDecide() needs to be a mirror of shootBall but with the conditions. I just thought why recreate a chunk of code. What do you think?

[kylelanman]

new ShootBall()

Simply creates an instance of the command. That instance needs to be scheduled by running ->Start()

Command* c = new ShootBall()
c->Start()


Another option is to build the command group on the fly. Here is an example of how we do this.


m_defenseChooser = new SendableChooser();
//Group A
m_defenseChooser->AddObject("Portcullis", new TraversePortcullisCommandGroup());
m_defenseChooser->AddObject("Cheval de Friese", new TraverseChevalFrieseCommandGroup());

//Group B
m_defenseChooser->AddObject("Moat", new TraverseMoatCommandGroup());
m_defenseChooser->AddObject("Ramparts", new TraverseRampartsCommandGroup());

//Group C
m_defenseChooser->AddObject("Drawbridge", new TraverseDrawbridgeCommandGroup());
m_defenseChooser->AddObject("Sally Port", new TraversePortCommandGroup());

//Group D
m_defenseChooser->AddObject("Rock Wall", new TraverseWallCommandGroup());
m_defenseChooser->AddObject("Rough Terrain", new TraverseTerrainCommandGroup());

m_defenseChooser->AddDefault("Nothing", (void*)0);

SmartDashboard::PutData("Defense Chooser",m_defenseChooser);

m_posChooser = new SendableChooser();
m_posChooser->AddDefault("Nothing", NULL);
// m_posChooser->AddObject("Pos 1 (Low Bar)", (void*)1);
m_posChooser->AddObject("Pos 2", (void*)2);
m_posChooser->AddObject("Pos 3", (void*)3);
m_posChooser->AddObject("Pos 4", (void*)4);
m_posChooser->AddObject("Pos 5", (void*)5);

SmartDashboard::PutData("Position Chooser", m_posChooser);


void AutonomousInit()
{
autonomousCommand.reset(new GeneratedAutoCommandGroup((Command*)m_defenseChoos er->GetSelected(),
(int)m_posChooser->GetSelected(),
(int)m_backChooser->GetSelected()));
if (autonomousCommand != NULL)
autonomousCommand->Start();
}



class GeneratedAutoCommandGroup: public CommandGroup
{
public:
GeneratedAutoCommandGroup(Command* traverseCommand, int position, int driveBack)
: CommandGroup("GeneratedAutoCommandGroup"){
double angleArray[6] = {0, 0, 30, 0, 0, -25};
if (traverseCommand){
AddSequential(new AutoBlockOneCommandGroup());
AddSequential(traverseCommand);
AddSequential(new RotateToAngleCommand(angleArray[position])); //disabling for middle pos
if (position == 2) {
AddSequential(new LockOnTargetCommand(CameraProcessor::LEFT_TARGET)) ;
} else {
AddSequential(new LockOnTargetCommand(CameraProcessor::RIGHT_TARGET) );
}
AddSequential(new AutoBlockTwoCommandGroup());
AddSequential(new RotateToAngleCommand(0));
AddSequential(new DriveDistanceCommand(.5, .5, 2000));
AddSequential(new Rotate180Command());
if (driveBack==1) {
AddSequential(new DriveDistanceCommand(.5, .5, 2000));
AddSequential(traverseCommand);
}
}
}
};

[rich2202]

If you don't want to modify shoot, then copy it to create auto_shoot, and modify that to remove checking the button. What is your reluctance? It may not be elegant, but you have more than enough code space for it.

[cpapplefamily]

If you don't want to modify shoot, then copy it to create auto_shoot, and modify that to remove checking the button. What is your reluctance? It may not be elegant, but you have more than enough code space for it.

"not be elegant" is the point. This is the way we might go but I will try some build on the fly since it seems to be possible. I will have to try and copy the code style in JAVA.

Thanks for the direction.

[cpapplefamily]

new ShootBall()

Simply creates an instance of the command. That instance needs to be scheduled by running ->Start()

Command* c = new ShootBall()
c->Start()


Another option is to build the command group on the fly. Here is an example of how we do this.


m_defenseChooser = new SendableChooser();
//Group A
m_defenseChooser->AddObject("Portcullis", new TraversePortcullisCommandGroup());
m_defenseChooser->AddObject("Cheval de Friese", new TraverseChevalFrieseCommandGroup());

//Group B
m_defenseChooser->AddObject("Moat", new TraverseMoatCommandGroup());
m_defenseChooser->AddObject("Ramparts", new TraverseRampartsCommandGroup());

//Group C
m_defenseChooser->AddObject("Drawbridge", new TraverseDrawbridgeCommandGroup());
m_defenseChooser->AddObject("Sally Port", new TraversePortCommandGroup());

//Group D
m_defenseChooser->AddObject("Rock Wall", new TraverseWallCommandGroup());
m_defenseChooser->AddObject("Rough Terrain", new TraverseTerrainCommandGroup());

m_defenseChooser->AddDefault("Nothing", (void*)0);

SmartDashboard::PutData("Defense Chooser",m_defenseChooser);

m_posChooser = new SendableChooser();
m_posChooser->AddDefault("Nothing", NULL);
// m_posChooser->AddObject("Pos 1 (Low Bar)", (void*)1);
m_posChooser->AddObject("Pos 2", (void*)2);
m_posChooser->AddObject("Pos 3", (void*)3);
m_posChooser->AddObject("Pos 4", (void*)4);
m_posChooser->AddObject("Pos 5", (void*)5);

SmartDashboard::PutData("Position Chooser", m_posChooser);


void AutonomousInit()
{
autonomousCommand.reset(new GeneratedAutoCommandGroup((Command*)m_defenseChoos er->GetSelected(),
(int)m_posChooser->GetSelected(),
(int)m_backChooser->GetSelected()));
if (autonomousCommand != NULL)
autonomousCommand->Start();
}



class GeneratedAutoCommandGroup: public CommandGroup
{
public:
GeneratedAutoCommandGroup(Command* traverseCommand, int position, int driveBack)
: CommandGroup("GeneratedAutoCommandGroup"){
double angleArray[6] = {0, 0, 30, 0, 0, -25};
if (traverseCommand){
AddSequential(new AutoBlockOneCommandGroup());
AddSequential(traverseCommand);
AddSequential(new RotateToAngleCommand(angleArray[position])); //disabling for middle pos
if (position == 2) {
AddSequential(new LockOnTargetCommand(CameraProcessor::LEFT_TARGET)) ;
} else {
AddSequential(new LockOnTargetCommand(CameraProcessor::RIGHT_TARGET) );
}
AddSequential(new AutoBlockTwoCommandGroup());
AddSequential(new RotateToAngleCommand(0));
AddSequential(new DriveDistanceCommand(.5, .5, 2000));
AddSequential(new Rotate180Command());
if (driveBack==1) {
AddSequential(new DriveDistanceCommand(.5, .5, 2000));
AddSequential(traverseCommand);
}
}
}
};

Ok so your rebuilding the command in the void AutonomousInit()
That makes since because with more testing now I found the command always use the status of the Sendable chooser when the Roborio booted up. When rebuilding the Command before autonomousCommand->Start()
you capture the current state of the sendable Choosers.

[cpapplefamily]

In an effort to continue down this rabbit hole here is where I sit.

I have continued with one of the original ideas before I attempt to try the build the autonomousCommand just before running it. To better discusse the code here is a link to the Github repo
https://github.com/GraniteCiyGearheads3244/FIRSTStronghold2016/tree/master/Copy%20of%20SirAntsABot_Talahi
If we drill to FIRSTStronghold2016/Copy of SirAntsABot_Talahi/src/org/usfirst/frc3244/SirAntsABot/commands/Auto_22_Demo_03_Drive_N_Score We can see we are driving the Bot with a few commands before we:addSequential(new Auto_Score_Ball_In_Goal()); This Code is now executing as I desired by only shooting if the goalChooser is not zero. Thanks to kylelanman for pointing it out we need to start the commands.

Now the trouble I'm having is getting the next sequence to continue when Auto_Score_Ball_In_Goal()is done. I have inserted some prints to show me who's end() and interrupted() methods are being called. The time out was just a test in Auto_Score_Ball_In_Goal(). I have also tried to use !c.isRunning. If you look at what c is its a command group called "PinBall_Cycle". This group sets the claw and shoots the ball. I also put system.out.println in these end methods to see if they are completing. It seems to me that everything works as planed and completing, only I'm not returning to the Auto_22_Demo_03_Drive_N_Score to finish the last two commands in the group.

Maybe someone can point me to or explain to me the life cycle of a command and what is needed to move onto the next sequence in a command group.

[Joe Ross]

Calling end() doesn't cause a command to end. Making isFinished() return true does.

I think the problem is that the commands you are starting are canceling Auto_22_Demo_03_Drive_N_Score because they require the same subsystems.