...

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. 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.                                                               */
/*----------------------------------------------------------------------------*/
#include <thread>//

#include <frc/AnalogInput.h>
#include <iostream>
#include <frc/PowerDistributionPanel.h>
#include <cameraserver/CameraServer.h>//
#include <opencv2/core/core.hpp>//
#include <opencv2/core/types.hpp>//
#include <opencv2/imgproc/imgproc.hpp>//
#include <wpi/raw_ostream.h>//
#include <frc/TimedRobot.h>
#include <wpi/raw_ostream.h>//
#include <frc/Joystick.h>
#include <frc/Timer.h>
#include <frc/PWMVictorSPX.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/SpeedControllerGroup.h>
#include <frc/DoubleSolenoid.h>
#include <frc/Solenoid.h>
#include <frc/Compressor.h>

class Robot : public frc::TimedRobot {
 public:

  void RobotInit() override {
  compressor.Stop ();
  frc::CameraServer::GetInstance()->StartAutomaticCapture(0);
  frc::CameraServer::GetInstance()->StartAutomaticCapture(1);
  }
  void RobotPeriodic() override {};
  void AutonomousInit() override {};
  void AutonomousPeriodic() override {};
  void TeleopInit() override {};

  void TeleopPeriodic() override {
//PDP Current Output
    //frc::SmartDashboard::PutString(PDP.GetCurrent(0));
//Compressor "Switch..."
    if (fightStick.GetRawButton(8) == 1) {
      compressor.Start();
    } else {
      compressor.Stop();
    }
  
//Drive(Works)
    /*(Second test, this is a test to switch from curvature drive to arcadedrive, arcade drive has the thrust set to 0 at all times)
    if (flightStick.GetRawButton(1) == 0) {
      Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -.5, flightStick.GetRawAxis(2) * .5, flightStick.GetRawButton(5)); // 0(X) or 2(Z)
    } else if (flightStick.GetRawButton(1) == 1) {
      Drive.TankDrive(flightStick.GetRawAxis(1) * .45, flightStick.GetRawAxis(2) * .45);
    } else {
      Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -.5, flightStick.GetRawAxis(2) * .5, flightStick.GetRawButton(5));
    }*/

    //(Original Code of drive, if none of the top works I can set the overall turn to a smaller number or I can try and see if multipling the 
    //statement that activates tank turn?)
    /*
    if (flightStick.GetRawButton(2) == 1) {
      Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -.7, flightStick.GetRawAxis(2) * .7, flightStick.GetRawButton(1));
    } else if (flightStick.GetRawButton(2) == 0) { 
       Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -.5, flightStick.GetRawAxis(2) * .7, flightStick.GetRawButton(1));
    } else {
      Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -.5, flightStick.GetRawAxis(2) * .7, flightStick.GetRawButton(1));
    }
    */
    if (flightStick.GetRawButton(2) == 1) {
      Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -.7, flightStick.GetRawAxis(2) * .7, flightStick.GetRawButton(1));
    } else if (flightStick.GetRawButton(2) == 0) { 
       Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -.5, flightStick.GetRawAxis(2) * .7, flightStick.GetRawButton(1));
    } else {
      Drive.CurvatureDrive(flightStick.GetRawAxis(1) * -.5, flightStick.GetRawAxis(2) * .7, flightStick.GetRawButton(1));
    }   
//HAB Lift(Works... Might now have???)
    //HAB lift
    //Font(Untested)
    if (flightStick.GetRawButton(7) == 1) {
      s_HABfront.Set(frc::DoubleSolenoid::kForward);
    } else if (flightStick.GetRawButton(7) == 0) {
      s_HABfront.Set(frc::DoubleSolenoid::kReverse);
    } else {
      s_HABfront.Set(frc::DoubleSolenoid::kOff);
    }
    //Back(Untested)
    if (flightStick.GetRawButton(8) == 1) {
      s_HABback.Set(frc::DoubleSolenoid::kForward);
    } else if (flightStick.GetRawButton(8) == 0) {
      s_HABback.Set(frc::DoubleSolenoid::kReverse);
    } else {
      s_HABback.Set(frc::DoubleSolenoid::kOff);
    }

