View Single Post
  #11   Spotlight this post!  
Unread 09-02-2017, 15:51
Robodawgs2221 Robodawgs2221 is online now
Registered User
FRC #2221
 
Join Date: Jan 2017
Location: Mandeville, Louisiana
Posts: 9
Robodawgs2221 is an unknown quantity at this point
Re: Timer.Delay stops motor movement?

Here it is:
Code:
package org.usfirst.frc.team2221.robot;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.RobotDrive.MotorType;


public class Robot extends IterativeRobot {
	/*--------------------------------------------------------------------*/
	/*----------------------Setup Talons---------------------*/
	Talon frontLeft = new Talon(0);
	Talon rearLeft = new Talon(1);
	Talon frontRight = new Talon(2);
	Talon rearRight = new Talon(3);
	
	Talon Spare = new Talon(4);
	/*--------------------------------------------------------------------*/
	/*---------------------CONTROLLERS-----------------------------*/
	XboxController xbox1 = new XboxController(0); // Driver Xbox (xboxController)
	XboxController xbox2 = new XboxController(1); // Manipulator Xbox (xboxController)
	/*--------------------------------------------------------------------*/
	/*----------------------Controller Settings----------------------------------*/
	double xbox1_value_x; // Xbox1 x-values
	double xbox1_value_y; // Xbox1 y-values 
	
	double xbox2_value_triggerRight = xbox2.getTriggerAxis(Hand.kRight);
	double xbox2_value_triggerLeft = xbox2.getTriggerAxis(Hand.kLeft);	
	
	/*--------------------------------------------------------------------*/
	/*---------------------Setup Arcade Drive-----------------------------*/
	RobotDrive arcadeDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
	/*--------------------------------------------------------------------*/
	/*----------------------Autonomous Chooser---------------------------*/
	SendableChooser<String> chooser = new SendableChooser<>();
	final String leftAuto = "Drive Left";
    final String straightAuto = "Drive Straight";
    final String rightAuto = "Drive Right";
    final String spyAuto = "Spybot";
    
	/*--------------------------------------------------------------------*/
	/*-----------------------Setup Compressor----------------------*/
	Compressor comp = new Compressor(0); // the Zero is the PCM NODE ID in the Web Environment
	/*--------------------------------------------------------------------*/
	/*---------------------Setup Solenoids (And Triggers)---------------*/
	DoubleSolenoid solenoid1 = new DoubleSolenoid(0, 1);
	DoubleSolenoid solenoid2 = new DoubleSolenoid(2, 3);
	DoubleSolenoid solenoid3 = new DoubleSolenoid(4, 5);
	boolean solenoid1_trigger;
	boolean solenoid2_trigger;
	boolean solenoid3_trigger;
	
	
	
	final String auto1 = "autoOne";
	String autoSelected;
	private double autoStartTime;
	public Robot() {
		arcadeDrive.setExpiration(0.1);
	}

	@Override
	public void robotInit() {
		comp.setClosedLoopControl(true);
		
		arcadeDrive.setInvertedMotor(MotorType.kFrontLeft, false);
		arcadeDrive.setInvertedMotor(MotorType.kFrontRight, false);
		arcadeDrive.setInvertedMotor(MotorType.kRearLeft, false);
		arcadeDrive.setInvertedMotor(MotorType.kRearRight, true);
		
	
		  
	}

	
	
	@Override
	public void autonomousInit(){
		autoStartTime = Timer.getFPGATimestamp();
		autoSelected = (String) chooser.getSelected();
    	System.out.println("Auto selected: " + autoSelected);
    	Timer timer = new Timer();
    	timer.start();
	}
	
