SidePegsBasedOnTime

LEFT SIDE:

package org.usfirst.frc.team6763.robot;

import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
//Imports the other files needed by the program
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.command.Command;

/*
THIS CODE IS FOR THE LEFT SIDE PEG
*/

public class Robot extends IterativeRobot {
	
	RobotDrive myRobot = new RobotDrive(0, 2);
	Joystick stick = new Joystick(0);
	Timer timer = new Timer();
	Jaguar ArmMotor = new Jaguar(9);
	SpeedController Climber = new Spark(7);
	Joystick mXboxController = new Joystick(1);
	
	//SendableChooser
	
	Encoder slaveEncoder; // Right Side Encoder
	Encoder masterEncoder; // Left Side Encoder
	
	float TicksPerInch = 108.333333F; // Number of Encoder Ticks per Inch
	int InchesPerFoot = 12; 
	
	double masterPower = 0.475;
	double slavePower = 0.5;
	double error = 0.0;
	double kp = 0.05;
	double maxEncoderError = TicksPerInch * 85;
	
	@Override
	public void robotInit() {
		slaveEncoder = new Encoder(2, 3);
    	slaveEncoder.setReverseDirection(true);
    	masterEncoder = new Encoder (0, 1);
    	masterEncoder.setReverseDirection(false);
    	CameraServer.getInstance().startAutomaticCapture();
    	
    	
	}

	@Override
	public void autonomousInit() {
		timer.reset();
		timer.start();
		System.out.println("Master Count,Slave Count,Error,KP,MasterPower,SlavePower");
		masterEncoder.reset();
		slaveEncoder.reset();
	}
	
	@Override
	public void autonomousPeriodic() {
		/*
		THIS IS FOR THE LEFT PEG
		*/
		
		ArmMotor.set(0.12);
		
		if (timer.get() < 5){
			masterPower = -0.533;
			slavePower = -0.5;
		} else if (timer.get() > 5 && timer.get() < 6.5) {
			masterPower = -0.65;
			slavePower = -0.5;
		} else if (timer.get() > 6.5 && timer.get() < 9) {
			masterPower = -0.533;
			slavePower = -0.5;
		} else {
			masterPower = 0;
			slavePower = 0;
		}
		
		/*
		if (masterEncoder.get() < (TicksPerInch * 76)) {
			masterPower = -0.533;
			slavePower = -0.5;
			error = masterEncoder.get() - slaveEncoder.get();
			if (error > 50){
				masterPower = -0.531;
			}
			else if (error < -50){
				masterPower = -0.535;
			}
			else if (error > -50 && error < 50){
				masterPower = -0.533;
			}
		}
		else {
			masterPower = 0;
			slavePower = 0;
		}
			
		myRobot.tankDrive(masterPower, slavePower);
		
		System.out.println(masterEncoder.get() + " , " + slaveEncoder.get() + " , " + error);
		*/
		/*
		if (masterEncoder.get() > 10500 && masterEncoder.get() < 11000){
			slavePower = -0.5;
			masterPower = -0.65;
		}
		else {
			masterPower = -0.533;
			slavePower = -0.5;
			error = masterEncoder.get() - slaveEncoder.get();
			if (error > 50){
				masterPower = -0.531;
			}
			else if (error < -50){
				masterPower = -0.535;
			}
			else if (error > -50 && error < 50){
				masterPower = -0.533;
			}
		}
		if (masterEncoder.get() > 11000) {
			myRobot.tankDrive(0, 0);
		}
		*/
		
		//System.out.println(masterEncoder.get() + "," + slaveEncoder.get() + "," + error + "," + kp + "," + masterPower + "," + slavePower);
		//System.out.println(masterEncoder.get() + "," + slaveEncoder.get());
	}
		
	@Override
	public void teleopInit() {
	}
	
	@Override
	public void teleopPeriodic() {
		myRobot.arcadeDrive(mXboxController);
		if (mXboxController.getRawAxis(5)*-1 < 0){
			Climber.set(0);
		}
		else{
			Climber.set(mXboxController.getRawAxis(5)*-1);
		}
		ArmMotor.set(stick.getRawAxis(1)*.25*-1);
	}

	@Override
	public void testPeriodic() {
		LiveWindow.run();
	}
	
	public void disabledPeriodic(){
		 //System.out.println("RightEncoder (slave) = " + slaveEncoder.get());
		 //System.out.println("LeftEncoder (master) = " + masterEncoder.get());
		//System.out.println(masterEncoder.get() + " , " + slaveEncoder.get());
	 }
}

