Autonomous Timer

[simon1636]

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!