	@Override
	public void autonomousPeriodic() {
		
		double currTime = Timer.getFPGATimestamp();
		double timeElapsed = currTime - autoStartTime;
		System.out.println("Auto selected: " + autoSelected);

		switch (autoSelected) {
		
		case leftAuto:
			
            // Disable watchdog when autonomous driving
            arcadeDrive.setSafetyEnabled(false);
            	
            System.out.print("You're left");
            if ( timeElapsed < 2)
            {
            arcadeDrive.drive(1, 0);
            }
            
            else if (timeElapsed < 4)
            {
            arcadeDrive.drive(0.75, .6);
            }
            
            else if (timeElapsed < 6)
            {
            arcadeDrive.drive(0, 0);
            solenoid2.set(DoubleSolenoid.Value.kForward);
            }
            
            else if (timeElapsed < 8.5)
            {
            solenoid2.set(DoubleSolenoid.Value.kReverse);
            arcadeDrive.drive(-1, .8);
		    } 
            
            else if (timeElapsed < 9.5)
            {
            arcadeDrive.drive(-.25, 0);
            }
         
            
            else if (timeElapsed > 11)
            {
            arcadeDrive.drive(0, 0);
            }
         
            
            arcadeDrive.setSafetyEnabled(true);
            break;
            
		case "straightAuto":
			
			 arcadeDrive.setSafetyEnabled(false);
			 System.out.print("you're straight");
			
			if(timeElapsed < 2)
			{
			arcadeDrive.drive(.5, 1);
			}
			
			else if(timeElapsed < 3)
			{
			arcadeDrive.drive(0, 0);
			}
			
			else if (timeElapsed < 3.5)
			{
			solenoid2.set(DoubleSolenoid.Value.kForward);
			}
			
			else if (timeElapsed < 4.5)
			{
			solenoid2.set(DoubleSolenoid.Value.kReverse);
			}
			
			else if (timeElapsed < 5)
			{
			arcadeDrive.drive(-.75, 0);
			}
			
			else if (timeElapsed > 8)
			{
			arcadeDrive.drive(0, 0);
			}
			
			else if (timeElapsed > 10)
			{
			arcadeDrive.setSafetyEnabled(true);
			}
			
			break;
			
		case "rightAuto":
			System.out.print("you're right");
			
			arcadeDrive.setSafetyEnabled(false);
			
			if(2.5 < timeElapsed && timeElapsed < 5)
			{
			arcadeDrive.drive(1, 0);
			}
			else if (timeElapsed < 7)
			{
			arcadeDrive.drive(0.75, -.6);
			solenoid2.set(DoubleSolenoid.Value.kForward);
			}
			else if (timeElapsed < 9){
			solenoid2.set(DoubleSolenoid.Value.kReverse);
			arcadeDrive.drive(-1, -.8);
			}
			else if (timeElapsed < 11 )
			{
			arcadeDrive.drive(-.25, 0);
			}
			else if (timeElapsed > 12)
			{
			arcadeDrive.drive(0, 0);
			arcadeDrive.setSafetyEnabled(true);
			}
			
			break;
			
			default:
				
	            arcadeDrive.setSafetyEnabled(false);
	            	
	            System.out.print("YOU'RE LEFT");
	            if ( timeElapsed < 2)
	            {
	            arcadeDrive.drive(1, 0);
	            }
	            
	            else if (timeElapsed < 4)
	            {
	            arcadeDrive.drive(0.75, .6);
	            }
	            
	            else if (timeElapsed < 6)
	            {
	            arcadeDrive.drive(0, 0);
	            solenoid2.set(DoubleSolenoid.Value.kForward);
	            }
	            
	            else if (timeElapsed < 8.5)
	            {
	            solenoid2.set(DoubleSolenoid.Value.kReverse);
	            arcadeDrive.drive(-1, .8);
			    } 
	            
	            else if (timeElapsed < 9.5)
	            {
	            arcadeDrive.drive(-.25, 0);
	            }
	         
	            
	            else if (timeElapsed > 11)
	            {
	            arcadeDrive.drive(0, 0);
	            }
	         // Enable watchdog on driving again
	            
	            arcadeDrive.setSafetyEnabled(true);
	            break;
				
		}
	}
	@Override
public void teleopInit(){
	}

	@Override
	public void teleopPeriodic() {
		arcadeDrive.setSafetyEnabled(true);
//-----------------------Driver Controller---------------------------------//
			if(xbox1.getAButton()){
				xbox1_value_x = xbox1.getX(Hand.kLeft) * 0.75;
				xbox1_value_y = xbox1.getY(Hand.kLeft) * 0.75;
			}
			else{
				xbox1_value_x = xbox1.getX(Hand.kLeft);
				xbox1_value_y = xbox1.getY(Hand.kLeft);
			}
			arcadeDrive.arcadeDrive(xbox1_value_x, xbox1_value_y);
			
//-----------------------Manipulator Controller---------------------------------//
			if(xbox2.getBButton()){
				if(solenoid1_trigger != true){
				solenoid1.set(DoubleSolenoid.Value.kForward);
				solenoid1_trigger = true;
				xbox2.setRumble(RumbleType.kRightRumble, 1);
				Timer.delay(.5);
				xbox2.setRumble(RumbleType.kRightRumble, 0);
				}
				else
				{
					solenoid1.set(DoubleSolenoid.Value.kReverse);
					solenoid1_trigger = false;
					Timer.delay(.5);
				}}
				
				else if(xbox2.getXButton()){
				if(solenoid2_trigger != true){
					solenoid2.set(DoubleSolenoid.Value.kForward);
					solenoid2_trigger = true;
					xbox2.setRumble(RumbleType.kRightRumble, 1);
					Timer.delay(.5);
					xbox2.setRumble(RumbleType.kRightRumble, 0);
					}
					else
					{
						solenoid2.set(DoubleSolenoid.Value.kReverse);
						solenoid2_trigger = false;
						Timer.delay(.5);
					}			
				}
				
			else if(xbox2.getYButton()){
				if(solenoid3_trigger != true){
					solenoid3.set(DoubleSolenoid.Value.kForward);
					solenoid3_trigger = true;
					xbox2.setRumble(RumbleType.kRightRumble, 1);
					Timer.delay(.5);
					xbox2.setRumble(RumbleType.kRightRumble, 0);
					}
					else
					{
						solenoid3.set(DoubleSolenoid.Value.kReverse);
						solenoid3_trigger = false;
						Timer.delay(.5);
					}			
				}
				
			if(xbox2.getBumper(Hand.kLeft) == true){
				Spare.set(xbox2_value_triggerLeft);
			}
			else if (xbox2.getBumper(Hand.kRight) == true){
				Spare.set(xbox2_value_triggerRight);
			}
		}
	}
Reply With Quote