//Forklift(Works) (B)
    //Forlift (Works)
    //POV is 0=UP 90=Right 180=Down 270=Left and 45 for all the angles
    //.GetPOV("0") is the number for the POV system things switch
    if (fightStick.GetRawButton(2) == 1) {
      if (fightStick.GetPOV(0) == 0) {
        if (fightStick.GetRawButton(5) == 1) {
          m_forklift.Set(-.5);
        } else if (fightStick.GetRawButton(5) == 0) {
          m_forklift.Set(-.4);
        } else {
          m_forklift.Set(-.4);
        }
      } else if (fightStick.GetPOV(0) == 180) {
        if (fightStick.GetRawButton(5) == 1) {
          m_forklift.Set(-.03);
        } else if (fightStick.GetRawButton(5) == 0) {
          m_forklift.Set(-.045);//.005 might be too small???
        } else {
          m_forklift.Set(-.045);
        } 
      } else {
          m_forklift.Set(-.2);
        }
      } else {
        m_forklift.Set(0);
       }

     
//Ball Intake(Works)
    if (fightStick.GetRawButton(1) == 1) {
      m_leftIntake.Set(.5);//+
      m_rightIntake.Set(-.5);
    } else if (fightStick.GetRawButton(3) == 1) {
      m_leftIntake.Set(-.5);
      m_rightIntake.Set(.5);
    } else {
      m_leftIntake.Set(0);
      m_rightIntake.Set(0);
    }

//Hatch Panel Launcher(Works)
    //Hatch Panel Launcher(Works)
    //Set for 2 solenoids, still deciding wither it will be 1 or 2 solenoids
    //4 = Y on Qanda Arcade
    //1
    if (fightStick.GetRawButton(4) == 0) {
      s_panelLauncher.Set(frc::DoubleSolenoid::kForward);
      s_panelGetter.Set(frc::DoubleSolenoid::kForward);
    } else if (fightStick.GetRawButton(4) == 1) {
      s_panelLauncher.Set(frc::DoubleSolenoid::kReverse);
      s_panelGetter.Set(frc::DoubleSolenoid::kReverse);
    } else {
      s_panelLauncher.Set(frc::DoubleSolenoid::kForward);
      s_panelGetter.Set(frc::DoubleSolenoid::kForward);
    }
    //input.GetVoltage();
  }
//Private
 private:
  //Analog PI (PWM)
  //frc::AnalogInput input{0};
  //Declartions
  frc::Joystick fightStick{1};
  frc::Joystick flightStick{2};
  frc::Compressor compressor;
  //frc::PowerDistributionPanel PDP;
  //Motors
  //Drivebase
  frc::PWMVictorSPX m_frontLeft{0};
  frc::PWMVictorSPX m_backLeft{1};
  frc::PWMVictorSPX m_frontRight{2};
  frc::PWMVictorSPX m_backRight{3};
  //Ball Intake
  frc::PWMVictorSPX m_leftIntake{4}; //No Gearbox Currently
  frc::PWMVictorSPX m_rightIntake{5};
  //Forlift
  frc::PWMVictorSPX m_forklift{6};
  //Groups
  //Drivebase
  frc::SpeedControllerGroup m_left{m_frontLeft, m_backLeft};
  frc::SpeedControllerGroup m_right{m_frontRight, m_backRight}; 
  //Setting Drive
  frc::DifferentialDrive Drive{m_left, m_right};
  //Pneumatics/Solenoids
  //Hatch Panel Launcher
  frc::DoubleSolenoid s_panelLauncher{0, 1};
  frc::DoubleSolenoid s_panelGetter{2, 3}; // will have to change this variable and the one above depending on if we have the Getter/Slider or not
  frc::DoubleSolenoid s_HABfront{4, 5};
  frc::DoubleSolenoid s_HABback{6, 7};
};


//Main
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif