Autonomous Timer

Technical Java
1

Hi! I’m working on some autonomous code for my robot. The code is to drive forward. In the code, it should be under the function driveForward(). Anyways, what I would like to do is to be able to make it drive forward at a certain speed for a certain amount of time. I have the speed down, and it’s driving. I just need to know how to do the timed stuff. Sorry if it’s a pretty basic question. I just started programming autonomous and I have never done this before.

Here is the code that includes driveForward():

package org.usfirst.frc.team4501.robot.subsystems;

import org.usfirst.frc.team4501.robot.RobotMap;
import org.usfirst.frc.team4501.robot.commands.DriveArcade;

import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 */
public class DriveTrain extends Subsystem {

	public enum ShifterState{
		SF_HIGH, SF_LOW;
	}
	
	RobotDrive drive;
	
	Talon rightTalon;
	Talon leftTalon;
	
	DoubleSolenoid shifter;
	ShifterState state; 
	
	AnalogGyro gyro;
	Encoder L_Encoder;
	Encoder R_Encoder;
	
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public DriveTrain(){
		this.leftTalon = new Talon(RobotMap.LEFTMOTOR);
		this.rightTalon = new Talon(RobotMap.RIGHTMOTOR);
		
		this.drive = new RobotDrive(leftTalon, rightTalon);
		this.shifter = new DoubleSolenoid(RobotMap.SOLENOID_HIGHGEAR, RobotMap.SOLENOID_LOWGEAR);
		
		this.gyro = new AnalogGyro(RobotMap.GYRO);
		
		this.L_Encoder = new Encoder(RobotMap.Encoders.L_A, RobotMap.Encoders.L_B);
		this.R_Encoder = new Encoder(RobotMap.Encoders.R_A, RobotMap.Encoders.R_B);
    }
    
    public void initDefaultCommand(){
    	setDefaultCommand(new DriveArcade());
    }
    
    public void arcadeDrive(double forward, double rotate){
    	drive.arcadeDrive(forward, rotate);
    }
    
    public void initGyro(double gSensitivity){
    	gyro.initGyro();
    	gyro.reset();
    	gyro.setSensitivity(gSensitivity); //volts Per Degree Per Second
    }
    
    public void sensorReset(){
    	gyro.reset();
    	L_Encoder.reset();
    	R_Encoder.reset();
    }
    

    public void getSensors(){
    	long gyroAngle = Math.round(gyro.getAngle());
    	SmartDashboard.putNumber("Gyro Angle", gyroAngle);
    	SmartDashboard.getNumber("Gyro Rate", gyro.getRate());
    	SmartDashboard.putNumber("Right Encoder Distance", this.R_Encoder.getDistance());
    	SmartDashboard.putNumber("Left Encoder Distance", this.L_Encoder.getDistance());
    	SmartDashboard.putNumber("Right Encoder Rate", this.R_Encoder.getRate());
    	SmartDashboard.putNumber("Left Encoder Rate", this.L_Encoder.getRate());
    }
    
    public void highGear() {
    	shifter.set(Value.kReverse);
    	state = ShifterState.SF_HIGH;
    }
    
    public void lowGear() {
    	shifter.set(Value.kForward);
    	state = ShifterState.SF_LOW;
    }
    
    public void set(ShifterState state) {
    	switch (state) {
    	case SF_HIGH:
    		this.highGear();
    		break;
    		
    	case SF_LOW:
    		this.lowGear();
    		break;
    	}
    }
    public ShifterState getState() {
    	return state;
    }
    
    public void forwardMove(double x, double y){
    	this.leftTalon.set(x);
    	this.rightTalon.set(y);
    	
    }
    
       
  
    
}

Here is the code which includes the autonomous section:

package org.usfirst.frc.team4501.robot;


import org.usfirst.frc.team4501.robot.commands.DriveArcade;
import org.usfirst.frc.team4501.robot.commands.DriveIdle;
import org.usfirst.frc.team4501.robot.subsystems.DriveTrain;

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;

/**
 * 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 {

	public static OI oi;

    Command autonomousCommand;
    
    DriveTrain drive;
    
    // Subsystems
    public static final DriveTrain driveTrain = new DriveTrain();
    
    public void robotInit() {
		oi = new OI();
		
		driveTrain.initGyro(.0069);
    }
	
	public void disabledPeriodic() {
		Scheduler.getInstance().run();
	}

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

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

    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();
        
        driveTrain.sensorReset();
        
        Scheduler.getInstance().add(new DriveArcade());
    }

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

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
    	driveTrain.getSensors();
        Scheduler.getInstance().run();
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        LiveWindow.run();
    }
}

Thanks!:]

2

My bad! The function is actually called forwardMove()!

3

You’re missing a few steps to do this the way the framework intends.

Pull the following out of autonomousPeriodic:

        driveTrain.forwardMove(0.05, 0.05);

It doesn’t belong there. You don’t want to control the robot from there, just use it to keep the scheduler running, like it does by default, and put code that updates SmartDashboard there, or moves data from sensors into global areas. Stuff like that.

Next change:

Command autonomousCommand;

to something like

Command autonomousCommand = new DriveForwardForXAtYCommand(0.80, 6000);

In the above my dummy values represent 80% power for 6 seconds. The DriveForwardForXAtYCommand class would look like:


/**
 * Imports skipped for brevity
 */
public class DriveStraightForXAtY extends Command {
    protected double power;
    protected double time;
    protected long endTime;

    public DriveStraightForDistance(double power, double timeInMillis) {
        this.power = power;
        this.time = timeInMillis;
        requires(Robot.driveTrain);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        long startTime = System.currentTimeMillis();
        endTime = startTime + this.time;
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        Robot.driveTrain.setPower(power, power);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return System.currentTimeInMillis() >= endTime;
    }

    // Called once after isFinished returns true
    protected void end() {
        Robot.driveTrain.setPower(0, 0);

    }

    protected void interrupted() {
        end();
    }
}

That’s roughly the right idea to get your design goal working.