Jade 2018 Code

jade2018.java

package org.usfirst.frc.team4151.robot;

/*Team coding work to post later, look into code with basic Java FRC Drive Code
Will be added to GitHub, but all edits and comments should be made on here.*/

//Comment like this or like I did above
import java.util.concurrent.TimeUnit;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.drive.*;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Encoder;


public class Robot extends IterativeRobot 
{
	
	
	Spark Lift1 = new Spark(2);
	Spark Lift2 = new Spark(3);
	Spark Climb1 = new Spark(5);
	Spark Climb2 = new Spark(4);
	Spark dR = new Spark(1);
	Spark dL = new Spark(0);
	
	public boolean pneu = false;
	
	DifferentialDrive driveT = new DifferentialDrive(dL, dR);
	SpeedControllerGroup lift = new SpeedControllerGroup(Lift1, Lift2);
	SpeedControllerGroup climb = new SpeedControllerGroup(Climb1, Climb2);
	
	Joystick tempR, tempL, dTBongo, uBongo, dPad;
	PowerDistributionPanel power;
	boolean z = false;
	
	//AnalogInput encoder = new AnalogInput(1);
	Compressor c;
	DoubleSolenoid fullExt;
	
	Encoder encode = new Encoder(8, 9);
	Encoder encodeUnder = new Encoder(6, 7);
	
	Timer time; 
	double num = -5.0;
	String test;
	
	double speedR;
	double speedL;
	
	public double driveSpeed = .6;
	public String liftDVoltage;
	
	public AnalogInput jEncL = new AnalogInput(0);
	public AnalogInput jEncR = new AnalogInput(1);
	
    public void robotInit() 
    {
    	driveT.setDeadband(0);
    	
    	tempL = new Joystick(0);	 		//The value in Joystick() is the order in which the Joysticks are plugged in. It can be checked on the USB section of the Drive Station
    	tempR = new Joystick(1);
    	dTBongo = new Joystick(2);
    	uBongo = new Joystick(3);
    	dPad = new Joystick(4);
    	
    	power = new PowerDistributionPanel();
    	power.clearStickyFaults();
    	
    	time = new Timer();
    	
    	c = new Compressor(0); //default for PCM
    	c.setClosedLoopControl(true);
    	
    	fullExt = new DoubleSolenoid(0, 1);
    	
    	//SmartDashboard.putNumber("current time", time.getFPGATimestamp());
    	
    	//encode.setDistancePerPulse(1);
    	encode.reset();
    	encode.setReverseDirection(true);
    	
    	//encodeUnder.setDistancePerPulse(1);
    	encode.reset();
		
		speedR=.8;
		speedL=.8;
    	
    	CameraServer.getInstance().startAutomaticCapture();
    	 
    }
    
    
    
    public void autonomousInit()
    {
    	time.reset();
    	time.start();
    	
    }

