/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package main;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.networktables.NetworkTable;

/**
 * @author Amanda & Shen
 */
public class BallLauncher extends SimpleRobot {
    DriveTrain dT = new DriveTrain();
    ArmMotor arm = new ArmMotor();
    VisionProcessor vP = new VisionProcessor();
    Joystick right = new Joystick(Map.joystickRight);
    Joystick left = new Joystick(Map.joystickLeft);
    Joystick shoot = new Joystick(Map.joystickShooter);
    NetworkTable nettable = NetworkTable.getTable("SmartDashboard");
    Encoder encA = new Encoder(Map.ENCODER_RIGHT_A, Map.ENCODER_RIGHT_B);
    Encoder encB = new Encoder(Map.ENCODER_LEFT_A, Map.ENCODER_LEFT_B);

    public void autonomous() {
        new Thread() {
            public void run() {
                Timer.delay(2.25);
                arm.forceOverride();
            }
        }.start();
        arm.setArm();
        Timer.delay(1);
        encB.reset();
        encB.setDistancePerPulse(1.0/243); //243.5
        encB.setSamplesToAverage(127);
        encB.start();
        dT.setWheels(-.7, -.7);
        while (encB.getDistance() >= -7.875 && encB.getDistance() <= 7.875) {
            System.out.println("EncB Dist:" + encB.getDistance());
        }
        System.out.println("Final EncB Dist:" + encB.getDistance());
        encB.reset();
        dT.setWheels(0, 0);
        Timer.delay(.75);
        while (vP.leftOrRight() == 0);
        if (vP.leftOrRight() == 1) {
            //if (right.getAxis(Joystick.AxisType.kY) < -.1) {
            dT.setWheels(0, 0);
            arm.throwArm();
        }
        if (vP.leftOrRight() == 2) {
            //if (right.getAxis(Joystick.AxisType.kY) > .1) {
            dT.setWheels(0, 0);
            arm.throwArm();
        }
        if (vP.leftOrRight() == 0) {
            dT.setWheels(0, -0);
        }

        /*while (encA.getDistance() < 10 && encB.getDistance() < 10) {
         dT.setWheels(1, 1);
         }*/
    }

    public void operatorControl() {
        new Thread() {
            boolean backwards = false;

            public void run() {
                while (isEnabled()) {
                    if (backwards) {
                        dT.setWheelsBackwards(right.getAxis(Joystick.AxisType.kY), left.getAxis(Joystick.AxisType.kY));
                    } else {
                        dT.setWheels(right.getAxis(Joystick.AxisType.kY), left.getAxis(Joystick.AxisType.kY));

                    }
                    if (buttons(3)) {
                        arm.forceOverride();
                    }
                    if (buttons(11)) {
                        backwards = !backwards;
                        //if (backwards) nettable.putString("Front", "arm/back");
                        //else nettable.putString("Front", "normal");
                        nettable.putString("Front", backwards ? "arm/back" : "normal");
                    }
                }
            }
        }.start();
        new Thread() {
            public void run() {
                while (isEnabled()) {
                    if (buttons(2)) {
                        arm.setArm();
                    } else if (buttons(1)) {
                        arm.throwArm();
                    }
                }
            }
        }.start();
    }

    public void test() {
    }

    public boolean buttons(int x) {
        return shoot.getRawButton(x);
    }
}

@@ -1369,182 +1369,255 @@
     
-new Thread() %7B%0A            public void run() %7B%0A                Timer.delay(2.25);%0A                arm.forceOverride();%0A            %7D%0A        %7D.start();%0A        arm.setArm();%0A
+//Timer.delay(1);%0A        encB.reset();%0A        encB.setDistancePerPulse(1.0/243); //243.5%0A        encB.setSamplesToAverage(127);%0A        encB.start();%0A        dT.setWheels(-.7, -.7);%0A        new Thread() %7B%0A            public void run() %7B%0A        
     
@@ -1624,33 +1624,36 @@
     Timer.delay(
-1
+2.25
 );%0A        encB.
@@ -1651,111 +1651,50 @@
     
-encB.reset();%0A        encB.setDistancePerPulse(1.0/243); //243.5%0A        encB.setSamplesToAverage(127);
+        arm.forceOverride();%0A            %7D
 %0A   
@@ -1690,36 +1690,33 @@
       %7D%0A        
-encB
+%7D
 .start();%0A      
@@ -1717,37 +1717,27 @@
         
-dT
+arm
 .set
-Wheels(-.7, -.7
+Arm(
 );%0A     
@@ -2046,37 +2046,141 @@
  while (
-vP.leftOrRight() == 0
+1 == 0 && isAutonomous() && isEnabled());//vP.leftOrRight()%0A        if (isAutonomous() && isEnabled())%0A        %7Barm.throwArm(
 );%0A     
@@ -2173,32 +2173,38 @@
 owArm();%0A       
+    /*
  if (vP.leftOrRi
@@ -2221,32 +2221,36 @@
 ) %7B%0A            
+    
 //if (right.getA
@@ -2292,36 +2292,8 @@
     
-        dT.setWheels(0, 0);%0A
     
@@ -2320,34 +2320,40 @@
 ();%0A        
-%7D%0A
+    %7D%0A  
         
 if (vP.leftO
@@ -2332,32 +2332,39 @@
     %7D%0A          
+  else 
 if (vP.leftOrRig
@@ -2372,24 +2372,28 @@
 t() == 2) %7B%0A
+    
             
@@ -2451,35 +2451,8 @@
     
-       dT.setWheels(0, 0);%0A
     
@@ -2450,33 +2450,32 @@
                 
- 
 arm.throwArm();%0A
@@ -2474,32 +2474,36 @@
 wArm();%0A        
+    
 %7D%0A        if (vP
@@ -2500,69 +2500,66 @@
     
-if (vP.leftOrRight() == 0) %7B%0A            dT.setWheels(0, -0);
+    else %7B%0A                arm.throwArm();%0A            %7D*/
 %0A   

Move and shoot in autonomous