package org.texastorque.texastorque20145;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.texastorque.texastorque20145.autonomous.AutoMode;
import org.texastorque.texastorque20145.autonomous.AutoPicker;
import org.texastorque.texastorque20145.constants.Constants;
import org.texastorque.texastorque20145.feedback.FeedbackSystem;
import org.texastorque.texastorque20145.feedback.SensorFeedback;
import org.texastorque.texastorque20145.input.DriverInput;
import org.texastorque.texastorque20145.input.InputSystem;
import org.texastorque.texastorque20145.subsystem.Clapper;
import org.texastorque.texastorque20145.subsystem.Drivebase;
import org.texastorque.texastorque20145.subsystem.RearIntake;
import org.texastorque.texastorque20145.subsystem.Shooter;
import org.texastorque.texastorque20145.torquelib.Parameters;

public class Robot extends TorqueIterative {

    Parameters params;

    Drivebase drivebase;
    Shooter shooter;
    Clapper clapper;
    RearIntake rearIntake;
    
    Compressor compressor;

    InputSystem input;

    FeedbackSystem feedback;
    FeedbackSystem sensorFeedback;

    Thread AutoThread;

    public void robotInit() {
        params = new Parameters();
        params.load();

        drivebase = new Drivebase();
        shooter = new Shooter();
        clapper = new Clapper();
        rearIntake = new RearIntake();
        
        compressor = new Compressor(1, Constants.compressorSwitch.getInt(), 1, Constants.compressorRelay.getInt());

        input = new DriverInput();

        sensorFeedback = new SensorFeedback();
        feedback = sensorFeedback;
    }

    public void teleopInit() {
        params.load();
        compressor.start();

        drivebase.setInputSystem(input);
        drivebase.enableOutput(true);
        
        shooter.setInputSystem(input);
        shooter.setFeedbackSystem(feedback);
        shooter.updateGains();
        
        clapper.setInputSystem(input);
        clapper.setFeedbackSystem(feedback);
        clapper.updateGains();
        
        rearIntake.setInputSystem(input);
        rearIntake.setFeedbackSystem(feedback);
        rearIntake.updateGains();
    }

    public void teleopPeriodic() {
        drivebase.update();
        shooter.update();
        clapper.update();
        rearIntake.update();
        
        drivebase.pushToDashboard();
        shooter.pushToDashboard();
        clapper.pushToDashboard();
        rearIntake.pushToDashboard();
    }

    public void teleopContinuous() {
        input.run();
        feedback.run();
        SmartDashboard.putNumber("rpm", feedback.getShooterRPM());
    }

    public void autonomousInit() {
        params.load();
        feedback = sensorFeedback;
        drivebase.setFeedbackSystem(feedback);

        AutoMode autoInput = AutoPicker.getAutoMode();
        autoInput.setFeedBackSystem(feedback);

        drivebase.setInputSystem(autoInput);

        drivebase.enableOutput(true);

        AutoThread = new Thread(autoInput);
        AutoThread.start();
    }

    public void autonomousPeriodic() {
        drivebase.update();
    }
    
    public void testInit()
    {
        feedback.resetFrontIntakeAngle();
        feedback.resetRearIntakeAngle();
    }

}

@@ -81,68 +81,8 @@
 or;%0A
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;%0A
 impo
@@ -630,16 +630,79 @@
 vebase;%0A
+import org.texastorque.texastorque20145.subsystem.FrontIntake;%0A
 import o
@@ -1041,16 +1041,45 @@
 Intake;%0A
+    FrontIntake frontIntake;%0A
     %0A   
@@ -1443,24 +1443,65 @@
 arIntake();%0A
+        frontIntake = new FrontIntake();%0A
         %0A   
@@ -2278,24 +2278,160 @@
 ateGains();%0A
+        %0A        frontIntake.setInputSystem(input);%0A        frontIntake.setFeedbackSystem(feedback);%0A        frontIntake.updateGains();%0A
     %7D%0A%0A    p
@@ -2561,32 +2561,151 @@
 ntake.update();%0A
+        frontIntake.update();%0A    %7D%0A%0A    public void teleopContinuous() %7B%0A        input.run();%0A        feedback.run();%0A
         %0A       
@@ -2850,157 +2850,40 @@
     
-%7D%0A%0A
     
-public void teleopContinuous() %7B%0A        input.run();%0A        feedback.run();%0A        SmartDashboard.putNumber(%22rpm%22, feedback.getShooterRPM()
+frontIntake.pushToDashboard(
 );%0A 
@@ -2938,32 +2938,41 @@
  params.load();%0A
+        %0A
         feedback
@@ -2994,56 +2994,8 @@
 ck;%0A
-        drivebase.setFeedbackSystem(feedback);%0A%0A
     
@@ -3092,16 +3092,71 @@
 dback);%0A
+        %0A        drivebase.setFeedbackSystem(feedback);
 %0A       
@@ -3193,17 +3193,16 @@
 Input);%0A
-%0A
         
@@ -3426,91 +3426,8 @@
   %7B%0A
-        feedback.resetFrontIntakeAngle();%0A        feedback.resetRearIntakeAngle();%0A
     

Add front intake