From 3983a7202e4fb1cebf1ef77266e2f215523b2e67 Mon Sep 17 00:00:00 2001
From: Yamilett Estrada <36552200+yyestrada-re@users.noreply.github.com>
Date: Mon, 4 Feb 2019 17:09:14 -0600
Subject: [PATCH 01/12] TankDrive code

---
 build.gradle                                  |   2 +-
 src/main/java/frc/robot/OI.java               |  16 ++-
 src/main/java/frc/robot/Robot.java            |  26 ++---
 .../java/frc/robot/commands/DriveCommand.java |  49 ++++++++
 .../{ExampleCommand.java => SetConstant.java} |  13 +--
 .../robot/subsystems/ExampleSubsystem.java    |  24 ----
 .../java/frc/robot/subsystems/TankDrive.java  | 110 ++++++++++++++++++
 vendordeps/Phoenix.json                       |  87 ++++++++++++++
 8 files changed, 278 insertions(+), 49 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/DriveCommand.java
 rename src/main/java/frc/robot/commands/{ExampleCommand.java => SetConstant.java} (82%)
 delete mode 100644 src/main/java/frc/robot/subsystems/ExampleSubsystem.java
 create mode 100644 src/main/java/frc/robot/subsystems/TankDrive.java
 create mode 100644 vendordeps/Phoenix.json