    //all our autonomous stuffs
    public void autonomousPeriodic() 
    {
    	
    	
	    {
	    	driveSpeed = .6;
	    	while(time.get() < 5.0)
	    	{
	    		driveT.tankDrive(-driveSpeed, -driveSpeed);
	    	}
	    	
	    }

    
    DriverStation ds = DriverStation.getInstance();
    String gameData = ds.getGameSpecificMessage(); //LEFT DRIVER STATION | Position To The Far Left RED ALI
	
    	
       
          		
		/*if(gameData.charAt(0) == 'L') //If our side of the switch is on the left
	  	{
	     driveT.tankDrive(0.8,.8);
        while(time.get() < 1)
	    {
			climb.set(0.8); //Lifts arm
		}
        while(time.get() < 1.2)
		{
			climb.set(0.0); //Stops lifting arm
        }
		while(time.get() < 2.2)
		{
			driveT.tankDrive(-0.7,-0.7); //Drives Robot forward
		}
		while(time.get() < 3.2)
		{
			driveT.tankDrive(0.6,-0.6); //Turns robot left
		}
		while(time.get() < 4.2)
		{
         driveT.tankDrive(-0.5,-0.5); //Drive forward
		}
		while(time.get() < 4.3)
        {
	     fullExt.set(DoubleSolenoid.Value.kReverse); //Releases cube
        }
        }
		/*If our side of the switch is on the right RED ALI
		if(gameData.charAt(0)== 'R' ) 
        {
        while(time.get() < )
        {
        	driveT.tankDrive(-.8,-.8); //Drives past switch
  	    }
        while(time.get() < )
        {
        	driveT.tankDrive(-0.6,0.6); //Turns to the right
        }
        while(time.get() < )
        {
        	driveT.tankDrive(-.8,-.8); // Drives to the right half of the field
  	    }
        while(time.get() < )
		{
        	driveT.tankDrive(-0.6,0.6); // Turns arm to be facing alliance station
	  	}
		while(time.get() < )
        {
			driveT.tankDrive(-0.6,-0.6); // Drives towards the alliance Station	
        }
        while(time.get() < )
		{
			climb.set(0.8); //Lifts arm
		}
		while(time.get() < ) 
		{
			climb.set(0.0); // Stops lifting arm
		}
        while(time.get() < )
		{
 	    driveT.set(-0.6,-0.6); // Drives forward to the switch
	  	}
        while(time.get() < )
        {  
         pneumat1.set(DoubleSolenoid.Value.kReverse); //Releases cube
        }
        }	


//RIGHT DRIVE STATION
	else if(gameData.charAt(0) == 'R') //If our side of the switch is on the right
	{
		speedR=.6;
		speedL=.6;
	while(time.get() < )
     {
		driveT.tankDrive(-.8 ,-.8 ); //Drive Forward
     }	
    while(time.get() < )	
	{  
    	driveT.tankDrive(0.6,-0.6); //Turns Left
	}
    while(time.get() < )	
	{
    	driveT.tankDrive(-0.6,-0.6);//Drive to the switch
	}
	while(time.get() < )
	{
     pneumat1.set(DoubleSolenoid.Value.kReverse); //Releases cube
    }
	}
	
	//If our side of the switch is on the left
	else if(gameData.charAt(0)=='L')
     {	
     while(time.get() < )
     {	
    	 driveT.tankDrive(-0.8,-0.8); //Drive Forward
     }
     while(time.get() < )
     {
    	 driveT.tankDrive(.6 , -.6); //Turn left
     } 
      while(time.get() < )
     {
    	  driveT.tankDrive(-.8 ,-.8 ); //Drive Forward
     }
      while(time.get() < )
     {
    	  driveT.tankDrive(.6 , -.6); //Turn facing drive station
     }
     while(time.get() < )
      {
    	 driveT.tankDrive(-.6 ,-.6 ); //Drive Forward
      }
     while(time.get() < )
      {
    	 driveT.tankDrive(.6 , -.6); //Turn facing Switch}
      }
     while(time.get() < )
	 {
		climb.set(0.8); //Lifts arm
	 }
	while(time.get() < )
	{
		climb.set(0.0); //Stops lifting arm
	}
    while(time.get() < )
    {
    	driveT.tankDrive(-.6 ,-.6 ); //Drive Forward
    }
    while(time.get() < )
    {
    pneumat1.set(DoubleSolenoid.Value.kReverse); //Releases Cube
    }
    }


//George Washington needs to stop the process of eating my CPU*/
	
//CENTER DRIVE STATION
         
	  /*if(gameData.charAt(0) == 'L') //If our side of the switch is on the left from center
     {
		speedL = .6;
		speedR= .6;
	 while(time.get() < )
	 {
		climb.set(0.8); //Lifts arm
     }
	while(time.get() < )
     {
		climb.set(0.0); //Stops lifting arm
     }
	while(time.get() < )
	 {
		driveT.tankDrive(-0.7,-0.7); //Drives Robot forward
	 }
	while(time.get() < )
	 {
		driveT.tankDrive(0.6,-0.6); //Turns robot to the switch
	 }
    while(time.get() < )			
     {                            
    	driveT.tankDrive(-0.7,-0.7); //Drives Robot forward
	 }
    while(time.get() < )		
     {
    	driveT.tankDrive(-0.6,0.6); //Turn robot to the switch
	 }
    while(time.get() < )   
     {                
    	driveT.tankDrive(-0.7,-0.7); //Drive robot forward
     }
    while(time.get() < )		
     { 
	  pneumat1.set(DoubleSolenoid.Value.kReverse); //Releases cube
     }
     }
		
//If our side of the switch is on the right
	else if(gameData.charAt(0)=='R')
	{
     driveT.setMaxOutput(0.8);
	while(time.get() < )
	{
		climb.set(0.8); //Lifts arm
	}
	while(time.get() < )
	{
		climb.set(0.0); //Stops lifting arm
    }
	while(time.get() < )
	{
		driveT.tankDrive(-0.7,-0.7); //Drives Robot forward
	}
	while(time.get() < )
	{
		driveT.tankDrive(-0.6,0.6); //Turns robot to the switch
	}
    while(time.get() < )		
    {                            
    	driveT.tankDrive(-0.7,-0.7); //Drives Robot forward
	}
    while(time.get() < )		
    {
    	driveT.tankDrive(0.6,-0.6); //Turn robot to the switch
    }
    while(time.get() < )
    {                
    	driveT.tankDrive(-0.7,-0.7); //Drive robot forward
	}
    while(time.get() < )		
    { 
		pneumat1.set(DoubleSolenoid.Value.kReverse); //Releases cube
    }
    }
    }*/
}
	
	
	public void teleopInit()
	{
	    double lastInput = 0.0;
	    
		time.reset();
	    time.start();
	}
	
