From fa2419c79e98ff3bca4a57c18adb05f8da081b24 Mon Sep 17 00:00:00 2001
From: CORE Programmers <core2062programmers@googlegroups.com>
Date: Sun, 8 Mar 2020 15:50:31 -0500
Subject: [PATCH 1/2] Created Ball sensor code

---
 src/ConveyorSubsystem.cpp | 3 +++
 src/ConveyorSubsystem.h   | 3 +++
 src/IntakeSubsystem.cpp   | 4 ++--
 3 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/ConveyorSubsystem.cpp b/src/ConveyorSubsystem.cpp
index 22179d3..90c5836 100644
--- a/src/ConveyorSubsystem.cpp
+++ b/src/ConveyorSubsystem.cpp
@@ -43,6 +43,9 @@ void ConveyorSubsystem::teleop(){
     } else {
         setUpperMotor(0.0);
     }
+
+    bool ballDetected = m_input.Get();
+    frc::SmartDashboard::PutBoolean("Ball Detected", ballDetected);
 }
 
 void ConveyorSubsystem::setLowerMotor(double lowerConveyorSpeed){
diff --git a/src/ConveyorSubsystem.h b/src/ConveyorSubsystem.h
index 08c8827..a564607 100644
--- a/src/ConveyorSubsystem.h
+++ b/src/ConveyorSubsystem.h
@@ -2,6 +2,8 @@
 
 #include <CORERobotLib.h>
 #include <ctre/Phoenix.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/DigitalInput.h>
 #include "Config.h"
 
 class ConveyorSubsystem : public CORESubsystem {
@@ -16,5 +18,6 @@ class ConveyorSubsystem : public CORESubsystem {
     COREConstant<double> lowerConveyorSpeed, upperConveyorSpeed;
 private: 
     TalonSRX m_lowerConveyorMotor, m_frontConveyorMotor, m_backConveyorMotor;
+    frc::DigitalInput m_input{0};
 };
 
diff --git a/src/IntakeSubsystem.cpp b/src/IntakeSubsystem.cpp
index 3461f36..4099a39 100644
--- a/src/IntakeSubsystem.cpp
+++ b/src/IntakeSubsystem.cpp
@@ -1,9 +1,9 @@
 #include "IntakeSubsystem.h"
 
 IntakeSubsystem::IntakeSubsystem() : 
+                                     intakeSpeed("Intake Speed", 0.45),
                                      m_intakeMotor(INTAKE_PORT), 
-                                     m_intakeSolenoid(LEFT_DRIVE_SHIFTER_PCM, INTAKE_DOWN_PORT, INTAKE_UP_PORT),
-                                     intakeSpeed("Intake Speed", 0.45){
+                                     m_intakeSolenoid(LEFT_DRIVE_SHIFTER_PCM, INTAKE_DOWN_PORT, INTAKE_UP_PORT){
 }
 
 void IntakeSubsystem::robotInit(){

From bf9701d5ff56b73f53f219820245a00cb541fdba Mon Sep 17 00:00:00 2001
From: CORE Programmers <core2062programmers@googlegroups.com>
Date: Sun, 8 Mar 2020 15:27:17 -0500
Subject: [PATCH 2/2] changes to test the ball sensor

---
 src/ConveyorSubsystem.cpp | 49 ++++++++++++++++++++++++++-------------
 src/ConveyorSubsystem.h   |  1 +
 2 files changed, 34 insertions(+), 16 deletions(-)

diff --git a/src/ConveyorSubsystem.cpp b/src/ConveyorSubsystem.cpp
index 90c5836..3cb4269 100644
--- a/src/ConveyorSubsystem.cpp
+++ b/src/ConveyorSubsystem.cpp
@@ -22,30 +22,47 @@ void ConveyorSubsystem::robotInit(){
     operatorJoystick->RegisterButton(CORE::COREJoystick::JoystickButton::RIGHT_BUTTON);
     operatorJoystick->RegisterButton(CORE::COREJoystick::JoystickButton::LEFT_TRIGGER);
     operatorJoystick->RegisterButton(CORE::COREJoystick::JoystickButton::LEFT_BUTTON);
+    operatorJoystick->RegisterButton(CORE::COREJoystick::JoystickButton::Y_BUTTON);
 }
 
-void ConveyorSubsystem::teleopInit() {}
+void ConveyorSubsystem::teleopInit() {
+    m_autoConveyor = false;
+}
 
 void ConveyorSubsystem::teleop(){
     
-    if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::RIGHT_TRIGGER)) {
-        setLowerMotor(lowerConveyorSpeed.Get());
-    } else if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::RIGHT_BUTTON)) {
-        setLowerMotor(-lowerConveyorSpeed.Get());
-    } else {
-        setLowerMotor(0.0);
-    }
-    
-    if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_TRIGGER)) {
-        setUpperMotor(upperConveyorSpeed.Get());
-    } else if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_BUTTON)) {
-        setUpperMotor(-upperConveyorSpeed.Get());
-    } else {
-        setUpperMotor(0.0);
-    }
 
     bool ballDetected = m_input.Get();
     frc::SmartDashboard::PutBoolean("Ball Detected", ballDetected);
+
+    if (operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::Y_BUTTON)) {
+        if (ballDetected){
+            setLowerMotor(-lowerConveyorSpeed.Get());
+            setUpperMotor(-upperConveyorSpeed.Get());
+        } else {
+            setLowerMotor(0.0);
+            setUpperMotor(0.0);
+        }
+        m_autoConveyor = true;
+    } else if (!operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::Y_BUTTON)) {
+        if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::RIGHT_TRIGGER)) {
+            setLowerMotor(lowerConveyorSpeed.Get());
+        } else if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::RIGHT_BUTTON)) {
+            setLowerMotor(-lowerConveyorSpeed.Get());
+        } else {
+            setLowerMotor(0.0);
+        }
+        
+        if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_TRIGGER)) {
+            setUpperMotor(upperConveyorSpeed.Get());
+        } else if(operatorJoystick->GetButton(CORE::COREJoystick::JoystickButton::LEFT_BUTTON)) {
+            setUpperMotor(-upperConveyorSpeed.Get());
+        } else {
+            setUpperMotor(0.0);
+        }
+
+    }
+    frc::SmartDashboard::PutBoolean("Conveyor Auto Run", m_autoConveyor);
 }
 
 void ConveyorSubsystem::setLowerMotor(double lowerConveyorSpeed){
diff --git a/src/ConveyorSubsystem.h b/src/ConveyorSubsystem.h
index a564607..772601f 100644
--- a/src/ConveyorSubsystem.h
+++ b/src/ConveyorSubsystem.h
@@ -19,5 +19,6 @@ class ConveyorSubsystem : public CORESubsystem {
 private: 
     TalonSRX m_lowerConveyorMotor, m_frontConveyorMotor, m_backConveyorMotor;
     frc::DigitalInput m_input{0};
+    bool m_autoConveyor;
 };