diff --git a/build.gradle b/build.gradle
index 9128ffc..2a7ec8c 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2019.1.1"
+    id "edu.wpi.first.GradleRIO" version "2019.2.1"
 }
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 5d57329..92dd319 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -7,6 +7,11 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.Button;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import frc.robot.commands.SetConstant;
+
 /**
  * This class is the glue that binds the controls on the physical operator
  * interface to the commands and command groups that allow control of the robot.
@@ -20,6 +25,10 @@ public class OI {
   // Joystick stick = new Joystick(port);
   // Button button = new JoystickButton(stick, buttonNumber);
 
+  public static Joystick joyLeft = new Joystick(0); // port subject to change
+  public static Joystick joyRight = new Joystick(1);
+
+  public static Button button = new JoystickButton(joyLeft, 3);
   // There are a few additional built in buttons you can use. Additionally,
   // by subclassing Button you can create custom triggers and bind those to
   // commands the same as any other Button.
@@ -34,9 +43,10 @@ public class OI {
 
   // Run the command while the button is being held down and interrupt it once
   // the button is released.
-  // button.whileHeld(new ExampleCommand());
-
+  public OI() {
+    button.whileHeld(new SetConstant()); 
   // Start the command when the button is released and let it run the command
   // until it is finished as determined by it's isFinished method.
-  // button.whenReleased(new ExampleCommand());
+    button.whenReleased(new SetConstant());
+  }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 0fc498a..b0a6c18 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -12,8 +12,7 @@
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.TankDrive;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -23,11 +22,11 @@
  * project.
  */
 public class Robot extends TimedRobot {
-  public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
-  public static OI m_oi;
+  public static TankDrive tankDriveSubsystem = new TankDrive();
+  public static OI oi;
 
-  Command m_autonomousCommand;
-  SendableChooser<Command> m_chooser = new SendableChooser<>();
+  Command autonomousCommand;
+  SendableChooser<Command> chooser = new SendableChooser<>();
 
   /**
    * This function is run when the robot is first started up and should be
@@ -35,10 +34,9 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotInit() {
-    m_oi = new OI();
-    m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
+    oi = new OI();
     // chooser.addOption("My Auto", new MyAutoCommand());
-    SmartDashboard.putData("Auto mode", m_chooser);
+    SmartDashboard.putData("Auto mode", chooser);
   }
 
   /**
@@ -80,7 +78,7 @@ public void disabledPeriodic() {
    */
   @Override
   public void autonomousInit() {
-    m_autonomousCommand = m_chooser.getSelected();
+    autonomousCommand = chooser.getSelected();
 
     /*
      * String autoSelected = SmartDashboard.getString("Auto Selector",
@@ -90,8 +88,8 @@ public void autonomousInit() {
      */
 
     // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.start();
+    if (autonomousCommand != null) {
+      autonomousCommand.start();
     }
   }
 
@@ -109,8 +107,8 @@ public void teleopInit() {
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
+    if (autonomousCommand != null) {
+      autonomousCommand.cancel();
     }
   }
 
diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java
new file mode 100644
index 0000000..d1df910
--- /dev/null
+++ b/src/main/java/frc/robot/commands/DriveCommand.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.Robot;
+
+public class DriveCommand extends Command {
+  public DriveCommand() {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    requires(Robot.tankDriveSubsystem);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.tankDriveSubsystem.init();
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    Robot.tankDriveSubsystem.driveWithJoystick();
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+    Robot.tankDriveSubsystem.stop();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/SetConstant.java
similarity index 82%
rename from src/main/java/frc/robot/commands/ExampleCommand.java
rename to src/main/java/frc/robot/commands/SetConstant.java
index 83a3fcc..3afc3ed 100644
--- a/src/main/java/frc/robot/commands/ExampleCommand.java
+++ b/src/main/java/frc/robot/commands/SetConstant.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 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.                                                               */
@@ -10,23 +10,22 @@
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.Robot;
 
-/**
- * An example command.  You can replace me with your own command.
- */
-public class ExampleCommand extends Command {
-  public ExampleCommand() {
+public class SetConstant extends Command {
+  public SetConstant() {
     // Use requires() here to declare subsystem dependencies
-    requires(Robot.m_subsystem);
+    requires(Robot.tankDriveSubsystem);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    Robot.tankDriveSubsystem.init();
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    Robot.tankDriveSubsystem.driveWithConstant();
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
deleted file mode 100644
index 7f5e1a4..0000000
--- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc.robot.subsystems;
-
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * An example subsystem.  You can replace me with your own Subsystem.
- */
-public class ExampleSubsystem extends Subsystem {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
-  @Override
-  public void initDefaultCommand() {
-    // Set the default command for a subsystem here.
-    // setDefaultCommand(new MySpecialCommand());
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/TankDrive.java b/src/main/java/frc/robot/subsystems/TankDrive.java
new file mode 100644
index 0000000..a3794d1
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/TankDrive.java
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.                                                               */
+/*----------------------------------------------------------------------------*/
+package frc.robot.subsystems;
+
+
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commands.DriveCommand;
+import frc.robot.OI;
+
+public class TankDrive extends Subsystem {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+ 
+  public WPI_TalonSRX topLeft = new WPI_TalonSRX(16); //ports subject to change
+  public WPI_TalonSRX middleLeft = new WPI_TalonSRX(15); 
+  public WPI_TalonSRX rearLeft = new WPI_TalonSRX(14); 
+
+  SpeedControllerGroup leftSide = new SpeedControllerGroup(topLeft, middleLeft, rearLeft);
+
+  public WPI_TalonSRX topRight = new WPI_TalonSRX(12); //ports subject to change'
+  public WPI_TalonSRX middleRight = new WPI_TalonSRX(13); 
+  public WPI_TalonSRX rearRight = new WPI_TalonSRX(10); 
+
+  SpeedControllerGroup rightSide = new SpeedControllerGroup(topRight, middleRight, rearRight);
+
+  DifferentialDrive difDrive = new DifferentialDrive(leftSide, rightSide);
+
+  @Override
+  public void initDefaultCommand() {
+    // Set the default command for a subsystem here.
+    // setDefaultCommand(new MySpecialCommand());
+    setDefaultCommand(new DriveCommand());
+  }
+
+  public void init() { // middleLeft and middleRight motor must go in opposite directions from the rest of the motors.
+    middleLeft.follow(topLeft);
+    rearLeft.follow(topLeft);
+
+    middleLeft.setInverted(true); // inverted reverses the direction = goes in the opposite direction
+    rearLeft.setInverted(true);
+
+    middleRight.follow(topRight);
+    rearRight.follow(topRight); 
+
+    middleRight.setInverted(true);
+    rearRight.setInverted(true);
+  }
+
+  public void driveWithJoystick() {
+    double leftSpeed = OI.joyLeft.getY() * 0.8;
+    double rightSpeed = OI.joyRight.getY() * 0.8;
+
+    if(Math.abs(leftSpeed) < 0.2) 
+      leftSpeed = 0;
+
+    if(Math.abs(rightSpeed) < 0.2)
+      rightSpeed = 0;
+
+    /*if(Math.abs(leftSpeed) > 0.8)
+      leftSpeed = 0.8;
+
+    if(Math.abs(rightSpeed) > 0.8)
+      rightSpeed = 0.8;*/
+
+    SmartDashboard.putNumber("Front Left Motor:", topLeft.get());
+    SmartDashboard.putNumber("Middle Left Motor:", middleLeft.get());
+    SmartDashboard.putNumber("Rear Left Motor:", rearLeft.get());
+
+    SmartDashboard.putNumber("Front Right Motor:", topRight.get());
+    SmartDashboard.putNumber("Middle Right Motor:", middleRight.get());
+    SmartDashboard.putNumber("Rear Right Motor:", rearRight.get());
+
+    difDrive.tankDrive(leftSpeed, rightSpeed);
+  }
+
+  public void driveWithConstant() { // to be utilized when button 3 is pressed on Left Joystick (joyLeft)
+    double leftSpeed = 0.4;
+    double rightSpeed = 0.4;
+    difDrive.tankDrive(leftSpeed, rightSpeed);
+
+    SmartDashboard.putNumber("Front Left Motor:", topLeft.get());
+    SmartDashboard.putNumber("Middle Left Motor:", middleLeft.get());
+    SmartDashboard.putNumber("Rear Left Motor:", rearLeft.get());
+
+    SmartDashboard.putNumber("Front Right Motor:", topRight.get());
+    SmartDashboard.putNumber("Middle Right Motor:", middleRight.get());
+    SmartDashboard.putNumber("Rear Right Motor:", rearRight.get());
+  }
+
+  public void stop() {
+    topLeft.stopMotor();
+    middleLeft.stopMotor();
+    rearLeft.stopMotor();
+
+    topRight.stopMotor();
+    middleRight.stopMotor();
+    rearRight.stopMotor();
+  }
+  
+}
diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json
new file mode 100644
index 0000000..a1654ec
--- /dev/null
+++ b/vendordeps/Phoenix.json
@@ -0,0 +1,87 @@
+{
+    "fileName": "Phoenix.json",
+    "name": "CTRE-Phoenix",
+    "version": "5.12.1",
+    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+    "mavenUrls": [
+        "http://devsite.ctr-electronics.com/maven/release/"
+    ],
+    "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-java",
+            "version": "5.12.1"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-java",
+            "version": "5.12.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.12.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-cpp",
+            "version": "5.12.1",
+            "libName": "CTRE_Phoenix_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-cpp",
+            "version": "5.12.1",
+            "libName": "CTRE_Phoenix",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.12.1",
+            "libName": "CTRE_PhoenixCCI",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "core",
+            "version": "5.12.1",
+            "libName": "CTRE_PhoenixCore",
+            "headerClassifier": "headers"
+        }
+    ]
+}
\ No newline at end of file

From 3f6f39f6097ba9b78d29d4d94866a5e284feea2f Mon Sep 17 00:00:00 2001
From: larguello365 <larguello365@gmail.com>
Date: Mon, 4 Feb 2019 18:16:24 -0600
Subject: [PATCH 02/12] Encoder code for driving forward a specified distance

---
 src/main/java/frc/robot/Robot.java            |  2 +
 .../frc/robot/commands/EncoderCommand.java    | 42 +++++++++++++++++++
 .../frc/robot/subsystems/EncoderTest.java     | 39 +++++++++++++++++
 3 files changed, 83 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/EncoderCommand.java
 create mode 100644 src/main/java/frc/robot/subsystems/EncoderTest.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index b0a6c18..d61312c 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.subsystems.EncoderTest;
 import frc.robot.subsystems.TankDrive;
 
 /**
@@ -23,6 +24,7 @@
  */
 public class Robot extends TimedRobot {
   public static TankDrive tankDriveSubsystem = new TankDrive();
+  public static EncoderTest encoderSub = new EncoderTest();
   public static OI oi;
 
   Command autonomousCommand;
diff --git a/src/main/java/frc/robot/commands/EncoderCommand.java b/src/main/java/frc/robot/commands/EncoderCommand.java
new file mode 100644
index 0000000..0724eac
--- /dev/null
+++ b/src/main/java/frc/robot/commands/EncoderCommand.java
@@ -0,0 +1,42 @@
+package frc.robot.commands;
+
+import frc.robot.Robot;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class EncoderCommand extends Command {
+    public EncoderCommand() {
+        requires(Robot.encoderSub);
+    }
+    // Called just before this Command runs the first time
+    @Override
+    protected void initialize() {
+        Robot.encoderSub.talon.setSelectedSensorPosition(0);
+
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    @Override
+    protected void execute() {
+        SmartDashboard.putNumber("Position", Robot.encoderSub.getPosition() );
+        Robot.encoderSub.goStraight(10); //Parameter is a placeholder
+
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return false;
+    }
+
+    // Called once after isFinished returns true
+    @Override
+    protected void end() {
+    
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    @Override
+    protected void interrupted() {
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/EncoderTest.java b/src/main/java/frc/robot/subsystems/EncoderTest.java
new file mode 100644
index 0000000..4d74c26
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/EncoderTest.java
@@ -0,0 +1,39 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.Robot;
+import frc.robot.commands.EncoderCommand;
+
+
+public class EncoderTest extends Subsystem {
+    public WPI_TalonSRX talon = new WPI_TalonSRX(15);
+
+    public EncoderTest() {
+        talon.setSensorPhase(true);
+        talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10);
+        talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10);
+    }
+
+    public double getPosition() {
+        return (double)talon.getSelectedSensorPosition();    
+    }
+
+    public void goStraight (int distance) {
+        Robot.tankDriveSubsystem.driveWithConstant(); //Drives with 0.4 speed
+        double rotations = distance / (15.24 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance
+        double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
+        if (getPosition() >= units) { //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
+            Robot.tankDriveSubsystem.stop(); //stops all motors
+        } 
+    }
+
+    @Override
+    protected void initDefaultCommand() {
+        setDefaultCommand(new EncoderCommand());
+    }
+
+}
\ No newline at end of file

From b227deb2cdaf2bedaf905912b74fc0a8dca52520 Mon Sep 17 00:00:00 2001
From: Yamilett Estrada <36552200+yyestrada-re@users.noreply.github.com>
Date: Mon, 4 Feb 2019 21:56:22 -0600
Subject: [PATCH 03/12] added turnToAngle method to EncoderTest

---
 src/main/java/frc/robot/subsystems/EncoderTest.java | 10 ++++++++--
 src/main/java/frc/robot/subsystems/TankDrive.java   |  2 --
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/EncoderTest.java b/src/main/java/frc/robot/subsystems/EncoderTest.java
index 4d74c26..e4b4f7a 100644
--- a/src/main/java/frc/robot/subsystems/EncoderTest.java
+++ b/src/main/java/frc/robot/subsystems/EncoderTest.java
@@ -22,15 +22,21 @@ public double getPosition() {
         return (double)talon.getSelectedSensorPosition();    
     }
 
-    public void goStraight (int distance) {
+    public void goStraight (double distance) { //where 15.24 is the circumference of the wheels and distance is in cm
         Robot.tankDriveSubsystem.driveWithConstant(); //Drives with 0.4 speed
-        double rotations = distance / (15.24 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance
+        double rotations = distance / (15.24 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (cm) (rotations = distance/circumference)
         double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
         if (getPosition() >= units) { //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
             Robot.tankDriveSubsystem.stop(); //stops all motors
         } 
     }
 
+    public void turnToAngle(int angleOfTurn) { /* new edits */
+        double circumferenceOfRobot = 57.76; //units are in centimeter
+        double distance = (double)(circumferenceOfRobot * (angleOfTurn/360)); //gets the distance needed to turn
+        goStraight(distance);
+    }
+
     @Override
     protected void initDefaultCommand() {
         setDefaultCommand(new EncoderCommand());
diff --git a/src/main/java/frc/robot/subsystems/TankDrive.java b/src/main/java/frc/robot/subsystems/TankDrive.java
index a3794d1..755a9a7 100644
--- a/src/main/java/frc/robot/subsystems/TankDrive.java
+++ b/src/main/java/frc/robot/subsystems/TankDrive.java
@@ -6,8 +6,6 @@
 /*----------------------------------------------------------------------------*/
 package frc.robot.subsystems;
 
-
-
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 import edu.wpi.first.wpilibj.SpeedControllerGroup;

From fa2d48aa7f460615289a8cb5ca1443e37670bcad Mon Sep 17 00:00:00 2001
From: Yamilett Estrada <36552200+yyestrada-re@users.noreply.github.com>
Date: Mon, 4 Feb 2019 21:57:47 -0600
Subject: [PATCH 04/12] Update EncoderTest.java

---
 src/main/java/frc/robot/subsystems/EncoderTest.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/EncoderTest.java b/src/main/java/frc/robot/subsystems/EncoderTest.java
index e4b4f7a..255de31 100644
--- a/src/main/java/frc/robot/subsystems/EncoderTest.java
+++ b/src/main/java/frc/robot/subsystems/EncoderTest.java
@@ -33,7 +33,7 @@ public void goStraight (double distance) { //where 15.24 is the circumference of
 
     public void turnToAngle(int angleOfTurn) { /* new edits */
         double circumferenceOfRobot = 57.76; //units are in centimeter
-        double distance = (double)(circumferenceOfRobot * (angleOfTurn/360)); //gets the distance needed to turn
+        double distance = (double)(circumferenceOfRobot * (angleOfTurn/360)); //gets the distance needed to turn a specified angle
         goStraight(distance);
     }
 
@@ -42,4 +42,4 @@ protected void initDefaultCommand() {
         setDefaultCommand(new EncoderCommand());
     }
 
-}
\ No newline at end of file
+}

From 79fafa45384ab07b36c06cbde917543ee728e320 Mon Sep 17 00:00:00 2001
From: Yamilett Estrada <36552200+yyestrada-re@users.noreply.github.com>
Date: Wed, 6 Feb 2019 15:24:02 -0600
Subject: [PATCH 05/12] Added gyroscope code

---
 src/main/java/frc/robot/OI.java               |  8 +---
 src/main/java/frc/robot/Robot.java            |  2 +
 .../frc/robot/commands/EncoderCommand.java    |  4 +-
 .../{SetConstant.java => GoToAngle.java}      | 32 +++++++++++----
 .../frc/robot/subsystems/EncoderTest.java     | 29 ++++++++-----
 .../java/frc/robot/subsystems/Gyroscope.java  | 34 +++++++++++++++
 .../java/frc/robot/subsystems/TankDrive.java  | 41 +++++++++++++++----
 7 files changed, 113 insertions(+), 37 deletions(-)
 rename src/main/java/frc/robot/commands/{SetConstant.java => GoToAngle.java} (55%)
 create mode 100644 src/main/java/frc/robot/subsystems/Gyroscope.java

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 92dd319..3837a83 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -10,7 +10,6 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
-import frc.robot.commands.SetConstant;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -28,7 +27,7 @@ public class OI {
   public static Joystick joyLeft = new Joystick(0); // port subject to change
   public static Joystick joyRight = new Joystick(1);
 
-  public static Button button = new JoystickButton(joyLeft, 3);
+  
   // There are a few additional built in buttons you can use. Additionally,
   // by subclassing Button you can create custom triggers and bind those to
   // commands the same as any other Button.
@@ -44,9 +43,6 @@ public class OI {
   // Run the command while the button is being held down and interrupt it once
   // the button is released.
   public OI() {
-    button.whileHeld(new SetConstant()); 
-  // Start the command when the button is released and let it run the command
-  // until it is finished as determined by it's isFinished method.
-    button.whenReleased(new SetConstant());
+    
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index d61312c..6b11446 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.EncoderTest;
+import frc.robot.subsystems.Gyroscope;
 import frc.robot.subsystems.TankDrive;
 
 /**
@@ -25,6 +26,7 @@
 public class Robot extends TimedRobot {
   public static TankDrive tankDriveSubsystem = new TankDrive();
   public static EncoderTest encoderSub = new EncoderTest();
+  public static Gyroscope gyroSub = new Gyroscope();
   public static OI oi;
 
   Command autonomousCommand;
diff --git a/src/main/java/frc/robot/commands/EncoderCommand.java b/src/main/java/frc/robot/commands/EncoderCommand.java
index 0724eac..bae63b1 100644
--- a/src/main/java/frc/robot/commands/EncoderCommand.java
+++ b/src/main/java/frc/robot/commands/EncoderCommand.java
@@ -12,15 +12,13 @@ public EncoderCommand() {
     @Override
     protected void initialize() {
         Robot.encoderSub.talon.setSelectedSensorPosition(0);
-
     }
 
     // Called repeatedly when this Command is scheduled to run
     @Override
     protected void execute() {
         SmartDashboard.putNumber("Position", Robot.encoderSub.getPosition() );
-        Robot.encoderSub.goStraight(10); //Parameter is a placeholder
-
+        Robot.encoderSub.goStraight(10); //Parameter is a placeholder = 10 inch
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/SetConstant.java b/src/main/java/frc/robot/commands/GoToAngle.java
similarity index 55%
rename from src/main/java/frc/robot/commands/SetConstant.java
rename to src/main/java/frc/robot/commands/GoToAngle.java
index 3afc3ed..8393b2e 100644
--- a/src/main/java/frc/robot/commands/SetConstant.java
+++ b/src/main/java/frc/robot/commands/GoToAngle.java
@@ -7,41 +7,57 @@
 
 package frc.robot.commands;
 
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 
-public class SetConstant extends Command {
-  public SetConstant() {
-    // Use requires() here to declare subsystem dependencies
-    requires(Robot.tankDriveSubsystem);
+public class GoToAngle extends Command {
+
+  public double finalAngle;
+  public double finalAngle1;
+  public double specifiedAngle;
+ 
+  public GoToAngle(double pfinalAngle1, double pfinalAngle2, double pSpecifiedAngle) {
+    requires(Robot.gyroSub);
+
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    Robot.tankDriveSubsystem.init();
+    Robot.gyroSub.gyro.reset();
+    Robot.tankDriveSubsystem.driveToAngle(specifiedAngle); //autonomous?
+    //Robot.gyroSub.motor.set(0.5);
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    Robot.tankDriveSubsystem.driveWithConstant();
+    SmartDashboard.putNumber("Current angle ", Robot.gyroSub.gyro.getAngle());
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+    SmartDashboard.putBoolean("finished", (Robot.gyroSub.gyro.getAngle() >= finalAngle  && Robot.gyroSub.gyro.getAngle() <= finalAngle1));
+    return (Robot.gyroSub.gyro.getAngle() >= finalAngle && Robot.gyroSub.gyro.getAngle() <= finalAngle1);
   }
 
   // Called once after isFinished returns true
   @Override
   protected void end() {
+    Robot.tankDriveSubsystem.stop();
+    Robot.gyroSub.stop();
+    
   }
 
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   @Override
   protected void interrupted() {
+    Robot.tankDriveSubsystem.stop();
+    Robot.gyroSub.stop();
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/EncoderTest.java b/src/main/java/frc/robot/subsystems/EncoderTest.java
index e4b4f7a..ae8d7be 100644
--- a/src/main/java/frc/robot/subsystems/EncoderTest.java
+++ b/src/main/java/frc/robot/subsystems/EncoderTest.java
@@ -5,9 +5,8 @@
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 import edu.wpi.first.wpilibj.command.Subsystem;
-import frc.robot.Robot;
 import frc.robot.commands.EncoderCommand;
-
+import frc.robot.Robot;
 
 public class EncoderTest extends Subsystem {
     public WPI_TalonSRX talon = new WPI_TalonSRX(15);
@@ -22,19 +21,27 @@ public double getPosition() {
         return (double)talon.getSelectedSensorPosition();    
     }
 
-    public void goStraight (double distance) { //where 15.24 is the circumference of the wheels and distance is in cm
-        Robot.tankDriveSubsystem.driveWithConstant(); //Drives with 0.4 speed
-        double rotations = distance / (15.24 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (cm) (rotations = distance/circumference)
+    public void goStraight(double distance) { //where 15.24 is the circumference of the wheels and distance is in in
+        double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
         double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
-        if (getPosition() >= units) { //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
+        Robot.tankDriveSubsystem.driveForward(); //Drives with 0.5 speed
+        if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
             Robot.tankDriveSubsystem.stop(); //stops all motors
-        } 
     }
 
-    public void turnToAngle(int angleOfTurn) { /* new edits */
-        double circumferenceOfRobot = 57.76; //units are in centimeter
-        double distance = (double)(circumferenceOfRobot * (angleOfTurn/360)); //gets the distance needed to turn
-        goStraight(distance);
+    public void turn(double distance, double angle) { 
+        double rotations = distance / (6 * Math.PI); 
+        double units = 400 * rotations;
+        Robot.tankDriveSubsystem.driveToAngle(angle); // depending on the angle, the speeds of the left/right sides of the tank drive will differ
+        if (getPosition() >= units) 
+            Robot.tankDriveSubsystem.stop(); 
+    }
+
+    public void distanceToAngle(double angle) { //using encoder
+        double circumferenceOfRobot = 22.74; //units are in centimeter
+        double distance = (double)(circumferenceOfRobot * (angle/360)); //gets the distance needed to turn
+        turn(distance, angle); //to turn, one side of the tankDrive must go faster than the opposing side. 
+        /* 90 degrees clockwise = 270 degrees cc*/
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/Gyroscope.java b/src/main/java/frc/robot/subsystems/Gyroscope.java
new file mode 100644
index 0000000..52d5e0b
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Gyroscope.java
@@ -0,0 +1,34 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.Robot;
+import frc.robot.commands.GoToAngle;
+
+public class Gyroscope extends Subsystem {
+
+  int count = 0;
+  public double finalAngle;
+  public double finalAngle1;
+  public double specifiedAngle;
+  public ADXRS450_Gyro gyro = new ADXRS450_Gyro();
+  
+  //public WPI_TalonSRX motor = new WPI_TalonSRX(11);
+
+  public Gyroscope() {
+    gyro.calibrate();
+  }
+  
+  public void reset() {
+    gyro.reset();
+  } 
+
+  @Override
+  public void initDefaultCommand() { 
+    setDefaultCommand(new GoToAngle(10,11,10));
+  }
+  public void stop() {
+    Robot.gyroSub.stop();
+    Robot.tankDriveSubsystem.stop();
+  }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/TankDrive.java b/src/main/java/frc/robot/subsystems/TankDrive.java
index 755a9a7..933201d 100644
--- a/src/main/java/frc/robot/subsystems/TankDrive.java
+++ b/src/main/java/frc/robot/subsystems/TankDrive.java
@@ -14,20 +14,21 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.DriveCommand;
 import frc.robot.OI;
+import frc.robot.Robot;
 
 public class TankDrive extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
  
-  public WPI_TalonSRX topLeft = new WPI_TalonSRX(16); //ports subject to change
-  public WPI_TalonSRX middleLeft = new WPI_TalonSRX(15); 
-  public WPI_TalonSRX rearLeft = new WPI_TalonSRX(14); 
+  public WPI_TalonSRX topLeft = new WPI_TalonSRX(3); //ports subject to change (16)
+  public WPI_TalonSRX middleLeft = new WPI_TalonSRX(4); //15
+  public WPI_TalonSRX rearLeft = new WPI_TalonSRX(10); //14
 
   SpeedControllerGroup leftSide = new SpeedControllerGroup(topLeft, middleLeft, rearLeft);
 
-  public WPI_TalonSRX topRight = new WPI_TalonSRX(12); //ports subject to change'
-  public WPI_TalonSRX middleRight = new WPI_TalonSRX(13); 
-  public WPI_TalonSRX rearRight = new WPI_TalonSRX(10); 
+  public WPI_TalonSRX topRight = new WPI_TalonSRX(5); //ports subject to change' (12)
+  public WPI_TalonSRX middleRight = new WPI_TalonSRX(13); //13
+  public WPI_TalonSRX rearRight = new WPI_TalonSRX(1); //10
 
   SpeedControllerGroup rightSide = new SpeedControllerGroup(topRight, middleRight, rearRight);
 
@@ -81,9 +82,31 @@ public void driveWithJoystick() {
     difDrive.tankDrive(leftSpeed, rightSpeed);
   }
 
-  public void driveWithConstant() { // to be utilized when button 3 is pressed on Left Joystick (joyLeft)
-    double leftSpeed = 0.4;
-    double rightSpeed = 0.4;
+  public void driveForward() { // to be utilized when button 3 is pressed on Left Joystick (joyLeft)
+    double leftSpeed = 0.5;
+    double rightSpeed = 0.5;
+    difDrive.tankDrive(leftSpeed, rightSpeed);
+
+    SmartDashboard.putNumber("Front Left Motor:", topLeft.get());
+    SmartDashboard.putNumber("Middle Left Motor:", middleLeft.get());
+    SmartDashboard.putNumber("Rear Left Motor:", rearLeft.get());
+
+    SmartDashboard.putNumber("Front Right Motor:", topRight.get());
+    SmartDashboard.putNumber("Middle Right Motor:", middleRight.get());
+    SmartDashboard.putNumber("Rear Right Motor:", rearRight.get());
+  }
+
+  public void driveToAngle(double angle) { // to be utilized when button 3 is pressed on Left Joystick (joyLeft)
+    double leftSpeed, rightSpeed;
+    //the speedcontroller changes depending on what angle
+    if(angle < 180.0) { //denotes it turning right. left side tank drive speed will be faster.
+      leftSpeed = 0.6;
+      rightSpeed = 0.5;
+    }
+    else { //denotes it turning left. right side tank drive will be faster
+      rightSpeed = 0.6;
+      leftSpeed = 0.5;
+    }
     difDrive.tankDrive(leftSpeed, rightSpeed);
 
     SmartDashboard.putNumber("Front Left Motor:", topLeft.get());

From 59c057c63bff01406853651e87ef412a67774750 Mon Sep 17 00:00:00 2001
From: larguello365 <larguello365@gmail.com>
Date: Wed, 6 Feb 2019 17:42:50 -0600
Subject: [PATCH 06/12] Update

---
 src/main/java/frc/robot/Robot.java            |  2 +
 .../frc/robot/commands/AutonDriveCommand.java | 45 +++++++++
 .../java/frc/robot/subsystems/PIDControl.java | 91 +++++++++++++++++++
 3 files changed, 138 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/AutonDriveCommand.java
 create mode 100644 src/main/java/frc/robot/subsystems/PIDControl.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 6b11446..80d776e 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.EncoderTest;
 import frc.robot.subsystems.Gyroscope;
+import frc.robot.subsystems.PIDControl;
 import frc.robot.subsystems.TankDrive;
 
 /**
@@ -27,6 +28,7 @@ public class Robot extends TimedRobot {
   public static TankDrive tankDriveSubsystem = new TankDrive();
   public static EncoderTest encoderSub = new EncoderTest();
   public static Gyroscope gyroSub = new Gyroscope();
+  public static PIDControl pidSub = new PIDControl(10);//place holder for distance
   public static OI oi;
 
   Command autonomousCommand;
diff --git a/src/main/java/frc/robot/commands/AutonDriveCommand.java b/src/main/java/frc/robot/commands/AutonDriveCommand.java
new file mode 100644
index 0000000..f66e586
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AutonDriveCommand.java
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.Robot;
+
+public class AutonDriveCommand extends Command {
+  public AutonDriveCommand() {
+    // Use requires() here to declare subsystem dependencies
+    requires(Robot.pidSub);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return Robot.pidSub.encoderPID.onTarget();
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/PIDControl.java b/src/main/java/frc/robot/subsystems/PIDControl.java
new file mode 100644
index 0000000..612e42d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/PIDControl.java
@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
+import frc.robot.Robot;
+import frc.robot.commands.AutonDriveCommand;
+import frc.robot.commands.DriveCommand;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.PIDController;
+
+/**
+ * Add your docs here.
+ */
+public class PIDControl extends PIDSubsystem {
+  /**
+   * Add your docs here.
+   */
+  public PIDController encoderPID = getPIDController();
+  
+  public WPI_TalonSRX talon = new WPI_TalonSRX(15);
+
+  public PIDControl(double distance) {
+    // Insert a subsystem name and PID values here
+    super("EncoderTest", 0, 0, 0);
+    talon.setSensorPhase(true);
+    talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10);
+    talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10);
+    setSetpoint(distance);      // Sets where the PID controller should move the system
+    enable(); // Enables the PID controller.
+  }
+
+  @Override
+  public void initDefaultCommand() {
+    // Set the default command for a subsystem here.
+    setDefaultCommand(new AutonDriveCommand());
+  }
+
+  public double getPosition() {
+    return (double)talon.getSelectedSensorPosition();    
+  }
+
+  public void goStraight(double distance) { //where 15.24 is the circumference of the wheels and distance is in in
+    double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
+    double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
+    Robot.tankDriveSubsystem.driveForward(); //Drives with 0.5 speed
+    if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
+        Robot.tankDriveSubsystem.stop(); //stops all motors
+  }
+
+  public void turnToAngle(double distance, double angle) { //where 15.24 is the circumference of the wheels and distance is in in
+    double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
+    double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
+    Robot.tankDriveSubsystem.driveToAngle(angle); //Drives with 0.5 speed
+    if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
+        Robot.tankDriveSubsystem.stop(); //stops all motors
+  }
+
+  public void turnToAngle(double angleOfTurn) { /* new edits */
+    double circumferenceOfRobot = 57.76; //units are in centimeter
+    double distance = (double)(circumferenceOfRobot * (angleOfTurn/360)); //gets the distance needed to turn a specified angle
+    turnToAngle(distance, angleOfTurn);;
+  }
+
+  public void reset() {
+    encoderPID.reset();
+  }
+
+  @Override
+  protected double returnPIDInput() {
+    // Return your input value for the PID loop
+    // e.g. a sensor, like a potentiometer:
+    // yourPot.getAverageVoltage() / kYourMaxVoltage;
+    return 0.0;
+  }
+
+  @Override
+  protected void usePIDOutput(double output) {
+    // Use output to drive your system, like a motor
+    // e.g. yourMotor.set(output);
+  }
+}

From 1c8cad028958484a29bf2159729348e9ca0f5e4f Mon Sep 17 00:00:00 2001
From: larguello365 <larguello365@gmail.com>
Date: Wed, 6 Feb 2019 18:36:42 -0600
Subject: [PATCH 07/12] update

---
 .../java/frc/robot/subsystems/PIDControl.java   | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/PIDControl.java b/src/main/java/frc/robot/subsystems/PIDControl.java
index 612e42d..91c5d0f 100644
--- a/src/main/java/frc/robot/subsystems/PIDControl.java
+++ b/src/main/java/frc/robot/subsystems/PIDControl.java
@@ -36,6 +36,7 @@ public PIDControl(double distance) {
     talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10);
     talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10);
     setSetpoint(distance);      // Sets where the PID controller should move the system
+    setPercentTolerance(5);
     enable(); // Enables the PID controller.
   }
 
@@ -49,12 +50,20 @@ public double getPosition() {
     return (double)talon.getSelectedSensorPosition();    
   }
 
-  public void goStraight(double distance) { //where 15.24 is the circumference of the wheels and distance is in in
-    double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
+  public void goStraight() { //where 6 is the diameter of the wheels and distance is in in
+    double i = 0;
+    double d = 0;
+    double lastError = 0;
+    double rotations = getSetpoint() / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
     double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
     Robot.tankDriveSubsystem.driveForward(); //Drives with 0.5 speed
-    if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
-        Robot.tankDriveSubsystem.stop(); //stops all motors
+    double p = units - getPosition(); //Represents p, how far we are from the setPosition
+    i += p; // Represents the sum of all error values
+    d = p - lastError;
+    lastError = p;
+    /*if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
+        Robot.tankDriveSubsystem.stop(); //stops all motors */
+        
   }
 
   public void turnToAngle(double distance, double angle) { //where 15.24 is the circumference of the wheels and distance is in in

From 7a52af42b0362900b32869fa3c53db429f077b74 Mon Sep 17 00:00:00 2001
From: larguello365 <larguello365@gmail.com>
Date: Thu, 7 Feb 2019 16:09:26 -0600
Subject: [PATCH 08/12] moved initDefaultCommand and added encoderPID. in
 PIDControl

---
 .../java/frc/robot/subsystems/EncoderTest.java  | 10 ++++++----
 .../java/frc/robot/subsystems/PIDControl.java   | 17 ++++++-----------
 2 files changed, 12 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/EncoderTest.java b/src/main/java/frc/robot/subsystems/EncoderTest.java
index d8008bf..570c0ed 100644
--- a/src/main/java/frc/robot/subsystems/EncoderTest.java
+++ b/src/main/java/frc/robot/subsystems/EncoderTest.java
@@ -17,6 +17,11 @@ public EncoderTest() {
         talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10);
     }
 
+    @Override
+    protected void initDefaultCommand() {
+        setDefaultCommand(new EncoderCommand());
+    }
+
     public double getPosition() {
         return (double)talon.getSelectedSensorPosition();    
     }
@@ -43,9 +48,6 @@ public void turnToAngle(double angleOfTurn) { /* new edits */
         turnToAngle(distance, angleOfTurn);;
     }
 
-    @Override
-    protected void initDefaultCommand() {
-        setDefaultCommand(new EncoderCommand());
-    }
+
 
 }
diff --git a/src/main/java/frc/robot/subsystems/PIDControl.java b/src/main/java/frc/robot/subsystems/PIDControl.java
index 91c5d0f..e236aac 100644
--- a/src/main/java/frc/robot/subsystems/PIDControl.java
+++ b/src/main/java/frc/robot/subsystems/PIDControl.java
@@ -35,9 +35,9 @@ public PIDControl(double distance) {
     talon.setSensorPhase(true);
     talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10);
     talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10);
-    setSetpoint(distance);      // Sets where the PID controller should move the system
-    setPercentTolerance(5);
-    enable(); // Enables the PID controller.
+    encoderPID.setSetpoint(distance);      // Sets where the PID controller should move the system
+    encoderPID.setPercentTolerance(5);
+    encoderPID.enable(); // Enables the PID controller.
   }
 
   @Override
@@ -51,18 +51,13 @@ public double getPosition() {
   }
 
   public void goStraight() { //where 6 is the diameter of the wheels and distance is in in
-    double i = 0;
-    double d = 0;
-    double lastError = 0;
     double rotations = getSetpoint() / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
     double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
     Robot.tankDriveSubsystem.driveForward(); //Drives with 0.5 speed
     double p = units - getPosition(); //Represents p, how far we are from the setPosition
-    i += p; // Represents the sum of all error values
-    d = p - lastError;
-    lastError = p;
-    /*if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
-        Robot.tankDriveSubsystem.stop(); //stops all motors */
+    if (encoderPID.onTarget()) {
+      Robot.tankDriveSubsystem.stop();
+    }
         
   }
 

From 5099bc493b1899ef5c9f65de7ac43bfad6595f1a Mon Sep 17 00:00:00 2001
From: Alexa Lagunas <amlagunas04@gmail.com>
Date: Thu, 7 Feb 2019 16:31:22 -0600
Subject: [PATCH 09/12] commeted out initdefaultcmmands

---
 src/main/java/frc/robot/OI.java                     | 2 +-
 src/main/java/frc/robot/commands/DriveCommand.java  | 1 +
 src/main/java/frc/robot/subsystems/EncoderTest.java | 2 +-
 src/main/java/frc/robot/subsystems/Gyroscope.java   | 2 +-
 4 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 903344c..3529bb0 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -30,7 +30,7 @@ public class OI {
   // commands the same as any other Button.
 
   //// TRIGGERING COMMANDS WITH BUTTONS
-  // Once you have a button, it's trivial to bind it to a button in one of
+  // Once you have a button, it's trivial to bind it to a button in oneP of
   // three ways:
 
   // Start the command when the button is pressed and let it run the command
diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java
index d1df910..c29bf72 100644
--- a/src/main/java/frc/robot/commands/DriveCommand.java
+++ b/src/main/java/frc/robot/commands/DriveCommand.java
@@ -8,6 +8,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 
 public class DriveCommand extends Command {
diff --git a/src/main/java/frc/robot/subsystems/EncoderTest.java b/src/main/java/frc/robot/subsystems/EncoderTest.java
index d8008bf..8bf2489 100644
--- a/src/main/java/frc/robot/subsystems/EncoderTest.java
+++ b/src/main/java/frc/robot/subsystems/EncoderTest.java
@@ -45,7 +45,7 @@ public void turnToAngle(double angleOfTurn) { /* new edits */
 
     @Override
     protected void initDefaultCommand() {
-        setDefaultCommand(new EncoderCommand());
+        //setDefaultCommand(new EncoderCommand());
     }
 
 }
diff --git a/src/main/java/frc/robot/subsystems/Gyroscope.java b/src/main/java/frc/robot/subsystems/Gyroscope.java
index 52d5e0b..74debe6 100644
--- a/src/main/java/frc/robot/subsystems/Gyroscope.java
+++ b/src/main/java/frc/robot/subsystems/Gyroscope.java
@@ -25,7 +25,7 @@ public void reset() {
 
   @Override
   public void initDefaultCommand() { 
-    setDefaultCommand(new GoToAngle(10,11,10));
+    //setDefaultCommand(new GoToAngle(10,11,10));
   }
   public void stop() {
     Robot.gyroSub.stop();

From c9cd5369ecd8314a20f06e301baccd4bb0bd1d6e Mon Sep 17 00:00:00 2001
From: larguello365 <larguello365@gmail.com>
Date: Thu, 7 Feb 2019 17:02:58 -0600
Subject: [PATCH 10/12] changed goStraight

---
 .../java/frc/robot/subsystems/PIDControl.java | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/PIDControl.java b/src/main/java/frc/robot/subsystems/PIDControl.java
index e236aac..1e5dd70 100644
--- a/src/main/java/frc/robot/subsystems/PIDControl.java
+++ b/src/main/java/frc/robot/subsystems/PIDControl.java
@@ -28,14 +28,18 @@ public class PIDControl extends PIDSubsystem {
   public PIDController encoderPID = getPIDController();
   
   public WPI_TalonSRX talon = new WPI_TalonSRX(15);
+  public double dis;
 
-  public PIDControl(double distance) {
+  public PIDControl(double distance) {//in inches {
     // Insert a subsystem name and PID values here
     super("EncoderTest", 0, 0, 0);
     talon.setSensorPhase(true);
     talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10);
     talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10);
-    encoderPID.setSetpoint(distance);      // Sets where the PID controller should move the system
+    dis = distance;
+    double rotations = distance / (6 * Math.PI);
+    double units = 360 * rotations; //encoder units
+    encoderPID.setSetpoint(units);
     encoderPID.setPercentTolerance(5);
     encoderPID.enable(); // Enables the PID controller.
   }
@@ -51,19 +55,15 @@ public double getPosition() {
   }
 
   public void goStraight() { //where 6 is the diameter of the wheels and distance is in in
-    double rotations = getSetpoint() / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
-    double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
     Robot.tankDriveSubsystem.driveForward(); //Drives with 0.5 speed
-    double p = units - getPosition(); //Represents p, how far we are from the setPosition
     if (encoderPID.onTarget()) {
       Robot.tankDriveSubsystem.stop();
-    }
-        
+    }      
   }
 
   public void turnToAngle(double distance, double angle) { //where 15.24 is the circumference of the wheels and distance is in in
     double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
-    double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
+    double units = 360 * rotations; // gets the total number of raw sensor units correlated with specified distance
     Robot.tankDriveSubsystem.driveToAngle(angle); //Drives with 0.5 speed
     if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
         Robot.tankDriveSubsystem.stop(); //stops all motors
@@ -84,12 +84,13 @@ protected double returnPIDInput() {
     // Return your input value for the PID loop
     // e.g. a sensor, like a potentiometer:
     // yourPot.getAverageVoltage() / kYourMaxVoltage;
-    return 0.0;
+    return talon.getSelectedSensorPosition(0);
   }
 
   @Override
   protected void usePIDOutput(double output) {
     // Use output to drive your system, like a motor
     // e.g. yourMotor.set(output);
+     talon.pidWrite(output);
   }
 }

From 0246bcf71cd78802740c8382f2f92764c4472c55 Mon Sep 17 00:00:00 2001
From: Yamilett Estrada <36552200+yyestrada-re@users.noreply.github.com>
Date: Sat, 16 Feb 2019 09:24:44 -0600
Subject: [PATCH 11/12] inverted rightSpeed on driveForward()

---
 src/main/java/frc/robot/subsystems/TankDrive.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/TankDrive.java b/src/main/java/frc/robot/subsystems/TankDrive.java
index dca1c83..f7af9db 100644
--- a/src/main/java/frc/robot/subsystems/TankDrive.java
+++ b/src/main/java/frc/robot/subsystems/TankDrive.java
@@ -83,7 +83,7 @@ public void driveWithJoystick() {
 
   public void driveForward() { // to be utilized when button 3 is pressed on Left Joystick (joyLeft)
     double leftSpeed = 0.5;
-    double rightSpeed = 0.5;
+    double rightSpeed = -0.5;
     difDrive.tankDrive(leftSpeed, rightSpeed);
 
     SmartDashboard.putNumber("Front Left Motor:", topLeft.get());

From 2101e7b65625902371bebbb30d4e26c7b4cc7fd6 Mon Sep 17 00:00:00 2001
From: Yamilett Estrada <36552200+yyestrada-re@users.noreply.github.com>
Date: Tue, 19 Feb 2019 16:26:49 -0600
Subject: [PATCH 12/12] commented each class

---
 build.gradle                                  |  2 +-
 src/main/java/frc/robot/RobotMap.java         |  4 +++
 .../frc/robot/commands/AutonDriveCommand.java |  4 +--
 .../java/frc/robot/commands/DriveCommand.java |  2 +-
 .../frc/robot/commands/EncoderCommand.java    | 11 +++----
 .../java/frc/robot/commands/GoToAngle.java    |  5 ++--
 .../frc/robot/subsystems/EncoderTest.java     | 29 +++++++++----------
 .../java/frc/robot/subsystems/Gyroscope.java  |  2 +-
 .../java/frc/robot/subsystems/PIDControl.java |  2 +-
 .../java/frc/robot/subsystems/TankDrive.java  | 22 +++++++-------
 10 files changed, 42 insertions(+), 41 deletions(-)

diff --git a/build.gradle b/build.gradle
index 2a7ec8c..abdb212 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2019.2.1"
+    id "edu.wpi.first.GradleRIO" version "2019.3.2"
 }
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 45360e4..48826fb 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -7,6 +7,8 @@
 
 package frc.robot;
 
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
 /**
  * The RobotMap is a mapping from the ports sensors and actuators are wired into
  * to a variable name. This provides flexibility changing wiring, makes checking
@@ -23,4 +25,6 @@ public class RobotMap {
   // number and the module. For example you with a rangefinder:
   // public static int rangefinderPort = 1;
   // public static int rangefinderModule = 1;
+
+  public static WPI_TalonSRX TANK_DRIVE_TOP_LEFT = new WPI_TalonSRX(15); //port # not yet known? to be attached with encoder
 }
diff --git a/src/main/java/frc/robot/commands/AutonDriveCommand.java b/src/main/java/frc/robot/commands/AutonDriveCommand.java
index f66e586..d6efa45 100644
--- a/src/main/java/frc/robot/commands/AutonDriveCommand.java
+++ b/src/main/java/frc/robot/commands/AutonDriveCommand.java
@@ -10,7 +10,7 @@
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.Robot;
 
-public class AutonDriveCommand extends Command {
+public class AutonDriveCommand extends Command { // yet to be tested
   public AutonDriveCommand() {
     // Use requires() here to declare subsystem dependencies
     requires(Robot.pidSub);
@@ -29,7 +29,7 @@ protected void execute() {
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return Robot.pidSub.encoderPID.onTarget();
+    return Robot.pidSub.encoderPID.onTarget(); // stop when PID reads that we've met designated position. onTarget() will == true
   }
 
   // Called once after isFinished returns true
diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java
index c29bf72..d63c621 100644
--- a/src/main/java/frc/robot/commands/DriveCommand.java
+++ b/src/main/java/frc/robot/commands/DriveCommand.java
@@ -11,7 +11,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 
-public class DriveCommand extends Command {
+public class DriveCommand extends Command { // yet to be tested
   public DriveCommand() {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
diff --git a/src/main/java/frc/robot/commands/EncoderCommand.java b/src/main/java/frc/robot/commands/EncoderCommand.java
index bae63b1..88f4560 100644
--- a/src/main/java/frc/robot/commands/EncoderCommand.java
+++ b/src/main/java/frc/robot/commands/EncoderCommand.java
@@ -1,24 +1,25 @@
 package frc.robot.commands;
 
 import frc.robot.Robot;
+import frc.robot.RobotMap;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-public class EncoderCommand extends Command {
+public class EncoderCommand extends Command { // yet to be tested
     public EncoderCommand() {
         requires(Robot.encoderSub);
     }
     // Called just before this Command runs the first time
     @Override
-    protected void initialize() {
-        Robot.encoderSub.talon.setSelectedSensorPosition(0);
+    protected void initialize() { 
+        RobotMap.TANK_DRIVE_TOP_LEFT.setSelectedSensorPosition(0); 
     }
 
     // Called repeatedly when this Command is scheduled to run
     @Override
     protected void execute() {
-        SmartDashboard.putNumber("Position", Robot.encoderSub.getPosition() );
-        Robot.encoderSub.goStraight(10); //Parameter is a placeholder = 10 inch
+        SmartDashboard.putNumber("Position", Robot.encoderSub.getPosition());
+        Robot.encoderSub.goStraight(10); //Parameter is a placeholder = 10 inch to move
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/GoToAngle.java b/src/main/java/frc/robot/commands/GoToAngle.java
index 776ad0e..9f8521a 100644
--- a/src/main/java/frc/robot/commands/GoToAngle.java
+++ b/src/main/java/frc/robot/commands/GoToAngle.java
@@ -11,7 +11,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 
-public class GoToAngle extends Command {
+public class GoToAngle extends Command { //yet to be tested
 
   public double finalAngle;
   public double finalAngle1;
@@ -19,7 +19,6 @@ public class GoToAngle extends Command {
  
   public GoToAngle(double pfinalAngle1, double pfinalAngle2, double pSpecifiedAngle) {
     requires(Robot.gyroSub);
-
   }
 
   // Called just before this Command runs the first time
@@ -40,7 +39,7 @@ protected void execute() {
   @Override
   protected boolean isFinished() {
     SmartDashboard.putBoolean("finished", (Robot.gyroSub.gyro.getAngle() >= finalAngle  && Robot.gyroSub.gyro.getAngle() <= finalAngle1));
-    return (Robot.gyroSub.gyro.getAngle() >= finalAngle && Robot.gyroSub.gyro.getAngle() <= finalAngle1);
+    return (Robot.gyroSub.gyro.getAngle() >= finalAngle && Robot.gyroSub.gyro.getAngle() <= finalAngle1); 
   }
 
   // Called once after isFinished returns true
diff --git a/src/main/java/frc/robot/subsystems/EncoderTest.java b/src/main/java/frc/robot/subsystems/EncoderTest.java
index 7d25ddb..278acb9 100644
--- a/src/main/java/frc/robot/subsystems/EncoderTest.java
+++ b/src/main/java/frc/robot/subsystems/EncoderTest.java
@@ -2,35 +2,32 @@
 
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 import edu.wpi.first.wpilibj.command.Subsystem;
-import frc.robot.commands.EncoderCommand;
 import frc.robot.Robot;
+import frc.robot.RobotMap;
 
-public class EncoderTest extends Subsystem {
-    public WPI_TalonSRX talon = new WPI_TalonSRX(15);
-
+public class EncoderTest extends Subsystem { // yet to be tested
+    
     public EncoderTest() {
-        talon.setSensorPhase(true);
-        talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10);
-        talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10);
+        RobotMap.TANK_DRIVE_TOP_LEFT.setSensorPhase(true); 
+        RobotMap.TANK_DRIVE_TOP_LEFT.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10);
+        RobotMap.TANK_DRIVE_TOP_LEFT.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10);
     }
 
-
     public double getPosition() {
-        return (double)talon.getSelectedSensorPosition();    
+        return (double)RobotMap.TANK_DRIVE_TOP_LEFT.getSelectedSensorPosition(); // where the robot is 
     }
 
-    public void goStraight(double distance) { //where 15.24 is the circumference of the wheels and distance is in in
-        double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
+    public void goStraight(double distance) { //where 6 is the circumference of the wheels and distance is in inches
+        double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (inch) (rotations = distance/circumference)
         double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
         Robot.tankDriveSubsystem.driveForward(); //Drives with 0.5 speed
         if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving
             Robot.tankDriveSubsystem.stop(); //stops all motors
     }
     
-    public void turnToAngle(double distance, double angle) { //where 15.24 is the circumference of the wheels and distance is in in
+    public void turnToAngle(double distance, double angle) {
         double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference)
         double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance
         Robot.tankDriveSubsystem.driveToAngle(angle); //Drives with 0.5 speed
@@ -38,10 +35,10 @@ public void turnToAngle(double distance, double angle) { //where 15.24 is the ci
             Robot.tankDriveSubsystem.stop(); //stops all motors
     }
 
-    public void turnToAngle(double angleOfTurn) { /* new edits */
-        double circumferenceOfRobot = 57.76; //units are in centimeter
+    public void turnToAngle(double angleOfTurn) { 
+        double circumferenceOfRobot = Math.PI * 22.74; //units are in inches, where 22.74 is a measure of the distance between each wheel
         double distance = (double)(circumferenceOfRobot * (angleOfTurn/360)); //gets the distance needed to turn a specified angle
-        turnToAngle(distance, angleOfTurn);;
+        turnToAngle(distance, angleOfTurn); // calls on auton tank drive for turning to angles
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/Gyroscope.java b/src/main/java/frc/robot/subsystems/Gyroscope.java
index 74debe6..5468981 100644
--- a/src/main/java/frc/robot/subsystems/Gyroscope.java
+++ b/src/main/java/frc/robot/subsystems/Gyroscope.java
@@ -5,7 +5,7 @@
 import frc.robot.Robot;
 import frc.robot.commands.GoToAngle;
 
-public class Gyroscope extends Subsystem {
+public class Gyroscope extends Subsystem { //yet to be tested
 
   int count = 0;
   public double finalAngle;
diff --git a/src/main/java/frc/robot/subsystems/PIDControl.java b/src/main/java/frc/robot/subsystems/PIDControl.java
index 1e5dd70..c883f7f 100644
--- a/src/main/java/frc/robot/subsystems/PIDControl.java
+++ b/src/main/java/frc/robot/subsystems/PIDControl.java
@@ -21,7 +21,7 @@
 /**
  * Add your docs here.
  */
-public class PIDControl extends PIDSubsystem {
+public class PIDControl extends PIDSubsystem { //yet to be tested
   /**
    * Add your docs here.
    */
diff --git a/src/main/java/frc/robot/subsystems/TankDrive.java b/src/main/java/frc/robot/subsystems/TankDrive.java
index f7af9db..326d088 100644
--- a/src/main/java/frc/robot/subsystems/TankDrive.java
+++ b/src/main/java/frc/robot/subsystems/TankDrive.java
@@ -19,17 +19,17 @@ public class TankDrive extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
  
-  public WPI_TalonSRX topLeft = new WPI_TalonSRX(3); //ports subject to change (16)
-  public WPI_TalonSRX middleLeft = new WPI_TalonSRX(4); //15
-  public WPI_TalonSRX rearLeft = new WPI_TalonSRX(10); //14
+  public WPI_TalonSRX topLeft = new WPI_TalonSRX(3); 
+  public WPI_TalonSRX middleLeft = new WPI_TalonSRX(4); 
+  public WPI_TalonSRX rearLeft = new WPI_TalonSRX(10); 
 
-  SpeedControllerGroup leftSide = new SpeedControllerGroup(topLeft, middleLeft, rearLeft);
+  SpeedControllerGroup leftSide = new SpeedControllerGroup(topLeft, middleLeft, rearLeft); // controls the left side motors of tank drive
 
-  public WPI_TalonSRX topRight = new WPI_TalonSRX(5); //ports subject to change' (12)
-  public WPI_TalonSRX middleRight = new WPI_TalonSRX(13); //13
-  public WPI_TalonSRX rearRight = new WPI_TalonSRX(1); //10
+  public WPI_TalonSRX topRight = new WPI_TalonSRX(5); 
+  public WPI_TalonSRX middleRight = new WPI_TalonSRX(13); 
+  public WPI_TalonSRX rearRight = new WPI_TalonSRX(1); 
 
-  SpeedControllerGroup rightSide = new SpeedControllerGroup(topRight, middleRight, rearRight);
+  SpeedControllerGroup rightSide = new SpeedControllerGroup(topRight, middleRight, rearRight); //controls the right side motors of tank drive
 
   DifferentialDrive difDrive = new DifferentialDrive(leftSide, rightSide);
 
@@ -55,7 +55,7 @@ public void init() { // middleLeft and middleRight motor must go in opposite dir
   }
 
   public void driveWithJoystick() {
-    double leftSpeed = OI.joyLeft.getY() * 0.8;
+    double leftSpeed = OI.joyLeft.getY() * 0.8; // motors cannot exceed 80%
     double rightSpeed = OI.joyRight.getY() * 0.8;
 
     if(Math.abs(leftSpeed) < 0.2) 
@@ -81,7 +81,7 @@ public void driveWithJoystick() {
     difDrive.tankDrive(leftSpeed, rightSpeed);
   }
 
-  public void driveForward() { // to be utilized when button 3 is pressed on Left Joystick (joyLeft)
+  public void driveForward() { 
     double leftSpeed = 0.5;
     double rightSpeed = -0.5;
     difDrive.tankDrive(leftSpeed, rightSpeed);
@@ -95,7 +95,7 @@ public void driveForward() { // to be utilized when button 3 is pressed on Left
     SmartDashboard.putNumber("Rear Right Motor:", rearRight.get());
   }
 
-  public void driveToAngle(double angle) { // to be utilized when button 3 is pressed on Left Joystick (joyLeft)
+  public void driveToAngle(double angle) { 
     double leftSpeed, rightSpeed;
     //the speedcontroller changes depending on what angle
     if(angle < 180.0) { //denotes it turning right. left side tank drive speed will be faster.