RIGHT SIDE:

package org.usfirst.frc.team6763.robot;

import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
//Imports the other files needed by the program
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.command.Command;

/*
THIS CODE IS FOR THE RIGHT SIDE PEG
*/

public class Robot extends IterativeRobot {
	
	RobotDrive myRobot = new RobotDrive(0, 2);
	Joystick stick = new Joystick(0);
	Timer timer = new Timer();
	Jaguar ArmMotor = new Jaguar(9);
	SpeedController Climber = new Spark(7);
	Joystick mXboxController = new Joystick(1);
	
	//SendableChooser
	
	Encoder slaveEncoder; // Right Side Encoder
	Encoder masterEncoder; // Left Side Encoder
	
	float TicksPerInch = 108.333333F; // Number of Encoder Ticks per Inch
	int InchesPerFoot = 12; 
	
	double masterPower = 0.475;
	double slavePower = 0.5;
	double error = 0.0;
	double kp = 0.05;
	double maxEncoderError = TicksPerInch * 85;
	
	@Override
	public void robotInit() {
		slaveEncoder = new Encoder(2, 3);
    	slaveEncoder.setReverseDirection(true);
    	masterEncoder = new Encoder (0, 1);
    	masterEncoder.setReverseDirection(false);
    	CameraServer.getInstance().startAutomaticCapture();
    	
    	
	}

	@Override
	public void autonomousInit() {
		timer.reset();
		timer.start();
		System.out.println("Master Count,Slave Count,Error,KP,MasterPower,SlavePower");
		masterEncoder.reset();
		slaveEncoder.reset();
	}
	
	@Override
	public void autonomousPeriodic() {
		/*
		THIS IS FOR THE RIGHT PEG
		*/
		
		ArmMotor.set(0.12);
		
		if (timer.get() < 5){
			masterPower = -0.533;
			slavePower = -0.5;
		} else if (timer.get() > 5 && timer.get() < 6.5) {
			masterPower = -0.533;
			slavePower = -0.65;
		} else if (timer.get() > 6.5 && timer.get() < 9) {
			masterPower = -0.533;
			slavePower = -0.5;
		} else {
			masterPower = 0;
			slavePower = 0;
		}
		
		/*
		if (masterEncoder.get() < (TicksPerInch * 76)) {
			masterPower = -0.533;
			slavePower = -0.5;
			error = masterEncoder.get() - slaveEncoder.get();
			if (error > 50){
				masterPower = -0.531;
			}
			else if (error < -50){
				masterPower = -0.535;
			}
			else if (error > -50 && error < 50){
				masterPower = -0.533;
			}
		}
		else {
			masterPower = 0;
			slavePower = 0;
		}
			
		myRobot.tankDrive(masterPower, slavePower);
		
		System.out.println(masterEncoder.get() + " , " + slaveEncoder.get() + " , " + error);
		*/
		
		
		/*
		if (masterEncoder.get() > 10500 && masterEncoder.get() < 11000){
			slavePower = -0.65;
			masterPower = -0.533;
		}
		else {
			masterPower = -0.533;
			slavePower = -0.5;
			error = masterEncoder.get() - slaveEncoder.get();
			if (error > 50){
				masterPower = -0.531;
			}
			else if (error < -50){
				masterPower = -0.535;
			}
			else if (error > -50 && error < 50){
				masterPower = -0.533;
			}
		}
		if (masterEncoder.get() > 11000) {
			myRobot.tankDrive(0, 0);
		}
		*/
		
		//System.out.println(masterEncoder.get() + "," + slaveEncoder.get() + "," + error + "," + kp + "," + masterPower + "," + slavePower);
		//System.out.println(masterEncoder.get() + "," + slaveEncoder.get());
	}
		
	@Override
	public void teleopInit() {
	}
	
	@Override
	public void teleopPeriodic() {
		myRobot.arcadeDrive(mXboxController);
		if (mXboxController.getRawAxis(5)*-1 < 0){
			Climber.set(0);
		}
		else{
			Climber.set(mXboxController.getRawAxis(5)*-1);
		}
		ArmMotor.set(stick.getRawAxis(1)*.25*-1);
	}

	@Override
	public void testPeriodic() {
		LiveWindow.run();
	}
	
	public void disabledPeriodic(){
		 //System.out.println("RightEncoder (slave) = " + slaveEncoder.get());
		 //System.out.println("LeftEncoder (master) = " + masterEncoder.get());
		//System.out.println(masterEncoder.get() + " , " + slaveEncoder.get());
	 }
}