	String REnc, LEnc, encodeStr, compressorState, encodeDist, encodeDist2, leftY, rightY;
	
    public void teleopPeriodic() 
    { //Joysticks Controls
    	if((tempL.getY() > -.1 && tempR.getY() < .1) || (tempL.getY() < .1 && tempR.getY() > -.1))
    	{
    		driveT.tankDrive(tempR.getY(), tempL.getY());
    	}
    	else
    	{
    		driveT.tankDrive(tempL.getY(), tempR.getY());
    	}
    	SmartDashboard.putData("Encoder", encode);
    	//System.out.println(encode.getDistance());
    	
    	REnc = jEncR.getVoltage() + "";
    	LEnc = jEncL.getVoltage() + "";
    	SmartDashboard.putString("Left pneumatic", LEnc);
    	SmartDashboard.putString("Right pneumatic", REnc);
    	
    	
    	leftY = "" + tempL.getY();
    	SmartDashboard.putString("Left Y", leftY);
    	
    	rightY = "" + tempR.getY();
    	SmartDashboard.putString("Rigth Y", rightY);
    	
    	System.out.println("Left Encoder"+jEncL.getVoltage());
    	System.out.println("Right Encoder" + jEncR.getVoltage());
    	
    	
    	
    		//System.out.println(encode.getDistance());
    		
    		if(tempL.getRawButton(4))
    		{
    			driveT.setMaxOutput(1);
    		}
    		else if(tempL.getRawButton(3))
    		{
    			driveT.setMaxOutput(.8);
    		}
    		else if(tempL.getRawButton(2))
    		{
    			driveT.setMaxOutput(.6);
    		}
    		
    			//arm
    		if(tempR.getRawButton(3) == true)//down
    		{
    			lift.set(1);
    			//climb.set(.7);
    		}


    			//SmartDashboard.putData("Encoder", encode);
    		
    		
    		if(tempR.getRawButton(2) == true)//up/
    		{
    			
    			lift.set(-.5);
    			//climb.set(-.7);
    			
    		}
    		else if(tempR.getRawButton(3) == false && tempR.getRawButton(2) == false)// Stop when let go
    		{
    				lift.set(0);
        			climb.set(0);
    		}
    		if(tempR.getRawButtonPressed(1) == true)//was 4 now open
    		{
    			pneu = !pneu;
    		}
    		
    		if(pneu)
    		{
    			fullExt.set(DoubleSolenoid.Value.kReverse);
    		}
    		else
    		{
    			fullExt.set(DoubleSolenoid.Value.kForward);
    		}
    		
    		//switch height 
    		//max height 2145
    		
    		if(c.getPressureSwitchValue())
    		{
    			compressorState = "Low Pressure";
    		}
    		else
    		{
    			compressorState = "Pressure ok";
    		}
    		
    		//Bongos
    		if(dTBongo.getRawButton(2) && dTBongo.getRawButton(3))
        	{
        		driveT.tankDrive(speedL, speedR);
        	}
    		else if(dTBongo.getRawButton(1) && dTBongo.getRawButton(4))
        	{
        		driveT.tankDrive(-speedL, -speedR);
        	}
    		else if(dTBongo.getRawButton(1) && dTBongo.getRawButton(3))
        	{
        		driveT.tankDrive(-speedL, speedR);
        	}
    		else if(dTBongo.getRawButton(2) && dTBongo.getRawButton(4))
        	{
        		driveT.tankDrive(speedL, -speedR);
        	}
    		
    		
    		
    		if(uBongo.getRawButton(3) == true)//down
    		{
    			lift.set(1);
    			//climb.set(.7);
    		}

    		if(uBongo.getRawButton(2) == true)//up/
    		{
    			
    			lift.set(-.5);
    			//climb.set(-.7);
    			
    		}
    		else if(uBongo.getRawButton(3) == false && uBongo.getRawButton(2) == false)// Stop when let go
    		{
    				lift.set(0);
        			climb.set(0);
    		}
    		if(uBongo.getRawButtonPressed(10) == true)//was 4 now open
    		{
    			pneu = !pneu;
    		}
    		
    		if(pneu)
    		{
    			fullExt.set(DoubleSolenoid.Value.kReverse);
    		}
    		else
    		{
    			fullExt.set(DoubleSolenoid.Value.kForward);
    		}
    		
    		//switch height 
    		//max height 2145
    		
    		if(c.getPressureSwitchValue())
    		{
    			compressorState = "Low Pressure";
    		}
    		else
    		{
    			compressorState = "Pressure ok";
    		}
    		
    		
    		
    		
    		
    		
    		//DPad Controls
    		
    		if(dPad.getRawButton(14) && dPad.getRawButton(8))
        	{
        		driveT.tankDrive(speedL, speedR);
        	}
    		else if(dPad.getRawButton(10) && dPad.getRawButton(3))
        	{
        		driveT.tankDrive(-speedL, -speedR);
        	}
    		else if(dPad.getRawButton(14) && dPad.getRawButton(3))
        	{
        		driveT.tankDrive(-speedL, speedR);
        	}
    		else if(dPad.getRawButton(10) && dPad.getRawButton(8))
        	{
        		driveT.tankDrive(speedL, -speedR);
        	}
    		
    		
    		
    		if(dPad.getRawButton(1) == true)//down
    		{
    			lift.set(1);
    			//climb.set(.7);
    		}

    		if(dPad.getRawButton(16) == true)//up/
    		{
    			
    			lift.set(-.5);
    			//climb.set(-.7);
    			
    		}
    		else if(dPad.getRawButton(1) == false && dPad.getRawButton(16) == false)// Stop when let go
    		{
    				lift.set(0);
        			climb.set(0);
    		}
    		if(dPad.getRawButtonPressed(4) == true || dPad.getRawButtonPressed(13) == true)//was 4 now open
    		{
    			pneu = !pneu;
    		}
    		
    		if(pneu)
    		{
    			fullExt.set(DoubleSolenoid.Value.kReverse);
    		}
    		else
    		{
    			fullExt.set(DoubleSolenoid.Value.kForward);
    		}
    		
    		//switch height 
    		//max height 2145
    		
    		if(c.getPressureSwitchValue())
    		{
    			compressorState = "Low Pressure";
    		}
    		else
    		{
    			compressorState = "Pressure ok";
    		}
   }
}