package cougartech.aerialassist.prototypes;

import edu.wpi.first.wpilibj.CANJaguar;
//import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import java.util.Date;

public class RobotMain extends IterativeRobot
{

    Joystick joy;
    CANJaguar motor;
    Date nowTime;
    boolean wantShoot = false;

    boolean reset = false;
    long sTime;
    long pTime;
    int movementPer = 61;
    long motorTimeOut = 5000;
    long timePer = 120;
    double scaledCurrent;
    double resistance;
        
    public void robotInit()
    {
        joy = new Joystick(1);
        
        try
        {
            motor = new CANJaguar(10);
        }
        catch(CANTimeoutException ex)
        {
            ex.printStackTrace();
        }
        
        nowTime = new Date();
        //encoder = new Encoder(1, 2);
    }

    public void autonomousPeriodic()
    {
    }

    public void teleopPeriodic()
    {
        nowTime = new Date();
    
        try
        {
            motor.enableControl();
        }
        catch(CANTimeoutException ex)
        {
            ex.printStackTrace();
        }
      
        if(wantShoot)
        {
                if(nowTime.getTime() < (sTime + timePer))
                {
                    try
                    {
                        motor.setX(1.0);
                    }
                    catch(CANTimeoutException ex)
                    {
                        ex.printStackTrace();
                    }
                    reset = true;
                }
                else
                {
                    try
                    {
                        motor.setX(0.0);
                    }
                    catch(CANTimeoutException ex)
                    {
                        ex.printStackTrace();
                    }
                }
         }
        else if(!wantShoot && joy.getRawButton(1))
        {
            sTime = nowTime.getTime();
            wantShoot = true;
            
        }
        
        if(reset)
        {
            
        }
    }
}

@@ -81,18 +81,16 @@
 aguar;%0D%0A
-//
 import e
@@ -110,23 +110,28 @@
 wpilibj.
-Encoder
+DigitalInput
 ;%0D%0Aimpor
@@ -385,16 +385,99 @@
 motor;%0D%0A
+    CANJaguar motor2;%0D%0A    DigitalInput stopSwitch;%0D%0A    DigitalInput ballDetect;%0D%0A
     Date
@@ -519,18 +519,16 @@
 false;%0D%0A
-%0D%0A
     bool
@@ -577,224 +577,184 @@
 ong 
-pTime;%0D%0A    int movementPer = 61;%0D%0A    long motorTimeOut = 5000;%0D%0A    long timePer = 120;%0D%0A    double scaledCurrent;%0D%0A    double resistance;%0D%0A        %0D%0A    public void robotInit()%0D%0A    %7B%0D%0A        joy = new Joystick(1
+timePer = 120;%0D%0A        %0D%0A    public void robotInit()%0D%0A    %7B%0D%0A        joy = new Joystick(1);%0D%0A        stopSwitch = new DigitalInput(1);%0D%0A        ballDetect = new DigitalInput(2
 );%0D%0A
@@ -823,24 +823,64 @@
 aguar(10);%0D%0A
+            motor = new CANJaguar(11);%0D%0A
         %7D%0D%0A 
@@ -978,89 +978,8 @@
  %7D%0D%0A
-        %0D%0A        nowTime = new Date();%0D%0A        //encoder = new Encoder(1, 2);%0D%0A
     
@@ -1171,24 +1171,61 @@
 Control();%0D%0A
+            motor2.enableControl();%0D%0A
         %7D%0D%0A 
@@ -1525,24 +1525,67 @@
 setX(1.0);%0D%0A
+                        motor2.setX(1.0);%0D%0A
             
@@ -1743,43 +1743,8 @@
  %7D%0D%0A
-                    reset = true;%0D%0A
     
@@ -1885,24 +1885,67 @@
 setX(0.0);%0D%0A
+                        motor2.setX(0.0);%0D%0A
             
@@ -2091,32 +2091,67 @@
              %7D%0D%0A
+                    reset = true;%0D%0A
                 
@@ -2214,16 +2214,37 @@
 utton(1)
+ && !ballDetect.get()
 )%0D%0A     
@@ -2321,24 +2321,30 @@
 true;%0D%0A     
+   %7D%0D%0A
        %0D%0A   
@@ -2340,33 +2340,41 @@
       %0D%0A        
-%7D
+if(reset)
 %0D%0A        %0D%0A    
@@ -2359,32 +2359,33 @@
 reset)%0D%0A        
+%7B
 %0D%0A        if(res
@@ -2374,33 +2374,768 @@
  %7B%0D%0A        
+ 
-if(reset)
+   if(stopSwitch.get())%0D%0A            %7B%0D%0A                try%0D%0A                %7B%0D%0A                    motor.setX(0.0);%0D%0A                    motor2.setX(0.0);%0D%0A                %7D%0D%0A                catch(CANTimeoutException ex)%0D%0A                %7B%0D%0A                    ex.printStackTrace();%0D%0A                %7D%0D%0A                reset  = false;%0D%0A                wantShoot = false;%0D%0A            %7D%0D%0A            else%0D%0A            %7B%0D%0A                try%0D%0A                %7B%0D%0A                    motor.setX(-0.25);%0D%0A                    motor2.setX(-0.25);%0D%0A                %7D%0D%0A                catch(CANTimeoutException ex)%0D%0A                %7B%0D%0A                    ex.printStackTrace();%0D%0A                %7D                %0D%0A            %7D%0D%0A        %7D%0D%0A        else
 %0D%0A        %7B%0D

Reset and ball detect is so perfect!