Autonomous won't do *anything* ?

FIRST General Forum
1

I’m trying to do the most absolutely basic wpilib autonomous, and I can’t get it to trigger. It isn’t doing anything, and I’m following the wpilib pretty closely…


// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc5858.SteamworksBot; 

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.usfirst.frc5858.SteamworksBot.commands.*;
import org.usfirst.frc5858.SteamworksBot.subsystems.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {

    Command autonomousCommand;

    public static OI oi;
    

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
    	RobotMap.init();
        oi = new OI();

        //autonomousCommand = new AutonomousCommand();
        autonomousCommand = new DebugAutoCommandOne();
    }

    /**
     * This function is called when the disabled button is hit.
     * You can use it to reset subsystems before shutting down.
     */
    public void disabledInit(){

    }

    public void disabledPeriodic() {
        Scheduler.getInstance().run();
    }

    public void autonomousInit() {
        // schedule the autonomous command (example)
        if (autonomousCommand != null) autonomousCommand.start();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }

    public void teleopInit() {
        // This makes sure that the autonomous stops running when
        // teleop starts running. If you want the autonomous to
        // continue until interrupted by another command, remove
        // this line or comment it out.
        if (autonomousCommand != null) autonomousCommand.cancel();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
    }

    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        LiveWindow.run();
    }
}



package org.usfirst.frc5858.SteamworksBot.commands;

import java.util.Timer;

import org.usfirst.frc5858.SteamworksBot.Robot;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 */
public class DebugAutoCommandOne extends Command {

	// we're using 8 so we can use 9 later during a sequential operation!
	public String debug_output = "DB/String 9";
	double start_time;
	double elapsed_time;
	
    public DebugAutoCommandOne() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	requires(Robot.drivetrain);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	SmartDashboard.putString(debug_output, "Debug one INIT.");
    	start_time = System.currentTimeMillis();
    	
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	elapsed_time = System.currentTimeMillis() - start_time;
    	SmartDashboard.putString(debug_output, "Debug 1 : " + elapsed_time + " ms");
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
    	SmartDashboard.putString(debug_output, "Debug one DONE.");
        return elapsed_time - start_time >= 2.0;
    }

    // Called once after isFinished returns true
    protected void end() {
    	SmartDashboard.putString(debug_output, "Debug one END.");
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	SmartDashboard.putString(debug_output, "Debug one INTERRUPTED.");
    }
}

I never see any indication it’s running, either in the debug boxes OR by debugging and setting breakpoints. It never hits any of the breakpoints at all… am I missing something that’s not covered in the wpilib documentation?

2

Okay, ignoring the dumb typo in my math above (which didn’t explain why I was never hitting to break point to evaluate it), the issue seems to be with having setDefaultCommand in a few subsystems - evidently having setDefaultCommand prevents queued commands from running at all…?

EDIT: Note, I cleaned my code up a little above and got rid of subsystems I didn’t think mattered… but I have a drivetrain subsystem and a computer vision networking subsystem, and if I setDefaultCommand in either one of them, my Autonomous command is never called… this is very strange…

3

The first thing that immediately pops up to me is that you’ve got some funky stuff going on with your time calculations. Firstly, you’re subtracting start_time from elapsed_time twice, once in execute() and once in isFinished(). You should drop it out of isFinished(), changing your return statement in isFinished() to just

return elapsed_time >= 2.0;

The other issue is there, with your comparison. System.currentTimeMillis() gives values in just that; milliseconds. Your comparison checks to see if 2ms have passed, not 2sec as I’m guessing you were probably going for. You could change your code to be return elapsed_time >= 2000; if you’re going for 2 seconds, or you could drop the use of System.currentTimeMillis() altogether and just use a WPILib Timer:

Create the Timer as you would any other object,

Timer timer = new Timer();

In initialize(), you should reset your timer to 0 seconds, and then start it

timer.reset();
timer.start();

in isFinished, change your code to

return timer.get() > 2;

**Note that the Timer object works in Seconds, not Milliseconds!

And lastly, to keep things tidy, you can throw a timer.stop(); into your end() and interrupted() methods if you want.

I’m not entirely sure why your current code isn’t doing anything, but these are a couple of things that should be cleaned up to get the debugging started.

4

Is your default command Interruptible? If the default command on the subsystem is not Interruptible, then any new commands including your Autonomous command will not execute until that default command ends.

Since what you have is a drivetrain, I’m guessing your default command does some kind of Teleop read of the joysticks to drive the bot around – and probably will never terminate on its own. You may need to make sure this default command can be interrupted by using the setInterruptible(boolean interruptible) method.

Without seeing the code of your default command, however, it’s hard to tell whats going on in your case.

5

Argh, yeah, I bet that’s it. I had no idea you had to setInterruptable.

And yeah, the timing stuff I had just thrown together ultra quick, I found the dumb bugs in that pretty fast.