Autonomous command not continuous

Technical Java
1

We are trying to write a gear autonomous command to drive the robot forward and have the robot slow down as it gets closer to the gear peg using motion profiling. However, when we run our auto command, the robot drives forward in short bursts instead of continuously driving forward. Even when we just set the drivetrain to drive a set value instead of having motion profiling determine our voltage, the robot still doesn’t drive continuously.

Our code:

public class AutoDriveCommand extends Command {
	private static RobotLogger logger = new RobotLogger(AutoDriveCommand.class);
	
	private double distanceCounts;
	private boolean finished = false;

    public AutoDriveCommand() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
    	//divide by twelve if needed in inches
    	//distanceCounts = distanceFeet * 2607.5;
    	requires(Robot.usbCameraSubsystem);
    	requires(Robot.drivetrainSubsystem);
    }
    
    double initd = 0.0;

    // Called just before this Command runs the first time
    protected void initialize() {
    	Robot.drivetrainSubsystem.Drive(0.0,  0.0);
//    	Robot.drivetrainSubsystem.autoDrive();
    	GearPipeline gtbr = new GearPipeline();
        //Scheduler.getInstance().run();
     	Mat mat = Robot.usbCameraSubsystem.getNthFrame(Robot.robotConfig.shooterConfig.nthFrame);
 		gtbr.process(mat);
 		ArrayList<Rect> gearRects = new ArrayList<Rect>();
 		for(MatOfPoint mop : gtbr.filterContoursOutput()) {
 			Rect rect = Imgproc.boundingRect(mop);
 			gearRects.add(rect);
 		}
 		logger.log(RobotLogger.LoggerLevel.debug, "NUMBER OF RECTS " + gearRects.size());
     	Rect rect = VisionUtility.getRectContainer(gearRects, Robot.robotConfig.usbCameraSettings.width, Robot.robotConfig.usbCameraSettings.height);
     	logger.log(RobotLogger.LoggerLevel.debug, rect.toString());
//     	double distance = VisionUtility.getGearDistance(rect.width);
     	Robot.drivetrainSubsystem.setDistance(VisionUtility.getGearDistance(VisionUtility.getRectWidthAvg(gearRects)));
     	logger.log(RobotLogger.LoggerLevel.debug, "Distance: " + Robot.drivetrainSubsystem.getDistance());
    	initd = Robot.drivetrainSubsystem.getDistance();
    }
    
    private static GearPipeline gtbr = new GearPipeline();

    // Called repeatedly when this Command is scheduled to run 
    protected void execute() {
		logger.log(RobotLogger.LoggerLevel.debug, "I AM HERE!!!!!!!!!!!!!!!!!!!!!!");
    	logger.log(RobotLogger.LoggerLevel.debug, "leftBackDriveMotor: "  + RobotMap.leftBackDriveMotor.getBusVoltage());
    	logger.log(RobotLogger.LoggerLevel.debug, "leftFrontDriveMotor: "  + RobotMap.leftFrontDriveMotor.getBusVoltage());
    	GearPipeline gtbr = new GearPipeline();
    	Mat mat = Robot.usbCameraSubsystem.getNthFrame(Robot.robotConfig.shooterConfig.nthFrame);
		gtbr.process(mat);
		ArrayList<Rect> gearRects = new ArrayList<Rect>();
		for(MatOfPoint mop : gtbr.filterContoursOutput()) {
			Rect rect = Imgproc.boundingRect(mop);
			gearRects.add(rect);
		}
		logger.log(RobotLogger.LoggerLevel.debug, "NUMBER OF RECTS " + gearRects.size());
    	Rect rect = VisionUtility.getRectContainer(gearRects, Robot.robotConfig.usbCameraSettings.width, Robot.robotConfig.usbCameraSettings.height);
    	logger.log(RobotLogger.LoggerLevel.debug, rect.toString());
//    	double distance = VisionUtility.getGearDistance(rect.width);
    	Robot.drivetrainSubsystem.setDistance(VisionUtility.getGearDistance(VisionUtility.getRectWidthAvg(gearRects)));
    	logger.log(RobotLogger.LoggerLevel.debug, "Distance: " + Robot.drivetrainSubsystem.getDistance());
    	double distance = Robot.drivetrainSubsystem.getDistance();
    	
    	double voltage = AutonomousUtil.VoltageProfile(distance, initd, 12.0, ProfileType.Linear, 1.0);

    	if(voltage < 0.5) {
    		voltage = 0.5;
    	}
		RobotMap.leftBackDriveMotor.set(10.0);
		RobotMap.leftFrontDriveMotor.set(10.0);
    }

    // Make this return true when this Command no longer needs to run execute()
    
    protected boolean isFinished() {
        return finished;
    }

    // Called once after isFinished returns true
    protected void end() {
    	Robot.drivetrainSubsystem.returnToTeleop();
    	Robot.drivetrainSubsystem.Drive(0, 0);
    	
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    	Robot.drivetrainSubsystem.returnToTeleop();
    	Robot.drivetrainSubsystem.Drive(0, 0);
    }
}

Thanks in advance.

2

My first blush suspicion is that the robot pauses when it reaches the watchdog timeout as it is processing each image. The image retrieval and processing should be in a separate thread, and the execute() method should check for a completed image process, and continually send the proper value based on the most recent camera info.

3

But when the motor isn’t receiving a new setpoint, shouldn’t it just stay at the same speed? It’s probably not good to have that image processing code there, but I don’t think it’s what’s causing this problem.

4

I agree with GeeTwo and I suspect it is the cause of your problem. The execute is taking a long time due to the image processing and as a result you are not updating your motors often enough and the system is turning them off thinking you code has become unresponsive. It is a safety thing.

Thanks,
Steve

5

Thanks a lot for the responses. I am sure this is the answer. We do receive an error on the console about “no comm with the motor/drivetrain in 125ms”.

6

You need to supply a value every 100 ms or so, or the RIO stops sending signals to the motor controller. The controller then shuts down the motor fairly quickly. This is a FIRST safety feature so that robots do not continue doing the last command they were given (possibly doing injury or damage) if the control software gets stuck in an endless loop. Implementing this is one of the reasons that specific firmware versions are required by FIRST.

7

The “safety feature” was indeed our issue. We put the Navx and Vision on their own threads and now our autonomous works as expected. Thanks a lot to all who replied!