From 5d39df1e8908bc9843f114f4ae9e80b69f1ab0f5 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Sat, 19 Jan 2019 15:22:53 -0500
Subject: [PATCH 01/50] build.gradle was updated to version 2019.2.1, old ver.
 2019.1.1

---
 build.gradle | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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"

From 9b394a8e4b82553dce2664206ff49feabb1ebeb7 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 21 Jan 2019 19:22:05 -0500
Subject: [PATCH 02/50] Pixy camera base code taken from BHSRobotix

---
 src/main/java/frc/robot/subsystems/Pixy.java  | 211 ++++++++++++++++++
 .../java/frc/robot/subsystems/PixyPacket.java |  18 ++
 2 files changed, 229 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/Pixy.java
 create mode 100644 src/main/java/frc/robot/subsystems/PixyPacket.java

diff --git a/src/main/java/frc/robot/subsystems/Pixy.java b/src/main/java/frc/robot/subsystems/Pixy.java
new file mode 100644
index 0000000..8b0d6cf
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Pixy.java
@@ -0,0 +1,211 @@
+/*----------------------------------------------------------------------------*/
+/* 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.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Pixy {
+  String name;
+	PixyPacket values;
+	I2C pixy;
+	Port port = Port.kOnboard;
+	PixyPacket[] packets;
+	String print;
+
+	public Pixy(String id, I2C argPixy, PixyPacket[] argPixyPacket, PixyPacket argValues) {
+		pixy = argPixy;
+		packets = argPixyPacket;
+		values = argValues;
+		name = "Pixy_" + id;
+  }
+  
+  public int cvt(byte upper, byte lower) {
+		return (((int) upper & 0xff) << 8) | ((int) lower & 0xff);
+  }
+  
+  // This method gathers data, then parses that data, and assigns the ints to global variables
+	// The signature should be which number object in pixymon you are trying to get data for
+  public PixyPacket readPacket(int signature) {
+		int Checksum;
+		int Sig;
+		byte[] rawData = new byte[32];
+		// SmartDashboard.putString("rawData", rawData[0] + " " + rawData[1] + "
+		// " + rawData[15] + " " + rawData[31]);
+		try {
+			pixy.readOnly(rawData, 32);
+		} catch (RuntimeException e) {
+			SmartDashboard.putString(name + "Status", e.toString());
+			System.out.println(name + "  " + e);
+		}
+		if (rawData.length < 32) {
+			SmartDashboard.putString(name + "Status", "raw data length " + rawData.length);
+			System.out.println("byte array length is broken length=" + rawData.length);
+			return null;
+		}
+		for (int i = 0; i <= 16; i++) {
+			int syncWord = cvt(rawData[i + 1], rawData[i + 0]); // Parse first 2
+																// bytes
+			if (syncWord == 0xaa55) { // Check is first 2 bytes equal a "sync
+										// word", which indicates the start of a
+										// packet of valid data
+				syncWord = cvt(rawData[i + 3], rawData[i + 2]); // Parse the
+																// next 2 bytes
+				if (syncWord != 0xaa55) { // Shifts everything in the case that
+											// one syncword is sent
+					i -= 2;
+				}
+				// This next block parses the rest of the rawData
+				Checksum = cvt(rawData[i + 5], rawData[i + 4]);
+				Sig = cvt(rawData[i + 7], rawData[i + 6]);
+				if (Sig <= 0 || Sig > packets.length) {
+					break;
+				}
+
+				packets[Sig - 1] = new PixyPacket();
+				packets[Sig - 1].x = cvt(rawData[i + 9], rawData[i + 8]);
+				packets[Sig - 1].y = cvt(rawData[i + 11], rawData[i + 10]);
+				packets[Sig - 1].width = cvt(rawData[i + 13], rawData[i + 12]);
+				packets[Sig - 1].height = cvt(rawData[i + 15], rawData[i + 14]);
+				// Checks whether the data is valid using the checksum *This if
+				// block should never be entered*
+				if (Checksum != Sig + packets[Sig - 1].x + packets[Sig - 1].y + packets[Sig - 1].width
+						+ packets[Sig - 1].height) {
+					packets[Sig - 1] = null;
+				}
+				break;
+			} else
+				SmartDashboard.putNumber("syncword: ", syncWord);
+		}
+		// Assigns our packet to a temp packet, then deletes data so that we
+		// dont return old data
+		PixyPacket pkt = packets[signature - 1];
+		packets[signature - 1] = null;
+		return pkt;
+	}
+
+	private byte[] readData(int len) {
+		byte[] rawData = new byte[len];
+		try {
+			pixy.readOnly(rawData, len);
+		} catch (RuntimeException e) {
+			SmartDashboard.putString(name + "Status", e.toString());
+			System.out.println(name + "  " + e);
+		}
+		if (rawData.length < len) {
+			SmartDashboard.putString(name + "Status", "raw data length " + rawData.length);
+			System.out.println("byte array length is broken length=" + rawData.length);
+			return null;
+		}
+		return rawData;
+	}
+
+	private int readWord() {
+		byte[] data = readData(2);
+		if (data == null) {
+			return 0;
+		}
+		return cvt(data[1], data[0]);
+	}
+
+	private PixyPacket readBlock(int checksum) {
+		// See Object block format section in
+		// http://www.cmucam.org/projects/cmucam5/wiki/Porting_Guide#Object-block-format
+		// Each block is 14 bytes, but we already read 2 bytes for checksum in
+		// caller so now we need to read rest.
+
+		byte[] data = readData(12);
+		if (data == null) {
+			return null;
+		}
+		PixyPacket block = new PixyPacket();
+		block.signature = cvt(data[1], data[0]);
+		if (block.signature <= 0 || block.signature > 7) {
+			return null;
+		}
+		block.x = cvt(data[3], data[2]);
+		block.y = cvt(data[5], data[4]);
+		block.width = cvt(data[7], data[6]);
+		block.height = cvt(data[9], data[8]);
+
+		int sum = block.signature + block.x + block.y + block.width + block.height;
+		if (sum != checksum) {
+			return null;
+		}
+		return block;
+	}
+
+	private final int MAx_signatureS = 7;
+	private final int OBJECT_SIZE = 14;
+	private final int START_WORD = 0xaa55;
+	private final int START_WORD_CC = 0xaa5;
+	private final int START_WORD_x = 0x55aa;
+
+	public boolean getStart() {
+		int numBytesRead = 0;
+		int lastWord = 0xffff;
+		// This while condition was originally true.. may not be a good idea if
+		// something goes wrong robot will be stuck in this loop forever. So
+		// let's read some number of bytes and give up so other stuff on robot
+		// can have a chance to run. What number of bytes to choose? Maybe size
+		// of a block * max number of signatures that can be detected? Or how
+		// about size of block and max number of blocks we are looking for?
+		while (numBytesRead < (OBJECT_SIZE * MAx_signatureS)) {
+			int word = readWord();
+			numBytesRead += 2;
+			if (word == 0 && lastWord == 0) {
+				return false;
+			} else if (word == START_WORD && lastWord == START_WORD) {
+				return true;
+			} else if (word == START_WORD_CC && lastWord == START_WORD) {
+				return true;
+			} else if (word == START_WORD_x) {
+				byte[] data = readData(1);
+				numBytesRead += 1;
+			}
+			lastWord = word;
+		}
+		return false;
+	}
+
+	private boolean skipStart = false;
+
+	public PixyPacket[] readBlocks() {
+		// This has to match the max block setting in pixymon?
+		int maxBlocks = 2;
+		PixyPacket[] blocks = new PixyPacket[maxBlocks];
+
+		if (!skipStart) {
+			if (getStart() == false) {
+				return null;
+			}
+		} else {
+			skipStart = false;
+		}
+		for (int i = 0; i < maxBlocks; i++) {
+			// Should we set to empty PixyPacket? To avoid having to check for
+			// null in callers?
+			blocks[i] = null;
+			int checksum = readWord();
+			if (checksum == START_WORD) {
+				// we've reached the beginning of the next frame
+				skipStart = true;
+				return blocks;
+			} else if (checksum == START_WORD_CC) {
+				// we've reached the beginning of the next frame
+				skipStart = true;
+				return blocks;
+			} else if (checksum == 0) {
+				return blocks;
+			}
+			blocks[i] = readBlock(checksum);
+		}
+		return blocks;
+  }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/PixyPacket.java b/src/main/java/frc/robot/subsystems/PixyPacket.java
new file mode 100644
index 0000000..8d82c87
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/PixyPacket.java
@@ -0,0 +1,18 @@
+package frc.robot.subsystems;
+
+public class PixyPacket {
+	public int signature;
+	public int x;
+	public int y;
+	public int width;
+	public int height;
+	public int angle;
+	public int age;
+	
+	//public int checksumError;
+	
+	public String toString() {
+		return "" + " S:" + signature + " X:" + x + " Y:" + 
+		y + " W:" + width + " H:" + height + " A:" + angle + " Age:" + age;
+	}
+}
\ No newline at end of file

From 2cef246c14c876ae3c11dc874852c49a3ae51dc0 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Sat, 2 Feb 2019 13:10:16 -0500
Subject: [PATCH 03/50] Deleted pixy code since Joey has already created
 working pixy code.

---
 src/main/java/frc/robot/subsystems/Pixy.java  | 211 ------------------
 .../java/frc/robot/subsystems/PixyPacket.java |  18 --
 2 files changed, 229 deletions(-)
 delete mode 100644 src/main/java/frc/robot/subsystems/Pixy.java
 delete mode 100644 src/main/java/frc/robot/subsystems/PixyPacket.java

diff --git a/src/main/java/frc/robot/subsystems/Pixy.java b/src/main/java/frc/robot/subsystems/Pixy.java
deleted file mode 100644
index 8b0d6cf..0000000
--- a/src/main/java/frc/robot/subsystems/Pixy.java
+++ /dev/null
@@ -1,211 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.I2C;
-import edu.wpi.first.wpilibj.I2C.Port;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-public class Pixy {
-  String name;
-	PixyPacket values;
-	I2C pixy;
-	Port port = Port.kOnboard;
-	PixyPacket[] packets;
-	String print;
-
-	public Pixy(String id, I2C argPixy, PixyPacket[] argPixyPacket, PixyPacket argValues) {
-		pixy = argPixy;
-		packets = argPixyPacket;
-		values = argValues;
-		name = "Pixy_" + id;
-  }
-  
-  public int cvt(byte upper, byte lower) {
-		return (((int) upper & 0xff) << 8) | ((int) lower & 0xff);
-  }
-  
-  // This method gathers data, then parses that data, and assigns the ints to global variables
-	// The signature should be which number object in pixymon you are trying to get data for
-  public PixyPacket readPacket(int signature) {
-		int Checksum;
-		int Sig;
-		byte[] rawData = new byte[32];
-		// SmartDashboard.putString("rawData", rawData[0] + " " + rawData[1] + "
-		// " + rawData[15] + " " + rawData[31]);
-		try {
-			pixy.readOnly(rawData, 32);
-		} catch (RuntimeException e) {
-			SmartDashboard.putString(name + "Status", e.toString());
-			System.out.println(name + "  " + e);
-		}
-		if (rawData.length < 32) {
-			SmartDashboard.putString(name + "Status", "raw data length " + rawData.length);
-			System.out.println("byte array length is broken length=" + rawData.length);
-			return null;
-		}
-		for (int i = 0; i <= 16; i++) {
-			int syncWord = cvt(rawData[i + 1], rawData[i + 0]); // Parse first 2
-																// bytes
-			if (syncWord == 0xaa55) { // Check is first 2 bytes equal a "sync
-										// word", which indicates the start of a
-										// packet of valid data
-				syncWord = cvt(rawData[i + 3], rawData[i + 2]); // Parse the
-																// next 2 bytes
-				if (syncWord != 0xaa55) { // Shifts everything in the case that
-											// one syncword is sent
-					i -= 2;
-				}
-				// This next block parses the rest of the rawData
-				Checksum = cvt(rawData[i + 5], rawData[i + 4]);
-				Sig = cvt(rawData[i + 7], rawData[i + 6]);
-				if (Sig <= 0 || Sig > packets.length) {
-					break;
-				}
-
-				packets[Sig - 1] = new PixyPacket();
-				packets[Sig - 1].x = cvt(rawData[i + 9], rawData[i + 8]);
-				packets[Sig - 1].y = cvt(rawData[i + 11], rawData[i + 10]);
-				packets[Sig - 1].width = cvt(rawData[i + 13], rawData[i + 12]);
-				packets[Sig - 1].height = cvt(rawData[i + 15], rawData[i + 14]);
-				// Checks whether the data is valid using the checksum *This if
-				// block should never be entered*
-				if (Checksum != Sig + packets[Sig - 1].x + packets[Sig - 1].y + packets[Sig - 1].width
-						+ packets[Sig - 1].height) {
-					packets[Sig - 1] = null;
-				}
-				break;
-			} else
-				SmartDashboard.putNumber("syncword: ", syncWord);
-		}
-		// Assigns our packet to a temp packet, then deletes data so that we
-		// dont return old data
-		PixyPacket pkt = packets[signature - 1];
-		packets[signature - 1] = null;
-		return pkt;
-	}
-
-	private byte[] readData(int len) {
-		byte[] rawData = new byte[len];
-		try {
-			pixy.readOnly(rawData, len);
-		} catch (RuntimeException e) {
-			SmartDashboard.putString(name + "Status", e.toString());
-			System.out.println(name + "  " + e);
-		}
-		if (rawData.length < len) {
-			SmartDashboard.putString(name + "Status", "raw data length " + rawData.length);
-			System.out.println("byte array length is broken length=" + rawData.length);
-			return null;
-		}
-		return rawData;
-	}
-
-	private int readWord() {
-		byte[] data = readData(2);
-		if (data == null) {
-			return 0;
-		}
-		return cvt(data[1], data[0]);
-	}
-
-	private PixyPacket readBlock(int checksum) {
-		// See Object block format section in
-		// http://www.cmucam.org/projects/cmucam5/wiki/Porting_Guide#Object-block-format
-		// Each block is 14 bytes, but we already read 2 bytes for checksum in
-		// caller so now we need to read rest.
-
-		byte[] data = readData(12);
-		if (data == null) {
-			return null;
-		}
-		PixyPacket block = new PixyPacket();
-		block.signature = cvt(data[1], data[0]);
-		if (block.signature <= 0 || block.signature > 7) {
-			return null;
-		}
-		block.x = cvt(data[3], data[2]);
-		block.y = cvt(data[5], data[4]);
-		block.width = cvt(data[7], data[6]);
-		block.height = cvt(data[9], data[8]);
-
-		int sum = block.signature + block.x + block.y + block.width + block.height;
-		if (sum != checksum) {
-			return null;
-		}
-		return block;
-	}
-
-	private final int MAx_signatureS = 7;
-	private final int OBJECT_SIZE = 14;
-	private final int START_WORD = 0xaa55;
-	private final int START_WORD_CC = 0xaa5;
-	private final int START_WORD_x = 0x55aa;
-
-	public boolean getStart() {
-		int numBytesRead = 0;
-		int lastWord = 0xffff;
-		// This while condition was originally true.. may not be a good idea if
-		// something goes wrong robot will be stuck in this loop forever. So
-		// let's read some number of bytes and give up so other stuff on robot
-		// can have a chance to run. What number of bytes to choose? Maybe size
-		// of a block * max number of signatures that can be detected? Or how
-		// about size of block and max number of blocks we are looking for?
-		while (numBytesRead < (OBJECT_SIZE * MAx_signatureS)) {
-			int word = readWord();
-			numBytesRead += 2;
-			if (word == 0 && lastWord == 0) {
-				return false;
-			} else if (word == START_WORD && lastWord == START_WORD) {
-				return true;
-			} else if (word == START_WORD_CC && lastWord == START_WORD) {
-				return true;
-			} else if (word == START_WORD_x) {
-				byte[] data = readData(1);
-				numBytesRead += 1;
-			}
-			lastWord = word;
-		}
-		return false;
-	}
-
-	private boolean skipStart = false;
-
-	public PixyPacket[] readBlocks() {
-		// This has to match the max block setting in pixymon?
-		int maxBlocks = 2;
-		PixyPacket[] blocks = new PixyPacket[maxBlocks];
-
-		if (!skipStart) {
-			if (getStart() == false) {
-				return null;
-			}
-		} else {
-			skipStart = false;
-		}
-		for (int i = 0; i < maxBlocks; i++) {
-			// Should we set to empty PixyPacket? To avoid having to check for
-			// null in callers?
-			blocks[i] = null;
-			int checksum = readWord();
-			if (checksum == START_WORD) {
-				// we've reached the beginning of the next frame
-				skipStart = true;
-				return blocks;
-			} else if (checksum == START_WORD_CC) {
-				// we've reached the beginning of the next frame
-				skipStart = true;
-				return blocks;
-			} else if (checksum == 0) {
-				return blocks;
-			}
-			blocks[i] = readBlock(checksum);
-		}
-		return blocks;
-  }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/PixyPacket.java b/src/main/java/frc/robot/subsystems/PixyPacket.java
deleted file mode 100644
index 8d82c87..0000000
--- a/src/main/java/frc/robot/subsystems/PixyPacket.java
+++ /dev/null
@@ -1,18 +0,0 @@
-package frc.robot.subsystems;
-
-public class PixyPacket {
-	public int signature;
-	public int x;
-	public int y;
-	public int width;
-	public int height;
-	public int angle;
-	public int age;
-	
-	//public int checksumError;
-	
-	public String toString() {
-		return "" + " S:" + signature + " X:" + x + " Y:" + 
-		y + " W:" + width + " H:" + height + " A:" + angle + " Age:" + age;
-	}
-}
\ No newline at end of file

From 4594e57834df94adb28e23692ba0c78cd9bc53e5 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Sat, 2 Feb 2019 14:44:48 -0500
Subject: [PATCH 04/50] Added ADIS gyro library, began code to get angles from
 gyro (buggy)

---
 src/main/java/frc/robot/Robot.java            | 20 ++++++++++++
 .../frc/robot/commands/JoystickDrive.java     |  2 ++
 .../frc/robot/commands/PneumaticsDrive.java   |  4 +--
 .../java/frc/robot/subsystems/DriveTrain.java |  9 ++++++
 vendordeps/ADIS16448.json                     | 32 +++++++++++++++++++
 5 files changed, 65 insertions(+), 2 deletions(-)
 create mode 100644 vendordeps/ADIS16448.json

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 819779a..608d9f6 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -18,6 +18,7 @@
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.Pneumatics;
+import com.analog.adis16448.frc.ADIS16448_IMU;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -31,6 +32,7 @@ public class Robot extends TimedRobot {
   public static Pneumatics m_pneumatics;
   public static OI m_oi;
   public static DriveTrain m_driveTrain;
+  //public static ADIS16448_IMU imu;
   
   Command m_autonomousCommand;
   SendableChooser<Command> m_chooser = new SendableChooser<>();
@@ -42,6 +44,7 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotInit() {
+    //imu = new ADIS16448_IMU();  
     m_oi = new OI();
     m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
     m_pneumatics = new Pneumatics();
@@ -63,6 +66,23 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
+    m_driveTrain.getGyroAngle();
+    /*
+    SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
+    SmartDashboard.putNumber("Gyro-Y", imu.getAngleY());
+    SmartDashboard.putNumber("Gyro-Z", imu.getAngleZ());
+    
+    SmartDashboard.putNumber("Accel-X", imu.getAccelX());
+    SmartDashboard.putNumber("Accel-Y", imu.getAccelY());
+    SmartDashboard.putNumber("Accel-Z", imu.getAccelZ());
+    
+    SmartDashboard.putNumber("Pitch", imu.getPitch());
+    SmartDashboard.putNumber("Roll", imu.getRoll());
+    SmartDashboard.putNumber("Yaw", imu.getYaw());
+    
+    SmartDashboard.putNumber("Pressure: ", imu.getBarometricPressure());
+    SmartDashboard.putNumber("Temperature: ", imu.getTemperature()); 
+    */
   }
 
   /**
diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index 3aafb5d..1600a3c 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.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.OI;
 import frc.robot.Robot;
 import frc.robot.subsystems.DriveTrain;
@@ -28,6 +29,7 @@ protected void initialize() {
   @Override
   protected void execute() {
     DriveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX());
+    // SmartDashboard.putNumber("Gryo-X: ", Robot.m_driveTrain.getGyroX());
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/commands/PneumaticsDrive.java b/src/main/java/frc/robot/commands/PneumaticsDrive.java
index 74cc8e9..406b30a 100644
--- a/src/main/java/frc/robot/commands/PneumaticsDrive.java
+++ b/src/main/java/frc/robot/commands/PneumaticsDrive.java
@@ -22,13 +22,13 @@ public PneumaticsDrive() {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    System.out.println("init");
+    //System.out.println("init");
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    System.out.println("exec");
+    //System.out.println("exec");
     if(OI.xBoxControl.getTriggerAxis(Hand.kLeft)>0) {
       Pneumatics.retract();
     }
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 8ac7d0e..234c88a 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -11,7 +11,9 @@
 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.JoystickDrive;
+import com.analog.adis16448.frc.ADIS16448_IMU;
 
 /**
  * Add your docs here.
@@ -19,6 +21,8 @@
 public class DriveTrain extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
+  public static final ADIS16448_IMU imu = new ADIS16448_IMU();
+
   public Spark sparkL1 = new Spark(0);
   public Spark sparkL2 = new Spark(1);
   public Spark sparkR1 = new Spark(2);
@@ -45,4 +49,9 @@ public void initDefaultCommand() {
     // Set the default command for a subsystem here.
     setDefaultCommand(new JoystickDrive());
   }
+  public void getGyroAngle() {
+    SmartDashboard.putNumber("Gyro-X", imu.getAngleX());
+    SmartDashboard.putNumber("Gyro-Y", imu.getAngleY());
+    SmartDashboard.putNumber("Gyro-Z", imu.getAngleZ());
+  }
 }
diff --git a/vendordeps/ADIS16448.json b/vendordeps/ADIS16448.json
new file mode 100644
index 0000000..3069c78
--- /dev/null
+++ b/vendordeps/ADIS16448.json
@@ -0,0 +1,32 @@
+{
+    "fileName": "ADIS16448.json",
+    "name": "ADIS16448-IMU",
+    "version": "2019-r1",
+    "uuid": "38c21ab6-aa8b-44bc-b844-8086c77f09ec",
+    "mavenUrls": [
+        "http://maven.highcurrent.io/maven"
+    ],
+    "jsonUrl": "http://maven.highcurrent.io/vendordeps/ADIS16448.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.github.juchong",
+            "artifactId": "adis16448-java",
+            "version": "2019-r1"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.github.juchong",
+            "artifactId": "adis16448-cpp",
+            "version": "2019-r1",
+            "libName": "libadis16448imu",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ]
+        }
+    ]
+}
\ No newline at end of file

From 543feacad6073120c4cb73f1bcba43eff5acb621 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Sat, 2 Feb 2019 15:56:43 -0500
Subject: [PATCH 05/50] Command to turn angle given by vision when button
 pressed, still buggy

---
 src/main/java/frc/robot/OI.java               |  5 ++
 src/main/java/frc/robot/Robot.java            | 12 +--
 .../frc/robot/commands/JoystickDrive.java     |  2 +-
 .../java/frc/robot/commands/TurnDegrees.java  | 74 +++++++++++++++++++
 .../java/frc/robot/subsystems/DriveTrain.java | 16 ++--
 5 files changed, 97 insertions(+), 12 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/TurnDegrees.java

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index ad7688e..541625a 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -7,10 +7,13 @@
 
 package frc.robot;
 
+import java.awt.event.KeyListener;
+
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import edu.wpi.first.wpilibj.buttons.Trigger;
+import frc.robot.commands.*;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -49,5 +52,7 @@ public class OI {
 
   public OI() {
    // Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
+   Button button = new JoystickButton(xBoxControl, 2);
+   button.whenPressed(new TurnDegrees(10));
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 608d9f6..ed5dd3f 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -32,7 +32,7 @@ public class Robot extends TimedRobot {
   public static Pneumatics m_pneumatics;
   public static OI m_oi;
   public static DriveTrain m_driveTrain;
-  //public static ADIS16448_IMU imu;
+  //public static final ADIS16448_IMU imu = new ADIS16448_IMU(); ;
   
   Command m_autonomousCommand;
   SendableChooser<Command> m_chooser = new SendableChooser<>();
@@ -43,9 +43,7 @@ public class Robot extends TimedRobot {
    * 
    */
   @Override
-  public void robotInit() {
-    //imu = new ADIS16448_IMU();  
-    m_oi = new OI();
+  public void robotInit() { 
     m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
     m_pneumatics = new Pneumatics();
     m_driveTrain = new DriveTrain();
@@ -53,7 +51,7 @@ public void robotInit() {
     SmartDashboard.putData("Auto mode", m_chooser);
     CameraServer.getInstance().startAutomaticCapture(0);
     CameraServer.getInstance().startAutomaticCapture(1);
-    
+    m_oi = new OI();
   }
 
   /**
@@ -66,7 +64,9 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
-    m_driveTrain.getGyroAngle();
+    SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
+    SmartDashboard.putNumber("Gyro-Y", m_driveTrain.getGyroY());
+    SmartDashboard.putNumber("Gyro-Z", m_driveTrain.getGyroZ());
     /*
     SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
     SmartDashboard.putNumber("Gyro-Y", imu.getAngleY());
diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index 1600a3c..f2a8978 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -28,7 +28,7 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    DriveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX());
+    Robot.m_driveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX());
     // SmartDashboard.putNumber("Gryo-X: ", Robot.m_driveTrain.getGyroX());
   }
 
diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java
new file mode 100644
index 0000000..9f1271d
--- /dev/null
+++ b/src/main/java/frc/robot/commands/TurnDegrees.java
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+import frc.robot.subsystems.DriveTrain;
+
+public class TurnDegrees extends Command {
+  public double target;
+  public double turnAngle;
+  public boolean isLeft;
+
+  public TurnDegrees(double target) {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    requires(Robot.m_driveTrain);
+    this.target = target;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    turnAngle = Robot.m_driveTrain.getGyroZ() + target;
+    isLeft = target<0;
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    double leftSpeed = 0;
+    double rightSpeed = 0;
+    if(isLeft) {
+      leftSpeed = -.5;
+      rightSpeed = .5;
+    }
+    else {
+      leftSpeed = .5;
+      rightSpeed = -.5;
+    }
+      
+    Robot.m_driveTrain.drive(leftSpeed, rightSpeed);
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    if(isLeft) {
+      if(Robot.m_driveTrain.getGyroZ() <= turnAngle)
+        return true;
+    }
+    else {
+      if(Robot.m_driveTrain.getGyroZ() >= turnAngle)
+        return true;
+    }
+    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() {
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 234c88a..1ff3b21 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -7,6 +7,7 @@
 
 package frc.robot.subsystems;
 
+import frc.robot.Robot;
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.command.Subsystem;
@@ -40,7 +41,7 @@ public DriveTrain() {
     diffDrive = new DifferentialDrive(leftGroup, rightGroup);
   }
 
-  public static void drive(double leftSpeed, double rightSpeed) {
+  public void drive(double leftSpeed, double rightSpeed) {
     diffDrive.arcadeDrive(leftSpeed, rightSpeed);
   }
 
@@ -49,9 +50,14 @@ public void initDefaultCommand() {
     // Set the default command for a subsystem here.
     setDefaultCommand(new JoystickDrive());
   }
-  public void getGyroAngle() {
-    SmartDashboard.putNumber("Gyro-X", imu.getAngleX());
-    SmartDashboard.putNumber("Gyro-Y", imu.getAngleY());
-    SmartDashboard.putNumber("Gyro-Z", imu.getAngleZ());
+
+  public double getGyroX() {
+    return imu.getAngleX();
+  }
+  public double getGyroY() {
+    return imu.getAngleY();
+  }
+  public double getGyroZ() {
+    return imu.getAngleZ();
   }
 }

From 78b7ee1db88152da86e031b3abfea4cd13dd7c94 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 4 Feb 2019 19:23:52 -0500
Subject: [PATCH 06/50] Added comments for TODO

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

diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 1ff3b21..4f8e077 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -41,6 +41,7 @@ public DriveTrain() {
     diffDrive = new DifferentialDrive(leftGroup, rightGroup);
   }
 
+  //maybe change back to static (broken code?)
   public void drive(double leftSpeed, double rightSpeed) {
     diffDrive.arcadeDrive(leftSpeed, rightSpeed);
   }

From ae106b6e865b676f6c08fdf0e64aa2471781a585 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 4 Feb 2019 19:35:55 -0500
Subject: [PATCH 07/50] Changed drive back to static

---
 src/main/java/frc/robot/commands/JoystickDrive.java | 2 +-
 src/main/java/frc/robot/commands/TurnDegrees.java   | 2 +-
 src/main/java/frc/robot/subsystems/DriveTrain.java  | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index f2a8978..1600a3c 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -28,7 +28,7 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    Robot.m_driveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX());
+    DriveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX());
     // SmartDashboard.putNumber("Gryo-X: ", Robot.m_driveTrain.getGyroX());
   }
 
diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java
index 9f1271d..bfbe996 100644
--- a/src/main/java/frc/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc/robot/commands/TurnDegrees.java
@@ -44,7 +44,7 @@ protected void execute() {
       rightSpeed = -.5;
     }
       
-    Robot.m_driveTrain.drive(leftSpeed, rightSpeed);
+    DriveTrain.drive(leftSpeed, rightSpeed);
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 4f8e077..d273721 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -42,7 +42,7 @@ public DriveTrain() {
   }
 
   //maybe change back to static (broken code?)
-  public void drive(double leftSpeed, double rightSpeed) {
+  public static void drive(double leftSpeed, double rightSpeed) {
     diffDrive.arcadeDrive(leftSpeed, rightSpeed);
   }
 

From 9e1a3d8b636e22ad4910952033e772a0cfc3f22a Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 4 Feb 2019 19:50:33 -0500
Subject: [PATCH 08/50] Pneumatics renaming

---
 src/main/java/frc/robot/Robot.java                    | 6 +++---
 src/main/java/frc/robot/commands/PneumaticsDrive.java | 6 +++---
 src/main/java/frc/robot/subsystems/pneumatics.java    | 4 ++--
 3 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 05fdc06..61a3d44 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -22,7 +22,7 @@
 import frc.robot.subsystems.Claw;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.ExampleSubsystem;
-import frc.robot.subsystems.Pneumatics;
+import frc.robot.subsystems.pneumatics;
 import com.analog.adis16448.frc.ADIS16448_IMU;
 import frc.robot.subsystems.SerialDistance;
 import frc.robot.subsystems.UltraSonic;
@@ -36,7 +36,7 @@
  */
 public class Robot extends TimedRobot {
   public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
-  public static Pneumatics m_pneumatics;
+  public static pneumatics m_pneumatics;
   public static OI m_oi;
   public static DriveTrain m_driveTrain;
   //public static final ADIS16448_IMU imu = new ADIS16448_IMU(); ;
@@ -61,7 +61,7 @@ public void robotInit() {
     claw = new Claw();
     m_driveTrain = new DriveTrain();
     m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
-    m_pneumatics = new Pneumatics();
+    m_pneumatics = new pneumatics();
     SmartDashboard.putData("Auto mode", m_chooser);
     CameraServer.getInstance().startAutomaticCapture(0);
     CameraServer.getInstance().startAutomaticCapture(1);
diff --git a/src/main/java/frc/robot/commands/PneumaticsDrive.java b/src/main/java/frc/robot/commands/PneumaticsDrive.java
index 7e16d47..edb83e7 100644
--- a/src/main/java/frc/robot/commands/PneumaticsDrive.java
+++ b/src/main/java/frc/robot/commands/PneumaticsDrive.java
@@ -11,7 +11,7 @@
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.OI;
 import frc.robot.Robot;
-import frc.robot.subsystems.Pneumatics;
+import frc.robot.subsystems.pneumatics;
 
 public class PneumaticsDrive extends Command {
   public PneumaticsDrive() {
@@ -29,10 +29,10 @@ protected void initialize() {
   @Override
   protected void execute() {
     if(OI.xBoxControl.getTriggerAxis(Hand.kLeft)>0) {
-      Pneumatics.retract();
+      pneumatics.retract();
     }
     if(OI.xBoxControl.getTriggerAxis(Hand.kRight)>0) {
-      Pneumatics.extend();
+      pneumatics.extend();
     }
     
   }
diff --git a/src/main/java/frc/robot/subsystems/pneumatics.java b/src/main/java/frc/robot/subsystems/pneumatics.java
index 08d2714..bc285de 100644
--- a/src/main/java/frc/robot/subsystems/pneumatics.java
+++ b/src/main/java/frc/robot/subsystems/pneumatics.java
@@ -15,12 +15,12 @@
 /**
  * Add your docs here.
  */
-public class Pneumatics extends Subsystem {
+public class pneumatics extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public static Compressor compressor = new Compressor(0);
   public static DoubleSolenoid doubleSol = new DoubleSolenoid(0, 1);
-  public Pneumatics(){
+  public pneumatics(){
     System.out.println("Compressor Init");
     compressor = new Compressor(0);
     compressor.setClosedLoopControl(true);

From 848dabe28201ac85c4d25ef51a727ce444599385 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 4 Feb 2019 20:20:48 -0500
Subject: [PATCH 09/50] Final fixes to isFinished method in TurnDegrees

---
 src/main/java/frc/robot/commands/TurnDegrees.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java
index f2d73a1..a558a6f 100644
--- a/src/main/java/frc/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc/robot/commands/TurnDegrees.java
@@ -61,6 +61,7 @@ protected boolean isFinished() {
       if(Robot.m_driveTrain.getGyroZ() >= turnAngle)
         return true;
     }
+    return false;
   }
 
   // Called once after isFinished returns true

From 73852f60909dc6548cc2303492331126189c57bd Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 4 Feb 2019 20:50:02 -0500
Subject: [PATCH 10/50] Fixed several bugs and made drive in DriveTrain
 non-static

---
 src/main/java/frc/robot/commands/JoystickDrive.java | 2 +-
 src/main/java/frc/robot/commands/TurnDegrees.java   | 7 +++----
 src/main/java/frc/robot/subsystems/Arm.java         | 6 ++++--
 src/main/java/frc/robot/subsystems/DriveTrain.java  | 4 ++--
 4 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index 59f6396..b1bb015 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -29,7 +29,7 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    DriveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX());
+    Robot.m_driveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX());
     // SmartDashboard.putNumber("Gryo-X: ", Robot.m_driveTrain.getGyroX());
   }
   
diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java
index a558a6f..49aefbd 100644
--- a/src/main/java/frc/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc/robot/commands/TurnDegrees.java
@@ -12,11 +12,11 @@
 import frc.robot.subsystems.DriveTrain;
 
 public class TurnDegrees extends Command {
-  public double target;
+  public int target;
   public double turnAngle;
   public boolean isLeft;
 
-  public TurnDegrees(double target) {
+  public TurnDegrees(int target) {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
     requires(Robot.m_driveTrain);
@@ -27,7 +27,6 @@ public TurnDegrees(double target) {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-
     turnAngle = Robot.m_driveTrain.getGyroZ() + target;
     isLeft = target<0;
   }
@@ -46,7 +45,7 @@ protected void execute() {
       rightSpeed = -.5;
     }
       
-    DriveTrain.drive(leftSpeed, rightSpeed);
+    Robot.m_driveTrain.drive(leftSpeed, rightSpeed);
 
   }
 
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 9e00504..a33c1a5 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -17,10 +17,12 @@
  * Add your docs here.
  */
 public class Arm extends Subsystem {
-  static Spark motor = new Spark(RobotMap.arm);
+  static Spark motor;
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
-
+  public Arm() {
+    motor = new Spark(7);
+  }
   @Override
   public void initDefaultCommand() {
   }
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 0138222..576686c 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -35,7 +35,7 @@ public class DriveTrain extends Subsystem {
   SpeedControllerGroup leftGroup;
   SpeedControllerGroup rightGroup;
 
-  public static DifferentialDrive diffDrive;
+  public DifferentialDrive diffDrive;
 
   public DriveTrain() {
     leftGroup = new SpeedControllerGroup(sparkL1, sparkL2);
@@ -46,7 +46,7 @@ public DriveTrain() {
   }
 
   //maybe change back to static (broken code?)
-  public static void drive(double leftSpeed, double rightSpeed) {
+  public void drive(double leftSpeed, double rightSpeed) {
     diffDrive.arcadeDrive(leftSpeed * speedModifier, rightSpeed * speedModifier);
     Shuffleboard.selectTab("Live Window");
     Shuffleboard.update();

From e1d8ed795ab1fb08dc431a63c18e89745f1fa786 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Wed, 6 Feb 2019 19:30:05 -0500
Subject: [PATCH 11/50] Added exit statement if target angle is 0 in
 TurnDegrees

---
 .../java/frc/robot/commands/TurnDegrees.java   | 18 +++++++++++-------
 1 file changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java
index 49aefbd..d197d27 100644
--- a/src/main/java/frc/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc/robot/commands/TurnDegrees.java
@@ -52,15 +52,19 @@ protected void execute() {
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    if(isLeft) {
-      if(Robot.m_driveTrain.getGyroZ() <= turnAngle)
-        return true;
-    }
+    if(target == 0)
+      return true;
     else {
-      if(Robot.m_driveTrain.getGyroZ() >= turnAngle)
-        return true;
+      if(isLeft) {
+        if(Robot.m_driveTrain.getGyroZ() <= turnAngle)
+          return true;
+      }
+      else {
+        if(Robot.m_driveTrain.getGyroZ() >= turnAngle)
+          return true;
+      }
+      return false;
     }
-    return false;
   }
 
   // Called once after isFinished returns true

From 736f90f7c9c47e279d7c69849962dcb361be4148 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Wed, 6 Feb 2019 20:45:33 -0500
Subject: [PATCH 12/50] Added GuideToTarget Command

---
 src/main/java/frc/robot/Robot.java            |  4 +-
 .../frc/robot/commands/ArmToNextPosition.java |  1 -
 .../frc/robot/commands/GuideToTarget.java     | 50 +++++++++++++++++++
 .../java/frc/robot/commands/TurnDegrees.java  | 20 ++++----
 .../robot/commands/updateAngleToTarget.java   |  2 +-
 .../java/frc/robot/subsystems/CameraI2c.java  |  4 +-
 .../java/frc/robot/subsystems/DriveTrain.java |  3 +-
 .../java/frc/robot/subsystems/Ncoder.java     | 32 ++++++++++++
 8 files changed, 99 insertions(+), 17 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/GuideToTarget.java
 create mode 100644 src/main/java/frc/robot/subsystems/Ncoder.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index f3531a8..a775277 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -53,7 +53,7 @@ public class Robot extends TimedRobot {
   public static UltraSonic m_ultraSonic;
   public static Claw claw;
   public static CameraI2c camera;
-  public static Arm arm;
+  //public static Arm arm;
   public static ArmLimitSwitch m_limitSwitch;
   public static BaseLimitSwitch m_baseSwitch;
   /**
@@ -66,7 +66,7 @@ public void robotInit() {
     com.setClosedLoopControl(true);
     com.start();
     claw = new Claw();
-    arm = new Arm();
+    //arm = new Arm();
     m_baseSwitch = new BaseLimitSwitch();
     m_limitSwitch = new ArmLimitSwitch();
     camera = new CameraI2c();
diff --git a/src/main/java/frc/robot/commands/ArmToNextPosition.java b/src/main/java/frc/robot/commands/ArmToNextPosition.java
index f141cef..21cb442 100644
--- a/src/main/java/frc/robot/commands/ArmToNextPosition.java
+++ b/src/main/java/frc/robot/commands/ArmToNextPosition.java
@@ -18,7 +18,6 @@ public class ArmToNextPosition extends Command {
   static final double	speed = 1;
   public ArmToNextPosition() {
     // Use requires() here to declare subsystem dependencies
-    requires(Robot.arm);
     requires(Robot.m_baseSwitch);
     requires(Robot.m_limitSwitch);
   }
diff --git a/src/main/java/frc/robot/commands/GuideToTarget.java b/src/main/java/frc/robot/commands/GuideToTarget.java
new file mode 100644
index 0000000..bbad157
--- /dev/null
+++ b/src/main/java/frc/robot/commands/GuideToTarget.java
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+import frc.robot.subsystems.CameraI2c;
+
+public class GuideToTarget extends Command {
+  public GuideToTarget() {
+    // Use requires() here to declare subsystem dependencies
+     requires(Robot.m_driveTrain);
+     requires(Robot.camera);
+  }
+
+  // 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() {
+    Robot.camera.updateAngle();
+    TurnDegrees turn = new TurnDegrees(CameraI2c.angle);
+
+  }
+
+  // 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() {
+  }
+}
diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java
index bfbe996..827176a 100644
--- a/src/main/java/frc/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc/robot/commands/TurnDegrees.java
@@ -12,11 +12,11 @@
 import frc.robot.subsystems.DriveTrain;
 
 public class TurnDegrees extends Command {
-  public double target;
-  public double turnAngle;
+  public int target;
+  public int turnAngle;
   public boolean isLeft;
 
-  public TurnDegrees(double target) {
+  public TurnDegrees(int target) {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
     requires(Robot.m_driveTrain);
@@ -26,22 +26,22 @@ public TurnDegrees(double target) {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    turnAngle = Robot.m_driveTrain.getGyroZ() + target;
+    turnAngle = (int) Robot.m_driveTrain.getGyroZ() + target;
     isLeft = target<0;
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    double leftSpeed = 0;
-    double rightSpeed = 0;
+    int leftSpeed = 0;
+    int rightSpeed = 0;
     if(isLeft) {
-      leftSpeed = -.5;
-      rightSpeed = .5;
+      leftSpeed = -1;
+      rightSpeed = 1;
     }
     else {
-      leftSpeed = .5;
-      rightSpeed = -.5;
+      leftSpeed = 1;
+      rightSpeed = -1;
     }
       
     DriveTrain.drive(leftSpeed, rightSpeed);
diff --git a/src/main/java/frc/robot/commands/updateAngleToTarget.java b/src/main/java/frc/robot/commands/updateAngleToTarget.java
index ead963e..80d000b 100644
--- a/src/main/java/frc/robot/commands/updateAngleToTarget.java
+++ b/src/main/java/frc/robot/commands/updateAngleToTarget.java
@@ -27,7 +27,7 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    CameraI2c.updateAngle();
+   Robot.camera.updateAngle();
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/subsystems/CameraI2c.java b/src/main/java/frc/robot/subsystems/CameraI2c.java
index bb45150..a63a032 100644
--- a/src/main/java/frc/robot/subsystems/CameraI2c.java
+++ b/src/main/java/frc/robot/subsystems/CameraI2c.java
@@ -27,13 +27,13 @@ public void initDefaultCommand() {
     // Set the default command for a subsystem here.
      setDefaultCommand(new updateAngleToTarget());
   }
-  public static void updateAngle(){
+  public void updateAngle(){
     if(true){
       read();
       SmartDashboard.putNumber("Angle to target", angle);
     }
   }
-  public static int read(){
+  public int read(){
     String received = "";
     char[] ch = new char[20];
     byte[] toSend = new byte[1];
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 4417cd1..3ee6e5c 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commands.GuideToTarget;
 import frc.robot.commands.JoystickDrive;
 import com.analog.adis16448.frc.*;
 
@@ -56,7 +57,7 @@ public static void drive(double leftSpeed, double rightSpeed) {
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
-    setDefaultCommand(new JoystickDrive());
+    setDefaultCommand(new GuideToTarget());
   }
   public void setFast(){
     speedModifier = 1.0;
diff --git a/src/main/java/frc/robot/subsystems/Ncoder.java b/src/main/java/frc/robot/subsystems/Ncoder.java
new file mode 100644
index 0000000..b43303d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Ncoder.java
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Subsystem;
+import edu.wpi.first.wpilibj.Encoder;
+
+/**
+ * Add your docs here.
+ */
+public class Ncoder extends Subsystem {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+
+  Encoder enemity = new Encoder(0, 0);
+
+  @Override
+  public void initDefaultCommand() {
+    // Set the default command for a subsystem here.
+    // setDefaultCommand(new MySpecialCommand());
+  }
+
+  public void displayValues(Encoder e)
+  {
+    //int val = e.getRate();
+  }
+}

From 687a4897ebe5a10c44c5b2e6a1fc4fe831e812b9 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Wed, 6 Feb 2019 20:51:12 -0500
Subject: [PATCH 13/50] Recommiting the merge

---
 src/main/java/frc/robot/OI.java                    | 10 ----------
 src/main/java/frc/robot/subsystems/DriveTrain.java |  4 ----
 2 files changed, 14 deletions(-)

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index e5af792..5d54ed3 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -12,17 +12,14 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
-<<<<<<< HEAD
 import edu.wpi.first.wpilibj.buttons.Trigger;
 import frc.robot.commands.*;
 import frc.robot.commands.ArmToNextPosition;
 import frc.robot.commands.ClawReverse;
-=======
 
 import edu.wpi.first.wpilibj.buttons.Trigger;
 import frc.robot.commands.*;
 
->>>>>>> e1d8ed795ab1fb08dc431a63c18e89745f1fa786
 import frc.robot.commands.MoveClaw;
 import frc.robot.commands.SetSpeed;
 
@@ -73,12 +70,5 @@ public OI() {
     b2.whenPressed(new SetSpeed(false));
     b2.close();
     Button aButton = new JoystickButton(xBoxControl, 1);
-<<<<<<< HEAD
-    aButton.whenPressed(new ArmToNextPosition());
-    aButton.close();
-    
-=======
-
->>>>>>> e1d8ed795ab1fb08dc431a63c18e89745f1fa786
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 229a691..cf4f7f0 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -44,12 +44,8 @@ public DriveTrain() {
 
   }
 
-<<<<<<< HEAD
-  public static void drive(double leftSpeed, double rightSpeed) {
-=======
   //maybe change back to static (broken code?)
   public void drive(double leftSpeed, double rightSpeed) {
->>>>>>> e1d8ed795ab1fb08dc431a63c18e89745f1fa786
     diffDrive.arcadeDrive(leftSpeed * speedModifier, rightSpeed * speedModifier);
     Shuffleboard.selectTab("Live Window");
     Shuffleboard.update();

From 4fb905b7ed7859b7faf9b2ce2b4af582f8241390 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Thu, 7 Feb 2019 19:54:34 -0500
Subject: [PATCH 14/50] ADIS16448 library updated to r2

---
 vendordeps/ADIS16448.json | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/vendordeps/ADIS16448.json b/vendordeps/ADIS16448.json
index 3069c78..09aa0e8 100644
--- a/vendordeps/ADIS16448.json
+++ b/vendordeps/ADIS16448.json
@@ -1,7 +1,7 @@
 {
     "fileName": "ADIS16448.json",
     "name": "ADIS16448-IMU",
-    "version": "2019-r1",
+    "version": "2019-r2",
     "uuid": "38c21ab6-aa8b-44bc-b844-8086c77f09ec",
     "mavenUrls": [
         "http://maven.highcurrent.io/maven"
@@ -11,7 +11,7 @@
         {
             "groupId": "com.github.juchong",
             "artifactId": "adis16448-java",
-            "version": "2019-r1"
+            "version": "2019-r2"
         }
     ],
     "jniDependencies": [],
@@ -19,7 +19,7 @@
         {
             "groupId": "com.github.juchong",
             "artifactId": "adis16448-cpp",
-            "version": "2019-r1",
+            "version": "2019-r2",
             "libName": "libadis16448imu",
             "headerClassifier": "headers",
             "sharedLibrary": false,

From bad0c7d99d3291afcba692529fa4af88fb68bd5d Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 9 Feb 2019 13:28:43 -0500
Subject: [PATCH 15/50] Got Encoder and Vision working

---
 src/main/java/frc/robot/OI.java               |  7 +-
 src/main/java/frc/robot/Robot.java            | 21 +++--
 .../frc/robot/commands/GuideToTarget.java     |  4 +-
 .../{ArmToNextPosition.java => MoveArm.java}  | 35 +++-----
 .../frc/robot/commands/PneumaticsDrive.java   |  4 +-
 .../java/frc/robot/commands/TurnDegrees.java  | 72 ++++++++++-----
 .../robot/commands/updateAngleToTarget.java   |  2 +-
 src/main/java/frc/robot/subsystems/Arm.java   | 26 ++++--
 .../frc/robot/subsystems/ArmLimitSwitch.java  | 30 -------
 .../frc/robot/subsystems/BaseLimitSwitch.java | 31 -------
 .../java/frc/robot/subsystems/CameraI2c.java  | 12 ++-
 .../java/frc/robot/subsystems/DriveTrain.java | 15 +++-
 vendordeps/Phoenix.json                       | 87 +++++++++++++++++++
 13 files changed, 205 insertions(+), 141 deletions(-)
 rename src/main/java/frc/robot/commands/{ArmToNextPosition.java => MoveArm.java} (65%)
 delete mode 100644 src/main/java/frc/robot/subsystems/ArmLimitSwitch.java
 delete mode 100644 src/main/java/frc/robot/subsystems/BaseLimitSwitch.java
 create mode 100644 vendordeps/Phoenix.json

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 5d54ed3..f9ec2f2 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -14,7 +14,6 @@
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import edu.wpi.first.wpilibj.buttons.Trigger;
 import frc.robot.commands.*;
-import frc.robot.commands.ArmToNextPosition;
 import frc.robot.commands.ClawReverse;
 
 import edu.wpi.first.wpilibj.buttons.Trigger;
@@ -22,6 +21,7 @@
 
 import frc.robot.commands.MoveClaw;
 import frc.robot.commands.SetSpeed;
+import frc.robot.subsystems.CameraI2c;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -59,9 +59,12 @@ public class OI {
   public static XboxController xBoxControl = new XboxController(0);
 
   public OI() {
+    int cameraRead = 0;
    // Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
    Button button = new JoystickButton(xBoxControl, 2);
-   button.whenPressed(new TurnDegrees(10));
+   button.whenPressed(new TurnDegrees());
+
+
     Button b1 = new JoystickButton(xBoxControl, 6);
     b1.whenPressed(new SetSpeed(true));
     //b1.whenReleased(new SetSpeed(false));
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index a775277..173deb8 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -17,11 +17,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.ExampleCommand;
 import frc.robot.commands.GetDistance;
-import frc.robot.commands.PneumaticsDrive;
 import frc.robot.commands.Teleop;
 import frc.robot.subsystems.Arm;
-import frc.robot.subsystems.ArmLimitSwitch;
-import frc.robot.subsystems.BaseLimitSwitch;
 import frc.robot.subsystems.CameraI2c;
 import frc.robot.subsystems.Claw;
 import frc.robot.subsystems.DriveTrain;
@@ -44,6 +41,11 @@ public class Robot extends TimedRobot {
   public static OI m_oi;
   public static DriveTrain m_driveTrain;
   //public static final ADIS16448_IMU imu = new ADIS16448_IMU(); ;
+/**
+ *
+ */
+
+  //private static final boolean PUT_NUMBER = SmartDashboard.putNumber("Gyro-Z", m_driveTrain.getGyroZ());
   
   public static Teleop m_teleop;
   Command m_autonomousCommand;
@@ -53,9 +55,7 @@ public class Robot extends TimedRobot {
   public static UltraSonic m_ultraSonic;
   public static Claw claw;
   public static CameraI2c camera;
-  //public static Arm arm;
-  public static ArmLimitSwitch m_limitSwitch;
-  public static BaseLimitSwitch m_baseSwitch;
+  public static Arm arm;
   /**
    * This function is run when the robot is first started up and should be used
    * for any initialization code.
@@ -66,9 +66,7 @@ public void robotInit() {
     com.setClosedLoopControl(true);
     com.start();
     claw = new Claw();
-    //arm = new Arm();
-    m_baseSwitch = new BaseLimitSwitch();
-    m_limitSwitch = new ArmLimitSwitch();
+    arm = new Arm();
     camera = new CameraI2c();
     m_driveTrain = new DriveTrain();
     m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
@@ -94,9 +92,13 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
+    //CameraI2c.read();
+    /*
     SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
     SmartDashboard.putNumber("Gyro-Y", m_driveTrain.getGyroY());
     SmartDashboard.putNumber("Gyro-Z", m_driveTrain.getGyroZ());
+    */
+
     /*
     SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
     SmartDashboard.putNumber("Gyro-Y", imu.getAngleY());
@@ -180,6 +182,7 @@ public void teleopInit() {
    */
   @Override
   public void teleopPeriodic() {
+    CameraI2c.read();
     Scheduler.getInstance().run();
   }
 
diff --git a/src/main/java/frc/robot/commands/GuideToTarget.java b/src/main/java/frc/robot/commands/GuideToTarget.java
index bbad157..8c3327d 100644
--- a/src/main/java/frc/robot/commands/GuideToTarget.java
+++ b/src/main/java/frc/robot/commands/GuideToTarget.java
@@ -21,13 +21,13 @@ public GuideToTarget() {
   // 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() {
-    Robot.camera.updateAngle();
-    TurnDegrees turn = new TurnDegrees(CameraI2c.angle);
+    
 
   }
 
diff --git a/src/main/java/frc/robot/commands/ArmToNextPosition.java b/src/main/java/frc/robot/commands/MoveArm.java
similarity index 65%
rename from src/main/java/frc/robot/commands/ArmToNextPosition.java
rename to src/main/java/frc/robot/commands/MoveArm.java
index 21cb442..a46aee1 100644
--- a/src/main/java/frc/robot/commands/ArmToNextPosition.java
+++ b/src/main/java/frc/robot/commands/MoveArm.java
@@ -7,49 +7,36 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.GenericHID.Hand;
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.OI;
 import frc.robot.Robot;
-import frc.robot.subsystems.Arm;
-import frc.robot.subsystems.ArmLimitSwitch;
-import frc.robot.subsystems.BaseLimitSwitch;
 
-public class ArmToNextPosition extends Command {
-  boolean startRaised = false;
-  static final double	speed = 1;
-  public ArmToNextPosition() {
+public class MoveArm extends Command {
+  double armInput = 0.0;
+  public MoveArm() {
     // Use requires() here to declare subsystem dependencies
-    requires(Robot.m_baseSwitch);
-    requires(Robot.m_limitSwitch);
+    requires(Robot.arm);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    startRaised = ArmLimitSwitch.getState();
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    if(startRaised){
-      Arm.raise(speed);
-    }else if(!startRaised){
-        Arm.raise(-speed);
-    }
+    armInput = OI.xBoxControl.getTriggerAxis(Hand.kLeft) - OI.xBoxControl.getTriggerAxis(Hand.kRight);
+    SmartDashboard.putNumber("Arm throttle", armInput);
+    Robot.arm.move(armInput);
+    SmartDashboard.putNumber("Encoder",Robot.arm.getRotations());
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    if(startRaised){
-      if(BaseLimitSwitch.getState()){
-        return true;
-      }
-    }else{
-      if(ArmLimitSwitch.getState()){
-        return true;
-      }
-    }
     return false;
   }
 
diff --git a/src/main/java/frc/robot/commands/PneumaticsDrive.java b/src/main/java/frc/robot/commands/PneumaticsDrive.java
index edb83e7..62086be 100644
--- a/src/main/java/frc/robot/commands/PneumaticsDrive.java
+++ b/src/main/java/frc/robot/commands/PneumaticsDrive.java
@@ -28,10 +28,10 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    if(OI.xBoxControl.getTriggerAxis(Hand.kLeft)>0) {
+    if(OI.xBoxControl.getBumperPressed(Hand.kLeft)) {
       pneumatics.retract();
     }
-    if(OI.xBoxControl.getTriggerAxis(Hand.kRight)>0) {
+    if(OI.xBoxControl.getBumperPressed(Hand.kRight)) {
       pneumatics.extend();
     }
     
diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java
index 5a1ae96..9668771 100644
--- a/src/main/java/frc/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc/robot/commands/TurnDegrees.java
@@ -8,62 +8,88 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
+import frc.robot.subsystems.CameraI2c;
 import frc.robot.subsystems.DriveTrain;
 
 public class TurnDegrees extends Command {
-  public int target;
+  boolean finished = false;
+  public double target;
   public double turnAngle;
   public boolean isLeft;
-
-  public TurnDegrees(int target) {
+  double leftSpeed = 0;
+  double rightSpeed = 0;
+  public TurnDegrees() {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
     requires(Robot.m_driveTrain);
-    this.target = target;
+    
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    turnAngle = Robot.m_driveTrain.getGyroZ() + target;
+    this.target = CameraI2c.read();
+    SmartDashboard.putNumber("Target", target);
     isLeft = target<0;
-  }
-
-  // Called repeatedly when this Command is scheduled to run
-  @Override
-  protected void execute() {
-    double leftSpeed = 0;
-    double rightSpeed = 0;
+    SmartDashboard.putBoolean("IsLeft", isLeft);
+    if(isLeft){
+      turnAngle = Robot.m_driveTrain.getGyroZ() - Math.abs(target);
+    }else{
+      turnAngle = Robot.m_driveTrain.getGyroZ() + Math.abs(target);
+    }
+    SmartDashboard.putNumber("Gyro angle of target", turnAngle);
+    SmartDashboard.putNumber("Angle to target", target);
+    
     if(isLeft) {
-      leftSpeed = -1;
-      rightSpeed = 1;
+      leftSpeed = -.5;
+      rightSpeed = .5;
     }
     else {
-      leftSpeed = 1;
-      rightSpeed = -1;
+      leftSpeed = .5;
+      rightSpeed = -.5;
     }
       
-    Robot.m_driveTrain.drive(leftSpeed, rightSpeed);
+   
+    
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed);
 
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
+    Double gyroZ = Robot.m_driveTrain.getGyroZ();
     if(target == 0)
-      return true;
+      finished = true;
     else {
       if(isLeft) {
-        if(Robot.m_driveTrain.getGyroZ() <= turnAngle)
-          return true;
+        if(gyroZ <= turnAngle + 6){
+          finished = true;  
+        }else{
+          finished = false;
+        }
       }
       else {
-        if(Robot.m_driveTrain.getGyroZ() >= turnAngle)
-          return true;
+        if(gyroZ >= turnAngle - 6){
+          
+          finished = true;
+        }else{
+          finished = false;
+        }
       }
-      return false;
     }
+    SmartDashboard.putNumber("GyroZ turnangle", gyroZ);
+    SmartDashboard.putNumber("TurnAngle",turnAngle);
+    SmartDashboard.putBoolean("Stopped", finished);
+    
+    return finished;
   }
 
   // Called once after isFinished returns true
diff --git a/src/main/java/frc/robot/commands/updateAngleToTarget.java b/src/main/java/frc/robot/commands/updateAngleToTarget.java
index 80d000b..f02ba2f 100644
--- a/src/main/java/frc/robot/commands/updateAngleToTarget.java
+++ b/src/main/java/frc/robot/commands/updateAngleToTarget.java
@@ -27,7 +27,7 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-   Robot.camera.updateAngle();
+   CameraI2c.read();
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index a33c1a5..8b980a2 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -7,28 +7,40 @@
 
 package frc.robot.subsystems;
 
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.Talon;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 import frc.robot.RobotMap;
-import frc.robot.commands.ArmToNextPosition;
+import frc.robot.commands.MoveArm;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 /**
  * Add your docs here.
  */
 public class Arm extends Subsystem {
-  static Spark motor;
+  SpeedController motor;
+  Encoder armEncoder;
+  DigitalInput enc;
+  
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public Arm() {
-    motor = new Spark(7);
+    armEncoder = new Encoder(2,3);
+    motor = new WPI_TalonSRX(1);
   }
   @Override
   public void initDefaultCommand() {
+    setDefaultCommand(new MoveArm());
   }
-  public static void raise(Double speed){
-    if(ArmLimitSwitch.getState()){
-      motor.set(speed);
-    }
+  public void move(Double speed){
+    motor.set(speed);
+  }
+  public double getRotations(){
+    return armEncoder.getDistance();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/ArmLimitSwitch.java b/src/main/java/frc/robot/subsystems/ArmLimitSwitch.java
deleted file mode 100644
index ceff811..0000000
--- a/src/main/java/frc/robot/subsystems/ArmLimitSwitch.java
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.DigitalInput;
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * Add your docs here.
- */
-public class ArmLimitSwitch extends Subsystem {
-  public static boolean state;
-  static DigitalInput Switch = new DigitalInput(3);
-  // 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());
-  }
-  public static boolean getState(){
-    return Switch.get();
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/BaseLimitSwitch.java b/src/main/java/frc/robot/subsystems/BaseLimitSwitch.java
deleted file mode 100644
index 26ef15b..0000000
--- a/src/main/java/frc/robot/subsystems/BaseLimitSwitch.java
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.DigitalInput;
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * Add your docs here.
- */
-public class BaseLimitSwitch extends Subsystem {
-  // Put methods for controlling this subsystem
-  public static boolean state;
-  static DigitalInput Switch = new DigitalInput(4);
-  
-
-  @Override
-  public void initDefaultCommand() {
-    // Set the default command for a subsystem here.
-    // setDefaultCommand(new MySpecialCommand());
-  }
-  public static boolean getState(){
-    return Switch.get();
-  }
-
-}
diff --git a/src/main/java/frc/robot/subsystems/CameraI2c.java b/src/main/java/frc/robot/subsystems/CameraI2c.java
index a63a032..726309d 100644
--- a/src/main/java/frc/robot/subsystems/CameraI2c.java
+++ b/src/main/java/frc/robot/subsystems/CameraI2c.java
@@ -25,15 +25,12 @@ public class CameraI2c extends Subsystem {
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
-     setDefaultCommand(new updateAngleToTarget());
+    // setDefaultCommand(new updateAngleToTarget());
   }
-  public void updateAngle(){
-    if(true){
-      read();
-      SmartDashboard.putNumber("Angle to target", angle);
-    }
+  public static int getAngle(){
+      return angle;
   }
-  public int read(){
+  public static int read(){
     String received = "";
     char[] ch = new char[20];
     byte[] toSend = new byte[1];
@@ -48,6 +45,7 @@ public int read(){
     if(!received.isBlank()){
       angle = Integer.parseInt(received);
     }
+    SmartDashboard.putNumber("Angle to target", angle);
     return angle;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index cf4f7f0..972ba29 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -41,24 +41,32 @@ public DriveTrain() {
     rightGroup = new SpeedControllerGroup(sparkR1, sparkR2);
 
     diffDrive = new DifferentialDrive(leftGroup, rightGroup);
-
+    imu.reset();
+    imu.calibrate();
   }
 
   //maybe change back to static (broken code?)
   public void drive(double leftSpeed, double rightSpeed) {
     diffDrive.arcadeDrive(leftSpeed * speedModifier, rightSpeed * speedModifier);
+    SmartDashboard.putNumber("Gyro-Z drive", imu.getAngleZ());
     Shuffleboard.selectTab("Live Window");
     Shuffleboard.update();
     SmartDashboard.putNumber("SpeedModifier", speedModifier);
+
+    
+  }
+  public void tankDrive(double leftSpeed, double rightSpeed){
+    diffDrive.tankDrive(leftSpeed, rightSpeed);
+    SmartDashboard.putNumber("Gyro-Z drive", imu.getAngleZ());
   }
   
 
   
 
   @Override
-  public void initDefaultCommand() {
+  public void initDefaultCommand() { 
     // Set the default command for a subsystem here.
-    setDefaultCommand(new GuideToTarget());
+    setDefaultCommand(new JoystickDrive());
   }
   public void setFast(){
     speedModifier = 1.0;
@@ -67,6 +75,7 @@ public void setSlow(){
     speedModifier = 0.5;
   }
   public double getGyroX(){
+    //SmartDashboard.putNumber("Imu Z axis", imu.getAngleZ());
     return imu.getAngleX();
   }
   public double getGyroY(){
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 11836038fe6dd0fc232298a4edfd47c937b50a47 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Mon, 11 Feb 2019 19:21:08 -0500
Subject: [PATCH 16/50] Tested the arm

---
 src/main/java/frc/robot/Robot.java            |  6 +++-
 .../java/frc/robot/commands/TurnDegrees.java  |  9 ++++--
 src/main/java/frc/robot/subsystems/Arm.java   |  9 ++++--
 .../java/frc/robot/subsystems/DriveTrain.java |  2 +-
 .../java/frc/robot/subsystems/Ncoder.java     | 32 -------------------
 5 files changed, 18 insertions(+), 40 deletions(-)
 delete mode 100644 src/main/java/frc/robot/subsystems/Ncoder.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 173deb8..37457a7 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -18,6 +18,7 @@
 import frc.robot.commands.ExampleCommand;
 import frc.robot.commands.GetDistance;
 import frc.robot.commands.Teleop;
+import frc.robot.commands.TurnDegrees;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.CameraI2c;
 import frc.robot.subsystems.Claw;
@@ -79,6 +80,7 @@ public void robotInit() {
    // m_ultraSonic = new UltraSonic();
     Scheduler.getInstance().add(new GetDistance());
     //OI must be init last
+    SmartDashboard.putNumber("Offset", TurnDegrees.offset);
     m_oi = new OI();
   }
 
@@ -92,13 +94,15 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
+
     //CameraI2c.read();
     /*
     SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
     SmartDashboard.putNumber("Gyro-Y", m_driveTrain.getGyroY());
     SmartDashboard.putNumber("Gyro-Z", m_driveTrain.getGyroZ());
     */
-
+    TurnDegrees.offset = SmartDashboard.getNumber("Offset",  TurnDegrees.offset);
+    SmartDashboard.putNumber("Offset", TurnDegrees.offset);
     /*
     SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
     SmartDashboard.putNumber("Gyro-Y", imu.getAngleY());
diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java
index 9668771..81bbee9 100644
--- a/src/main/java/frc/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc/robot/commands/TurnDegrees.java
@@ -8,6 +8,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 import frc.robot.subsystems.CameraI2c;
@@ -15,6 +16,7 @@
 
 public class TurnDegrees extends Command {
   boolean finished = false;
+  public static double offset = 6;
   public double target;
   public double turnAngle;
   public boolean isLeft;
@@ -59,7 +61,8 @@ protected void initialize() {
   @Override
   protected void execute() {
     Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed);
-
+    SmartDashboard.putNumber("Offset", offset);
+    SmartDashboard.getNumber("Offset", offset);
   }
 
   // Make this return true when this Command no longer needs to run execute()
@@ -70,14 +73,14 @@ protected boolean isFinished() {
       finished = true;
     else {
       if(isLeft) {
-        if(gyroZ <= turnAngle + 6){
+        if(gyroZ <= turnAngle + offset){
           finished = true;  
         }else{
           finished = false;
         }
       }
       else {
-        if(gyroZ >= turnAngle - 6){
+        if(gyroZ >= turnAngle - offset){
           
           finished = true;
         }else{
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 8b980a2..e11f2cd 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -17,21 +17,24 @@
 import frc.robot.Robot;
 import frc.robot.RobotMap;
 import frc.robot.commands.MoveArm;
+
+import com.ctre.phoenix.motorcontrol.can.VictorSPX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 /**
  * Add your docs here.
  */
 public class Arm extends Subsystem {
-  SpeedController motor;
+  WPI_TalonSRX motor;
   Encoder armEncoder;
   DigitalInput enc;
+  double armModifier = .1;
   
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public Arm() {
     armEncoder = new Encoder(2,3);
-    motor = new WPI_TalonSRX(1);
+    motor = new WPI_TalonSRX(3);
   }
   @Override
   public void initDefaultCommand() {
@@ -43,4 +46,4 @@ public void move(Double speed){
   public double getRotations(){
     return armEncoder.getDistance();
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 972ba29..a399670 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -72,7 +72,7 @@ public void setFast(){
     speedModifier = 1.0;
   }
   public void setSlow(){
-    speedModifier = 0.5;
+    speedModifier = 0.65;
   }
   public double getGyroX(){
     //SmartDashboard.putNumber("Imu Z axis", imu.getAngleZ());
diff --git a/src/main/java/frc/robot/subsystems/Ncoder.java b/src/main/java/frc/robot/subsystems/Ncoder.java
deleted file mode 100644
index b43303d..0000000
--- a/src/main/java/frc/robot/subsystems/Ncoder.java
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.Subsystem;
-import edu.wpi.first.wpilibj.Encoder;
-
-/**
- * Add your docs here.
- */
-public class Ncoder extends Subsystem {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
-  Encoder enemity = new Encoder(0, 0);
-
-  @Override
-  public void initDefaultCommand() {
-    // Set the default command for a subsystem here.
-    // setDefaultCommand(new MySpecialCommand());
-  }
-
-  public void displayValues(Encoder e)
-  {
-    //int val = e.getRate();
-  }
-}

From f0572118bfd86b1422296bce52b98c343f1bcd5a Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 11 Feb 2019 19:26:54 -0500
Subject: [PATCH 17/50] Ultrasonic code test

---
 src/main/java/frc/robot/Robot.java                     | 5 ++++-
 src/main/java/frc/robot/subsystems/SerialDistance.java | 8 +++++---
 2 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 173deb8..f8b550b 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -75,7 +75,9 @@ public void robotInit() {
     CameraServer.getInstance().startAutomaticCapture(0);
     CameraServer.getInstance().startAutomaticCapture(1);
     Scheduler.getInstance().add(new Teleop());
-   // m_serialPort = new SerialDistance();
+
+    m_serialPort = new SerialDistance(); //TESTING***************************************
+
    // m_ultraSonic = new UltraSonic();
     Scheduler.getInstance().add(new GetDistance());
     //OI must be init last
@@ -92,6 +94,7 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
+    m_serialPort.getString();
     //CameraI2c.read();
     /*
     SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
diff --git a/src/main/java/frc/robot/subsystems/SerialDistance.java b/src/main/java/frc/robot/subsystems/SerialDistance.java
index 0c2443a..78ef299 100644
--- a/src/main/java/frc/robot/subsystems/SerialDistance.java
+++ b/src/main/java/frc/robot/subsystems/SerialDistance.java
@@ -8,6 +8,7 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.GetDistance;
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.SerialPort;
@@ -19,7 +20,7 @@ public class SerialDistance extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
-  //SerialPort sensor = new SerialPort(9600, SerialPort.Port.kOnboard, 8, SerialPort.Parity.kNone, SerialPort.StopBits.kOne);
+  SerialPort sensor = new SerialPort(9600, SerialPort.Port.kOnboard, 8, SerialPort.Parity.kNone, SerialPort.StopBits.kOne);
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
@@ -28,7 +29,8 @@ public void initDefaultCommand() {
 
   public String getString()
   {
-   // return sensor.readString();
-   return "";
+    SmartDashboard.putString("SerialDistance String", sensor.readString());
+    return sensor.readString();
+   //return "";
   }
 }

From ac03d1959cd173be6a157ed47c9a3ee11a17b2a5 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Wed, 13 Feb 2019 19:40:35 -0500
Subject: [PATCH 18/50] Ultrasonic sensor switched to serial (Still buggy)

---
 src/main/java/frc/robot/OI.java                     |  3 +++
 src/main/java/frc/robot/Robot.java                  |  9 ++++++---
 src/main/java/frc/robot/commands/GetDistance.java   | 13 ++++++-------
 src/main/java/frc/robot/subsystems/Arm.java         | 10 +++++-----
 .../java/frc/robot/subsystems/SerialDistance.java   |  3 +--
 5 files changed, 21 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index f9ec2f2..bd5f9f4 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import edu.wpi.first.wpilibj.buttons.Trigger;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.*;
 import frc.robot.commands.ClawReverse;
 
@@ -73,5 +74,7 @@ public OI() {
     b2.whenPressed(new SetSpeed(false));
     b2.close();
     Button aButton = new JoystickButton(xBoxControl, 1);
+
+    SmartDashboard.putData("Get Distance", new GetDistance());
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 5d04395..c301119 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -57,6 +57,8 @@ public class Robot extends TimedRobot {
   public static Claw claw;
   public static CameraI2c camera;
   public static Arm arm;
+  
+  public static SerialDistance m_serialDistance;
   /**
    * This function is run when the robot is first started up and should be used
    * for any initialization code.
@@ -79,8 +81,8 @@ public void robotInit() {
 
     m_serialPort = new SerialDistance(); //TESTING***************************************
 
-   // m_ultraSonic = new UltraSonic();
-    Scheduler.getInstance().add(new GetDistance());
+    //m_ultraSonic = new UltraSonic();
+    //Scheduler.getInstance().add(new GetDistance());
     //OI must be init last
     SmartDashboard.putNumber("Offset", TurnDegrees.offset);
     m_oi = new OI();
@@ -96,13 +98,14 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
-    m_serialPort.getString();
+    //m_serialPort.getString();  //REMOVE LATER LSKJDFLKSDFLJAFHSDGJOEWJ9248&(*#@!^%(*!@&$(*@&$37)))
     //CameraI2c.read();
     /*
     SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
     SmartDashboard.putNumber("Gyro-Y", m_driveTrain.getGyroY());
     SmartDashboard.putNumber("Gyro-Z", m_driveTrain.getGyroZ());
     */
+    /*
     TurnDegrees.offset = SmartDashboard.getNumber("Offset",  TurnDegrees.offset);
     SmartDashboard.putNumber("Offset", TurnDegrees.offset);
     /*
diff --git a/src/main/java/frc/robot/commands/GetDistance.java b/src/main/java/frc/robot/commands/GetDistance.java
index 8484aeb..af6a1ab 100644
--- a/src/main/java/frc/robot/commands/GetDistance.java
+++ b/src/main/java/frc/robot/commands/GetDistance.java
@@ -7,6 +7,7 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.Timer;             //TESTING)@9815831597438*@^$*&^@$*&@
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
@@ -14,26 +15,24 @@
 
 public class GetDistance extends Command {
 
-    String distance;
-    Double distanceDouble;
-
   public GetDistance() {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
+    requires(Robot.m_serialPort);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    //SmartDashboard.putString("asldjfdsSerialDistance String", Robot.m_serialPort.getString());
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    distance = Robot.m_serialPort.getString();
-    SmartDashboard.putString("distance", distance.replace("R",""));
-    distanceDouble = Robot.m_ultraSonic.distance();
-    SmartDashboard.putString("Distance double", distanceDouble +"");
+    SmartDashboard.putString("SerialDistance String", Robot.m_serialPort.getString());//.replace("R", "") );
+    //distance = Robot.m_serialPort.getString();
+    //SmartDashboard.putString("distance", distance.replace("R",""));
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index e11f2cd..e3cb8ae 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -18,14 +18,14 @@
 import frc.robot.RobotMap;
 import frc.robot.commands.MoveArm;
 
-import com.ctre.phoenix.motorcontrol.can.VictorSPX;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+//import com.ctre.phoenix.motorcontrol.can.VictorSPX;
+//import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 /**
  * Add your docs here.
  */
 public class Arm extends Subsystem {
-  WPI_TalonSRX motor;
+  //WPI_TalonSRX motor;
   Encoder armEncoder;
   DigitalInput enc;
   double armModifier = .1;
@@ -34,14 +34,14 @@ public class Arm extends Subsystem {
   // here. Call these from Commands.
   public Arm() {
     armEncoder = new Encoder(2,3);
-    motor = new WPI_TalonSRX(3);
+    //motor = new WPI_TalonSRX(3);
   }
   @Override
   public void initDefaultCommand() {
     setDefaultCommand(new MoveArm());
   }
   public void move(Double speed){
-    motor.set(speed);
+    //motor.set(speed);
   }
   public double getRotations(){
     return armEncoder.getDistance();
diff --git a/src/main/java/frc/robot/subsystems/SerialDistance.java b/src/main/java/frc/robot/subsystems/SerialDistance.java
index 78ef299..ff0c574 100644
--- a/src/main/java/frc/robot/subsystems/SerialDistance.java
+++ b/src/main/java/frc/robot/subsystems/SerialDistance.java
@@ -24,12 +24,11 @@ public class SerialDistance extends Subsystem {
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
-     setDefaultCommand(new GetDistance());
+    setDefaultCommand(new GetDistance());
   }
 
   public String getString()
   {
-    SmartDashboard.putString("SerialDistance String", sensor.readString());
     return sensor.readString();
    //return "";
   }

From edc5acb7c6cc54c047fe45ef67983ce0c28126c1 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Wed, 13 Feb 2019 19:40:52 -0500
Subject: [PATCH 19/50] Commiting, arm works

---
 src/main/java/frc/robot/commands/MoveArm.java   |  2 +-
 src/main/java/frc/robot/subsystems/Arm.java     | 17 +++++++++++------
 .../java/frc/robot/subsystems/DriveTrain.java   |  2 +-
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/commands/MoveArm.java b/src/main/java/frc/robot/commands/MoveArm.java
index a46aee1..1d9842e 100644
--- a/src/main/java/frc/robot/commands/MoveArm.java
+++ b/src/main/java/frc/robot/commands/MoveArm.java
@@ -28,7 +28,7 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    armInput = OI.xBoxControl.getTriggerAxis(Hand.kLeft) - OI.xBoxControl.getTriggerAxis(Hand.kRight);
+    armInput = (OI.xBoxControl.getTriggerAxis(Hand.kLeft) + OI.xBoxControl.getTriggerAxis(Hand.kRight))*.5;
     SmartDashboard.putNumber("Arm throttle", armInput);
     Robot.arm.move(armInput);
     SmartDashboard.putNumber("Encoder",Robot.arm.getRotations());
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index e11f2cd..5e48186 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -12,20 +12,21 @@
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.Talon;
+import edu.wpi.first.wpilibj.GenericHID.Hand;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.OI;
 import frc.robot.Robot;
 import frc.robot.RobotMap;
 import frc.robot.commands.MoveArm;
-
-import com.ctre.phoenix.motorcontrol.can.VictorSPX;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+//import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 /**
  * Add your docs here.
  */
 public class Arm extends Subsystem {
-  WPI_TalonSRX motor;
+  //WPI_TalonSRX motor;
+  Spark spark;
   Encoder armEncoder;
   DigitalInput enc;
   double armModifier = .1;
@@ -34,14 +35,18 @@ public class Arm extends Subsystem {
   // here. Call these from Commands.
   public Arm() {
     armEncoder = new Encoder(2,3);
-    motor = new WPI_TalonSRX(3);
+    //motor = new WPI_TalonSRX(3);
+    spark = new Spark(0);
   }
   @Override
   public void initDefaultCommand() {
     setDefaultCommand(new MoveArm());
   }
   public void move(Double speed){
-    motor.set(speed);
+    SmartDashboard.putNumber("Arm", speed);
+    spark.set((OI.xBoxControl.getTriggerAxis(Hand.kRight) -
+     OI.xBoxControl.getTriggerAxis(Hand.kLeft))*.5);
+    //motor.set(speed);
   }
   public double getRotations(){
     return armEncoder.getDistance();
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index a399670..5a56a45 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -26,7 +26,7 @@ public class DriveTrain extends Subsystem {
   public static final ADIS16448_IMU imu = new ADIS16448_IMU();
   public static boolean fast;
   private static Double speedModifier = 1.0;
-  public Spark sparkL1 = new Spark(0);
+  public Spark sparkL1 = new Spark(8);
   public Spark sparkL2 = new Spark(1);
   public Spark sparkR1 = new Spark(2);
   public Spark sparkR2 = new Spark(3);

From 4c127c4b03006490645d85471aacba04d15c478e Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Thu, 14 Feb 2019 21:53:24 -0500
Subject: [PATCH 20/50] Ultrasonic get distance now returns correct number in
 mm.

---
 src/main/java/frc/robot/OI.java               |  2 --
 src/main/java/frc/robot/Robot.java            | 22 +++++++-----
 .../java/frc/robot/commands/GetDistance.java  | 28 +++++++++------
 .../frc/robot/subsystems/SerialDistance.java  | 35 -------------------
 .../java/frc/robot/subsystems/UltraSonic.java | 15 +++++---
 5 files changed, 42 insertions(+), 60 deletions(-)
 delete mode 100644 src/main/java/frc/robot/subsystems/SerialDistance.java

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index bd5f9f4..e8b13ae 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -74,7 +74,5 @@ public OI() {
     b2.whenPressed(new SetSpeed(false));
     b2.close();
     Button aButton = new JoystickButton(xBoxControl, 1);
-
-    SmartDashboard.putData("Get Distance", new GetDistance());
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index c301119..2426a7a 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -13,6 +13,8 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.ExampleCommand;
@@ -25,8 +27,11 @@
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.pneumatics;
+
+import java.awt.Color;
+import java.util.Map;
+
 import com.analog.adis16448.frc.ADIS16448_IMU;
-import frc.robot.subsystems.SerialDistance;
 import frc.robot.subsystems.UltraSonic;
 
 /**
@@ -52,13 +57,12 @@ public class Robot extends TimedRobot {
   Command m_autonomousCommand;
   Compressor com = new Compressor(0);
   SendableChooser<Command> m_chooser = new SendableChooser<>();
-  public static SerialDistance m_serialPort;
-  public static UltraSonic m_ultraSonic;
   public static Claw claw;
   public static CameraI2c camera;
   public static Arm arm;
-  
-  public static SerialDistance m_serialDistance;
+
+  public static UltraSonic m_ultraSonic;
+
   /**
    * This function is run when the robot is first started up and should be used
    * for any initialization code.
@@ -79,12 +83,12 @@ public void robotInit() {
     CameraServer.getInstance().startAutomaticCapture(1);
     Scheduler.getInstance().add(new Teleop());
 
-    m_serialPort = new SerialDistance(); //TESTING***************************************
-
-    //m_ultraSonic = new UltraSonic();
+    m_ultraSonic = new UltraSonic();
+    
     //Scheduler.getInstance().add(new GetDistance());
     //OI must be init last
-    SmartDashboard.putNumber("Offset", TurnDegrees.offset);
+
+    SmartDashboard.putNumber("Offset", TurnDegrees.offset); 
     m_oi = new OI();
   }
 
diff --git a/src/main/java/frc/robot/commands/GetDistance.java b/src/main/java/frc/robot/commands/GetDistance.java
index af6a1ab..789df38 100644
--- a/src/main/java/frc/robot/commands/GetDistance.java
+++ b/src/main/java/frc/robot/commands/GetDistance.java
@@ -7,32 +7,40 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.Timer;             //TESTING)@9815831597438*@^$*&^@$*&@
+import java.awt.Color;
+import java.util.Map;
+
+import edu.wpi.first.wpilibj.Timer; //TESTING)@9815831597438*@^$*&^@$*&@
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
-import frc.robot.subsystems.SerialDistance;
 
 public class GetDistance extends Command {
-
+  double distance;
+  boolean warning;
   public GetDistance() {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
-    requires(Robot.m_serialPort);
+    requires(Robot.m_ultraSonic);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    //SmartDashboard.putString("asldjfdsSerialDistance String", Robot.m_serialPort.getString());
+    // Called repeatedly when this Command is scheduled to run
   }
-
-  // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    SmartDashboard.putString("SerialDistance String", Robot.m_serialPort.getString());//.replace("R", "") );
-    //distance = Robot.m_serialPort.getString();
-    //SmartDashboard.putString("distance", distance.replace("R",""));
+    //Color distanceColor = new Color((int)(Robot.m_ultraSonic.voltage()*51), 50, 100);
+    distance = Robot.m_ultraSonic.distance();
+    warning = false;
+    if(distance < 360) {
+      warning = true;
+    }
+    SmartDashboard.putBoolean("WARNING", warning);
+    SmartDashboard.putNumber("Ultrasonic Distance", distance);
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/subsystems/SerialDistance.java b/src/main/java/frc/robot/subsystems/SerialDistance.java
deleted file mode 100644
index ff0c574..0000000
--- a/src/main/java/frc/robot/subsystems/SerialDistance.java
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.Subsystem;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.commands.GetDistance;
-import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.SerialPort;
-
-/**
- * Add your docs here.
- */
-public class SerialDistance extends Subsystem {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
-  SerialPort sensor = new SerialPort(9600, SerialPort.Port.kOnboard, 8, SerialPort.Parity.kNone, SerialPort.StopBits.kOne);
-  @Override
-  public void initDefaultCommand() {
-    // Set the default command for a subsystem here.
-    setDefaultCommand(new GetDistance());
-  }
-
-  public String getString()
-  {
-    return sensor.readString();
-   //return "";
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/UltraSonic.java b/src/main/java/frc/robot/subsystems/UltraSonic.java
index 327b291..8f0a311 100644
--- a/src/main/java/frc/robot/subsystems/UltraSonic.java
+++ b/src/main/java/frc/robot/subsystems/UltraSonic.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commands.GetDistance;
 
 /**
  * Add your docs here.
@@ -19,21 +20,27 @@ public class UltraSonic extends Subsystem {
   // here. Call these from Commands.
   
   AnalogInput sonicSensor = new AnalogInput(0);
+  private double distance = 0.0;
+  private double volt = 0.0;
+  private double raw = 0.0;
 
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
     // setDefaultCommand(new MySpecialCommand());
-    
+    setDefaultCommand(new GetDistance());
   }
   public Double distance(){
-    return getVoltage()* 100/512;//("volt" * scale factor/sensitivity)
+    distance = voltage()*1024;
+    return distance; //distance(mm) = volt*1024
   }
 
-  public Double getVoltage()
+  public Double voltage()
   {
-      Double volt = sonicSensor.getVoltage();
+      volt = sonicSensor.getVoltage();
+      raw = sonicSensor.getValue();
       SmartDashboard.putNumber("AnalogInput Voltage", volt);
+      SmartDashboard.putNumber("AnalogInput Raw Value", raw);
       return volt;
   }
 }

From c0ea901ee590577f42ff2018f01976c9116f0be6 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 16 Feb 2019 11:21:40 -0500
Subject: [PATCH 21/50] Added Halleffect and other subsystems

---
 src/main/java/frc/robot/OI.java               |  6 +-
 src/main/java/frc/robot/Robot.java            | 13 ++---
 ...etDistance.java => HalleffectCounter.java} | 18 +++---
 .../frc/robot/commands/JoystickDrive.java     |  8 ++-
 src/main/java/frc/robot/commands/Rumbler.java | 58 +++++++++++++++++++
 .../java/frc/robot/commands/TeleMoveArm.java  | 48 +++++++++++++++
 src/main/java/frc/robot/subsystems/Arm.java   |  9 ++-
 src/main/java/frc/robot/subsystems/Claw.java  |  4 +-
 .../java/frc/robot/subsystems/DriveTrain.java |  2 +-
 .../java/frc/robot/subsystems/Hallefect.java  | 37 ++++++++++++
 ...erialDistance.java => TelescopingArm.java} | 25 ++++----
 .../java/frc/robot/subsystems/UltraSonic.java |  3 +-
 12 files changed, 191 insertions(+), 40 deletions(-)
 rename src/main/java/frc/robot/commands/{GetDistance.java => HalleffectCounter.java} (70%)
 create mode 100644 src/main/java/frc/robot/commands/Rumbler.java
 create mode 100644 src/main/java/frc/robot/commands/TeleMoveArm.java
 create mode 100644 src/main/java/frc/robot/subsystems/Hallefect.java
 rename src/main/java/frc/robot/subsystems/{SerialDistance.java => TelescopingArm.java} (62%)

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index bd5f9f4..f73cadb 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -17,8 +17,6 @@
 import frc.robot.commands.*;
 import frc.robot.commands.ClawReverse;
 
-import edu.wpi.first.wpilibj.buttons.Trigger;
-import frc.robot.commands.*;
 
 import frc.robot.commands.MoveClaw;
 import frc.robot.commands.SetSpeed;
@@ -60,7 +58,6 @@ public class OI {
   public static XboxController xBoxControl = new XboxController(0);
 
   public OI() {
-    int cameraRead = 0;
    // Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
    Button button = new JoystickButton(xBoxControl, 2);
    button.whenPressed(new TurnDegrees());
@@ -74,7 +71,6 @@ public OI() {
     b2.whenPressed(new SetSpeed(false));
     b2.close();
     Button aButton = new JoystickButton(xBoxControl, 1);
-
-    SmartDashboard.putData("Get Distance", new GetDistance());
+    aButton.whileHeld(new TeleMoveArm());
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index c301119..5aa6fd2 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -16,7 +16,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.ExampleCommand;
-import frc.robot.commands.GetDistance;
 import frc.robot.commands.Teleop;
 import frc.robot.commands.TurnDegrees;
 import frc.robot.subsystems.Arm;
@@ -24,9 +23,10 @@
 import frc.robot.subsystems.Claw;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.Hallefect;
+import frc.robot.subsystems.TelescopingArm;
 import frc.robot.subsystems.pneumatics;
 import com.analog.adis16448.frc.ADIS16448_IMU;
-import frc.robot.subsystems.SerialDistance;
 import frc.robot.subsystems.UltraSonic;
 
 /**
@@ -52,13 +52,12 @@ public class Robot extends TimedRobot {
   Command m_autonomousCommand;
   Compressor com = new Compressor(0);
   SendableChooser<Command> m_chooser = new SendableChooser<>();
-  public static SerialDistance m_serialPort;
   public static UltraSonic m_ultraSonic;
   public static Claw claw;
   public static CameraI2c camera;
   public static Arm arm;
-  
-  public static SerialDistance m_serialDistance;
+  public static TelescopingArm m_teleArm;
+  public static Hallefect m_hallEfect;
   /**
    * This function is run when the robot is first started up and should be used
    * for any initialization code.
@@ -78,8 +77,8 @@ public void robotInit() {
     CameraServer.getInstance().startAutomaticCapture(0);
     CameraServer.getInstance().startAutomaticCapture(1);
     Scheduler.getInstance().add(new Teleop());
-
-    m_serialPort = new SerialDistance(); //TESTING***************************************
+    m_teleArm = new TelescopingArm();
+    m_hallEfect = new Hallefect();
 
     //m_ultraSonic = new UltraSonic();
     //Scheduler.getInstance().add(new GetDistance());
diff --git a/src/main/java/frc/robot/commands/GetDistance.java b/src/main/java/frc/robot/commands/HalleffectCounter.java
similarity index 70%
rename from src/main/java/frc/robot/commands/GetDistance.java
rename to src/main/java/frc/robot/commands/HalleffectCounter.java
index af6a1ab..0d2849e 100644
--- a/src/main/java/frc/robot/commands/GetDistance.java
+++ b/src/main/java/frc/robot/commands/HalleffectCounter.java
@@ -7,32 +7,30 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.Timer;             //TESTING)@9815831597438*@^$*&^@$*&@
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
-import frc.robot.subsystems.SerialDistance;
+import frc.robot.subsystems.Hallefect;
 
-public class GetDistance extends Command {
-
-  public GetDistance() {
+public class HalleffectCounter extends Command {
+  public HalleffectCounter() {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
-    requires(Robot.m_serialPort);
+    requires(Robot.m_hallEfect);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    //SmartDashboard.putString("asldjfdsSerialDistance String", Robot.m_serialPort.getString());
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    SmartDashboard.putString("SerialDistance String", Robot.m_serialPort.getString());//.replace("R", "") );
-    //distance = Robot.m_serialPort.getString();
-    //SmartDashboard.putString("distance", distance.replace("R",""));
+    if(Robot.m_hallEfect.getPulse()){
+      Hallefect.setCount(Hallefect.getCount()+1);
+    }
+    SmartDashboard.putNumber("Count", Hallefect.getCount());
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index b1bb015..e8d5ce8 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -29,8 +29,12 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    Robot.m_driveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX());
-    // SmartDashboard.putNumber("Gryo-X: ", Robot.m_driveTrain.getGyroX());
+    //Robot.m_driveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX());
+    double leftSpeed = OI.xBoxControl.getY(Hand.kLeft);
+    double rightSpeed = OI.xBoxControl.getY(Hand.kRight);
+    Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed);
+    SmartDashboard.putNumber("Left Speed: ", leftSpeed);
+    SmartDashboard.putNumber("Right Speed: ", rightSpeed);
   }
   
 
diff --git a/src/main/java/frc/robot/commands/Rumbler.java b/src/main/java/frc/robot/commands/Rumbler.java
new file mode 100644
index 0000000..d2c97a8
--- /dev/null
+++ b/src/main/java/frc/robot/commands/Rumbler.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* 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.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.OI;
+import frc.robot.Robot;
+import frc.robot.subsystems.UltraSonic;
+
+public class Rumbler extends Command {
+public static double zone = 18;
+
+  public Rumbler() {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    requires(Robot.m_ultraSonic);
+  }
+
+  // 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() {
+    double distance = Robot.m_ultraSonic.distance();
+
+    if(distance < zone){
+      distance %= zone;
+      OI.xBoxControl.setRumble(RumbleType.kLeftRumble, zone - (distance/zone));
+      OI.xBoxControl.setRumble(RumbleType.kRightRumble, zone - (distance/zone));
+    }
+  }
+
+  // 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() {
+  }
+}
diff --git a/src/main/java/frc/robot/commands/TeleMoveArm.java b/src/main/java/frc/robot/commands/TeleMoveArm.java
new file mode 100644
index 0000000..a7769d8
--- /dev/null
+++ b/src/main/java/frc/robot/commands/TeleMoveArm.java
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+import frc.robot.subsystems.TelescopingArm;
+
+public class TeleMoveArm extends Command {
+  public TeleMoveArm() {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.m_teleArm.moveArm();
+  }
+
+  // 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 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() {
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index d0f62e9..beb6fd6 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -26,12 +26,15 @@
  */
 public class Arm extends Subsystem {
   //WPI_TalonSRX motor;
+  private static final double leftMod = .95;
+  private static final double rightMod = 1;
   Spark spark;
   Encoder armEncoder;
   DigitalInput enc;
   double armModifier = .1;
   DigitalInput switchBottom;
   DigitalInput switchTop;
+  Spark sparkR;
   
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
@@ -39,15 +42,17 @@ public Arm() {
     armEncoder = new Encoder(2,3);
     //motor = new WPI_TalonSRX(3);
     spark = new Spark(4);
+    sparkR = new Spark(6);
   }
   @Override
   public void initDefaultCommand() {
     setDefaultCommand(new MoveArm());
   }
   public void move(){
-    double speed = (-OI.xBoxControl.getTriggerAxis(Hand.kRight) + OI.xBoxControl.getTriggerAxis(Hand.kLeft))*.5;
+    double speed = (OI.xBoxControl.getTriggerAxis(Hand.kRight) - OI.xBoxControl.getTriggerAxis(Hand.kLeft))*1;
     SmartDashboard.putNumber("Arm", speed);
-    spark.setSpeed(speed);
+    spark.setSpeed(speed * leftMod);
+    //sparkR.setSpeed(speed * rightMod);
     //motor.set(speed);
   }
   public double getRotations(){
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index 8f0e687..c66fd90 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -25,8 +25,8 @@ public void initDefaultCommand() {
     // Set the default command for a subsystem here.
      setDefaultCommand(new MoveClaw());
   }
-  static Spark claw1 = new Spark(4);
-  static Spark claw2 = new Spark(5);
+  static Spark claw1 = new Spark(9);
+  static Spark claw2 = new Spark(8);
   public static void moveClaw(double speed){
     claw1.set(speed * speedModifier);
     claw2.set(speed * speedModifier);
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 5a56a45..eaad90c 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -26,7 +26,7 @@ public class DriveTrain extends Subsystem {
   public static final ADIS16448_IMU imu = new ADIS16448_IMU();
   public static boolean fast;
   private static Double speedModifier = 1.0;
-  public Spark sparkL1 = new Spark(8);
+  public Spark sparkL1 = new Spark(7);
   public Spark sparkL2 = new Spark(1);
   public Spark sparkR1 = new Spark(2);
   public Spark sparkR2 = new Spark(3);
diff --git a/src/main/java/frc/robot/subsystems/Hallefect.java b/src/main/java/frc/robot/subsystems/Hallefect.java
new file mode 100644
index 0000000..bc1276c
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Hallefect.java
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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.DigitalInput;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.commands.HalleffectCounter;
+
+/**
+ * Add your docs here.
+ */
+public class Hallefect extends Subsystem {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+  DigitalInput sensor = new DigitalInput(0);
+  static double m_count = 0.0;
+
+  @Override
+  public void initDefaultCommand() {
+    // Set the default command for a subsystem here.
+     setDefaultCommand(new HalleffectCounter());
+  }
+  public boolean getPulse(){
+    return sensor.get();
+  }
+  public static double getCount(){
+    return m_count;
+  }
+  public static void setCount(double count){
+    m_count = count;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/SerialDistance.java b/src/main/java/frc/robot/subsystems/TelescopingArm.java
similarity index 62%
rename from src/main/java/frc/robot/subsystems/SerialDistance.java
rename to src/main/java/frc/robot/subsystems/TelescopingArm.java
index ff0c574..485400c 100644
--- a/src/main/java/frc/robot/subsystems/SerialDistance.java
+++ b/src/main/java/frc/robot/subsystems/TelescopingArm.java
@@ -7,29 +7,34 @@
 
 package frc.robot.subsystems;
 
+import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.commands.GetDistance;
-import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.SerialPort;
 
 /**
  * Add your docs here.
  */
-public class SerialDistance extends Subsystem {
+public class TelescopingArm extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
-  SerialPort sensor = new SerialPort(9600, SerialPort.Port.kOnboard, 8, SerialPort.Parity.kNone, SerialPort.StopBits.kOne);
+  //Spark spark = new Spark(3);
+  double speed = 0.5;
+
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
-    setDefaultCommand(new GetDistance());
+    // setDefaultCommand(new MySpecialCommand());
   }
 
-  public String getString()
+  public void moveArm()
   {
-    return sensor.readString();
-   //return "";
+    //spark.setSpeed(speed);
+  }
+  public void setForward(){
+    speed = Math.abs(speed);
+  }
+  public void setBackwards(){
+    speed = -Math.abs(speed);
   }
 }
+
diff --git a/src/main/java/frc/robot/subsystems/UltraSonic.java b/src/main/java/frc/robot/subsystems/UltraSonic.java
index 327b291..1145702 100644
--- a/src/main/java/frc/robot/subsystems/UltraSonic.java
+++ b/src/main/java/frc/robot/subsystems/UltraSonic.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commands.Rumbler;
 
 /**
  * Add your docs here.
@@ -24,7 +25,7 @@ public class UltraSonic extends Subsystem {
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
     // setDefaultCommand(new MySpecialCommand());
-    
+    setDefaultCommand(new Rumbler());
   }
   public Double distance(){
     return getVoltage()* 100/512;//("volt" * scale factor/sensitivity)

From d22583d96f6cf6d4dfc8d8095c5fb316f88da891 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 16 Feb 2019 15:00:05 -0500
Subject: [PATCH 22/50] Got photoResistor working, got telecoping arm working,
 and got the arm working

---
 src/main/java/frc/robot/OI.java               | 11 ++---
 src/main/java/frc/robot/Robot.java            |  3 ++
 .../frc/robot/commands/HalleffectCounter.java | 11 +++--
 src/main/java/frc/robot/commands/MoveArm.java |  1 +
 .../java/frc/robot/commands/StopTeleArm.java  | 46 ++++++++++++++++++
 .../java/frc/robot/commands/TeleMoveArm.java  | 13 +++--
 .../robot/commands/getPhotoresistorValue.java | 47 +++++++++++++++++++
 src/main/java/frc/robot/subsystems/Arm.java   | 22 +++++++--
 src/main/java/frc/robot/subsystems/Claw.java  |  8 ++--
 .../java/frc/robot/subsystems/DriveTrain.java |  6 +--
 .../frc/robot/subsystems/Photoresistor.java   | 30 ++++++++++++
 .../frc/robot/subsystems/TelescopingArm.java  |  9 ++--
 12 files changed, 181 insertions(+), 26 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/StopTeleArm.java
 create mode 100644 src/main/java/frc/robot/commands/getPhotoresistorValue.java
 create mode 100644 src/main/java/frc/robot/subsystems/Photoresistor.java

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index f73cadb..97fca92 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -15,11 +15,6 @@
 import edu.wpi.first.wpilibj.buttons.Trigger;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.*;
-import frc.robot.commands.ClawReverse;
-
-
-import frc.robot.commands.MoveClaw;
-import frc.robot.commands.SetSpeed;
 import frc.robot.subsystems.CameraI2c;
 
 /**
@@ -71,6 +66,10 @@ public OI() {
     b2.whenPressed(new SetSpeed(false));
     b2.close();
     Button aButton = new JoystickButton(xBoxControl, 1);
-    aButton.whileHeld(new TeleMoveArm());
+    aButton.whenPressed(new TeleMoveArm(false));
+    aButton.whenReleased(new StopTeleArm());
+    Button xButton = new JoystickButton(xBoxControl, 3);
+    xButton.whenPressed(new TeleMoveArm(true));
+    xButton.whenReleased(new StopTeleArm());
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 5aa6fd2..ea8d82c 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -24,6 +24,7 @@
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.Hallefect;
+import frc.robot.subsystems.Photoresistor;
 import frc.robot.subsystems.TelescopingArm;
 import frc.robot.subsystems.pneumatics;
 import com.analog.adis16448.frc.ADIS16448_IMU;
@@ -58,6 +59,7 @@ public class Robot extends TimedRobot {
   public static Arm arm;
   public static TelescopingArm m_teleArm;
   public static Hallefect m_hallEfect;
+  public static Photoresistor resistor;
   /**
    * This function is run when the robot is first started up and should be used
    * for any initialization code.
@@ -79,6 +81,7 @@ public void robotInit() {
     Scheduler.getInstance().add(new Teleop());
     m_teleArm = new TelescopingArm();
     m_hallEfect = new Hallefect();
+    resistor = new Photoresistor();
 
     //m_ultraSonic = new UltraSonic();
     //Scheduler.getInstance().add(new GetDistance());
diff --git a/src/main/java/frc/robot/commands/HalleffectCounter.java b/src/main/java/frc/robot/commands/HalleffectCounter.java
index 0d2849e..ea4d5ba 100644
--- a/src/main/java/frc/robot/commands/HalleffectCounter.java
+++ b/src/main/java/frc/robot/commands/HalleffectCounter.java
@@ -13,6 +13,7 @@
 import frc.robot.subsystems.Hallefect;
 
 public class HalleffectCounter extends Command {
+
   public HalleffectCounter() {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
@@ -22,15 +23,17 @@ public HalleffectCounter() {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    if(Robot.m_hallEfect.getPulse()){
+      Hallefect.setCount(Hallefect.getCount()+1);
+    }
+    SmartDashboard.putNumber("Count", Hallefect.getCount());
+    
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    if(Robot.m_hallEfect.getPulse()){
-      Hallefect.setCount(Hallefect.getCount()+1);
-    }
-    SmartDashboard.putNumber("Count", Hallefect.getCount());
+    
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/commands/MoveArm.java b/src/main/java/frc/robot/commands/MoveArm.java
index 0ab63ff..b47aefe 100644
--- a/src/main/java/frc/robot/commands/MoveArm.java
+++ b/src/main/java/frc/robot/commands/MoveArm.java
@@ -29,6 +29,7 @@ protected void initialize() {
   @Override
   protected void execute() {
     Robot.arm.move();
+    SmartDashboard.putBoolean("Limit switch", Robot.arm.getSwitch());
     SmartDashboard.putNumber("Encoder",Robot.arm.getRotations());
   }
 
diff --git a/src/main/java/frc/robot/commands/StopTeleArm.java b/src/main/java/frc/robot/commands/StopTeleArm.java
new file mode 100644
index 0000000..12e06bb
--- /dev/null
+++ b/src/main/java/frc/robot/commands/StopTeleArm.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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 StopTeleArm extends Command {
+  public StopTeleArm() {
+    // Use requires() here to declare subsystem dependencies
+    requires(Robot.m_teleArm);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.m_teleArm.stop();
+  }
+
+  // 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 true;
+  }
+
+  // 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/commands/TeleMoveArm.java b/src/main/java/frc/robot/commands/TeleMoveArm.java
index a7769d8..8e386d2 100644
--- a/src/main/java/frc/robot/commands/TeleMoveArm.java
+++ b/src/main/java/frc/robot/commands/TeleMoveArm.java
@@ -12,14 +12,21 @@
 import frc.robot.subsystems.TelescopingArm;
 
 public class TeleMoveArm extends Command {
-  public TeleMoveArm() {
+  boolean setForward;
+  public TeleMoveArm(boolean setForward) {
     // Use requires() here to declare subsystem dependencies
-    // eg. requires(chassis);
+    this.setForward = setForward;
+    requires(Robot.m_teleArm);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    if(setForward){
+      Robot.m_teleArm.setForward();
+    }else{
+      Robot.m_teleArm.setBackwards();
+    }
     Robot.m_teleArm.moveArm();
   }
 
@@ -32,7 +39,7 @@ protected void execute() {
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+    return true;
   }
 
   // Called once after isFinished returns true
diff --git a/src/main/java/frc/robot/commands/getPhotoresistorValue.java b/src/main/java/frc/robot/commands/getPhotoresistorValue.java
new file mode 100644
index 0000000..701760e
--- /dev/null
+++ b/src/main/java/frc/robot/commands/getPhotoresistorValue.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Robot;
+
+public class getPhotoresistorValue extends Command {
+  public getPhotoresistorValue() {
+    // Use requires() here to declare subsystem dependencies
+    requires(Robot.resistor);
+  }
+
+  // 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() {
+    SmartDashboard.putNumber("PhotoResistor value", Robot.resistor.value());
+  }
+
+  // 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() {
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index beb6fd6..14d06e2 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -32,7 +32,7 @@ public class Arm extends Subsystem {
   Encoder armEncoder;
   DigitalInput enc;
   double armModifier = .1;
-  DigitalInput switchBottom;
+  static DigitalInput switchBottom;
   DigitalInput switchTop;
   Spark sparkR;
   
@@ -41,8 +41,9 @@ public class Arm extends Subsystem {
   public Arm() {
     armEncoder = new Encoder(2,3);
     //motor = new WPI_TalonSRX(3);
-    spark = new Spark(4);
+    spark = new Spark(5);
     sparkR = new Spark(6);
+    switchBottom = new DigitalInput(1);
   }
   @Override
   public void initDefaultCommand() {
@@ -51,11 +52,26 @@ public void initDefaultCommand() {
   public void move(){
     double speed = (OI.xBoxControl.getTriggerAxis(Hand.kRight) - OI.xBoxControl.getTriggerAxis(Hand.kLeft))*1;
     SmartDashboard.putNumber("Arm", speed);
-    spark.setSpeed(speed * leftMod);
+    if(speed > 0){
+      if(getSwitch()){
+        spark.setSpeed(0);
+        sparkR.setSpeed(0);
+      }else{
+        spark.setSpeed(speed * leftMod);
+        sparkR.setSpeed(speed * rightMod);
+      }
+    }else if(speed < 0){
+      spark.setSpeed(speed * leftMod);
+      sparkR.setSpeed(speed * rightMod);
+    }
+    
     //sparkR.setSpeed(speed * rightMod);
     //motor.set(speed);
   }
   public double getRotations(){
     return armEncoder.getDistance();
   }
+  public static boolean getSwitch(){
+    return switchBottom.get();
+  }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index c66fd90..ef0b2ab 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -25,11 +25,11 @@ public void initDefaultCommand() {
     // Set the default command for a subsystem here.
      setDefaultCommand(new MoveClaw());
   }
-  static Spark claw1 = new Spark(9);
-  static Spark claw2 = new Spark(8);
+  //static Spark claw1 = new Spark(9);
+  //static Spark claw2 = new Spark(8);
   public static void moveClaw(double speed){
-    claw1.set(speed * speedModifier);
-    claw2.set(speed * speedModifier);
+   // claw1.set(speed * speedModifier);
+   // claw2.set(speed * speedModifier);
     SmartDashboard.putNumber("Claw Speed", speed);
     
   }
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index eaad90c..8cca4a5 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -26,10 +26,10 @@ public class DriveTrain extends Subsystem {
   public static final ADIS16448_IMU imu = new ADIS16448_IMU();
   public static boolean fast;
   private static Double speedModifier = 1.0;
-  public Spark sparkL1 = new Spark(7);
+  public Spark sparkL1 = new Spark(0);
   public Spark sparkL2 = new Spark(1);
-  public Spark sparkR1 = new Spark(2);
-  public Spark sparkR2 = new Spark(3);
+  public Spark sparkR1 = new Spark(8);
+  public Spark sparkR2 = new Spark(9);
 
   SpeedControllerGroup leftGroup;
   SpeedControllerGroup rightGroup;
diff --git a/src/main/java/frc/robot/subsystems/Photoresistor.java b/src/main/java/frc/robot/subsystems/Photoresistor.java
new file mode 100644
index 0000000..837688b
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Photoresistor.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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.AnalogInput;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.commands.getPhotoresistorValue;
+
+/**
+ * Add your docs here.
+ */
+public class Photoresistor extends Subsystem {
+  AnalogInput sensor = new AnalogInput(2);
+  // 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 getPhotoresistorValue());
+  }
+  public double value(){
+    return sensor.getVoltage();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/TelescopingArm.java b/src/main/java/frc/robot/subsystems/TelescopingArm.java
index 485400c..3e9bf29 100644
--- a/src/main/java/frc/robot/subsystems/TelescopingArm.java
+++ b/src/main/java/frc/robot/subsystems/TelescopingArm.java
@@ -17,8 +17,8 @@ public class TelescopingArm extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
-  //Spark spark = new Spark(3);
-  double speed = 0.5;
+  Spark spark = new Spark(2);
+  double speed = 1;
 
   @Override
   public void initDefaultCommand() {
@@ -28,7 +28,7 @@ public void initDefaultCommand() {
 
   public void moveArm()
   {
-    //spark.setSpeed(speed);
+    spark.set(speed);
   }
   public void setForward(){
     speed = Math.abs(speed);
@@ -36,5 +36,8 @@ public void setForward(){
   public void setBackwards(){
     speed = -Math.abs(speed);
   }
+  public void stop(){
+    spark.set(0);
+  }
 }
 

From 3ab95e0469cf7b6fdfe81f62aab8f160a9d5f9eb Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 18 Feb 2019 14:39:06 -0500
Subject: [PATCH 23/50] RobotMap created and deployed

---
 src/main/java/frc/robot/RobotMap.java          | 18 +++++++++++-------
 src/main/java/frc/robot/subsystems/Arm.java    |  2 +-
 src/main/java/frc/robot/subsystems/Claw.java   |  5 +++--
 .../java/frc/robot/subsystems/DriveTrain.java  | 15 +++++++--------
 4 files changed, 22 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 8baa409..696c2ba 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -24,11 +24,15 @@ 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 int UltraSonic = 1;
-  public static int ArmLimitSwitch = 2;
-  public static int leftMotor1 = 0;
-  public static int leftMotor2 = 1;
-  public static int leftMotor3 = 2;
-  public static int leftMotor4 = 3;
-  public static int arm = 4;
+  public final static int ULTRASONIC = 1;
+  public final static int ARMLIMITSWITCH = 2;
+  
+  public final static int LEFTMOTOR_1 = 0;
+  public final static int LEFTMOTOR_2 = 1;
+  public final static int RIGHTMOTOR_1 = 2;
+  public final static int RIGHTMOTOR_2 = 3;
+  public final static int ARM = 4;
+  public final static int CLAW_1 = 5;
+  public final static int CLAW_2 = 6;
+
 }
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index d0f62e9..160e32f 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -38,7 +38,7 @@ public class Arm extends Subsystem {
   public Arm() {
     armEncoder = new Encoder(2,3);
     //motor = new WPI_TalonSRX(3);
-    spark = new Spark(4);
+    spark = new Spark(RobotMap.ARM);
   }
   @Override
   public void initDefaultCommand() {
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index 8f0e687..f1cd753 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.RobotMap;
 import frc.robot.commands.MoveClaw;
 
 /**
@@ -25,8 +26,8 @@ public void initDefaultCommand() {
     // Set the default command for a subsystem here.
      setDefaultCommand(new MoveClaw());
   }
-  static Spark claw1 = new Spark(4);
-  static Spark claw2 = new Spark(5);
+  static Spark claw1 = new Spark(RobotMap.CLAW_1);
+  static Spark claw2 = new Spark(RobotMap.CLAW_2);
   public static void moveClaw(double speed){
     claw1.set(speed * speedModifier);
     claw2.set(speed * speedModifier);
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 5a56a45..9adee13 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.RobotMap;
 import frc.robot.commands.GuideToTarget;
 import frc.robot.commands.JoystickDrive;
 import com.analog.adis16448.frc.*;
@@ -24,12 +25,11 @@ public class DriveTrain extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public static final ADIS16448_IMU imu = new ADIS16448_IMU();
-  public static boolean fast;
-  private static Double speedModifier = 1.0;
-  public Spark sparkL1 = new Spark(8);
-  public Spark sparkL2 = new Spark(1);
-  public Spark sparkR1 = new Spark(2);
-  public Spark sparkR2 = new Spark(3);
+  public static Double speedModifier = 1.0;
+  public Spark sparkL1 = new Spark(RobotMap.LEFTMOTOR_1);
+  public Spark sparkL2 = new Spark(RobotMap.LEFTMOTOR_2);
+  public Spark sparkR1 = new Spark(RobotMap.RIGHTMOTOR_1);
+  public Spark sparkR2 = new Spark(RobotMap.RIGHTMOTOR_2);
 
   SpeedControllerGroup leftGroup;
   SpeedControllerGroup rightGroup;
@@ -48,12 +48,11 @@ public DriveTrain() {
   //maybe change back to static (broken code?)
   public void drive(double leftSpeed, double rightSpeed) {
     diffDrive.arcadeDrive(leftSpeed * speedModifier, rightSpeed * speedModifier);
+
     SmartDashboard.putNumber("Gyro-Z drive", imu.getAngleZ());
     Shuffleboard.selectTab("Live Window");
     Shuffleboard.update();
     SmartDashboard.putNumber("SpeedModifier", speedModifier);
-
-    
   }
   public void tankDrive(double leftSpeed, double rightSpeed){
     diffDrive.tankDrive(leftSpeed, rightSpeed);

From 18f740d601a10ada3d81e81ddb26d2c5db3eef81 Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 18 Feb 2019 16:39:46 -0500
Subject: [PATCH 24/50] Sparks for DriveTrain replaced with Talons (untested),
 code cleaned up for SmartDashboard and ShuffleBoard.

---
 .../java/frc/robot/commands/ClawReverse.java  |  1 -
 .../java/frc/robot/commands/GetDistance.java  |  5 +-
 src/main/java/frc/robot/commands/MoveArm.java |  2 +-
 .../java/frc/robot/commands/MoveClaw.java     |  2 +-
 .../java/frc/robot/commands/TurnDegrees.java  | 14 ++--
 src/main/java/frc/robot/subsystems/Arm.java   |  5 --
 src/main/java/frc/robot/subsystems/Claw.java  |  4 +-
 .../java/frc/robot/subsystems/DriveTrain.java | 33 +++++++---
 .../java/frc/robot/subsystems/UltraSonic.java |  4 --
 .../java/frc/robot/subsystems/pneumatics.java |  3 +-
 vendordeps/Phoenix.json                       | 64 ++++++++++++++++---
 11 files changed, 93 insertions(+), 44 deletions(-)

diff --git a/src/main/java/frc/robot/commands/ClawReverse.java b/src/main/java/frc/robot/commands/ClawReverse.java
index 775042c..c9fba70 100644
--- a/src/main/java/frc/robot/commands/ClawReverse.java
+++ b/src/main/java/frc/robot/commands/ClawReverse.java
@@ -22,7 +22,6 @@ public ClawReverse() {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    SmartDashboard.putBoolean("Claw Reversed", true);
     Claw.moveClaw(1);
   }
 
diff --git a/src/main/java/frc/robot/commands/GetDistance.java b/src/main/java/frc/robot/commands/GetDistance.java
index 789df38..aaa0213 100644
--- a/src/main/java/frc/robot/commands/GetDistance.java
+++ b/src/main/java/frc/robot/commands/GetDistance.java
@@ -10,7 +10,6 @@
 import java.awt.Color;
 import java.util.Map;
 
-import edu.wpi.first.wpilibj.Timer; //TESTING)@9815831597438*@^$*&^@$*&@
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
@@ -35,10 +34,12 @@ protected void initialize() {
   protected void execute() {
     //Color distanceColor = new Color((int)(Robot.m_ultraSonic.voltage()*51), 50, 100);
     distance = Robot.m_ultraSonic.distance();
-    warning = false;
     if(distance < 360) {
       warning = true;
     }
+    else {
+      warning = false;
+    }
     SmartDashboard.putBoolean("WARNING", warning);
     SmartDashboard.putNumber("Ultrasonic Distance", distance);
   }
diff --git a/src/main/java/frc/robot/commands/MoveArm.java b/src/main/java/frc/robot/commands/MoveArm.java
index 0ab63ff..606eb56 100644
--- a/src/main/java/frc/robot/commands/MoveArm.java
+++ b/src/main/java/frc/robot/commands/MoveArm.java
@@ -29,7 +29,7 @@ protected void initialize() {
   @Override
   protected void execute() {
     Robot.arm.move();
-    SmartDashboard.putNumber("Encoder",Robot.arm.getRotations());
+    //SmartDashboard.putNumber("Encoder",Robot.arm.getRotations());
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/commands/MoveClaw.java b/src/main/java/frc/robot/commands/MoveClaw.java
index 46ec6bc..9b91edb 100644
--- a/src/main/java/frc/robot/commands/MoveClaw.java
+++ b/src/main/java/frc/robot/commands/MoveClaw.java
@@ -22,7 +22,7 @@ public MoveClaw() {
   @Override
   protected void initialize() {
     Claw.moveClaw(-1);
-    SmartDashboard.putBoolean("Claw Forward", true);
+    //SmartDashboard.putBoolean("Claw Forward", true);
   }
 
   // Called repeatedly when this Command is scheduled to run
diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java
index 81bbee9..c9f2dfb 100644
--- a/src/main/java/frc/robot/commands/TurnDegrees.java
+++ b/src/main/java/frc/robot/commands/TurnDegrees.java
@@ -33,16 +33,16 @@ public TurnDegrees() {
   @Override
   protected void initialize() {
     this.target = CameraI2c.read();
-    SmartDashboard.putNumber("Target", target);
+    //SmartDashboard.putNumber("Target", target);
     isLeft = target<0;
-    SmartDashboard.putBoolean("IsLeft", isLeft);
+    //SmartDashboard.putBoolean("IsLeft", isLeft);
     if(isLeft){
       turnAngle = Robot.m_driveTrain.getGyroZ() - Math.abs(target);
     }else{
       turnAngle = Robot.m_driveTrain.getGyroZ() + Math.abs(target);
     }
-    SmartDashboard.putNumber("Gyro angle of target", turnAngle);
-    SmartDashboard.putNumber("Angle to target", target);
+    //SmartDashboard.putNumber("Gyro angle of target", turnAngle);
+    //SmartDashboard.putNumber("Angle to target", target);
     
     if(isLeft) {
       leftSpeed = -.5;
@@ -88,9 +88,9 @@ protected boolean isFinished() {
         }
       }
     }
-    SmartDashboard.putNumber("GyroZ turnangle", gyroZ);
-    SmartDashboard.putNumber("TurnAngle",turnAngle);
-    SmartDashboard.putBoolean("Stopped", finished);
+    //SmartDashboard.putNumber("GyroZ turnangle", gyroZ);
+    //SmartDashboard.putNumber("TurnAngle",turnAngle);
+    //SmartDashboard.putBoolean("Stopped", finished);
     
     return finished;
   }
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 160e32f..26fc884 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -19,13 +19,11 @@
 import frc.robot.Robot;
 import frc.robot.RobotMap;
 import frc.robot.commands.MoveArm;
-//import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 /**
  * Add your docs here.
  */
 public class Arm extends Subsystem {
-  //WPI_TalonSRX motor;
   Spark spark;
   Encoder armEncoder;
   DigitalInput enc;
@@ -37,7 +35,6 @@ public class Arm extends Subsystem {
   // here. Call these from Commands.
   public Arm() {
     armEncoder = new Encoder(2,3);
-    //motor = new WPI_TalonSRX(3);
     spark = new Spark(RobotMap.ARM);
   }
   @Override
@@ -46,9 +43,7 @@ public void initDefaultCommand() {
   }
   public void move(){
     double speed = (-OI.xBoxControl.getTriggerAxis(Hand.kRight) + OI.xBoxControl.getTriggerAxis(Hand.kLeft))*.5;
-    SmartDashboard.putNumber("Arm", speed);
     spark.setSpeed(speed);
-    //motor.set(speed);
   }
   public double getRotations(){
     return armEncoder.getDistance();
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index f1cd753..22ca4a4 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -30,8 +30,6 @@ public void initDefaultCommand() {
   static Spark claw2 = new Spark(RobotMap.CLAW_2);
   public static void moveClaw(double speed){
     claw1.set(speed * speedModifier);
-    claw2.set(speed * speedModifier);
-    SmartDashboard.putNumber("Claw Speed", speed);
-    
+    claw2.set(speed * speedModifier);    
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 9adee13..e130509 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -18,18 +18,28 @@
 import frc.robot.commands.JoystickDrive;
 import com.analog.adis16448.frc.*;
 
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
 /**
  * Add your docs here.
  */
 public class DriveTrain extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
+
+  public WPI_TalonSRX talonL1;
+  public WPI_TalonSRX talonL2;
+  public WPI_TalonSRX talonR1;
+  public WPI_TalonSRX talonR2;
+
   public static final ADIS16448_IMU imu = new ADIS16448_IMU();
-  public static Double speedModifier = 1.0;
+  public static Double speedModifier;
+  /*
   public Spark sparkL1 = new Spark(RobotMap.LEFTMOTOR_1);
   public Spark sparkL2 = new Spark(RobotMap.LEFTMOTOR_2);
   public Spark sparkR1 = new Spark(RobotMap.RIGHTMOTOR_1);
   public Spark sparkR2 = new Spark(RobotMap.RIGHTMOTOR_2);
+  */
 
   SpeedControllerGroup leftGroup;
   SpeedControllerGroup rightGroup;
@@ -37,8 +47,15 @@ public class DriveTrain extends Subsystem {
   public DifferentialDrive diffDrive;
 
   public DriveTrain() {
-    leftGroup = new SpeedControllerGroup(sparkL1, sparkL2);
-    rightGroup = new SpeedControllerGroup(sparkR1, sparkR2);
+    talonL1 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_1);
+    talonL2 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_2);
+    talonR1 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_1);
+    talonR2 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_2);
+
+    speedModifier = 1.0;
+    
+    leftGroup = new SpeedControllerGroup(talonL1, talonL2);
+    rightGroup = new SpeedControllerGroup(talonR1, talonR2);
 
     diffDrive = new DifferentialDrive(leftGroup, rightGroup);
     imu.reset();
@@ -48,15 +65,9 @@ public DriveTrain() {
   //maybe change back to static (broken code?)
   public void drive(double leftSpeed, double rightSpeed) {
     diffDrive.arcadeDrive(leftSpeed * speedModifier, rightSpeed * speedModifier);
-
-    SmartDashboard.putNumber("Gyro-Z drive", imu.getAngleZ());
-    Shuffleboard.selectTab("Live Window");
-    Shuffleboard.update();
-    SmartDashboard.putNumber("SpeedModifier", speedModifier);
   }
   public void tankDrive(double leftSpeed, double rightSpeed){
     diffDrive.tankDrive(leftSpeed, rightSpeed);
-    SmartDashboard.putNumber("Gyro-Z drive", imu.getAngleZ());
   }
   
 
@@ -83,6 +94,8 @@ public double getGyroY(){
   public double getGyroZ(){
     return imu.getAngleZ();
   }
-
+  public void resetGyro() {
+    imu.reset();
+  }
   
 }
diff --git a/src/main/java/frc/robot/subsystems/UltraSonic.java b/src/main/java/frc/robot/subsystems/UltraSonic.java
index 8f0a311..607b63a 100644
--- a/src/main/java/frc/robot/subsystems/UltraSonic.java
+++ b/src/main/java/frc/robot/subsystems/UltraSonic.java
@@ -22,7 +22,6 @@ public class UltraSonic extends Subsystem {
   AnalogInput sonicSensor = new AnalogInput(0);
   private double distance = 0.0;
   private double volt = 0.0;
-  private double raw = 0.0;
 
   @Override
   public void initDefaultCommand() {
@@ -38,9 +37,6 @@ public Double distance(){
   public Double voltage()
   {
       volt = sonicSensor.getVoltage();
-      raw = sonicSensor.getValue();
-      SmartDashboard.putNumber("AnalogInput Voltage", volt);
-      SmartDashboard.putNumber("AnalogInput Raw Value", raw);
       return volt;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/pneumatics.java b/src/main/java/frc/robot/subsystems/pneumatics.java
index bc285de..dea0a82 100644
--- a/src/main/java/frc/robot/subsystems/pneumatics.java
+++ b/src/main/java/frc/robot/subsystems/pneumatics.java
@@ -20,8 +20,7 @@ public class pneumatics extends Subsystem {
   // here. Call these from Commands.
   public static Compressor compressor = new Compressor(0);
   public static DoubleSolenoid doubleSol = new DoubleSolenoid(0, 1);
-  public pneumatics(){
-    System.out.println("Compressor Init");
+  public pneumatics() {
     compressor = new Compressor(0);
     compressor.setClosedLoopControl(true);
     compressor.start();
diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json
index a1654ec..c12b5aa 100644
--- a/vendordeps/Phoenix.json
+++ b/vendordeps/Phoenix.json
@@ -1,7 +1,7 @@
 {
     "fileName": "Phoenix.json",
     "name": "CTRE-Phoenix",
-    "version": "5.12.1",
+    "version": "5.14.0",
     "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
     "mavenUrls": [
         "http://devsite.ctr-electronics.com/maven/release/"
@@ -11,19 +11,19 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "api-java",
-            "version": "5.12.1"
+            "version": "5.14.0"
         },
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "wpiapi-java",
-            "version": "5.12.1"
+            "version": "5.14.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "cci",
-            "version": "5.12.1",
+            "version": "5.14.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -31,13 +31,35 @@
                 "windowsx86-64",
                 "linuxx86-64"
             ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "canutils",
+            "version": "5.14.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "platform-stub",
+            "version": "5.14.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
         }
     ],
     "cppDependencies": [
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "wpiapi-cpp",
-            "version": "5.12.1",
+            "version": "5.14.0",
             "libName": "CTRE_Phoenix_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -51,7 +73,7 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "api-cpp",
-            "version": "5.12.1",
+            "version": "5.14.0",
             "libName": "CTRE_Phoenix",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -65,7 +87,7 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "cci",
-            "version": "5.12.1",
+            "version": "5.14.0",
             "libName": "CTRE_PhoenixCCI",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -76,10 +98,36 @@
                 "linuxx86-64"
             ]
         },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "canutils",
+            "version": "5.14.0",
+            "libName": "CTRE_PhoenixCanutils",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "platform-stub",
+            "version": "5.14.0",
+            "libName": "CTRE_PhoenixPlatform",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64"
+            ]
+        },
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "core",
-            "version": "5.12.1",
+            "version": "5.14.0",
             "libName": "CTRE_PhoenixCore",
             "headerClassifier": "headers"
         }

From a4ca15ee33b6a68b3b01b6a2ade5b1f8a8f444ae Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 18 Feb 2019 18:02:30 -0500
Subject: [PATCH 25/50] Updated ultrasonic display on Shuffleboard to Rumbler

---
 src/main/java/frc/robot/Robot.java            |  2 +-
 src/main/java/frc/robot/RobotMap.java         |  4 ++--
 src/main/java/frc/robot/commands/Rumbler.java | 11 +++++++++++
 3 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 29e641e..cc16c7e 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -203,7 +203,7 @@ public void teleopInit() {
    */
   @Override
   public void teleopPeriodic() {
-    CameraI2c.read();
+    //CameraI2c.read();
     Scheduler.getInstance().run();
   }
 
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 939aca6..ece5874 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -27,14 +27,14 @@ public class RobotMap {
 
   //Analog
   public final static int PHOTORESISTOR = 0;  
-  public final static int ULTRASONIC = 1;
+  public final static int ULTRASONIC = 3;
   //public final static int ARMLIMITSWITCH = 2;
 
   //DIO
   public final static int ARM_ENCODER_A = 2;  
   public final static int ARM_ENCODER_B = 3;
   public final static int ARM_SWITCHBOTTOM = 1;
-  public final static int HALLEFFECT = 3; //UNUSED(?)
+  public final static int HALLEFFECT = 0; //UNUSED(?)
 
   //PWM
   public final static int ARM_L = 0;
diff --git a/src/main/java/frc/robot/commands/Rumbler.java b/src/main/java/frc/robot/commands/Rumbler.java
index d2c97a8..71af5a7 100644
--- a/src/main/java/frc/robot/commands/Rumbler.java
+++ b/src/main/java/frc/robot/commands/Rumbler.java
@@ -9,12 +9,14 @@
 
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.OI;
 import frc.robot.Robot;
 import frc.robot.subsystems.UltraSonic;
 
 public class Rumbler extends Command {
 public static double zone = 18;
+public static boolean warn = false;
 
   public Rumbler() {
     // Use requires() here to declare subsystem dependencies
@@ -37,6 +39,15 @@ protected void execute() {
       OI.xBoxControl.setRumble(RumbleType.kLeftRumble, zone - (distance/zone));
       OI.xBoxControl.setRumble(RumbleType.kRightRumble, zone - (distance/zone));
     }
+    if(distance < 360) {
+      warn = true;
+    }
+    else {
+      warn = false;
+    }
+    
+    SmartDashboard.putNumber("Ultrasonic Distance", distance);
+    SmartDashboard.putBoolean("WARNING", warn);
   }
 
   // Make this return true when this Command no longer needs to run execute()

From 2aa3cf46bc84f3c7c5859b706f84eb159cef5b08 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Mon, 18 Feb 2019 18:51:35 -0500
Subject: [PATCH 26/50] Fixed claw

---
 src/main/java/frc/robot/commands/ClawReverse.java |  3 +--
 src/main/java/frc/robot/commands/MoveClaw.java    |  2 +-
 src/main/java/frc/robot/subsystems/Claw.java      | 12 ++++++------
 3 files changed, 8 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/commands/ClawReverse.java b/src/main/java/frc/robot/commands/ClawReverse.java
index 775042c..62e2217 100644
--- a/src/main/java/frc/robot/commands/ClawReverse.java
+++ b/src/main/java/frc/robot/commands/ClawReverse.java
@@ -10,7 +10,6 @@
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
-import frc.robot.subsystems.Claw;
 
 public class ClawReverse extends Command {
   public ClawReverse() {
@@ -23,7 +22,7 @@ public ClawReverse() {
   @Override
   protected void initialize() {
     SmartDashboard.putBoolean("Claw Reversed", true);
-    Claw.moveClaw(1);
+    Robot.claw.moveClaw();
   }
 
   // Called repeatedly when this Command is scheduled to run
diff --git a/src/main/java/frc/robot/commands/MoveClaw.java b/src/main/java/frc/robot/commands/MoveClaw.java
index 46ec6bc..12b9229 100644
--- a/src/main/java/frc/robot/commands/MoveClaw.java
+++ b/src/main/java/frc/robot/commands/MoveClaw.java
@@ -21,7 +21,7 @@ public MoveClaw() {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    Claw.moveClaw(-1);
+    Robot.claw.moveClaw();
     SmartDashboard.putBoolean("Claw Forward", true);
   }
 
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index ef0b2ab..35d361a 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -19,18 +19,18 @@
 public class Claw extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
+  double speed = .5;
   static double speedModifier = 1.0;
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
      setDefaultCommand(new MoveClaw());
   }
-  //static Spark claw1 = new Spark(9);
-  //static Spark claw2 = new Spark(8);
-  public static void moveClaw(double speed){
-   // claw1.set(speed * speedModifier);
-   // claw2.set(speed * speedModifier);
-    SmartDashboard.putNumber("Claw Speed", speed);
+    static Spark claw1 = new Spark(9);
+    static Spark claw2 = new Spark(8);
+  public void moveClaw(){
+   claw1.set(speed * speedModifier);
+   claw2.set(speed * speedModifier);
     
   }
 }

From 4d25d7438c898b648a8eaaf78423179bbdb53c23 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Tue, 19 Feb 2019 20:49:09 -0500
Subject: [PATCH 27/50] Exponential drive added

---
 src/main/java/frc/robot/OI.java               |  7 +-
 src/main/java/frc/robot/Robot.java            | 12 ++++
 src/main/java/frc/robot/RobotMap.java         |  7 +-
 .../java/frc/robot/commands/LowerRamps.java   | 46 +++++++++++++
 src/main/java/frc/robot/commands/Rumbler.java | 10 +--
 .../java/frc/robot/subsystems/CameraI2c.java  |  1 +
 .../java/frc/robot/subsystems/DriveTrain.java | 53 +++++++--------
 .../frc/robot/subsystems/LaserFinder.java     | 51 +++++++++++++++
 src/main/java/frc/robot/subsystems/Ramps.java | 36 +++++++++++
 .../java/frc/robot/subsystems/UltraSonic.java |  2 +-
 vendordeps/Phoenix.json                       | 64 +++----------------
 11 files changed, 191 insertions(+), 98 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/LowerRamps.java
 create mode 100644 src/main/java/frc/robot/subsystems/LaserFinder.java
 create mode 100644 src/main/java/frc/robot/subsystems/Ramps.java

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 97fca92..82bb607 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -58,18 +58,15 @@ public OI() {
    button.whenPressed(new TurnDegrees());
 
 
-    Button b1 = new JoystickButton(xBoxControl, 6);
-    b1.whenPressed(new SetSpeed(true));
     //b1.whenReleased(new SetSpeed(false));
 
-    Button b2 = new JoystickButton(xBoxControl, 5);
-    b2.whenPressed(new SetSpeed(false));
-    b2.close();
     Button aButton = new JoystickButton(xBoxControl, 1);
     aButton.whenPressed(new TeleMoveArm(false));
     aButton.whenReleased(new StopTeleArm());
     Button xButton = new JoystickButton(xBoxControl, 3);
     xButton.whenPressed(new TeleMoveArm(true));
     xButton.whenReleased(new StopTeleArm());
+    Button yButton = new JoystickButton(xBoxControl, 4);
+    yButton.whenPressed(new LowerRamps());
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index cc16c7e..b24c219 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
@@ -27,6 +28,7 @@
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.Hallefect;
 import frc.robot.subsystems.Photoresistor;
+import frc.robot.subsystems.Ramps;
 import frc.robot.subsystems.TelescopingArm;
 import frc.robot.subsystems.pneumatics;
 
@@ -66,6 +68,8 @@ public class Robot extends TimedRobot {
   public static TelescopingArm m_teleArm;
   public static Hallefect m_hallEfect;
   public static Photoresistor resistor;
+  public static Timer m_timer;
+  //public static Ramps m_ramps;
   /**
    * This function is run when the robot is first started up and should be used
    * for any initialization code.
@@ -88,6 +92,8 @@ public void robotInit() {
     m_teleArm = new TelescopingArm();
     m_hallEfect = new Hallefect();
     resistor = new Photoresistor();
+    m_timer = new Timer();
+    //m_ramps = new Ramps();
 
     //m_ultraSonic = new UltraSonic();
 
@@ -110,6 +116,12 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
+    //--------------------------Do 10 times per Second --------------------------------------------------
+    if(m_timer.hasPeriodPassed(.1)){
+      CameraI2c.read();
+      m_timer.reset();
+    }
+    ////////////////////////////////////////////////////////////////////////////////////////////////////
     //m_serialPort.getString();  //REMOVE LATER LSKJDFLKSDFLJAFHSDGJOEWJ9248&(*#@!^%(*!@&$(*@&$37)))
     //CameraI2c.read();
     /*
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index ece5874..62d34ca 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -37,11 +37,12 @@ public class RobotMap {
   public final static int HALLEFFECT = 0; //UNUSED(?)
 
   //PWM
-  public final static int ARM_L = 0;
-  public final static int ARM_R = 1;
-  public final static int TELE_ARM = 2;
+  public final static int ARM_L = 4;
+  public final static int ARM_R = 5;
+  public final static int TELE_ARM = 6;
   //public final static int CLAW_1 = 6;
   //public final static int CLAW_2 = 7;
+  public static final int RAMPS = 9;
   
   //CAN
   public final static int LEFTMOTOR_1 = 0;  
diff --git a/src/main/java/frc/robot/commands/LowerRamps.java b/src/main/java/frc/robot/commands/LowerRamps.java
new file mode 100644
index 0000000..46c4a37
--- /dev/null
+++ b/src/main/java/frc/robot/commands/LowerRamps.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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 LowerRamps extends Command {
+  public LowerRamps() {
+    // Use requires() here to declare subsystem dependencies
+   // requires(Robot.m_ramps);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    //Robot.m_ramps.lower();
+  }
+
+  // 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 true;
+  }
+
+  // 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/commands/Rumbler.java b/src/main/java/frc/robot/commands/Rumbler.java
index 71af5a7..dde7a13 100644
--- a/src/main/java/frc/robot/commands/Rumbler.java
+++ b/src/main/java/frc/robot/commands/Rumbler.java
@@ -35,11 +35,13 @@ protected void execute() {
     double distance = Robot.m_ultraSonic.distance();
 
     if(distance < zone){
-      distance %= zone;
-      OI.xBoxControl.setRumble(RumbleType.kLeftRumble, zone - (distance/zone));
-      OI.xBoxControl.setRumble(RumbleType.kRightRumble, zone - (distance/zone));
+      OI.xBoxControl.setRumble(RumbleType.kLeftRumble, 1 - (distance/zone));
+      OI.xBoxControl.setRumble(RumbleType.kRightRumble, 1 - (distance/zone));
+    }else{
+      OI.xBoxControl.setRumble(RumbleType.kLeftRumble, 0);
+      OI.xBoxControl.setRumble(RumbleType.kRightRumble, 0);
     }
-    if(distance < 360) {
+    if(distance < zone) {
       warn = true;
     }
     else {
diff --git a/src/main/java/frc/robot/subsystems/CameraI2c.java b/src/main/java/frc/robot/subsystems/CameraI2c.java
index 726309d..62d043b 100644
--- a/src/main/java/frc/robot/subsystems/CameraI2c.java
+++ b/src/main/java/frc/robot/subsystems/CameraI2c.java
@@ -45,6 +45,7 @@ public static int read(){
     if(!received.isBlank()){
       angle = Integer.parseInt(received);
     }
+    SmartDashboard.putString("RAW from arduino", received);
     SmartDashboard.putNumber("Angle to target", angle);
     return angle;
   }
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index e130509..1c3001d 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -18,7 +18,7 @@
 import frc.robot.commands.JoystickDrive;
 import com.analog.adis16448.frc.*;
 
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+//import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 /**
  * Add your docs here.
@@ -27,19 +27,20 @@ public class DriveTrain extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
-  public WPI_TalonSRX talonL1;
-  public WPI_TalonSRX talonL2;
-  public WPI_TalonSRX talonR1;
-  public WPI_TalonSRX talonR2;
-
+  //public WPI_TalonSRX talonL1;
+  //public WPI_TalonSRX talonL2;
+  //public WPI_TalonSRX talonR1;
+  //public WPI_TalonSRX talonR2;
+  
   public static final ADIS16448_IMU imu = new ADIS16448_IMU();
   public static Double speedModifier;
-  /*
-  public Spark sparkL1 = new Spark(RobotMap.LEFTMOTOR_1);
-  public Spark sparkL2 = new Spark(RobotMap.LEFTMOTOR_2);
-  public Spark sparkR1 = new Spark(RobotMap.RIGHTMOTOR_1);
-  public Spark sparkR2 = new Spark(RobotMap.RIGHTMOTOR_2);
-  */
+  private final double exponentialGrowth = .5;
+  
+  public Spark sparkL1;
+  public Spark sparkL2;
+  public Spark sparkR1;
+  public Spark sparkR2;
+  
 
   SpeedControllerGroup leftGroup;
   SpeedControllerGroup rightGroup;
@@ -47,15 +48,19 @@ public class DriveTrain extends Subsystem {
   public DifferentialDrive diffDrive;
 
   public DriveTrain() {
-    talonL1 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_1);
-    talonL2 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_2);
-    talonR1 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_1);
-    talonR2 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_2);
+    sparkL1 = new Spark(RobotMap.LEFTMOTOR_1);
+    sparkL2 = new Spark(RobotMap.LEFTMOTOR_2);
+    sparkR1 = new Spark(RobotMap.RIGHTMOTOR_1);
+    sparkR2 = new Spark(RobotMap.RIGHTMOTOR_2);
+    //talonL1 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_1);
+    //talonL2 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_2);
+    //talonR1 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_1);
+    //talonR2 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_2);
 
     speedModifier = 1.0;
     
-    leftGroup = new SpeedControllerGroup(talonL1, talonL2);
-    rightGroup = new SpeedControllerGroup(talonR1, talonR2);
+    leftGroup = new SpeedControllerGroup(sparkL1, sparkL2);
+    rightGroup = new SpeedControllerGroup(sparkR1, sparkR2);
 
     diffDrive = new DifferentialDrive(leftGroup, rightGroup);
     imu.reset();
@@ -67,7 +72,7 @@ public void drive(double leftSpeed, double rightSpeed) {
     diffDrive.arcadeDrive(leftSpeed * speedModifier, rightSpeed * speedModifier);
   }
   public void tankDrive(double leftSpeed, double rightSpeed){
-    diffDrive.tankDrive(leftSpeed, rightSpeed);
+    diffDrive.tankDrive((Math.pow(rightSpeed - 1, 3) + .5*(rightSpeed-1) + 1.5) * .75, (Math.pow(leftSpeed - 1, 3) + .5*(leftSpeed - 1) + 1.5) * .75);
   }
   
 
@@ -84,18 +89,8 @@ public void setFast(){
   public void setSlow(){
     speedModifier = 0.65;
   }
-  public double getGyroX(){
-    //SmartDashboard.putNumber("Imu Z axis", imu.getAngleZ());
-    return imu.getAngleX();
-  }
-  public double getGyroY(){
-    return imu.getAngleY();
-  }
   public double getGyroZ(){
     return imu.getAngleZ();
   }
-  public void resetGyro() {
-    imu.reset();
-  }
   
 }
diff --git a/src/main/java/frc/robot/subsystems/LaserFinder.java b/src/main/java/frc/robot/subsystems/LaserFinder.java
new file mode 100644
index 0000000..c5ef448
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/LaserFinder.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Add your docs here.
+ */
+public class LaserFinder extends Subsystem {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+  public static I2C Wire;
+  public static int distance;
+  public LaserFinder(){
+    Wire = new I2C(Port.kOnboard, 0x35);
+  }
+  @Override
+  public void initDefaultCommand() {
+    // Set the default command for a subsystem here.
+    // setDefaultCommand(new MySpecialCommand());
+  }
+  
+    public static int read(){
+      String received = "";
+      char[] ch = new char[20];
+      byte[] toSend = new byte[1];
+      byte[] fromArduino = new byte[20];
+      Wire.transaction(toSend, 1, fromArduino, 16);
+      for(int i = 0; i < fromArduino.length; i++){
+        ch[i] = (char) fromArduino[i];
+        if(ch[i] == '-' || ch[i] == '0' || ch[i] == '1' || ch[i] == '2' || ch[i] == '3'|| ch[i] == '4' || ch[i] == '5' || ch[i] == '6' || ch[i] == '7'|| ch[i] == '8' || ch[i] == '9'){
+          received += ch[i];
+        }
+      }
+      if(!received.isBlank()){
+        distance = Integer.parseInt(received);
+      }
+      SmartDashboard.putNumber("Distance from laser", distance);
+      return distance;
+    }
+  
+}
diff --git a/src/main/java/frc/robot/subsystems/Ramps.java b/src/main/java/frc/robot/subsystems/Ramps.java
new file mode 100644
index 0000000..8f9f98b
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Ramps.java
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Spark;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.RobotMap;
+
+/**
+ * Add your docs here.
+ */
+public class Ramps extends Subsystem {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+  private final double speed = .5;
+  Spark motor;
+  @Override
+  public void initDefaultCommand() {
+    // Set the default command for a subsystem here.
+    // setDefaultCommand(new MySpecialCommand());
+  }
+  public Ramps(){
+    motor = new Spark(RobotMap.RAMPS);
+  }
+  public void lower(){
+    motor.set(speed);
+  }
+  public void raise(){
+    motor.setSpeed(speed);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/UltraSonic.java b/src/main/java/frc/robot/subsystems/UltraSonic.java
index 23226fb..9a6c2da 100644
--- a/src/main/java/frc/robot/subsystems/UltraSonic.java
+++ b/src/main/java/frc/robot/subsystems/UltraSonic.java
@@ -31,7 +31,7 @@ public void initDefaultCommand() {
     setDefaultCommand(new Rumbler());
   }
   public Double distance(){
-    distance = voltage()*1024;
+    distance = voltage()*1024 /25.4;
     return distance; //distance(mm) = volt*1024
   }
 
diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json
index c12b5aa..a1654ec 100644
--- a/vendordeps/Phoenix.json
+++ b/vendordeps/Phoenix.json
@@ -1,7 +1,7 @@
 {
     "fileName": "Phoenix.json",
     "name": "CTRE-Phoenix",
-    "version": "5.14.0",
+    "version": "5.12.1",
     "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
     "mavenUrls": [
         "http://devsite.ctr-electronics.com/maven/release/"
@@ -11,19 +11,19 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "api-java",
-            "version": "5.14.0"
+            "version": "5.12.1"
         },
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "wpiapi-java",
-            "version": "5.14.0"
+            "version": "5.12.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "cci",
-            "version": "5.14.0",
+            "version": "5.12.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -31,35 +31,13 @@
                 "windowsx86-64",
                 "linuxx86-64"
             ]
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "canutils",
-            "version": "5.14.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "platform-stub",
-            "version": "5.14.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64"
-            ]
         }
     ],
     "cppDependencies": [
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "wpiapi-cpp",
-            "version": "5.14.0",
+            "version": "5.12.1",
             "libName": "CTRE_Phoenix_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -73,7 +51,7 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "api-cpp",
-            "version": "5.14.0",
+            "version": "5.12.1",
             "libName": "CTRE_Phoenix",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -87,7 +65,7 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "cci",
-            "version": "5.14.0",
+            "version": "5.12.1",
             "libName": "CTRE_PhoenixCCI",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -98,36 +76,10 @@
                 "linuxx86-64"
             ]
         },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "canutils",
-            "version": "5.14.0",
-            "libName": "CTRE_PhoenixCanutils",
-            "headerClassifier": "headers",
-            "sharedLibrary": false,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64"
-            ]
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "platform-stub",
-            "version": "5.14.0",
-            "libName": "CTRE_PhoenixPlatform",
-            "headerClassifier": "headers",
-            "sharedLibrary": false,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64"
-            ]
-        },
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "core",
-            "version": "5.14.0",
+            "version": "5.12.1",
             "libName": "CTRE_PhoenixCore",
             "headerClassifier": "headers"
         }

From a3f2bbc0256eac455f5e9da090148b3d741330a8 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Tue, 19 Feb 2019 21:38:13 -0500
Subject: [PATCH 28/50] TankDrive is now on a switch

---
 src/main/java/frc/robot/Robot.java            |  3 +++
 .../frc/robot/commands/JoystickDrive.java     | 19 +++++++++++++++----
 .../frc/robot/subsystems/LaserFinder.java     |  6 +++---
 3 files changed, 21 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index b24c219..fbaec8d 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -27,6 +27,7 @@
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.Hallefect;
+import frc.robot.subsystems.LaserFinder;
 import frc.robot.subsystems.Photoresistor;
 import frc.robot.subsystems.Ramps;
 import frc.robot.subsystems.TelescopingArm;
@@ -93,6 +94,7 @@ public void robotInit() {
     m_hallEfect = new Hallefect();
     resistor = new Photoresistor();
     m_timer = new Timer();
+    SmartDashboard.putBoolean("TankDrive", false);
     //m_ramps = new Ramps();
 
     //m_ultraSonic = new UltraSonic();
@@ -119,6 +121,7 @@ public void robotPeriodic() {
     //--------------------------Do 10 times per Second --------------------------------------------------
     if(m_timer.hasPeriodPassed(.1)){
       CameraI2c.read();
+      LaserFinder.read();
       m_timer.reset();
     }
     ////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index ccbadbb..e19da53 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -29,10 +29,21 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    //Robot.m_driveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX());
-    double leftSpeed = OI.xBoxControl.getY(Hand.kLeft);
-    double rightSpeed = OI.xBoxControl.getY(Hand.kRight);
-    Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed);
+    double rightSpeed = 0.0;
+    double leftSpeed = 0.0;
+    boolean tankDrive = false;
+    SmartDashboard.getBoolean("TankDrive", tankDrive);
+    //
+    if(tankDrive){
+      leftSpeed = OI.xBoxControl.getY(Hand.kLeft);
+      rightSpeed = OI.xBoxControl.getY(Hand.kRight);
+      Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed);
+    }else{
+      leftSpeed = OI.xBoxControl.getX(Hand.kRight);
+      rightSpeed = OI.xBoxControl.getY(Hand.kRight);
+      Robot.m_driveTrain.drive(rightSpeed, leftSpeed);
+    }
+    
     //SmartDashboard.putNumber("Left Speed: ", leftSpeed);
     //SmartDashboard.putNumber("Right Speed: ", rightSpeed);
   }
diff --git a/src/main/java/frc/robot/subsystems/LaserFinder.java b/src/main/java/frc/robot/subsystems/LaserFinder.java
index c5ef448..52e7f12 100644
--- a/src/main/java/frc/robot/subsystems/LaserFinder.java
+++ b/src/main/java/frc/robot/subsystems/LaserFinder.java
@@ -19,7 +19,7 @@ public class LaserFinder extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public static I2C Wire;
-  public static int distance;
+  public static double distance;
   public LaserFinder(){
     Wire = new I2C(Port.kOnboard, 0x35);
   }
@@ -29,7 +29,7 @@ public void initDefaultCommand() {
     // setDefaultCommand(new MySpecialCommand());
   }
   
-    public static int read(){
+    public static double read(){
       String received = "";
       char[] ch = new char[20];
       byte[] toSend = new byte[1];
@@ -42,7 +42,7 @@ public static int read(){
         }
       }
       if(!received.isBlank()){
-        distance = Integer.parseInt(received);
+        distance = Double.parseDouble(received);
       }
       SmartDashboard.putNumber("Distance from laser", distance);
       return distance;

From fda9c2ed533e1cc85a9312f54f1b48e64973c91e Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sun, 24 Feb 2019 16:32:04 -0500
Subject: [PATCH 29/50] Commiting recent changes

---
 src/main/java/frc/robot/Robot.java            |  6 +-
 src/main/java/frc/robot/RobotMap.java         | 14 +++--
 .../java/frc/robot/commands/ArmPositions.java | 59 +++++++++++++++++++
 .../frc/robot/commands/JoystickDrive.java     |  3 +-
 .../java/frc/robot/commands/laserUpdater.java | 46 +++++++++++++++
 src/main/java/frc/robot/subsystems/Claw.java  |  4 +-
 .../java/frc/robot/subsystems/DriveTrain.java | 12 ++--
 .../frc/robot/subsystems/LaserFinder.java     | 24 ++++----
 .../java/frc/robot/subsystems/pneumatics.java |  4 ++
 9 files changed, 143 insertions(+), 29 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/ArmPositions.java
 create mode 100644 src/main/java/frc/robot/commands/laserUpdater.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index fbaec8d..93c8d48 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -70,6 +70,7 @@ public class Robot extends TimedRobot {
   public static Hallefect m_hallEfect;
   public static Photoresistor resistor;
   public static Timer m_timer;
+  public static LaserFinder m_laser;
   //public static Ramps m_ramps;
   /**
    * This function is run when the robot is first started up and should be used
@@ -81,7 +82,7 @@ public void robotInit() {
     com.setClosedLoopControl(true);
     com.start();
     claw = new Claw();
-    arm = new Arm();
+    //arm = new Arm();
     camera = new CameraI2c();
     m_driveTrain = new DriveTrain();
     m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
@@ -94,6 +95,7 @@ public void robotInit() {
     m_hallEfect = new Hallefect();
     resistor = new Photoresistor();
     m_timer = new Timer();
+    //m_laser = new LaserFinder();
     SmartDashboard.putBoolean("TankDrive", false);
     //m_ramps = new Ramps();
 
@@ -121,7 +123,7 @@ public void robotPeriodic() {
     //--------------------------Do 10 times per Second --------------------------------------------------
     if(m_timer.hasPeriodPassed(.1)){
       CameraI2c.read();
-      LaserFinder.read();
+      
       m_timer.reset();
     }
     ////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 62d34ca..faf6edd 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -37,12 +37,12 @@ public class RobotMap {
   public final static int HALLEFFECT = 0; //UNUSED(?)
 
   //PWM
-  public final static int ARM_L = 4;
-  public final static int ARM_R = 5;
-  public final static int TELE_ARM = 6;
-  //public final static int CLAW_1 = 6;
-  //public final static int CLAW_2 = 7;
-  public static final int RAMPS = 9;
+  public final static int ARM_L = 3;//
+  public final static int ARM_R = 4;//
+  public final static int TELE_ARM = 1;//
+  public final static int CLAW_1 = 6;//
+  public final static int CLAW_2 = 5;//
+  public static final int RAMPS = 1;//
   
   //CAN
   public final static int LEFTMOTOR_1 = 0;  
@@ -54,4 +54,6 @@ public class RobotMap {
   public final static int PNEUMATIC_COMPRESSOR = 0;
   public final static int DOUBLESOL_FORWARD = 0;
   public final static int DOUBLESOL_REVERSE = 1;
+  public final static int DOUBLESOL_FORWARD1 = 2;
+  public static final int DOUBLESOL_REVERSE1 = 3;
 }
diff --git a/src/main/java/frc/robot/commands/ArmPositions.java b/src/main/java/frc/robot/commands/ArmPositions.java
new file mode 100644
index 0000000..4a7ec0c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ArmPositions.java
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+import frc.robot.subsystems.Arm;
+
+public class ArmPositions extends Command {
+  private static double pos1 = 0.0;
+  private static double pos2 = 0.1;
+  private static double pos3 = 0.2;
+  private static double pos4 = 0.4;
+  private static double pos5 = 0.0;
+  private static double pos6 = 0.1;
+  private static double pos7 = 0.2;
+  private static double pos8 = 0.4;
+  private static double[] positions;
+  public ArmPositions(int positionToMoveTo) {
+    // Use requires() here to declare subsystem dependencies
+    requires(Robot.arm);
+    requires(Robot.m_laser);
+  }
+
+  // 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() {
+    if(true){
+      return true;
+    }
+    return true;
+  }
+
+  // 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/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index e19da53..ba2a70d 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -32,8 +32,7 @@ protected void execute() {
     double rightSpeed = 0.0;
     double leftSpeed = 0.0;
     boolean tankDrive = false;
-    SmartDashboard.getBoolean("TankDrive", tankDrive);
-    //
+    tankDrive = SmartDashboard.getBoolean("TankDrive", tankDrive);
     if(tankDrive){
       leftSpeed = OI.xBoxControl.getY(Hand.kLeft);
       rightSpeed = OI.xBoxControl.getY(Hand.kRight);
diff --git a/src/main/java/frc/robot/commands/laserUpdater.java b/src/main/java/frc/robot/commands/laserUpdater.java
new file mode 100644
index 0000000..16d074e
--- /dev/null
+++ b/src/main/java/frc/robot/commands/laserUpdater.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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 laserUpdater extends Command {
+  public laserUpdater() {
+    // Use requires() here to declare subsystem dependencies
+    requires(Robot.m_laser);
+  }
+
+  // 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() {
+    Robot.m_laser.read();
+  }
+
+  // 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() {
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index 6eec358..5ea5978 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -27,8 +27,8 @@ public void initDefaultCommand() {
     // Set the default command for a subsystem here.
      setDefaultCommand(new MoveClaw()); 
   }
-    static Spark claw1 = new Spark(9);
-    static Spark claw2 = new Spark(8);
+    static Spark claw1 = new Spark(RobotMap.CLAW_1);
+    static Spark claw2 = new Spark(RobotMap.CLAW_2);
   public void moveClaw(){
    claw1.set(speed * speedModifier);
    claw2.set(speed * speedModifier);
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 1c3001d..8a47ba4 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -18,7 +18,7 @@
 import frc.robot.commands.JoystickDrive;
 import com.analog.adis16448.frc.*;
 
-//import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 /**
  * Add your docs here.
@@ -30,7 +30,7 @@ public class DriveTrain extends Subsystem {
   //public WPI_TalonSRX talonL1;
   //public WPI_TalonSRX talonL2;
   //public WPI_TalonSRX talonR1;
-  //public WPI_TalonSRX talonR2;
+  public WPI_TalonSRX talonR2;
   
   public static final ADIS16448_IMU imu = new ADIS16448_IMU();
   public static Double speedModifier;
@@ -49,17 +49,17 @@ public class DriveTrain extends Subsystem {
 
   public DriveTrain() {
     sparkL1 = new Spark(RobotMap.LEFTMOTOR_1);
-    sparkL2 = new Spark(RobotMap.LEFTMOTOR_2);
+    //sparkL2 = new Spark(RobotMap.LEFTMOTOR_2);
     sparkR1 = new Spark(RobotMap.RIGHTMOTOR_1);
     sparkR2 = new Spark(RobotMap.RIGHTMOTOR_2);
     //talonL1 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_1);
-    //talonL2 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_2);
+   // talonL2 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_2);
     //talonR1 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_1);
-    //talonR2 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_2);
+    talonR2 = new WPI_TalonSRX(0);
 
     speedModifier = 1.0;
     
-    leftGroup = new SpeedControllerGroup(sparkL1, sparkL2);
+    leftGroup = new SpeedControllerGroup(sparkL1, talonR2);
     rightGroup = new SpeedControllerGroup(sparkR1, sparkR2);
 
     diffDrive = new DifferentialDrive(leftGroup, rightGroup);
diff --git a/src/main/java/frc/robot/subsystems/LaserFinder.java b/src/main/java/frc/robot/subsystems/LaserFinder.java
index 52e7f12..63332cb 100644
--- a/src/main/java/frc/robot/subsystems/LaserFinder.java
+++ b/src/main/java/frc/robot/subsystems/LaserFinder.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj.I2C.Port;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commands.laserUpdater;
 
 /**
  * Add your docs here.
@@ -18,33 +19,34 @@
 public class LaserFinder extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
-  public static I2C Wire;
-  public static double distance;
+  public  I2C Wire;
+  public  double distance;
   public LaserFinder(){
-    Wire = new I2C(Port.kOnboard, 0x35);
+    Wire = new I2C(Port.kOnboard, 0x53);
   }
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
-    // setDefaultCommand(new MySpecialCommand());
+     setDefaultCommand(new laserUpdater());
   }
   
-    public static double read(){
-      String received = "";
-      char[] ch = new char[20];
+    public  double read(){
+      String received = "Error";
+      char[] ch = new char[16];
       byte[] toSend = new byte[1];
-      byte[] fromArduino = new byte[20];
+      byte[] fromArduino = new byte[16];
       Wire.transaction(toSend, 1, fromArduino, 16);
       for(int i = 0; i < fromArduino.length; i++){
         ch[i] = (char) fromArduino[i];
         if(ch[i] == '-' || ch[i] == '0' || ch[i] == '1' || ch[i] == '2' || ch[i] == '3'|| ch[i] == '4' || ch[i] == '5' || ch[i] == '6' || ch[i] == '7'|| ch[i] == '8' || ch[i] == '9'){
           received += ch[i];
-        }
+        }else{received += ch[i];}
+        
       }
       if(!received.isBlank()){
-        distance = Double.parseDouble(received);
+        //distance = Double.parseDouble(received);
       }
-      SmartDashboard.putNumber("Distance from laser", distance);
+      SmartDashboard.putString("Distance from laser", received);
       return distance;
     }
   
diff --git a/src/main/java/frc/robot/subsystems/pneumatics.java b/src/main/java/frc/robot/subsystems/pneumatics.java
index 72dcda5..f217692 100644
--- a/src/main/java/frc/robot/subsystems/pneumatics.java
+++ b/src/main/java/frc/robot/subsystems/pneumatics.java
@@ -21,6 +21,7 @@ public class pneumatics extends Subsystem {
   // here. Call these from Commands.
   public static Compressor compressor;
   public static DoubleSolenoid doubleSol = new DoubleSolenoid(RobotMap.DOUBLESOL_FORWARD, RobotMap.DOUBLESOL_REVERSE);
+  public static DoubleSolenoid doubleSol2 = new DoubleSolenoid(RobotMap.DOUBLESOL_FORWARD1, RobotMap.DOUBLESOL_REVERSE1);
   public pneumatics() {
     compressor = new Compressor(RobotMap.PNEUMATIC_COMPRESSOR);
     compressor.setClosedLoopControl(true);
@@ -35,11 +36,14 @@ public void initDefaultCommand() {
 
   public static void extend() {
     doubleSol.set(DoubleSolenoid.Value.kForward);
+    doubleSol2.set(DoubleSolenoid.Value.kForward);
   }
   public static void retract() {
     doubleSol.set(DoubleSolenoid.Value.kReverse);
+    doubleSol2.set(DoubleSolenoid.Value.kReverse);
   }
   public static void stop() {
     doubleSol.set(DoubleSolenoid.Value.kOff);
+    doubleSol2.set(DoubleSolenoid.Value.kOff);
   }
 }

From 4c49d98f14e00e4126cccab28c71ff22ef202930 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Mon, 25 Feb 2019 19:32:35 -0500
Subject: [PATCH 30/50] Mapped all of the buttons and added the string pot

---
 src/main/java/frc/robot/OI.java               | 12 ++---
 src/main/java/frc/robot/Robot.java            |  5 +-
 src/main/java/frc/robot/RobotMap.java         |  7 +--
 .../{MoveClaw.java => ClawDrive.java}         | 10 ++--
 src/main/java/frc/robot/commands/DownArm.java | 46 +++++++++++++++++++
 src/main/java/frc/robot/commands/StopArm.java | 46 +++++++++++++++++++
 .../java/frc/robot/commands/StopClaw.java     | 46 +++++++++++++++++++
 .../java/frc/robot/commands/StopRamps.java    | 45 ++++++++++++++++++
 ...{ClawReverse.java => StringpotDriver.java} | 12 ++---
 src/main/java/frc/robot/subsystems/Arm.java   | 16 ++++---
 src/main/java/frc/robot/subsystems/Claw.java  | 33 +++++++++----
 .../java/frc/robot/subsystems/DriveTrain.java | 30 ++++++------
 src/main/java/frc/robot/subsystems/Ramps.java |  3 ++
 .../java/frc/robot/subsystems/StringPot.java  | 30 ++++++++++++
 .../frc/robot/subsystems/TelescopingArm.java  |  5 +-
 15 files changed, 290 insertions(+), 56 deletions(-)
 rename src/main/java/frc/robot/commands/{MoveClaw.java => ClawDrive.java} (86%)
 create mode 100644 src/main/java/frc/robot/commands/DownArm.java
 create mode 100644 src/main/java/frc/robot/commands/StopArm.java
 create mode 100644 src/main/java/frc/robot/commands/StopClaw.java
 create mode 100644 src/main/java/frc/robot/commands/StopRamps.java
 rename src/main/java/frc/robot/commands/{ClawReverse.java => StringpotDriver.java} (85%)
 create mode 100644 src/main/java/frc/robot/subsystems/StringPot.java

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 82bb607..51ed7cf 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -54,12 +54,12 @@ public class OI {
 
   public OI() {
    // Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
-   Button button = new JoystickButton(xBoxControl, 2);
-   button.whenPressed(new TurnDegrees());
-
-
-    //b1.whenReleased(new SetSpeed(false));
-
+    Button button = new JoystickButton(xBoxControl, 2);
+    button.whenPressed(new TurnDegrees());
+    Button bumper = new JoystickButton(xBoxControl, 6);
+    bumper.whenPressed(new MoveArm());
+    Button bumper2 = new JoystickButton(xBoxControl, 5);
+    bumper2.whenPressed(new DownArm());
     Button aButton = new JoystickButton(xBoxControl, 1);
     aButton.whenPressed(new TeleMoveArm(false));
     aButton.whenReleased(new StopTeleArm());
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 93c8d48..08f2997 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -30,6 +30,7 @@
 import frc.robot.subsystems.LaserFinder;
 import frc.robot.subsystems.Photoresistor;
 import frc.robot.subsystems.Ramps;
+import frc.robot.subsystems.StringPot;
 import frc.robot.subsystems.TelescopingArm;
 import frc.robot.subsystems.pneumatics;
 
@@ -71,6 +72,7 @@ public class Robot extends TimedRobot {
   public static Photoresistor resistor;
   public static Timer m_timer;
   public static LaserFinder m_laser;
+  public static StringPot m_stringPot;
   //public static Ramps m_ramps;
   /**
    * This function is run when the robot is first started up and should be used
@@ -95,6 +97,7 @@ public void robotInit() {
     m_hallEfect = new Hallefect();
     resistor = new Photoresistor();
     m_timer = new Timer();
+    m_stringPot = new StringPot();
     //m_laser = new LaserFinder();
     SmartDashboard.putBoolean("TankDrive", false);
     //m_ramps = new Ramps();
@@ -105,7 +108,7 @@ public void robotInit() {
     
     //Scheduler.getInstance().add(new GetDistance());
     //OI must be init last
-
+    SmartDashboard.putNumber("Claw Speed", claw.speed);
     SmartDashboard.putNumber("Offset", TurnDegrees.offset); 
     m_oi = new OI();
   }
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index faf6edd..96d31d9 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -28,6 +28,7 @@ public class RobotMap {
   //Analog
   public final static int PHOTORESISTOR = 0;  
   public final static int ULTRASONIC = 3;
+  public static final int STRING_POT = 2;
   //public final static int ARMLIMITSWITCH = 2;
 
   //DIO
@@ -39,10 +40,10 @@ public class RobotMap {
   //PWM
   public final static int ARM_L = 3;//
   public final static int ARM_R = 4;//
-  public final static int TELE_ARM = 1;//
-  public final static int CLAW_1 = 6;//
+  public final static int TELE_ARM = 6;//
+  public final static int CLAW_1 = 2;//
   public final static int CLAW_2 = 5;//
-  public static final int RAMPS = 1;//
+  public static final int RAMPS = 0;//
   
   //CAN
   public final static int LEFTMOTOR_1 = 0;  
diff --git a/src/main/java/frc/robot/commands/MoveClaw.java b/src/main/java/frc/robot/commands/ClawDrive.java
similarity index 86%
rename from src/main/java/frc/robot/commands/MoveClaw.java
rename to src/main/java/frc/robot/commands/ClawDrive.java
index 4480cbc..274f54c 100644
--- a/src/main/java/frc/robot/commands/MoveClaw.java
+++ b/src/main/java/frc/robot/commands/ClawDrive.java
@@ -8,12 +8,10 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
-import frc.robot.subsystems.Claw;
 
-public class MoveClaw extends Command {
-  public MoveClaw() {
+public class ClawDrive extends Command {
+  public ClawDrive() {
     // Use requires() here to declare subsystem dependencies
     requires(Robot.claw);
   }
@@ -21,18 +19,18 @@ public MoveClaw() {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    Robot.claw.moveClaw();
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    Robot.claw.move();
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return true;
+    return false;
   }
 
   // Called once after isFinished returns true
diff --git a/src/main/java/frc/robot/commands/DownArm.java b/src/main/java/frc/robot/commands/DownArm.java
new file mode 100644
index 0000000..445d120
--- /dev/null
+++ b/src/main/java/frc/robot/commands/DownArm.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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 DownArm extends Command {
+  public DownArm() {
+    // Use requires() here to declare subsystem dependencies
+    requires(Robot.arm);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.arm.moveDown();
+  }
+
+  // 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 true;
+  }
+
+  // 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/commands/StopArm.java b/src/main/java/frc/robot/commands/StopArm.java
new file mode 100644
index 0000000..9582c83
--- /dev/null
+++ b/src/main/java/frc/robot/commands/StopArm.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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 StopArm extends Command {
+  public StopArm() {
+    // Use requires() here to declare subsystem dependencies
+    requires(Robot.arm);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.arm.stop();
+  }
+
+  // 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 true;
+  }
+
+  // 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/commands/StopClaw.java b/src/main/java/frc/robot/commands/StopClaw.java
new file mode 100644
index 0000000..0c8d444
--- /dev/null
+++ b/src/main/java/frc/robot/commands/StopClaw.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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 StopClaw extends Command {
+  public StopClaw() {
+    // Use requires() here to declare subsystem dependencies
+    requires(Robot.claw);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.claw.stop();
+  }
+
+  // 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 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() {
+  }
+}
diff --git a/src/main/java/frc/robot/commands/StopRamps.java b/src/main/java/frc/robot/commands/StopRamps.java
new file mode 100644
index 0000000..8ff06e0
--- /dev/null
+++ b/src/main/java/frc/robot/commands/StopRamps.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;
+
+public class StopRamps extends Command {
+  public StopRamps() {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    //Robot.m_ramps.stop
+  }
+
+  // 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 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() {
+  }
+}
diff --git a/src/main/java/frc/robot/commands/ClawReverse.java b/src/main/java/frc/robot/commands/StringpotDriver.java
similarity index 85%
rename from src/main/java/frc/robot/commands/ClawReverse.java
rename to src/main/java/frc/robot/commands/StringpotDriver.java
index 62e2217..86d7db1 100644
--- a/src/main/java/frc/robot/commands/ClawReverse.java
+++ b/src/main/java/frc/robot/commands/StringpotDriver.java
@@ -11,29 +11,27 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 
-public class ClawReverse extends Command {
-  public ClawReverse() {
+public class StringpotDriver extends Command {
+  public StringpotDriver() {
     // Use requires() here to declare subsystem dependencies
-    // eg. requires(chassis);
-    requires(Robot.claw);
+    requires(Robot.m_stringPot);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    SmartDashboard.putBoolean("Claw Reversed", true);
-    Robot.claw.moveClaw();
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    SmartDashboard.putNumber("String Potentiometer", Robot.m_stringPot.readPos());
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return true;
+    return false;
   }
 
   // Called once after isFinished returns true
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index caf0df4..333d1f9 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -35,7 +35,7 @@ public class Arm extends Subsystem {
   DigitalInput switchTop;
   Spark sparkL;
   Spark sparkR;
-  
+  double speed = 1;
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public Arm() {
@@ -50,9 +50,7 @@ public void initDefaultCommand() {
     setDefaultCommand(new MoveArm());
   }
   public void move(){
-    double speed = (OI.xBoxControl.getTriggerAxis(Hand.kRight) - OI.xBoxControl.getTriggerAxis(Hand.kLeft))*1;
     //SmartDashboard.putNumber("Arm", speed);
-    if(speed > 0){
       if(getSwitch()){
         sparkL.setSpeed(0);
         sparkR.setSpeed(0);
@@ -60,10 +58,6 @@ public void move(){
         sparkL.setSpeed(speed * leftMod);
         sparkR.setSpeed(speed * rightMod);
       }
-    } else if (speed < 0) {
-      sparkL.setSpeed(speed * leftMod);
-      sparkR.setSpeed(speed * rightMod);
-    }
     
     //sparkR.setSpeed(speed * rightMod);
     //motor.set(speed);
@@ -74,4 +68,12 @@ public double getRotations(){
   public static boolean getSwitch(){
     return switchBottom.get();
   }
+  public void moveDown(){
+    sparkL.setSpeed(speed * leftMod);
+    sparkR.setSpeed(speed * rightMod);
+  }
+  public void stop(){
+    sparkL.setSpeed(0);
+    sparkR.setSpeed(0);
+  }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index 5ea5978..88a8077 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -9,29 +9,44 @@
 
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.GenericHID.Hand;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.OI;
 import frc.robot.RobotMap;
-import frc.robot.commands.MoveClaw;
-
+import frc.robot.commands.ClawDrive;
 /**
  * Add your docs here.
  */
 public class Claw extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
-  double speed = .5;
-  static double speedModifier = 1.0;
+  public double speed = .5;
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
-     setDefaultCommand(new MoveClaw()); 
+     setDefaultCommand(new ClawDrive()); 
   }
     static Spark claw1 = new Spark(RobotMap.CLAW_1);
     static Spark claw2 = new Spark(RobotMap.CLAW_2);
-  public void moveClaw(){
-   claw1.set(speed * speedModifier);
-   claw2.set(speed * speedModifier);
-    
+  public void moveForward(){
+    speed = SmartDashboard.getNumber("Claw Speed", speed);
+    claw2.set(speed);
+    claw1.set(speed);
+  }
+  public void moveBackward(){
+    speed = SmartDashboard.getNumber("Claw Speed", speed);
+    claw2.set(-speed);
+    claw1.set(-speed);
+  }
+  public void stop(){
+    claw1.set(0);
+    claw2.set(0);
+  }
+  public void move(){
+    double speed = OI.xBoxControl.getTriggerAxis(Hand.kLeft);
+    double backSpeed = OI.xBoxControl.getTriggerAxis(Hand.kRight);
+    claw1.set(speed - backSpeed);
+    claw2.set(speed - backSpeed);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index 8a47ba4..c99600b 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -27,19 +27,19 @@ public class DriveTrain extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
-  //public WPI_TalonSRX talonL1;
-  //public WPI_TalonSRX talonL2;
-  //public WPI_TalonSRX talonR1;
+  public WPI_TalonSRX talonL1;
+  public WPI_TalonSRX talonL2;
+  public WPI_TalonSRX talonR1;
   public WPI_TalonSRX talonR2;
   
   public static final ADIS16448_IMU imu = new ADIS16448_IMU();
   public static Double speedModifier;
   private final double exponentialGrowth = .5;
   
-  public Spark sparkL1;
-  public Spark sparkL2;
-  public Spark sparkR1;
-  public Spark sparkR2;
+  //public Spark sparkL1;
+  //public Spark sparkL2;
+  //public Spark sparkR1;
+  //public Spark sparkR2;
   
 
   SpeedControllerGroup leftGroup;
@@ -48,19 +48,19 @@ public class DriveTrain extends Subsystem {
   public DifferentialDrive diffDrive;
 
   public DriveTrain() {
-    sparkL1 = new Spark(RobotMap.LEFTMOTOR_1);
+    //sparkL1 = new Spark(RobotMap.LEFTMOTOR_1);
     //sparkL2 = new Spark(RobotMap.LEFTMOTOR_2);
-    sparkR1 = new Spark(RobotMap.RIGHTMOTOR_1);
-    sparkR2 = new Spark(RobotMap.RIGHTMOTOR_2);
-    //talonL1 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_1);
-   // talonL2 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_2);
-    //talonR1 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_1);
+    //sparkR1 = new Spark(RobotMap.RIGHTMOTOR_1);
+    //sparkR2 = new Spark(RobotMap.RIGHTMOTOR_2);
+    talonL1 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_1);
+    talonL2 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_2);
+    talonR1 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_1);
     talonR2 = new WPI_TalonSRX(0);
 
     speedModifier = 1.0;
     
-    leftGroup = new SpeedControllerGroup(sparkL1, talonR2);
-    rightGroup = new SpeedControllerGroup(sparkR1, sparkR2);
+    leftGroup = new SpeedControllerGroup(talonL1, talonL2);
+    rightGroup = new SpeedControllerGroup(talonR1, talonR2);
 
     diffDrive = new DifferentialDrive(leftGroup, rightGroup);
     imu.reset();
diff --git a/src/main/java/frc/robot/subsystems/Ramps.java b/src/main/java/frc/robot/subsystems/Ramps.java
index 8f9f98b..685238a 100644
--- a/src/main/java/frc/robot/subsystems/Ramps.java
+++ b/src/main/java/frc/robot/subsystems/Ramps.java
@@ -33,4 +33,7 @@ public void lower(){
   public void raise(){
     motor.setSpeed(speed);
   }
+  public void stop(){
+    motor.set(0);
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/StringPot.java b/src/main/java/frc/robot/subsystems/StringPot.java
new file mode 100644
index 0000000..bcf3e1f
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/StringPot.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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.AnalogInput;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.RobotMap;
+import frc.robot.commands.StringpotDriver;
+
+/**
+ * Add your docs here.
+ */
+public class StringPot extends Subsystem {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+  AnalogInput sensor = new AnalogInput(RobotMap.STRING_POT);
+  @Override
+  public void initDefaultCommand() {
+    // Set the default command for a subsystem here.
+    setDefaultCommand(new StringpotDriver());
+  }
+  public double readPos(){
+    return sensor.getValue();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/TelescopingArm.java b/src/main/java/frc/robot/subsystems/TelescopingArm.java
index 3cf17d2..a10a8b0 100644
--- a/src/main/java/frc/robot/subsystems/TelescopingArm.java
+++ b/src/main/java/frc/robot/subsystems/TelescopingArm.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import frc.robot.RobotMap;
+import frc.robot.commands.StopTeleArm;
 
 /**
  * Add your docs here.
@@ -19,12 +20,12 @@ public class TelescopingArm extends Subsystem {
   // here. Call these from Commands.
 
   Spark spark = new Spark(RobotMap.TELE_ARM);
-  double speed = 1;
+  double speed = 0;
 
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
-    // setDefaultCommand(new MySpecialCommand());
+     setDefaultCommand(new StopTeleArm());
   }
 
   public void moveArm()

From 78157d0214bc59d050fa48c29b7b42b78d842245 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Mon, 25 Feb 2019 19:50:40 -0500
Subject: [PATCH 31/50] Created Ultrasonic w/ echo

---
 src/main/java/frc/robot/RobotMap.java         |  4 +++-
 .../java/frc/robot/subsystems/UltraSonic.java | 22 +++++++------------
 2 files changed, 11 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 96d31d9..54094e7 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -27,7 +27,7 @@ public class RobotMap {
 
   //Analog
   public final static int PHOTORESISTOR = 0;  
-  public final static int ULTRASONIC = 3;
+  //public final static int ULTRASONIC = 3;
   public static final int STRING_POT = 2;
   //public final static int ARMLIMITSWITCH = 2;
 
@@ -36,6 +36,8 @@ public class RobotMap {
   public final static int ARM_ENCODER_B = 3;
   public final static int ARM_SWITCHBOTTOM = 1;
   public final static int HALLEFFECT = 0; //UNUSED(?)
+  public final static int ULTRASONIC = 4;
+  public final static int ULTRASONIC_ECHO = 5;
 
   //PWM
   public final static int ARM_L = 3;//
diff --git a/src/main/java/frc/robot/subsystems/UltraSonic.java b/src/main/java/frc/robot/subsystems/UltraSonic.java
index 9a6c2da..b1fcff1 100644
--- a/src/main/java/frc/robot/subsystems/UltraSonic.java
+++ b/src/main/java/frc/robot/subsystems/UltraSonic.java
@@ -8,6 +8,8 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.Ultrasonic;
+import edu.wpi.first.wpilibj.Ultrasonic.Unit;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.RobotMap;
@@ -19,10 +21,12 @@
 public class UltraSonic extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
-  
-  AnalogInput sonicSensor = new AnalogInput(RobotMap.ULTRASONIC);
-  private double distance = 0.0;
-  private double volt = 0.0;
+
+  Ultrasonic us = new Ultrasonic (RobotMap.ULTRASONIC, RobotMap.ULTRASONIC_ECHO, Unit.kInches);
+
+  public double getDistance(){
+    return us.getRangeInches();
+  }
 
   @Override
   public void initDefaultCommand() {
@@ -30,14 +34,4 @@ public void initDefaultCommand() {
     // setDefaultCommand(new MySpecialCommand());
     setDefaultCommand(new Rumbler());
   }
-  public Double distance(){
-    distance = voltage()*1024 /25.4;
-    return distance; //distance(mm) = volt*1024
-  }
-
-  public Double voltage()
-  {
-      volt = sonicSensor.getVoltage();
-      return volt;
-  }
 }

From 77452e18bd8d913bc7210e08effabf58462bc0f7 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 2 Mar 2019 08:26:24 -0500
Subject: [PATCH 32/50] Robot works

---
 src/main/java/frc/robot/OI.java               |  8 +--
 src/main/java/frc/robot/Robot.java            | 12 ++--
 src/main/java/frc/robot/RobotMap.java         | 20 +++----
 .../commands/{MoveArm.java => ArmDrive.java}  | 18 ++++--
 .../java/frc/robot/commands/ArmPositions.java | 59 -------------------
 .../{DownArm.java => ExtendPiston.java}       |  8 +--
 .../frc/robot/commands/JoystickDrive.java     |  4 +-
 .../frc/robot/commands/PneumaticsDrive.java   |  4 +-
 .../{StopClaw.java => RetractPiston.java}     | 10 ++--
 src/main/java/frc/robot/commands/Rumbler.java | 10 +++-
 src/main/java/frc/robot/subsystems/Arm.java   | 15 ++---
 .../frc/robot/subsystems/LaserFinder.java     |  2 +-
 .../java/frc/robot/subsystems/pneumatics.java | 14 ++++-
 13 files changed, 74 insertions(+), 110 deletions(-)
 rename src/main/java/frc/robot/commands/{MoveArm.java => ArmDrive.java} (76%)
 delete mode 100644 src/main/java/frc/robot/commands/ArmPositions.java
 rename src/main/java/frc/robot/commands/{DownArm.java => ExtendPiston.java} (90%)
 rename src/main/java/frc/robot/commands/{StopClaw.java => RetractPiston.java} (88%)

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 51ed7cf..98db826 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -54,12 +54,12 @@ public class OI {
 
   public OI() {
    // Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
+    Button pistonExtend = new JoystickButton(xBoxControl, 8);
+    pistonExtend.whenPressed(new ExtendPiston());
+    Button pistonRetract = new JoystickButton(xBoxControl, 7);
+    pistonRetract.whenPressed(new RetractPiston());
     Button button = new JoystickButton(xBoxControl, 2);
     button.whenPressed(new TurnDegrees());
-    Button bumper = new JoystickButton(xBoxControl, 6);
-    bumper.whenPressed(new MoveArm());
-    Button bumper2 = new JoystickButton(xBoxControl, 5);
-    bumper2.whenPressed(new DownArm());
     Button aButton = new JoystickButton(xBoxControl, 1);
     aButton.whenPressed(new TeleMoveArm(false));
     aButton.whenReleased(new StopTeleArm());
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 08f2997..80104f0 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -18,6 +18,7 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commands.ArmDrive;
 import frc.robot.commands.ExampleCommand;
 import frc.robot.commands.Teleop;
 import frc.robot.commands.TurnDegrees;
@@ -81,10 +82,11 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotInit() {
+    SmartDashboard.putBoolean("Arm moving",false);
     com.setClosedLoopControl(true);
     com.start();
     claw = new Claw();
-    //arm = new Arm();
+    arm = new Arm();
     camera = new CameraI2c();
     m_driveTrain = new DriveTrain();
     m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
@@ -98,11 +100,10 @@ public void robotInit() {
     resistor = new Photoresistor();
     m_timer = new Timer();
     m_stringPot = new StringPot();
-    //m_laser = new LaserFinder();
+    m_laser = new LaserFinder();
     SmartDashboard.putBoolean("TankDrive", false);
     //m_ramps = new Ramps();
-
-    //m_ultraSonic = new UltraSonic();
+    ArmDrive armDrive = new ArmDrive();
 
     m_ultraSonic = new UltraSonic();
     
@@ -111,6 +112,7 @@ public void robotInit() {
     SmartDashboard.putNumber("Claw Speed", claw.speed);
     SmartDashboard.putNumber("Offset", TurnDegrees.offset); 
     m_oi = new OI();
+    m_timer.start();
   }
 
   /**
@@ -126,7 +128,7 @@ public void robotPeriodic() {
     //--------------------------Do 10 times per Second --------------------------------------------------
     if(m_timer.hasPeriodPassed(.1)){
       CameraI2c.read();
-      
+      m_laser.read();
       m_timer.reset();
     }
     ////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 54094e7..26f596c 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -34,18 +34,18 @@ public class RobotMap {
   //DIO
   public final static int ARM_ENCODER_A = 2;  
   public final static int ARM_ENCODER_B = 3;
-  public final static int ARM_SWITCHBOTTOM = 1;
-  public final static int HALLEFFECT = 0; //UNUSED(?)
+  public final static int ARM_SWITCHBOTTOM = 0;
+  public final static int HALLEFFECT = 1; //UNUSED(?)
   public final static int ULTRASONIC = 4;
   public final static int ULTRASONIC_ECHO = 5;
 
   //PWM
-  public final static int ARM_L = 3;//
-  public final static int ARM_R = 4;//
-  public final static int TELE_ARM = 6;//
-  public final static int CLAW_1 = 2;//
-  public final static int CLAW_2 = 5;//
-  public static final int RAMPS = 0;//
+  public final static int ARM_L = 2;//
+  public final static int ARM_R = 0;//
+  public final static int TELE_ARM = 5;//
+  public final static int CLAW_1 = 3;//
+  public final static int CLAW_2 = 1;//
+  public static final int RAMPS = 4;//
   
   //CAN
   public final static int LEFTMOTOR_1 = 0;  
@@ -57,6 +57,6 @@ public class RobotMap {
   public final static int PNEUMATIC_COMPRESSOR = 0;
   public final static int DOUBLESOL_FORWARD = 0;
   public final static int DOUBLESOL_REVERSE = 1;
-  public final static int DOUBLESOL_FORWARD1 = 2;
-  public static final int DOUBLESOL_REVERSE1 = 3;
+  public final static int DOUBLESOL_FORWARD1 = 3;
+  public static final int DOUBLESOL_REVERSE1 = 2;
 }
diff --git a/src/main/java/frc/robot/commands/MoveArm.java b/src/main/java/frc/robot/commands/ArmDrive.java
similarity index 76%
rename from src/main/java/frc/robot/commands/MoveArm.java
rename to src/main/java/frc/robot/commands/ArmDrive.java
index 606eb56..34b91a8 100644
--- a/src/main/java/frc/robot/commands/MoveArm.java
+++ b/src/main/java/frc/robot/commands/ArmDrive.java
@@ -13,9 +13,9 @@
 import frc.robot.OI;
 import frc.robot.Robot;
 
-public class MoveArm extends Command {
-  double armInput = 0.0;
-  public MoveArm() {
+public class ArmDrive extends Command {
+  int count = 1;
+  public ArmDrive() {
     // Use requires() here to declare subsystem dependencies
     requires(Robot.arm);
   }
@@ -28,8 +28,16 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    Robot.arm.move();
-    //SmartDashboard.putNumber("Encoder",Robot.arm.getRotations());
+    if(OI.xBoxControl.getBumper(Hand.kRight)){
+      Robot.arm.move();
+      SmartDashboard.putBoolean("Arm moving", true);
+    }else if(OI.xBoxControl.getBumper(Hand.kLeft)){
+      Robot.arm.moveDown();
+      SmartDashboard.putBoolean("Arm moving", true);
+    }else{
+      SmartDashboard.putBoolean("Arm moving", false);
+      Robot.arm.stop();
+    }
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/commands/ArmPositions.java b/src/main/java/frc/robot/commands/ArmPositions.java
deleted file mode 100644
index 4a7ec0c..0000000
--- a/src/main/java/frc/robot/commands/ArmPositions.java
+++ /dev/null
@@ -1,59 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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;
-import frc.robot.subsystems.Arm;
-
-public class ArmPositions extends Command {
-  private static double pos1 = 0.0;
-  private static double pos2 = 0.1;
-  private static double pos3 = 0.2;
-  private static double pos4 = 0.4;
-  private static double pos5 = 0.0;
-  private static double pos6 = 0.1;
-  private static double pos7 = 0.2;
-  private static double pos8 = 0.4;
-  private static double[] positions;
-  public ArmPositions(int positionToMoveTo) {
-    // Use requires() here to declare subsystem dependencies
-    requires(Robot.arm);
-    requires(Robot.m_laser);
-  }
-
-  // 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() {
-    if(true){
-      return true;
-    }
-    return true;
-  }
-
-  // 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/commands/DownArm.java b/src/main/java/frc/robot/commands/ExtendPiston.java
similarity index 90%
rename from src/main/java/frc/robot/commands/DownArm.java
rename to src/main/java/frc/robot/commands/ExtendPiston.java
index 445d120..d18f15b 100644
--- a/src/main/java/frc/robot/commands/DownArm.java
+++ b/src/main/java/frc/robot/commands/ExtendPiston.java
@@ -10,16 +10,16 @@
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.Robot;
 
-public class DownArm extends Command {
-  public DownArm() {
+public class ExtendPiston extends Command {
+  public ExtendPiston() {
     // Use requires() here to declare subsystem dependencies
-    requires(Robot.arm);
+    requires(Robot.m_pneumatics);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    Robot.arm.moveDown();
+    Robot.m_pneumatics.extend();
   }
 
   // Called repeatedly when this Command is scheduled to run
diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index ba2a70d..27ada34 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -42,8 +42,8 @@ protected void execute() {
       rightSpeed = OI.xBoxControl.getY(Hand.kRight);
       Robot.m_driveTrain.drive(rightSpeed, leftSpeed);
     }
-    
-    //SmartDashboard.putNumber("Left Speed: ", leftSpeed);
+    SmartDashboard.putNumber("GyroAngle", Robot.m_driveTrain.getGyroZ());
+    //SmartDaShboard.putNumber("Left Speed: ", leftSpeed);
     //SmartDashboard.putNumber("Right Speed: ", rightSpeed);
   }
   
diff --git a/src/main/java/frc/robot/commands/PneumaticsDrive.java b/src/main/java/frc/robot/commands/PneumaticsDrive.java
index 62086be..7352ffe 100644
--- a/src/main/java/frc/robot/commands/PneumaticsDrive.java
+++ b/src/main/java/frc/robot/commands/PneumaticsDrive.java
@@ -28,13 +28,13 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    if(OI.xBoxControl.getBumperPressed(Hand.kLeft)) {
+    /*if(OI.xBoxControl.getBumperPressed(Hand.kLeft)) {
       pneumatics.retract();
     }
     if(OI.xBoxControl.getBumperPressed(Hand.kRight)) {
       pneumatics.extend();
     }
-    
+    */
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/commands/StopClaw.java b/src/main/java/frc/robot/commands/RetractPiston.java
similarity index 88%
rename from src/main/java/frc/robot/commands/StopClaw.java
rename to src/main/java/frc/robot/commands/RetractPiston.java
index 0c8d444..3876b73 100644
--- a/src/main/java/frc/robot/commands/StopClaw.java
+++ b/src/main/java/frc/robot/commands/RetractPiston.java
@@ -10,16 +10,16 @@
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.Robot;
 
-public class StopClaw extends Command {
-  public StopClaw() {
+public class RetractPiston extends Command {
+  public RetractPiston() {
     // Use requires() here to declare subsystem dependencies
-    requires(Robot.claw);
+    requires(Robot.m_pneumatics);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    Robot.claw.stop();
+    Robot.m_pneumatics.retract();
   }
 
   // Called repeatedly when this Command is scheduled to run
@@ -30,7 +30,7 @@ protected void execute() {
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+    return true;
   }
 
   // Called once after isFinished returns true
diff --git a/src/main/java/frc/robot/commands/Rumbler.java b/src/main/java/frc/robot/commands/Rumbler.java
index dde7a13..f0945b1 100644
--- a/src/main/java/frc/robot/commands/Rumbler.java
+++ b/src/main/java/frc/robot/commands/Rumbler.java
@@ -17,7 +17,7 @@
 public class Rumbler extends Command {
 public static double zone = 18;
 public static boolean warn = false;
-
+int count = 0;
   public Rumbler() {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
@@ -32,8 +32,8 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    double distance = Robot.m_ultraSonic.distance();
-
+    double distance = Robot.m_ultraSonic.getDistance();
+    if(count >= 5){
     if(distance < zone){
       OI.xBoxControl.setRumble(RumbleType.kLeftRumble, 1 - (distance/zone));
       OI.xBoxControl.setRumble(RumbleType.kRightRumble, 1 - (distance/zone));
@@ -50,6 +50,10 @@ protected void execute() {
     
     SmartDashboard.putNumber("Ultrasonic Distance", distance);
     SmartDashboard.putBoolean("WARNING", warn);
+    count = 0;
+  }
+  count++;
+
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 333d1f9..0b5d477 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -18,7 +18,7 @@
 import frc.robot.OI;
 import frc.robot.Robot;
 import frc.robot.RobotMap;
-import frc.robot.commands.MoveArm;
+import frc.robot.commands.ArmDrive;
 
 /**
  * Add your docs here.
@@ -47,16 +47,17 @@ public Arm() {
   }
   @Override
   public void initDefaultCommand() {
-    setDefaultCommand(new MoveArm());
+    setDefaultCommand(new ArmDrive());
   }
   public void move(){
     //SmartDashboard.putNumber("Arm", speed);
+    
       if(getSwitch()){
         sparkL.setSpeed(0);
         sparkR.setSpeed(0);
       } else {
-        sparkL.setSpeed(speed * leftMod);
-        sparkR.setSpeed(speed * rightMod);
+        sparkL.setSpeed(leftMod);
+        sparkR.setSpeed(rightMod);
       }
     
     //sparkR.setSpeed(speed * rightMod);
@@ -65,12 +66,12 @@ public void move(){
   public double getRotations(){
     return armEncoder.getDistance();
   }
-  public static boolean getSwitch(){
+  public boolean getSwitch(){
     return switchBottom.get();
   }
   public void moveDown(){
-    sparkL.setSpeed(speed * leftMod);
-    sparkR.setSpeed(speed * rightMod);
+    sparkL.setSpeed(-leftMod);
+    sparkR.setSpeed(-rightMod);
   }
   public void stop(){
     sparkL.setSpeed(0);
diff --git a/src/main/java/frc/robot/subsystems/LaserFinder.java b/src/main/java/frc/robot/subsystems/LaserFinder.java
index 63332cb..e3de695 100644
--- a/src/main/java/frc/robot/subsystems/LaserFinder.java
+++ b/src/main/java/frc/robot/subsystems/LaserFinder.java
@@ -22,7 +22,7 @@ public class LaserFinder extends Subsystem {
   public  I2C Wire;
   public  double distance;
   public LaserFinder(){
-    Wire = new I2C(Port.kOnboard, 0x53);
+    Wire = new I2C(Port.kOnboard, 8);
   }
   @Override
   public void initDefaultCommand() {
diff --git a/src/main/java/frc/robot/subsystems/pneumatics.java b/src/main/java/frc/robot/subsystems/pneumatics.java
index f217692..cffcddf 100644
--- a/src/main/java/frc/robot/subsystems/pneumatics.java
+++ b/src/main/java/frc/robot/subsystems/pneumatics.java
@@ -9,6 +9,7 @@
 
 import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import frc.robot.RobotMap;
 import frc.robot.commands.PneumaticsDrive;
@@ -20,6 +21,9 @@ public class pneumatics extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public static Compressor compressor;
+  //public static Solenoid forward = new Solenoid(RobotMap.DOUBLESOL_FORWARD);
+  ///public static Solenoid backwards = new Solenoid(RobotMap.DOUBLESOL_REVERSE);
+  
   public static DoubleSolenoid doubleSol = new DoubleSolenoid(RobotMap.DOUBLESOL_FORWARD, RobotMap.DOUBLESOL_REVERSE);
   public static DoubleSolenoid doubleSol2 = new DoubleSolenoid(RobotMap.DOUBLESOL_FORWARD1, RobotMap.DOUBLESOL_REVERSE1);
   public pneumatics() {
@@ -34,15 +38,19 @@ public void initDefaultCommand() {
     setDefaultCommand(new PneumaticsDrive());
   }
 
-  public static void extend() {
+  public void extend() {
+    //backwards.set(false);
+    //forward.set(true);
     doubleSol.set(DoubleSolenoid.Value.kForward);
     doubleSol2.set(DoubleSolenoid.Value.kForward);
   }
-  public static void retract() {
+  public void retract() {
+    //forward.set(false);
+    //backwards.set(true);
     doubleSol.set(DoubleSolenoid.Value.kReverse);
     doubleSol2.set(DoubleSolenoid.Value.kReverse);
   }
-  public static void stop() {
+  public void stop() {
     doubleSol.set(DoubleSolenoid.Value.kOff);
     doubleSol2.set(DoubleSolenoid.Value.kOff);
   }

From a1082b4d2eb1a0beda5b0a1f80ad33a7f16d15c0 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 2 Mar 2019 10:09:47 -0500
Subject: [PATCH 33/50] Robot works

---
 build.gradle                                     | 2 +-
 src/main/java/frc/robot/Robot.java               | 4 ++--
 src/main/java/frc/robot/RobotMap.java            | 4 ++--
 src/main/java/frc/robot/commands/ArmDrive.java   | 4 ++--
 src/main/java/frc/robot/commands/LowerRamps.java | 4 ++--
 src/main/java/frc/robot/subsystems/Arm.java      | 8 ++++----
 src/main/java/frc/robot/subsystems/Ramps.java    | 4 +---
 7 files changed, 14 insertions(+), 16 deletions(-)

diff --git a/build.gradle b/build.gradle
index 2a7ec8c..f4529be 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.4.1"
 }
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 80104f0..3fd498d 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -59,7 +59,7 @@ public class Robot extends TimedRobot {
  */
 
   //private static final boolean PUT_NUMBER = SmartDashboard.putNumber("Gyro-Z", m_driveTrain.getGyroZ());
-  
+  public static Ramps m_ramps;
   public static Teleop m_teleop;
   Command m_autonomousCommand;
   Compressor com = new Compressor(0);
@@ -102,7 +102,7 @@ public void robotInit() {
     m_stringPot = new StringPot();
     m_laser = new LaserFinder();
     SmartDashboard.putBoolean("TankDrive", false);
-    //m_ramps = new Ramps();
+    m_ramps = new Ramps();
     ArmDrive armDrive = new ArmDrive();
 
     m_ultraSonic = new UltraSonic();
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 26f596c..f1e6a14 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -57,6 +57,6 @@ public class RobotMap {
   public final static int PNEUMATIC_COMPRESSOR = 0;
   public final static int DOUBLESOL_FORWARD = 0;
   public final static int DOUBLESOL_REVERSE = 1;
-  public final static int DOUBLESOL_FORWARD1 = 3;
-  public static final int DOUBLESOL_REVERSE1 = 2;
+  public final static int DOUBLESOL_FORWARD1 = 2;
+  public static final int DOUBLESOL_REVERSE1 = 3;
 }
diff --git a/src/main/java/frc/robot/commands/ArmDrive.java b/src/main/java/frc/robot/commands/ArmDrive.java
index 34b91a8..fbc5d3e 100644
--- a/src/main/java/frc/robot/commands/ArmDrive.java
+++ b/src/main/java/frc/robot/commands/ArmDrive.java
@@ -28,10 +28,10 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    if(OI.xBoxControl.getBumper(Hand.kRight)){
+    if(OI.xBoxControl.getBumper(Hand.kLeft)){
       Robot.arm.move();
       SmartDashboard.putBoolean("Arm moving", true);
-    }else if(OI.xBoxControl.getBumper(Hand.kLeft)){
+    }else if(OI.xBoxControl.getBumper(Hand.kRight)){
       Robot.arm.moveDown();
       SmartDashboard.putBoolean("Arm moving", true);
     }else{
diff --git a/src/main/java/frc/robot/commands/LowerRamps.java b/src/main/java/frc/robot/commands/LowerRamps.java
index 46c4a37..6679513 100644
--- a/src/main/java/frc/robot/commands/LowerRamps.java
+++ b/src/main/java/frc/robot/commands/LowerRamps.java
@@ -13,13 +13,13 @@
 public class LowerRamps extends Command {
   public LowerRamps() {
     // Use requires() here to declare subsystem dependencies
-   // requires(Robot.m_ramps);
+    requires(Robot.m_ramps);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    //Robot.m_ramps.lower();
+    Robot.m_ramps.lower();
   }
 
   // Called repeatedly when this Command is scheduled to run
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 0b5d477..2135b0c 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -56,8 +56,8 @@ public void move(){
         sparkL.setSpeed(0);
         sparkR.setSpeed(0);
       } else {
-        sparkL.setSpeed(leftMod);
-        sparkR.setSpeed(rightMod);
+        sparkL.setSpeed(-leftMod);
+        sparkR.setSpeed(-rightMod);
       }
     
     //sparkR.setSpeed(speed * rightMod);
@@ -70,8 +70,8 @@ public boolean getSwitch(){
     return switchBottom.get();
   }
   public void moveDown(){
-    sparkL.setSpeed(-leftMod);
-    sparkR.setSpeed(-rightMod);
+    sparkL.setSpeed(leftMod);
+    sparkR.setSpeed(rightMod);
   }
   public void stop(){
     sparkL.setSpeed(0);
diff --git a/src/main/java/frc/robot/subsystems/Ramps.java b/src/main/java/frc/robot/subsystems/Ramps.java
index 685238a..68cec0a 100644
--- a/src/main/java/frc/robot/subsystems/Ramps.java
+++ b/src/main/java/frc/robot/subsystems/Ramps.java
@@ -30,9 +30,7 @@ public Ramps(){
   public void lower(){
     motor.set(speed);
   }
-  public void raise(){
-    motor.setSpeed(speed);
-  }
+
   public void stop(){
     motor.set(0);
   }

From 8c1f546575f57a012cea2049fbd167f77978b44a Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 9 Mar 2019 09:40:21 -0500
Subject: [PATCH 34/50] All changes made at comp

---
 src/main/java/frc/robot/OI.java                     | 11 +++--------
 src/main/java/frc/robot/RobotMap.java               |  4 ++--
 src/main/java/frc/robot/commands/JoystickDrive.java |  9 +++++----
 src/main/java/frc/robot/commands/LowerRamps.java    |  4 ++--
 src/main/java/frc/robot/subsystems/Arm.java         |  8 ++++----
 src/main/java/frc/robot/subsystems/DriveTrain.java  |  6 +++---
 src/main/java/frc/robot/subsystems/Ramps.java       | 12 ++++++++++--
 7 files changed, 29 insertions(+), 25 deletions(-)

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 98db826..a200f8a 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -60,13 +60,8 @@ public OI() {
     pistonRetract.whenPressed(new RetractPiston());
     Button button = new JoystickButton(xBoxControl, 2);
     button.whenPressed(new TurnDegrees());
-    Button aButton = new JoystickButton(xBoxControl, 1);
-    aButton.whenPressed(new TeleMoveArm(false));
-    aButton.whenReleased(new StopTeleArm());
-    Button xButton = new JoystickButton(xBoxControl, 3);
-    xButton.whenPressed(new TeleMoveArm(true));
-    xButton.whenReleased(new StopTeleArm());
-    Button yButton = new JoystickButton(xBoxControl, 4);
-    yButton.whenPressed(new LowerRamps());
+    /*Button aButton = new JoystickButton(xBoxControl, 1);
+    aButton.whenPressed(new ExtendPiston());
+    aButton.whenReleased(new StopTeleArm());*/
   }
 }
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index f1e6a14..19f6b17 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -48,10 +48,10 @@ public class RobotMap {
   public static final int RAMPS = 4;//
   
   //CAN
-  public final static int LEFTMOTOR_1 = 0;  
+  public final static int LEFTMOTOR_1 = 3;  
   public final static int LEFTMOTOR_2 = 1;
   public final static int RIGHTMOTOR_1 = 2;
-  public final static int RIGHTMOTOR_2 = 3; 
+  public final static int RIGHTMOTOR_2 = 0; 
 
    //Pneumatics
   public final static int PNEUMATIC_COMPRESSOR = 0;
diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index 27ada34..ceb7fc1 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -15,6 +15,7 @@
 import frc.robot.subsystems.DriveTrain;
 
 public class JoystickDrive extends Command {
+  boolean tankDrive = false;
   public JoystickDrive() {
     // Use requires() here to declare subsystem dependencies
     // eg. requires(chassis);
@@ -31,17 +32,17 @@ protected void initialize() {
   protected void execute() {
     double rightSpeed = 0.0;
     double leftSpeed = 0.0;
-    boolean tankDrive = false;
+    
     tankDrive = SmartDashboard.getBoolean("TankDrive", tankDrive);
-    if(tankDrive){
+    /*if(tankDrive){
       leftSpeed = OI.xBoxControl.getY(Hand.kLeft);
       rightSpeed = OI.xBoxControl.getY(Hand.kRight);
       Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed);
-    }else{
+    }else{*/
       leftSpeed = OI.xBoxControl.getX(Hand.kRight);
       rightSpeed = OI.xBoxControl.getY(Hand.kRight);
       Robot.m_driveTrain.drive(rightSpeed, leftSpeed);
-    }
+    //}
     SmartDashboard.putNumber("GyroAngle", Robot.m_driveTrain.getGyroZ());
     //SmartDaShboard.putNumber("Left Speed: ", leftSpeed);
     //SmartDashboard.putNumber("Right Speed: ", rightSpeed);
diff --git a/src/main/java/frc/robot/commands/LowerRamps.java b/src/main/java/frc/robot/commands/LowerRamps.java
index 6679513..537ac76 100644
--- a/src/main/java/frc/robot/commands/LowerRamps.java
+++ b/src/main/java/frc/robot/commands/LowerRamps.java
@@ -19,18 +19,18 @@ public LowerRamps() {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    Robot.m_ramps.lower();
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    Robot.m_ramps.lower();
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return true;
+    return false;
   }
 
   // Called once after isFinished returns true
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 2135b0c..0b5d477 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -56,8 +56,8 @@ public void move(){
         sparkL.setSpeed(0);
         sparkR.setSpeed(0);
       } else {
-        sparkL.setSpeed(-leftMod);
-        sparkR.setSpeed(-rightMod);
+        sparkL.setSpeed(leftMod);
+        sparkR.setSpeed(rightMod);
       }
     
     //sparkR.setSpeed(speed * rightMod);
@@ -70,8 +70,8 @@ public boolean getSwitch(){
     return switchBottom.get();
   }
   public void moveDown(){
-    sparkL.setSpeed(leftMod);
-    sparkR.setSpeed(rightMod);
+    sparkL.setSpeed(-leftMod);
+    sparkR.setSpeed(-rightMod);
   }
   public void stop(){
     sparkL.setSpeed(0);
diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java
index c99600b..b8bc7ed 100644
--- a/src/main/java/frc/robot/subsystems/DriveTrain.java
+++ b/src/main/java/frc/robot/subsystems/DriveTrain.java
@@ -55,7 +55,7 @@ public DriveTrain() {
     talonL1 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_1);
     talonL2 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_2);
     talonR1 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_1);
-    talonR2 = new WPI_TalonSRX(0);
+    talonR2 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_2);
 
     speedModifier = 1.0;
     
@@ -69,10 +69,10 @@ public DriveTrain() {
 
   //maybe change back to static (broken code?)
   public void drive(double leftSpeed, double rightSpeed) {
-    diffDrive.arcadeDrive(leftSpeed * speedModifier, rightSpeed * speedModifier);
+    diffDrive.arcadeDrive(-leftSpeed * speedModifier, -rightSpeed * speedModifier);
   }
   public void tankDrive(double leftSpeed, double rightSpeed){
-    diffDrive.tankDrive((Math.pow(rightSpeed - 1, 3) + .5*(rightSpeed-1) + 1.5) * .75, (Math.pow(leftSpeed - 1, 3) + .5*(leftSpeed - 1) + 1.5) * .75);
+    diffDrive.tankDrive(-(Math.pow(rightSpeed - 1, 3) + .5*(rightSpeed-1) + 1.5) * .75, -(Math.pow(leftSpeed - 1, 3) + .5*(leftSpeed - 1) + 1.5) * .75);
   }
   
 
diff --git a/src/main/java/frc/robot/subsystems/Ramps.java b/src/main/java/frc/robot/subsystems/Ramps.java
index 68cec0a..95326be 100644
--- a/src/main/java/frc/robot/subsystems/Ramps.java
+++ b/src/main/java/frc/robot/subsystems/Ramps.java
@@ -9,7 +9,9 @@
 
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.OI;
 import frc.robot.RobotMap;
+import frc.robot.commands.LowerRamps;
 
 /**
  * Add your docs here.
@@ -22,13 +24,19 @@ public class Ramps extends Subsystem {
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
-    // setDefaultCommand(new MySpecialCommand());
+     setDefaultCommand(new LowerRamps());
   }
   public Ramps(){
     motor = new Spark(RobotMap.RAMPS);
   }
   public void lower(){
-    motor.set(speed);
+    if(OI.xBoxControl.getYButton()){
+      motor.set(speed);
+    }else if(OI.xBoxControl.getXButton()){
+      motor.set(-speed);
+    }else{
+      motor.set(0);
+    }
   }
 
   public void stop(){

From d882a516d5a231968f25647356ff6664a8061c9c Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Sat, 9 Mar 2019 14:41:50 -0500
Subject: [PATCH 35/50] Wrist subsystem and turn wrist amount of degrees (not
 tested)

---
 src/main/java/frc/robot/Robot.java            | 10 +--
 src/main/java/frc/robot/RobotMap.java         |  3 +
 .../java/frc/robot/commands/TurnWrist.java    | 55 ++++++++++++
 src/main/java/frc/robot/subsystems/Wrist.java | 83 +++++++++++++++++++
 4 files changed, 145 insertions(+), 6 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/TurnWrist.java
 create mode 100644 src/main/java/frc/robot/subsystems/Wrist.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 3fd498d..880716d 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -40,6 +40,7 @@
 
 import com.analog.adis16448.frc.ADIS16448_IMU;
 import frc.robot.subsystems.UltraSonic;
+import frc.robot.subsystems.Wrist;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -53,12 +54,7 @@ public class Robot extends TimedRobot {
   public static pneumatics m_pneumatics;
   public static OI m_oi;
   public static DriveTrain m_driveTrain;
-  //public static final ADIS16448_IMU imu = new ADIS16448_IMU(); ;
-/**
- *
- */
-
-  //private static final boolean PUT_NUMBER = SmartDashboard.putNumber("Gyro-Z", m_driveTrain.getGyroZ());
+  //public static final ADIS16448_IMU imu = new ADIS16448_IMU();
   public static Ramps m_ramps;
   public static Teleop m_teleop;
   Command m_autonomousCommand;
@@ -74,6 +70,7 @@ public class Robot extends TimedRobot {
   public static Timer m_timer;
   public static LaserFinder m_laser;
   public static StringPot m_stringPot;
+  public static Wrist m_wrist;
   //public static Ramps m_ramps;
   /**
    * This function is run when the robot is first started up and should be used
@@ -104,6 +101,7 @@ public void robotInit() {
     SmartDashboard.putBoolean("TankDrive", false);
     m_ramps = new Ramps();
     ArmDrive armDrive = new ArmDrive();
+    m_wrist = new Wrist();
 
     m_ultraSonic = new UltraSonic();
     
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 19f6b17..befd730 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -38,6 +38,8 @@ public class RobotMap {
   public final static int HALLEFFECT = 1; //UNUSED(?)
   public final static int ULTRASONIC = 4;
   public final static int ULTRASONIC_ECHO = 5;
+  public final static int WRIST_ENCODER_A = 6;
+  public final static int WRIST_ENCODER_B = 7;
 
   //PWM
   public final static int ARM_L = 2;//
@@ -52,6 +54,7 @@ public class RobotMap {
   public final static int LEFTMOTOR_2 = 1;
   public final static int RIGHTMOTOR_1 = 2;
   public final static int RIGHTMOTOR_2 = 0; 
+  public final static int WRISTMOTOR = 4;
 
    //Pneumatics
   public final static int PNEUMATIC_COMPRESSOR = 0;
diff --git a/src/main/java/frc/robot/commands/TurnWrist.java b/src/main/java/frc/robot/commands/TurnWrist.java
new file mode 100644
index 0000000..3e3c024
--- /dev/null
+++ b/src/main/java/frc/robot/commands/TurnWrist.java
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* 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 TurnWrist extends Command {
+  private double targetAngle;
+  public TurnWrist(double targetAngle) {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    requires(Robot.m_wrist);
+    this.targetAngle = targetAngle;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.m_wrist.setTarget(targetAngle);
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    Robot.m_wrist.move();
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    if(Robot.m_wrist.getAngle() == targetAngle) {
+      return true;
+    }
+    else {
+      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() {
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java
new file mode 100644
index 0000000..ec2b5f9
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Wrist.java
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* 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.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.RobotMap;
+
+/**
+ * Add your docs here.
+ */
+public class Wrist extends Subsystem {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+  private final static double SPEED = .5;
+  //num of angles = 1 rotation   is it 360? no one knows
+  private final static double ANGLE_TO_ROTATION = 360;
+
+  private double targetAngle;
+
+  Encoder wristEncoder;
+  DigitalInput switchBottom;
+  DigitalInput switchTop;
+
+  WPI_TalonSRX wristTalon;
+
+  @Override
+  public void initDefaultCommand() {
+    // Set the default command for a subsystem here.
+    // setDefaultCommand(new MySpecialCommand());
+  }
+
+  public Wrist() {
+    wristEncoder = new Encoder(RobotMap.WRIST_ENCODER_A, RobotMap.WRIST_ENCODER_B);
+    wristTalon = new WPI_TalonSRX(RobotMap.WRISTMOTOR);
+  }
+
+  public void move() {
+    if(getBottom()) {
+      wristEncoder.reset();
+      wristTalon.set(0);
+    }
+    else if(getTop()) {
+      wristTalon.set(0);
+    }
+    //forward = down  backwards = up
+    else if(getAngle() < targetAngle) {
+      wristTalon.set(-1*SPEED);
+    }
+    else if(getAngle() > targetAngle) {
+      wristTalon.set(SPEED);
+    }
+    else if(getAngle() == targetAngle) {
+      wristTalon.set(0);
+    }
+  }
+
+  public boolean atTarget() {
+    return getAngle() == targetAngle;
+  }
+  public void setTarget(double target) {
+    targetAngle = target;
+  }
+
+  public boolean getTop() {
+    return switchTop.get();
+  }
+  public boolean getBottom() {
+    return switchBottom.get();
+  }
+
+  public double getAngle(){
+    return wristEncoder.getDistance()*ANGLE_TO_ROTATION;
+  }
+}

From 82bf5e8e16f0c559f12058975b6a6432fb1250f7 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 9 Mar 2019 14:43:00 -0500
Subject: [PATCH 36/50] Added climb subsystem

---
 src/main/java/frc/robot/RobotMap.java         |  4 +++
 src/main/java/frc/robot/subsystems/Climb.java | 35 +++++++++++++++++++
 2 files changed, 39 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/Climb.java

diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 19f6b17..ee69fd0 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -59,4 +59,8 @@ public class RobotMap {
   public final static int DOUBLESOL_REVERSE = 1;
   public final static int DOUBLESOL_FORWARD1 = 2;
   public static final int DOUBLESOL_REVERSE1 = 3;
+  public static final int CLIMBSOL_FRONT_FORWARD = 4;
+  public static final int CLIMBSOL_FRONT_REVERSE = 5;
+  public static final int CLIMBSOL_BACK_FORWARD = 6;
+  public static final int CLIMBSOL_BACK_REVERSE =7;
 }
diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java
new file mode 100644
index 0000000..7747387
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Climb.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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.DoubleSolenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.RobotMap;
+
+/**
+ * Add your docs here.
+ */
+public class Climb extends Subsystem {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+  DoubleSolenoid doubleSol = new DoubleSolenoid(RobotMap.CLIMBSOL_FRONT_FORWARD, RobotMap.CLIMBSOL_FRONT_REVERSE);
+  DoubleSolenoid doubleSolBack = new DoubleSolenoid(RobotMap.CLIMBSOL_BACK_FORWARD, RobotMap.CLIMBSOL_BACK_REVERSE);
+  @Override
+  public void initDefaultCommand() {
+    // Set the default command for a subsystem here.
+    // setDefaultCommand(new MySpecialCommand());
+  }
+  public void raiseFront(){
+    doubleSol.set(DoubleSolenoid.Value.kForward);
+    doubleSolBack.set(DoubleSolenoid.Value.kReverse);
+  }
+  public void raiseBack(){
+    doubleSol.set(DoubleSolenoid.Value.kReverse);
+    doubleSolBack.set(DoubleSolenoid.Value.kForward);
+  }
+}

From 79d47e53c7aa794fb96a29981b175db52123ed40 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 9 Mar 2019 15:36:00 -0500
Subject: [PATCH 37/50] Wrist debugged

---
 src/main/java/frc/robot/subsystems/Wrist.java | 34 +++++++++++++------
 1 file changed, 24 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java
index ec2b5f9..58a234a 100644
--- a/src/main/java/frc/robot/subsystems/Wrist.java
+++ b/src/main/java/frc/robot/subsystems/Wrist.java
@@ -23,7 +23,7 @@ public class Wrist extends Subsystem {
   private final static double SPEED = .5;
   //num of angles = 1 rotation   is it 360? no one knows
   private final static double ANGLE_TO_ROTATION = 360;
-
+  private boolean finished = false;
   private double targetAngle;
 
   Encoder wristEncoder;
@@ -44,6 +44,8 @@ public Wrist() {
   }
 
   public void move() {
+    finished = false;
+    boolean down = getAngle()>targetAngle;
     if(getBottom()) {
       wristEncoder.reset();
       wristTalon.set(0);
@@ -52,15 +54,25 @@ else if(getTop()) {
       wristTalon.set(0);
     }
     //forward = down  backwards = up
-    else if(getAngle() < targetAngle) {
-      wristTalon.set(-1*SPEED);
-    }
-    else if(getAngle() > targetAngle) {
-      wristTalon.set(SPEED);
-    }
-    else if(getAngle() == targetAngle) {
-      wristTalon.set(0);
+    if(down){
+      if(getAngle() < targetAngle){
+        finished = true;
+        wristTalon.set(0);
+      }else{
+        finished = false;
+        wristTalon.set(SPEED);
+      }
+    }else{
+      if(getAngle() < targetAngle) {
+        wristTalon.set(-SPEED);
+      }
+      else {
+        finished = true;
+        wristTalon.set(0);
+      }
+      
     }
+    
   }
 
   public boolean atTarget() {
@@ -76,7 +88,9 @@ public boolean getTop() {
   public boolean getBottom() {
     return switchBottom.get();
   }
-
+  public boolean getFinished(){
+    return finished;
+  }
   public double getAngle(){
     return wristEncoder.getDistance()*ANGLE_TO_ROTATION;
   }

From f9d1fabe710f24d4c93518bcdccec423b2dd4d1f Mon Sep 17 00:00:00 2001
From: Bad Joke <32912199+Badly-Timed-Joke@users.noreply.github.com>
Date: Mon, 11 Mar 2019 20:49:38 -0400
Subject: [PATCH 38/50] Position arm command created, need to input actual
 distance in terms of StringPot in Arm class, not tested

---
 src/main/java/frc/robot/OI.java               | 22 ++++++++-
 src/main/java/frc/robot/Robot.java            |  3 --
 .../{StringpotDriver.java => MoveArmTo.java}  | 49 ++++++++++++++++---
 src/main/java/frc/robot/subsystems/Arm.java   | 21 +++++++-
 .../java/frc/robot/subsystems/StringPot.java  | 30 ------------
 5 files changed, 83 insertions(+), 42 deletions(-)
 rename src/main/java/frc/robot/commands/{StringpotDriver.java => MoveArmTo.java} (60%)
 delete mode 100644 src/main/java/frc/robot/subsystems/StringPot.java

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index a200f8a..65758fb 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -9,12 +9,14 @@
 
 import java.awt.event.KeyListener;
 
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import edu.wpi.first.wpilibj.buttons.Trigger;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.*;
+import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.CameraI2c;
 
 /**
@@ -51,15 +53,33 @@ public class OI {
   // button.whenReleased(new ExampleCommand());
 
   public static XboxController xBoxControl = new XboxController(0);
+  public static XboxController xBoxControlArm = new XboxController(1);
 
   public OI() {
-   // Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
+    //Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
     Button pistonExtend = new JoystickButton(xBoxControl, 8);
     pistonExtend.whenPressed(new ExtendPiston());
     Button pistonRetract = new JoystickButton(xBoxControl, 7);
     pistonRetract.whenPressed(new RetractPiston());
     Button button = new JoystickButton(xBoxControl, 2);
     button.whenPressed(new TurnDegrees());
+
+    Button lowRocket_c = new JoystickButton(xBoxControlArm, 1);
+    lowRocket_c.whenPressed(new MoveArmTo(Arm.POSITION[0]));
+    Button midRocket_c = new JoystickButton(xBoxControlArm, 2);
+    midRocket_c.whenPressed(new MoveArmTo(Arm.POSITION[1]));
+    Button highRocket_c = new JoystickButton(xBoxControlArm, 3);
+    highRocket_c.whenPressed(new MoveArmTo(Arm.POSITION[2]));
+
+    Button lowRocket_h = new JoystickButton(xBoxControlArm, 4);
+    lowRocket_h.whenPressed(new MoveArmTo(Arm.POSITION[3]));
+    Button midRocket_h = new JoystickButton(xBoxControlArm, 5);
+    midRocket_h.whenPressed(new MoveArmTo(Arm.POSITION[4]));
+    //not possible to do high rocket hatch due to no teleArm
+
+    Button cargoShip_h = new JoystickButton(xBoxControlArm, 6);
+    cargoShip_h.whenPressed(new MoveArmTo(Arm.POSITION[5]));
+
     /*Button aButton = new JoystickButton(xBoxControl, 1);
     aButton.whenPressed(new ExtendPiston());
     aButton.whenReleased(new StopTeleArm());*/
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 880716d..7d37c6b 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -31,7 +31,6 @@
 import frc.robot.subsystems.LaserFinder;
 import frc.robot.subsystems.Photoresistor;
 import frc.robot.subsystems.Ramps;
-import frc.robot.subsystems.StringPot;
 import frc.robot.subsystems.TelescopingArm;
 import frc.robot.subsystems.pneumatics;
 
@@ -69,7 +68,6 @@ public class Robot extends TimedRobot {
   public static Photoresistor resistor;
   public static Timer m_timer;
   public static LaserFinder m_laser;
-  public static StringPot m_stringPot;
   public static Wrist m_wrist;
   //public static Ramps m_ramps;
   /**
@@ -96,7 +94,6 @@ public void robotInit() {
     m_hallEfect = new Hallefect();
     resistor = new Photoresistor();
     m_timer = new Timer();
-    m_stringPot = new StringPot();
     m_laser = new LaserFinder();
     SmartDashboard.putBoolean("TankDrive", false);
     m_ramps = new Ramps();
diff --git a/src/main/java/frc/robot/commands/StringpotDriver.java b/src/main/java/frc/robot/commands/MoveArmTo.java
similarity index 60%
rename from src/main/java/frc/robot/commands/StringpotDriver.java
rename to src/main/java/frc/robot/commands/MoveArmTo.java
index 86d7db1..0420d64 100644
--- a/src/main/java/frc/robot/commands/StringpotDriver.java
+++ b/src/main/java/frc/robot/commands/MoveArmTo.java
@@ -8,30 +8,63 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 
-public class StringpotDriver extends Command {
-  public StringpotDriver() {
+public class MoveArmTo extends Command {
+  int target;
+  boolean isDown;
+  boolean finished;
+  int currentPos;
+
+  public MoveArmTo(int target) {
     // Use requires() here to declare subsystem dependencies
-    requires(Robot.m_stringPot);
+    requires(Robot.arm);
+    this.target = target;
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    currentPos = 1024-Robot.arm.readPos();
+    isDown = currentPos > target;
+    finished = false;
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    SmartDashboard.putNumber("String Potentiometer", Robot.m_stringPot.readPos());
+    finished = false;
+    currentPos = 1024-Robot.arm.readPos();
+    if(isDown) {
+
+      if(currentPos <= target) {
+        Robot.arm.stop();
+        finished = true;
+      }
+      else {
+        Robot.arm.moveDown();
+        finished = false;
+      }
+
+    }
+    else {
+
+      if(currentPos >= target) {
+        Robot.arm.stop();
+        finished = true;
+      }
+      else {
+        Robot.arm.move();
+        finished = false;
+      }
+
+    }
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+    return finished;
   }
 
   // Called once after isFinished returns true
@@ -44,4 +77,8 @@ protected void end() {
   @Override
   protected void interrupted() {
   }
+
+  public void setTarget(int target) {
+    this.target = target;
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 0b5d477..5726903 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -7,6 +7,7 @@
 
 package frc.robot.subsystems;
 
+import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Spark;
@@ -24,10 +25,14 @@
  * Add your docs here.
  */
 public class Arm extends Subsystem {
+  //REMEMBER TO REPLACE WITH ACTUAL MEASUREMENTS))(*@&*&%(*($*!@&$@!*&#^*&@!$
+  public static final int POSITION[] = {0,1,2,3,4,5,6,7};
+
   //WPI_TalonSRX motor;
   private static final double leftMod = .95;
   private static final double rightMod = 1;
   
+  AnalogInput sensor = new AnalogInput(RobotMap.STRING_POT);
   Encoder armEncoder;
   DigitalInput enc;
   double armModifier = .1;
@@ -39,6 +44,8 @@ public class Arm extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public Arm() {
+    
+    //0 = lorwRockCarg  1 = midRockCarg 2 = highRockCarg  3 = lowRockHatch  4 = midRockHatch  5 = cargoShipHatch
     armEncoder = new Encoder(RobotMap.ARM_ENCODER_A, RobotMap.ARM_ENCODER_B);
     //motor = new WPI_TalonSRX(3);
     sparkL = new Spark(RobotMap.ARM_L);
@@ -49,6 +56,7 @@ public Arm() {
   public void initDefaultCommand() {
     setDefaultCommand(new ArmDrive());
   }
+
   public void move(){
     //SmartDashboard.putNumber("Arm", speed);
     
@@ -63,6 +71,10 @@ public void move(){
     //sparkR.setSpeed(speed * rightMod);
     //motor.set(speed);
   }
+  
+  public int readPos(){
+    return sensor.getValue();
+  }
   public double getRotations(){
     return armEncoder.getDistance();
   }
@@ -70,8 +82,13 @@ public boolean getSwitch(){
     return switchBottom.get();
   }
   public void moveDown(){
-    sparkL.setSpeed(-leftMod);
-    sparkR.setSpeed(-rightMod);
+    if(getSwitch()) {
+      stop();
+    }
+    else {
+      sparkL.setSpeed(-leftMod);
+      sparkR.setSpeed(-rightMod);
+    }
   }
   public void stop(){
     sparkL.setSpeed(0);
diff --git a/src/main/java/frc/robot/subsystems/StringPot.java b/src/main/java/frc/robot/subsystems/StringPot.java
deleted file mode 100644
index bcf3e1f..0000000
--- a/src/main/java/frc/robot/subsystems/StringPot.java
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.AnalogInput;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import frc.robot.RobotMap;
-import frc.robot.commands.StringpotDriver;
-
-/**
- * Add your docs here.
- */
-public class StringPot extends Subsystem {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-  AnalogInput sensor = new AnalogInput(RobotMap.STRING_POT);
-  @Override
-  public void initDefaultCommand() {
-    // Set the default command for a subsystem here.
-    setDefaultCommand(new StringpotDriver());
-  }
-  public double readPos(){
-    return sensor.getValue();
-  }
-}

From 66ca72420162a52f9ec6a1dac994fcf183cf7c02 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Wed, 13 Mar 2019 19:36:56 -0400
Subject: [PATCH 39/50] Fixed arm positions, still not tested

---
 src/main/java/frc/robot/OI.java             | 12 ++++-----
 src/main/java/frc/robot/subsystems/Arm.java | 27 ++++++++++++---------
 2 files changed, 21 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 65758fb..f562d46 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -65,20 +65,20 @@ public OI() {
     button.whenPressed(new TurnDegrees());
 
     Button lowRocket_c = new JoystickButton(xBoxControlArm, 1);
-    lowRocket_c.whenPressed(new MoveArmTo(Arm.POSITION[0]));
+    lowRocket_c.whenPressed(new MoveArmTo(1));
     Button midRocket_c = new JoystickButton(xBoxControlArm, 2);
-    midRocket_c.whenPressed(new MoveArmTo(Arm.POSITION[1]));
+    midRocket_c.whenPressed(new MoveArmTo(2));
     Button highRocket_c = new JoystickButton(xBoxControlArm, 3);
-    highRocket_c.whenPressed(new MoveArmTo(Arm.POSITION[2]));
+    highRocket_c.whenPressed(new MoveArmTo(3));
 
     Button lowRocket_h = new JoystickButton(xBoxControlArm, 4);
-    lowRocket_h.whenPressed(new MoveArmTo(Arm.POSITION[3]));
+    lowRocket_h.whenPressed(new MoveArmTo(4));
     Button midRocket_h = new JoystickButton(xBoxControlArm, 5);
-    midRocket_h.whenPressed(new MoveArmTo(Arm.POSITION[4]));
+    midRocket_h.whenPressed(new MoveArmTo(5));
     //not possible to do high rocket hatch due to no teleArm
 
     Button cargoShip_h = new JoystickButton(xBoxControlArm, 6);
-    cargoShip_h.whenPressed(new MoveArmTo(Arm.POSITION[5]));
+    cargoShip_h.whenPressed(new MoveArmTo(6));
 
     /*Button aButton = new JoystickButton(xBoxControl, 1);
     aButton.whenPressed(new ExtendPiston());
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 5726903..b5890af 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -11,13 +11,7 @@
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Spark;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.Talon;
-import edu.wpi.first.wpilibj.GenericHID.Hand;
 import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.OI;
-import frc.robot.Robot;
 import frc.robot.RobotMap;
 import frc.robot.commands.ArmDrive;
 
@@ -26,13 +20,14 @@
  */
 public class Arm extends Subsystem {
   //REMEMBER TO REPLACE WITH ACTUAL MEASUREMENTS))(*@&*&%(*($*!@&$@!*&#^*&@!$
-  public static final int POSITION[] = {0,1,2,3,4,5,6,7};
-
+  public static final int POSISTIONS[] = {0,1,2,3,4,5,6,7};
+  public int position = 0;
+  public static int MARGIN_OF_ERROR = 30;
   //WPI_TalonSRX motor;
   private static final double leftMod = .95;
   private static final double rightMod = 1;
   
-  AnalogInput sensor = new AnalogInput(RobotMap.STRING_POT);
+  AnalogInput sensor;
   Encoder armEncoder;
   DigitalInput enc;
   double armModifier = .1;
@@ -44,13 +39,21 @@ public class Arm extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public Arm() {
-    
-    //0 = lorwRockCarg  1 = midRockCarg 2 = highRockCarg  3 = lowRockHatch  4 = midRockHatch  5 = cargoShipHatch
-    armEncoder = new Encoder(RobotMap.ARM_ENCODER_A, RobotMap.ARM_ENCODER_B);
     //motor = new WPI_TalonSRX(3);
+    sensor = new AnalogInput(RobotMap.STRING_POT);
     sparkL = new Spark(RobotMap.ARM_L);
     sparkR = new Spark(RobotMap.ARM_R);
     switchBottom = new DigitalInput(RobotMap.ARM_SWITCHBOTTOM);
+    // Go through positions, and find the one you are close to
+    for(int i : POSISTIONS){
+      if(Math.abs(readPos() - MARGIN_OF_ERROR) <= i){
+        for(int index = 0; index < POSISTIONS.length; index++){
+          if(POSISTIONS[index] == i){
+            position = index;
+          }
+        }
+      }
+    }
   }
   @Override
   public void initDefaultCommand() {

From d54eea095a73fca9b585f8f545a3e102243603b8 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Wed, 13 Mar 2019 20:46:09 -0400
Subject: [PATCH 40/50] Added arm positions, and custom controller

---
 src/main/java/frc/robot/OI.java                 | 4 +++-
 src/main/java/frc/robot/Robot.java              | 4 +++-
 src/main/java/frc/robot/commands/MoveArmTo.java | 9 +++++----
 src/main/java/frc/robot/subsystems/Arm.java     | 9 +++++++--
 4 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index f562d46..01063e6 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -8,7 +8,9 @@
 package frc.robot;
 
 import java.awt.event.KeyListener;
+import java.util.ResourceBundle.Control;
 
+import edu.wpi.first.wpilibj.Controller;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.buttons.Button;
@@ -53,7 +55,7 @@ public class OI {
   // button.whenReleased(new ExampleCommand());
 
   public static XboxController xBoxControl = new XboxController(0);
-  public static XboxController xBoxControlArm = new XboxController(1);
+  public static Joystick xBoxControlArm = new Joystick(1);
 
   public OI() {
     //Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 7d37c6b..5af3fc7 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -120,6 +120,7 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
+    SmartDashboard.putNumber("String pot", arm.readPos());
     //--------------------------Do 10 times per Second --------------------------------------------------
     if(m_timer.hasPeriodPassed(.1)){
       CameraI2c.read();
@@ -132,7 +133,8 @@ public void robotPeriodic() {
     /*
     SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX());
     SmartDashboard.putNumber("Gyro-Y", m_driveTrain.getGyroY());
-    SmartDashboard.putNumber("Gyro-Z", m_driveTrain.getGyroZ());
+    SmartDashboard.putN
+    umber("Gyro-Z", m_driveTrain.getGyroZ());
     */
     /*
     TurnDegrees.offset = SmartDashboard.getNumber("Offset",  TurnDegrees.offset);
diff --git a/src/main/java/frc/robot/commands/MoveArmTo.java b/src/main/java/frc/robot/commands/MoveArmTo.java
index 0420d64..0756944 100644
--- a/src/main/java/frc/robot/commands/MoveArmTo.java
+++ b/src/main/java/frc/robot/commands/MoveArmTo.java
@@ -9,6 +9,7 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.Robot;
+import frc.robot.subsystems.Arm;
 
 public class MoveArmTo extends Command {
   int target;
@@ -25,8 +26,8 @@ public MoveArmTo(int target) {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    currentPos = 1024-Robot.arm.readPos();
-    isDown = currentPos > target;
+    currentPos = 430-Robot.arm.readPos();
+    isDown = currentPos > Arm.POSISTIONS[target];
     finished = false;
   }
 
@@ -34,7 +35,7 @@ protected void initialize() {
   @Override
   protected void execute() {
     finished = false;
-    currentPos = 1024-Robot.arm.readPos();
+    currentPos = 430-Robot.arm.readPos();
     if(isDown) {
 
       if(currentPos <= target) {
@@ -64,7 +65,7 @@ protected void execute() {
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return finished;
+    return false;
   }
 
   // Called once after isFinished returns true
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index b5890af..07b4656 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -12,8 +12,10 @@
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.RobotMap;
 import frc.robot.commands.ArmDrive;
+import frc.robot.commands.MoveArmTo;
 
 /**
  * Add your docs here.
@@ -44,6 +46,7 @@ public Arm() {
     sparkL = new Spark(RobotMap.ARM_L);
     sparkR = new Spark(RobotMap.ARM_R);
     switchBottom = new DigitalInput(RobotMap.ARM_SWITCHBOTTOM);
+    SmartDashboard.putNumber("String pot", readPos());
     // Go through positions, and find the one you are close to
     for(int i : POSISTIONS){
       if(Math.abs(readPos() - MARGIN_OF_ERROR) <= i){
@@ -57,12 +60,14 @@ public Arm() {
   }
   @Override
   public void initDefaultCommand() {
-    setDefaultCommand(new ArmDrive());
+    setDefaultCommand(new MoveArmTo(100));
+    
+    //setDefaultCommand(new ArmDrive());
   }
 
   public void move(){
     //SmartDashboard.putNumber("Arm", speed);
-    
+    SmartDashboard.putNumber("String pot", readPos());
       if(getSwitch()){
         sparkL.setSpeed(0);
         sparkR.setSpeed(0);

From 91d2f648823420cc7358a166b2a0dfc24cb96d5f Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Tue, 19 Mar 2019 20:12:23 -0400
Subject: [PATCH 41/50] Got rid of telearm and made ArmMoveTo work

---
 src/main/java/frc/robot/OI.java               | 32 +++++++----
 src/main/java/frc/robot/Robot.java            | 12 ----
 src/main/java/frc/robot/RobotMap.java         | 13 ++---
 .../java/frc/robot/commands/LowerRamps.java   | 46 ----------------
 .../java/frc/robot/commands/MoveArmTo.java    | 30 +++++++---
 .../java/frc/robot/commands/StopTeleArm.java  | 46 ----------------
 .../java/frc/robot/commands/TeleMoveArm.java  | 55 -------------------
 src/main/java/frc/robot/subsystems/Arm.java   |  8 +--
 src/main/java/frc/robot/subsystems/Ramps.java | 45 ---------------
 .../frc/robot/subsystems/TelescopingArm.java  | 45 ---------------
 10 files changed, 51 insertions(+), 281 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/LowerRamps.java
 delete mode 100644 src/main/java/frc/robot/commands/StopTeleArm.java
 delete mode 100644 src/main/java/frc/robot/commands/TeleMoveArm.java
 delete mode 100644 src/main/java/frc/robot/subsystems/Ramps.java
 delete mode 100644 src/main/java/frc/robot/subsystems/TelescopingArm.java

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index 01063e6..cdc7c3a 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -56,30 +56,38 @@ public class OI {
 
   public static XboxController xBoxControl = new XboxController(0);
   public static Joystick xBoxControlArm = new Joystick(1);
-
+  Button pistonExtend = new JoystickButton(xBoxControl, 8);
+  Button pistonRetract = new JoystickButton(xBoxControl, 7);
+  Button lowRocket_c = new JoystickButton(xBoxControlArm, 1);
+  Button midRocket_c = new JoystickButton(xBoxControlArm, 2);
+  Button button = new JoystickButton(xBoxControl, 2);
+  Button highRocket_c = new JoystickButton(xBoxControlArm, 3);
+  Button lowRocket_h = new JoystickButton(xBoxControlArm, 4);
+  Button cargoShip_h = new JoystickButton(xBoxControlArm, 6);
+  Button midRocket_h = new JoystickButton(xBoxControlArm, 5);
   public OI() {
     //Button rTrig = new JoystickButton(xBoxControl, buttonNumber)
-    Button pistonExtend = new JoystickButton(xBoxControl, 8);
+    
     pistonExtend.whenPressed(new ExtendPiston());
-    Button pistonRetract = new JoystickButton(xBoxControl, 7);
+    
     pistonRetract.whenPressed(new RetractPiston());
-    Button button = new JoystickButton(xBoxControl, 2);
+    
     button.whenPressed(new TurnDegrees());
 
-    Button lowRocket_c = new JoystickButton(xBoxControlArm, 1);
+    
     lowRocket_c.whenPressed(new MoveArmTo(1));
-    Button midRocket_c = new JoystickButton(xBoxControlArm, 2);
-    midRocket_c.whenPressed(new MoveArmTo(2));
-    Button highRocket_c = new JoystickButton(xBoxControlArm, 3);
-    highRocket_c.whenPressed(new MoveArmTo(3));
+    
+    midRocket_c.whenActive(new MoveArmTo(2));
+
+    highRocket_c.whenActive(new MoveArmTo(3));
+
 
-    Button lowRocket_h = new JoystickButton(xBoxControlArm, 4);
     lowRocket_h.whenPressed(new MoveArmTo(4));
-    Button midRocket_h = new JoystickButton(xBoxControlArm, 5);
+
     midRocket_h.whenPressed(new MoveArmTo(5));
     //not possible to do high rocket hatch due to no teleArm
 
-    Button cargoShip_h = new JoystickButton(xBoxControlArm, 6);
+
     cargoShip_h.whenPressed(new MoveArmTo(6));
 
     /*Button aButton = new JoystickButton(xBoxControl, 1);
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 5af3fc7..013841b 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -30,14 +30,7 @@
 import frc.robot.subsystems.Hallefect;
 import frc.robot.subsystems.LaserFinder;
 import frc.robot.subsystems.Photoresistor;
-import frc.robot.subsystems.Ramps;
-import frc.robot.subsystems.TelescopingArm;
 import frc.robot.subsystems.pneumatics;
-
-import java.awt.Color;
-import java.util.Map;
-
-import com.analog.adis16448.frc.ADIS16448_IMU;
 import frc.robot.subsystems.UltraSonic;
 import frc.robot.subsystems.Wrist;
 
@@ -54,7 +47,6 @@ public class Robot extends TimedRobot {
   public static OI m_oi;
   public static DriveTrain m_driveTrain;
   //public static final ADIS16448_IMU imu = new ADIS16448_IMU();
-  public static Ramps m_ramps;
   public static Teleop m_teleop;
   Command m_autonomousCommand;
   Compressor com = new Compressor(0);
@@ -63,7 +55,6 @@ public class Robot extends TimedRobot {
   public static Claw claw;
   public static CameraI2c camera;
   public static Arm arm;
-  public static TelescopingArm m_teleArm;
   public static Hallefect m_hallEfect;
   public static Photoresistor resistor;
   public static Timer m_timer;
@@ -90,14 +81,11 @@ public void robotInit() {
     CameraServer.getInstance().startAutomaticCapture(0);
     CameraServer.getInstance().startAutomaticCapture(1);
     Scheduler.getInstance().add(new Teleop());
-    m_teleArm = new TelescopingArm();
     m_hallEfect = new Hallefect();
     resistor = new Photoresistor();
     m_timer = new Timer();
     m_laser = new LaserFinder();
     SmartDashboard.putBoolean("TankDrive", false);
-    m_ramps = new Ramps();
-    ArmDrive armDrive = new ArmDrive();
     m_wrist = new Wrist();
 
     m_ultraSonic = new UltraSonic();
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index b466dc9..9a5705d 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -41,20 +41,17 @@ public class RobotMap {
   public final static int WRIST_ENCODER_A = 6;
   public final static int WRIST_ENCODER_B = 7;
 
-  //PWM
-  public final static int ARM_L = 2;//
-  public final static int ARM_R = 0;//
-  public final static int TELE_ARM = 5;//
+  //CAN
+  public final static int ARM_L = 4;//
+  public final static int ARM_R = 5;//
   public final static int CLAW_1 = 3;//
   public final static int CLAW_2 = 1;//
-  public static final int RAMPS = 4;//
-  
-  //CAN
+ // public static final int RAMPS = 4;// NOT USED
   public final static int LEFTMOTOR_1 = 3;  
   public final static int LEFTMOTOR_2 = 1;
   public final static int RIGHTMOTOR_1 = 2;
   public final static int RIGHTMOTOR_2 = 0; 
-  public final static int WRISTMOTOR = 4;
+  public final static int WRISTMOTOR = 8;
 
    //Pneumatics
   public final static int PNEUMATIC_COMPRESSOR = 0;
diff --git a/src/main/java/frc/robot/commands/LowerRamps.java b/src/main/java/frc/robot/commands/LowerRamps.java
deleted file mode 100644
index 537ac76..0000000
--- a/src/main/java/frc/robot/commands/LowerRamps.java
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 LowerRamps extends Command {
-  public LowerRamps() {
-    // Use requires() here to declare subsystem dependencies
-    requires(Robot.m_ramps);
-  }
-
-  // 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() {
-    Robot.m_ramps.lower();
-  }
-
-  // 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() {
-  }
-}
diff --git a/src/main/java/frc/robot/commands/MoveArmTo.java b/src/main/java/frc/robot/commands/MoveArmTo.java
index 0756944..07b9045 100644
--- a/src/main/java/frc/robot/commands/MoveArmTo.java
+++ b/src/main/java/frc/robot/commands/MoveArmTo.java
@@ -7,26 +7,32 @@
 
 package frc.robot.commands;
 
+
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Wrist;
 
 public class MoveArmTo extends Command {
   int target;
   boolean isDown;
   boolean finished;
   int currentPos;
+  double WRIST_ANGLE_TOP = 15;
+  double WRIST_ANGLE_BOTTOM = 1;
 
   public MoveArmTo(int target) {
     // Use requires() here to declare subsystem dependencies
     requires(Robot.arm);
+    requires(Robot.m_wrist);
     this.target = target;
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    currentPos = 430-Robot.arm.readPos();
+    currentPos = 500-Robot.arm.readPos();
     isDown = currentPos > Arm.POSISTIONS[target];
     finished = false;
   }
@@ -34,23 +40,24 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    SmartDashboard.putNumber("Position to go to", target);
     finished = false;
-    currentPos = 430-Robot.arm.readPos();
+    currentPos = 500-Robot.arm.readPos();
+    SmartDashboard.putNumber("Arm position", currentPos);
     if(isDown) {
 
-      if(currentPos <= target) {
+      if(currentPos <=  Arm.POSISTIONS[target]) {
         Robot.arm.stop();
         finished = true;
+        
       }
       else {
         Robot.arm.moveDown();
         finished = false;
       }
-
     }
     else {
-
-      if(currentPos >= target) {
+      if(currentPos >= Arm.POSISTIONS[target]) {
         Robot.arm.stop();
         finished = true;
       }
@@ -58,14 +65,21 @@ protected void execute() {
         Robot.arm.move();
         finished = false;
       }
-
+      if(target == 7){
+        Robot.m_wrist.setTarget(WRIST_ANGLE_TOP);
+      }
+      if(target != 7){
+        Robot.m_wrist.setTarget(WRIST_ANGLE_BOTTOM);
+      }
     }
+    Robot.m_wrist.move();
+    SmartDashboard.putBoolean("Arm Done", finished);
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    return false;
+    return finished && Robot.m_wrist.getFinished();
   }
 
   // Called once after isFinished returns true
diff --git a/src/main/java/frc/robot/commands/StopTeleArm.java b/src/main/java/frc/robot/commands/StopTeleArm.java
deleted file mode 100644
index 12e06bb..0000000
--- a/src/main/java/frc/robot/commands/StopTeleArm.java
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 StopTeleArm extends Command {
-  public StopTeleArm() {
-    // Use requires() here to declare subsystem dependencies
-    requires(Robot.m_teleArm);
-  }
-
-  // Called just before this Command runs the first time
-  @Override
-  protected void initialize() {
-    Robot.m_teleArm.stop();
-  }
-
-  // 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 true;
-  }
-
-  // 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/commands/TeleMoveArm.java b/src/main/java/frc/robot/commands/TeleMoveArm.java
deleted file mode 100644
index 8e386d2..0000000
--- a/src/main/java/frc/robot/commands/TeleMoveArm.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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;
-import frc.robot.subsystems.TelescopingArm;
-
-public class TeleMoveArm extends Command {
-  boolean setForward;
-  public TeleMoveArm(boolean setForward) {
-    // Use requires() here to declare subsystem dependencies
-    this.setForward = setForward;
-    requires(Robot.m_teleArm);
-  }
-
-  // Called just before this Command runs the first time
-  @Override
-  protected void initialize() {
-    if(setForward){
-      Robot.m_teleArm.setForward();
-    }else{
-      Robot.m_teleArm.setBackwards();
-    }
-    Robot.m_teleArm.moveArm();
-  }
-
-  // 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 true;
-  }
-
-  // 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/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 07b4656..d647ecf 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -22,7 +22,7 @@
  */
 public class Arm extends Subsystem {
   //REMEMBER TO REPLACE WITH ACTUAL MEASUREMENTS))(*@&*&%(*($*!@&$@!*&#^*&@!$
-  public static final int POSISTIONS[] = {0,1,2,3,4,5,6,7};
+  public static final int POSISTIONS[] = {20,90,130,160,210,300,480};
   public int position = 0;
   public static int MARGIN_OF_ERROR = 30;
   //WPI_TalonSRX motor;
@@ -60,15 +60,15 @@ public Arm() {
   }
   @Override
   public void initDefaultCommand() {
-    setDefaultCommand(new MoveArmTo(100));
+    //setDefaultCommand(new MoveArmTo(7));
     
-    //setDefaultCommand(new ArmDrive());
+    setDefaultCommand(new ArmDrive());
   }
 
   public void move(){
     //SmartDashboard.putNumber("Arm", speed);
     SmartDashboard.putNumber("String pot", readPos());
-      if(getSwitch()){
+     if(getSwitch()){
         sparkL.setSpeed(0);
         sparkR.setSpeed(0);
       } else {
diff --git a/src/main/java/frc/robot/subsystems/Ramps.java b/src/main/java/frc/robot/subsystems/Ramps.java
deleted file mode 100644
index 95326be..0000000
--- a/src/main/java/frc/robot/subsystems/Ramps.java
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.Spark;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import frc.robot.OI;
-import frc.robot.RobotMap;
-import frc.robot.commands.LowerRamps;
-
-/**
- * Add your docs here.
- */
-public class Ramps extends Subsystem {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-  private final double speed = .5;
-  Spark motor;
-  @Override
-  public void initDefaultCommand() {
-    // Set the default command for a subsystem here.
-     setDefaultCommand(new LowerRamps());
-  }
-  public Ramps(){
-    motor = new Spark(RobotMap.RAMPS);
-  }
-  public void lower(){
-    if(OI.xBoxControl.getYButton()){
-      motor.set(speed);
-    }else if(OI.xBoxControl.getXButton()){
-      motor.set(-speed);
-    }else{
-      motor.set(0);
-    }
-  }
-
-  public void stop(){
-    motor.set(0);
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/TelescopingArm.java b/src/main/java/frc/robot/subsystems/TelescopingArm.java
deleted file mode 100644
index a10a8b0..0000000
--- a/src/main/java/frc/robot/subsystems/TelescopingArm.java
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.Spark;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import frc.robot.RobotMap;
-import frc.robot.commands.StopTeleArm;
-
-/**
- * Add your docs here.
- */
-public class TelescopingArm extends Subsystem {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
-  Spark spark = new Spark(RobotMap.TELE_ARM);
-  double speed = 0;
-
-  @Override
-  public void initDefaultCommand() {
-    // Set the default command for a subsystem here.
-     setDefaultCommand(new StopTeleArm());
-  }
-
-  public void moveArm()
-  {
-    spark.set(speed);
-  }
-  public void setForward(){
-    speed = Math.abs(speed);
-  }
-  public void setBackwards(){
-    speed = -Math.abs(speed);
-  }
-  public void stop(){
-    spark.set(0);
-  }
-}
-

From 116dd87ecb1dee609d734306332218185c96d459 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Wed, 20 Mar 2019 19:39:51 -0400
Subject: [PATCH 42/50] Added arm reed switches instead of the sting pot

---
 src/main/java/frc/robot/Robot.java            |  8 +--
 src/main/java/frc/robot/RobotMap.java         | 21 +++---
 .../frc/robot/commands/HalleffectCounter.java | 55 ---------------
 .../java/frc/robot/commands/MoveArmTo.java    | 49 ++++++-------
 src/main/java/frc/robot/subsystems/Arm.java   | 69 +++++++------------
 .../java/frc/robot/subsystems/Hallefect.java  | 38 ----------
 src/main/java/frc/robot/subsystems/Wrist.java | 11 ++-
 7 files changed, 66 insertions(+), 185 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/HalleffectCounter.java
 delete mode 100644 src/main/java/frc/robot/subsystems/Hallefect.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 013841b..d7fd395 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -9,16 +9,12 @@
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.Compressor;
-import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.commands.ArmDrive;
 import frc.robot.commands.ExampleCommand;
 import frc.robot.commands.Teleop;
 import frc.robot.commands.TurnDegrees;
@@ -27,7 +23,6 @@
 import frc.robot.subsystems.Claw;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.ExampleSubsystem;
-import frc.robot.subsystems.Hallefect;
 import frc.robot.subsystems.LaserFinder;
 import frc.robot.subsystems.Photoresistor;
 import frc.robot.subsystems.pneumatics;
@@ -55,7 +50,6 @@ public class Robot extends TimedRobot {
   public static Claw claw;
   public static CameraI2c camera;
   public static Arm arm;
-  public static Hallefect m_hallEfect;
   public static Photoresistor resistor;
   public static Timer m_timer;
   public static LaserFinder m_laser;
@@ -81,7 +75,6 @@ public void robotInit() {
     CameraServer.getInstance().startAutomaticCapture(0);
     CameraServer.getInstance().startAutomaticCapture(1);
     Scheduler.getInstance().add(new Teleop());
-    m_hallEfect = new Hallefect();
     resistor = new Photoresistor();
     m_timer = new Timer();
     m_laser = new LaserFinder();
@@ -108,6 +101,7 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
+    SmartDashboard.putNumber("Wrist Encoder", m_wrist.getAngle());
     SmartDashboard.putNumber("String pot", arm.readPos());
     //--------------------------Do 10 times per Second --------------------------------------------------
     if(m_timer.hasPeriodPassed(.1)){
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 9a5705d..052a1de 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -32,10 +32,10 @@ public class RobotMap {
   //public final static int ARMLIMITSWITCH = 2;
 
   //DIO
-  public final static int ARM_ENCODER_A = 2;  
-  public final static int ARM_ENCODER_B = 3;
+  public final static int ARM_REED_2 = 2;  
+  public final static int ARM_REED_3 = 3;
   public final static int ARM_SWITCHBOTTOM = 0;
-  public final static int HALLEFFECT = 1; //UNUSED(?)
+  public final static int ARM_REED_1 = 1; //UNUSED(?)
   public final static int ULTRASONIC = 4;
   public final static int ULTRASONIC_ECHO = 5;
   public final static int WRIST_ENCODER_A = 6;
@@ -44,14 +44,13 @@ public class RobotMap {
   //CAN
   public final static int ARM_L = 4;//
   public final static int ARM_R = 5;//
-  public final static int CLAW_1 = 3;//
-  public final static int CLAW_2 = 1;//
- // public static final int RAMPS = 4;// NOT USED
-  public final static int LEFTMOTOR_1 = 3;  
-  public final static int LEFTMOTOR_2 = 1;
-  public final static int RIGHTMOTOR_1 = 2;
-  public final static int RIGHTMOTOR_2 = 0; 
-  public final static int WRISTMOTOR = 8;
+  public final static int CLAW_1 = 6;//
+  public final static int CLAW_2 = 7;//
+  public final static int LEFTMOTOR_1 = 3;  //
+  public final static int LEFTMOTOR_2 = 1;//
+  public final static int RIGHTMOTOR_1 = 2;//
+  public final static int RIGHTMOTOR_2 = 0; //
+  public final static int WRISTMOTOR = 8;//
 
    //Pneumatics
   public final static int PNEUMATIC_COMPRESSOR = 0;
diff --git a/src/main/java/frc/robot/commands/HalleffectCounter.java b/src/main/java/frc/robot/commands/HalleffectCounter.java
deleted file mode 100644
index f9cec35..0000000
--- a/src/main/java/frc/robot/commands/HalleffectCounter.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.Robot;
-import frc.robot.subsystems.Hallefect;
-
-public class HalleffectCounter extends Command {
-
-  public HalleffectCounter() {
-    // Use requires() here to declare subsystem dependencies
-    // eg. requires(chassis);
-    requires(Robot.m_hallEfect);
-  }
-
-  // Called just before this Command runs the first time
-  @Override
-  protected void initialize() {
-    if(Robot.m_hallEfect.getPulse()){
-      Hallefect.setCount(Hallefect.getCount()+1);
-    }
-    SmartDashboard.putNumber("Count", Hallefect.getCount());
-    
-  }
-  @Override
-  protected void execute() {
-    
-  }
-
-  // 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() {
-  }
-}
diff --git a/src/main/java/frc/robot/commands/MoveArmTo.java b/src/main/java/frc/robot/commands/MoveArmTo.java
index 07b9045..d7c5ba7 100644
--- a/src/main/java/frc/robot/commands/MoveArmTo.java
+++ b/src/main/java/frc/robot/commands/MoveArmTo.java
@@ -32,47 +32,38 @@ public MoveArmTo(int target) {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    currentPos = 500-Robot.arm.readPos();
-    isDown = currentPos > Arm.POSISTIONS[target];
+    if(Robot.arm.readPos() < target){
+      isDown = false;
+    }else if(Robot.arm.readPos() > target){
+      isDown = true;
+    }
     finished = false;
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    SmartDashboard.putNumber("Position to go to", target);
-    finished = false;
-    currentPos = 500-Robot.arm.readPos();
-    SmartDashboard.putNumber("Arm position", currentPos);
-    if(isDown) {
-
-      if(currentPos <=  Arm.POSISTIONS[target]) {
-        Robot.arm.stop();
-        finished = true;
-        
-      }
-      else {
-        Robot.arm.moveDown();
-        finished = false;
-      }
+    if(Robot.arm.readPos() == 1 && target == 1){
+      finished = true;
+    }else if(Robot.arm.readPos() == 2 && target == 2){
+      finished = true;
+    }else if(Robot.arm.readPos() == 3 && target == 3){
+      finished = true;
+    }else{
+      finished = false;
     }
-    else {
-      if(currentPos >= Arm.POSISTIONS[target]) {
-        Robot.arm.stop();
-        finished = true;
-      }
-      else {
-        Robot.arm.move();
-        finished = false;
-      }
       if(target == 7){
         Robot.m_wrist.setTarget(WRIST_ANGLE_TOP);
       }
       if(target != 7){
         Robot.m_wrist.setTarget(WRIST_ANGLE_BOTTOM);
       }
-    }
-    Robot.m_wrist.move();
+      if(!isDown){
+        Robot.m_wrist.move();
+      }else{
+        Robot.arm.moveDown();
+      }
+    
     SmartDashboard.putBoolean("Arm Done", finished);
   }
 
@@ -85,6 +76,8 @@ protected boolean isFinished() {
   // Called once after isFinished returns true
   @Override
   protected void end() {
+    Robot.arm.stop();
+    Robot.m_wrist.stop();
   }
 
   // Called when another command which requires one or more of the same
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index d647ecf..f0f4a2a 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -7,6 +7,10 @@
 
 package frc.robot.subsystems;
 
+import static org.junit.Assume.assumeNoException;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
@@ -22,70 +26,49 @@
  */
 public class Arm extends Subsystem {
   //REMEMBER TO REPLACE WITH ACTUAL MEASUREMENTS))(*@&*&%(*($*!@&$@!*&#^*&@!$
-  public static final int POSISTIONS[] = {20,90,130,160,210,300,480};
   public int position = 0;
   public static int MARGIN_OF_ERROR = 30;
   //WPI_TalonSRX motor;
   private static final double leftMod = .95;
   private static final double rightMod = 1;
-  
-  AnalogInput sensor;
-  Encoder armEncoder;
-  DigitalInput enc;
-  double armModifier = .1;
-  static DigitalInput switchBottom;
-  DigitalInput switchTop;
-  Spark sparkL;
-  Spark sparkR;
+  private static WPI_TalonSRX motorL = new WPI_TalonSRX(RobotMap.ARM_L);
+  private static WPI_TalonSRX motorR = new WPI_TalonSRX(RobotMap.ARM_R);
   double speed = 1;
+  DigitalInput switchBottom = new DigitalInput(RobotMap.ARM_SWITCHBOTTOM);
+  DigitalInput reedSwitch_1 = new DigitalInput(RobotMap.ARM_REED_1);
+  DigitalInput reedSwitch_2 = new DigitalInput(RobotMap.ARM_REED_2);
+  DigitalInput reedSwitch_3 = new DigitalInput(RobotMap.ARM_REED_3);
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public Arm() {
     //motor = new WPI_TalonSRX(3);
-    sensor = new AnalogInput(RobotMap.STRING_POT);
-    sparkL = new Spark(RobotMap.ARM_L);
-    sparkR = new Spark(RobotMap.ARM_R);
-    switchBottom = new DigitalInput(RobotMap.ARM_SWITCHBOTTOM);
-    SmartDashboard.putNumber("String pot", readPos());
-    // Go through positions, and find the one you are close to
-    for(int i : POSISTIONS){
-      if(Math.abs(readPos() - MARGIN_OF_ERROR) <= i){
-        for(int index = 0; index < POSISTIONS.length; index++){
-          if(POSISTIONS[index] == i){
-            position = index;
-          }
-        }
-      }
-    }
   }
   @Override
   public void initDefaultCommand() {
     //setDefaultCommand(new MoveArmTo(7));
-    
     setDefaultCommand(new ArmDrive());
   }
 
   public void move(){
-    //SmartDashboard.putNumber("Arm", speed);
-    SmartDashboard.putNumber("String pot", readPos());
      if(getSwitch()){
-        sparkL.setSpeed(0);
-        sparkR.setSpeed(0);
+       motorL.set(speed);
+       motorR.set(speed);
       } else {
-        sparkL.setSpeed(leftMod);
-        sparkR.setSpeed(rightMod);
+        stop();
       }
-    
-    //sparkR.setSpeed(speed * rightMod);
-    //motor.set(speed);
   }
   
   public int readPos(){
-    return sensor.getValue();
-  }
-  public double getRotations(){
-    return armEncoder.getDistance();
+    if(reedSwitch_1.get()){
+      position = 1;
+    }else if(reedSwitch_2.get()){
+      position = 2;
+    }else if(reedSwitch_3.get()){
+      position = 3;
+    }
+    return position;
   }
+
   public boolean getSwitch(){
     return switchBottom.get();
   }
@@ -94,12 +77,12 @@ public void moveDown(){
       stop();
     }
     else {
-      sparkL.setSpeed(-leftMod);
-      sparkR.setSpeed(-rightMod);
+      motorL.set(speed);
+      motorR.set(speed);
     }
   }
   public void stop(){
-    sparkL.setSpeed(0);
-    sparkR.setSpeed(0);
+    motorL.set(0);
+    motorR.set(0);
   }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Hallefect.java b/src/main/java/frc/robot/subsystems/Hallefect.java
deleted file mode 100644
index 2b9a150..0000000
--- a/src/main/java/frc/robot/subsystems/Hallefect.java
+++ /dev/null
@@ -1,38 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.DigitalInput;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import frc.robot.RobotMap;
-import frc.robot.commands.HalleffectCounter;
-
-/**
- * Add your docs here.
- */
-public class Hallefect extends Subsystem {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-  DigitalInput sensor = new DigitalInput(RobotMap.HALLEFFECT);
-  static double m_count = 0.0;
-
-  @Override
-  public void initDefaultCommand() {
-    // Set the default command for a subsystem here.
-     setDefaultCommand(new HalleffectCounter());
-  }
-  public boolean getPulse(){
-    return sensor.get();
-  }
-  public static double getCount(){
-    return m_count;
-  }
-  public static void setCount(double count){
-    m_count = count;
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java
index 58a234a..51818ab 100644
--- a/src/main/java/frc/robot/subsystems/Wrist.java
+++ b/src/main/java/frc/robot/subsystems/Wrist.java
@@ -20,9 +20,11 @@
 public class Wrist extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
-  private final static double SPEED = .5;
+  private final static double SPEED = .1;
   //num of angles = 1 rotation   is it 360? no one knows
   private final static double ANGLE_TO_ROTATION = 360;
+  public static final double TOP = 100;
+  public static final double BOTTOM = 0;
   private boolean finished = false;
   private double targetAngle;
 
@@ -55,7 +57,7 @@ else if(getTop()) {
     }
     //forward = down  backwards = up
     if(down){
-      if(getAngle() < targetAngle){
+      if(getAngle() < targetAngle && getAngle() >= BOTTOM){
         finished = true;
         wristTalon.set(0);
       }else{
@@ -63,7 +65,7 @@ else if(getTop()) {
         wristTalon.set(SPEED);
       }
     }else{
-      if(getAngle() < targetAngle) {
+      if(getAngle() < targetAngle && getAngle() <= TOP) {
         wristTalon.set(-SPEED);
       }
       else {
@@ -94,4 +96,7 @@ public boolean getFinished(){
   public double getAngle(){
     return wristEncoder.getDistance()*ANGLE_TO_ROTATION;
   }
+  public void stop(){
+    wristTalon.set(0);
+  }
 }

From 8548800028baac9c7175004130b754ebfa71df44 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Fri, 22 Mar 2019 19:19:24 -0400
Subject: [PATCH 43/50] Added auto climb, fixed ultrasonic to be analog, and
 added hatch panel grabber

---
 src/main/java/frc/robot/Robot.java            |  4 +
 src/main/java/frc/robot/RobotMap.java         |  4 +-
 .../java/frc/robot/commands/ArmDrive.java     | 18 +++++
 .../java/frc/robot/commands/ClimbStair.java   | 79 +++++++++++++++++++
 .../java/frc/robot/commands/MoveArmTo.java    |  2 +
 .../frc/robot/commands/PneumaticsDrive.java   | 13 ++-
 src/main/java/frc/robot/subsystems/Arm.java   | 18 +++--
 src/main/java/frc/robot/subsystems/Climb.java | 21 +++--
 .../java/frc/robot/subsystems/UltraSonic.java |  7 +-
 .../java/frc/robot/subsystems/pneumatics.java |  7 ++
 10 files changed, 152 insertions(+), 21 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/ClimbStair.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index d7fd395..ba26fd6 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -21,6 +21,7 @@
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.CameraI2c;
 import frc.robot.subsystems.Claw;
+import frc.robot.subsystems.Climb;
 import frc.robot.subsystems.DriveTrain;
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.LaserFinder;
@@ -54,6 +55,7 @@ public class Robot extends TimedRobot {
   public static Timer m_timer;
   public static LaserFinder m_laser;
   public static Wrist m_wrist;
+  public static Climb m_climb;
   //public static Ramps m_ramps;
   /**
    * This function is run when the robot is first started up and should be used
@@ -65,6 +67,7 @@ public void robotInit() {
     SmartDashboard.putBoolean("Arm moving",false);
     com.setClosedLoopControl(true);
     com.start();
+    m_climb = new Climb();
     claw = new Claw();
     arm = new Arm();
     camera = new CameraI2c();
@@ -102,6 +105,7 @@ public void robotInit() {
   @Override
   public void robotPeriodic() {
     SmartDashboard.putNumber("Wrist Encoder", m_wrist.getAngle());
+    SmartDashboard.putNumber("Reed switch", arm.readPos());
     SmartDashboard.putNumber("String pot", arm.readPos());
     //--------------------------Do 10 times per Second --------------------------------------------------
     if(m_timer.hasPeriodPassed(.1)){
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 052a1de..baede11 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -27,7 +27,7 @@ public class RobotMap {
 
   //Analog
   public final static int PHOTORESISTOR = 0;  
-  //public final static int ULTRASONIC = 3;
+  public final static int ULTRASONIC = 3;
   public static final int STRING_POT = 2;
   //public final static int ARMLIMITSWITCH = 2;
 
@@ -36,7 +36,7 @@ public class RobotMap {
   public final static int ARM_REED_3 = 3;
   public final static int ARM_SWITCHBOTTOM = 0;
   public final static int ARM_REED_1 = 1; //UNUSED(?)
-  public final static int ULTRASONIC = 4;
+  //public final static int ULTRASONIC = 4;
   public final static int ULTRASONIC_ECHO = 5;
   public final static int WRIST_ENCODER_A = 6;
   public final static int WRIST_ENCODER_B = 7;
diff --git a/src/main/java/frc/robot/commands/ArmDrive.java b/src/main/java/frc/robot/commands/ArmDrive.java
index fbc5d3e..9dd3267 100644
--- a/src/main/java/frc/robot/commands/ArmDrive.java
+++ b/src/main/java/frc/robot/commands/ArmDrive.java
@@ -14,6 +14,7 @@
 import frc.robot.Robot;
 
 public class ArmDrive extends Command {
+  public static final double WRIST_POSISTION_STRAIGHT = 15;
   int count = 1;
   public ArmDrive() {
     // Use requires() here to declare subsystem dependencies
@@ -28,6 +29,10 @@ protected void initialize() {
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
+    SmartDashboard.putBoolean("Swtich one", Robot.arm.switchOne());
+    SmartDashboard.putBoolean("Switch two", Robot.arm.switchTwo());
+    SmartDashboard.putBoolean("Switch three", Robot.arm.switchThree());
+    SmartDashboard.putNumber("Arm position", Robot.arm.readPos());
     if(OI.xBoxControl.getBumper(Hand.kLeft)){
       Robot.arm.move();
       SmartDashboard.putBoolean("Arm moving", true);
@@ -38,6 +43,19 @@ protected void execute() {
       SmartDashboard.putBoolean("Arm moving", false);
       Robot.arm.stop();
     }
+    if(OI.xBoxControl.getY(Hand.kLeft) > 0){
+      Robot.m_wrist.setTarget(Robot.m_wrist.getAngle() + 1);
+      Robot.m_wrist.move();
+    }else if(OI.xBoxControl.getY(Hand.kLeft) < 0){
+      Robot.m_wrist.setTarget(Robot.m_wrist.getAngle() + 1);
+      Robot.m_wrist.move();
+    }else{
+      Robot.m_wrist.stop();
+    }
+    if(OI.xBoxControl.getStickButton(Hand.kLeft)){
+      Robot.m_wrist.setTarget(WRIST_POSISTION_STRAIGHT);
+      Robot.m_wrist.move();
+    }
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/commands/ClimbStair.java b/src/main/java/frc/robot/commands/ClimbStair.java
new file mode 100644
index 0000000..554b3f1
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ClimbStair.java
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* 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 ClimbStair extends Command {
+  boolean finished = false;
+  int step = 0;
+  final static double firstDistance = 100;
+  final static double secondDistance = 50;
+  public ClimbStair() {
+    // Use requires() here to declare subsystem dependencies
+    requires(Robot.m_climb);
+    requires(Robot.m_driveTrain);
+    requires(Robot.m_ultraSonic);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    Robot.m_climb.liftFront();
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    if(step == 0){
+      if(Robot.m_ultraSonic.getDistance() > firstDistance){
+        step++;
+      }else{
+        Robot.m_driveTrain.drive(.75, 0);
+      }
+    }else if(step == 1){
+      Robot.m_climb.dropFront();
+      step++;
+    }else if(step == 2){
+      Robot.m_climb.liftBack();
+      step++;
+    }else if(step == 3){
+      if(Robot.m_ultraSonic.getDistance() > secondDistance){
+        Robot.m_driveTrain.drive(.75, 0);
+      }
+    }else if(step == 4){
+      Robot.m_climb.dropBack();
+    }else if(step == 5){
+      if(Robot.m_ultraSonic.getDistance() > 10){
+        Robot.m_driveTrain.drive(.5, 0);
+      }else{
+        step++;
+      }
+    }else if(step == 6){
+      finished = true;
+    }
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return finished;
+  }
+
+  // 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/commands/MoveArmTo.java b/src/main/java/frc/robot/commands/MoveArmTo.java
index d7c5ba7..0eb5774 100644
--- a/src/main/java/frc/robot/commands/MoveArmTo.java
+++ b/src/main/java/frc/robot/commands/MoveArmTo.java
@@ -15,6 +15,7 @@
 import frc.robot.subsystems.Wrist;
 
 public class MoveArmTo extends Command {
+  int position = 0;
   int target;
   boolean isDown;
   boolean finished;
@@ -32,6 +33,7 @@ public MoveArmTo(int target) {
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    position = Robot.arm.readPos();
     if(Robot.arm.readPos() < target){
       isDown = false;
     }else if(Robot.arm.readPos() > target){
diff --git a/src/main/java/frc/robot/commands/PneumaticsDrive.java b/src/main/java/frc/robot/commands/PneumaticsDrive.java
index 7352ffe..8609018 100644
--- a/src/main/java/frc/robot/commands/PneumaticsDrive.java
+++ b/src/main/java/frc/robot/commands/PneumaticsDrive.java
@@ -6,12 +6,9 @@
 /*----------------------------------------------------------------------------*/
 
 package frc.robot.commands;
-
-import edu.wpi.first.wpilibj.GenericHID.Hand;
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.OI;
 import frc.robot.Robot;
-import frc.robot.subsystems.pneumatics;
 
 public class PneumaticsDrive extends Command {
   public PneumaticsDrive() {
@@ -35,6 +32,16 @@ protected void execute() {
       pneumatics.extend();
     }
     */
+    if(OI.xBoxControl.getYButton()){
+      Robot.m_pneumatics.toggleOut();
+      if(Robot.m_pneumatics.getOut()){
+        Robot.m_pneumatics.retract();
+      }else{
+        Robot.m_pneumatics.extend();
+      }
+    }
+
+    
   }
 
   // Make this return true when this Command no longer needs to run execute()
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index f0f4a2a..e6d6a69 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -7,7 +7,6 @@
 
 package frc.robot.subsystems;
 
-import static org.junit.Assume.assumeNoException;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
@@ -19,7 +18,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.RobotMap;
 import frc.robot.commands.ArmDrive;
-import frc.robot.commands.MoveArmTo;
 
 /**
  * Add your docs here.
@@ -59,11 +57,12 @@ public void move(){
   }
   
   public int readPos(){
-    if(reedSwitch_1.get()){
+    if(!reedSwitch_1.get()){
+      SmartDashboard.putBoolean("Arm position 1", true);
       position = 1;
-    }else if(reedSwitch_2.get()){
+    }else if(!reedSwitch_2.get()){
       position = 2;
-    }else if(reedSwitch_3.get()){
+    }else if(!reedSwitch_3.get()){
       position = 3;
     }
     return position;
@@ -85,4 +84,13 @@ public void stop(){
     motorL.set(0);
     motorR.set(0);
   }
+  public boolean switchOne(){
+    return reedSwitch_1.get();
+  }
+  public boolean switchTwo(){
+    return reedSwitch_2.get();
+  }
+  public boolean switchThree(){
+    return reedSwitch_3.get();
+  }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java
index 7747387..dd2ca5b 100644
--- a/src/main/java/frc/robot/subsystems/Climb.java
+++ b/src/main/java/frc/robot/subsystems/Climb.java
@@ -17,19 +17,24 @@
 public class Climb extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
-  DoubleSolenoid doubleSol = new DoubleSolenoid(RobotMap.CLIMBSOL_FRONT_FORWARD, RobotMap.CLIMBSOL_FRONT_REVERSE);
-  DoubleSolenoid doubleSolBack = new DoubleSolenoid(RobotMap.CLIMBSOL_BACK_FORWARD, RobotMap.CLIMBSOL_BACK_REVERSE);
+  DoubleSolenoid frontPiston = new DoubleSolenoid(RobotMap.CLIMBSOL_FRONT_FORWARD, RobotMap.CLIMBSOL_FRONT_REVERSE);
+  DoubleSolenoid backPiston = new DoubleSolenoid(RobotMap.CLIMBSOL_BACK_FORWARD, RobotMap.CLIMBSOL_BACK_REVERSE);
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
     // setDefaultCommand(new MySpecialCommand());
   }
-  public void raiseFront(){
-    doubleSol.set(DoubleSolenoid.Value.kForward);
-    doubleSolBack.set(DoubleSolenoid.Value.kReverse);
+  public void liftFront(){
+    frontPiston.set(DoubleSolenoid.Value.kForward);
+    backPiston.set(DoubleSolenoid.Value.kReverse);
   }
-  public void raiseBack(){
-    doubleSol.set(DoubleSolenoid.Value.kReverse);
-    doubleSolBack.set(DoubleSolenoid.Value.kForward);
+  public void dropFront(){
+    frontPiston.set(DoubleSolenoid.Value.kReverse);
+  }
+  public void liftBack(){
+    backPiston.set(DoubleSolenoid.Value.kForward);
+  }
+  public void dropBack(){
+    backPiston.set(DoubleSolenoid.Value.kReverse);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/UltraSonic.java b/src/main/java/frc/robot/subsystems/UltraSonic.java
index b1fcff1..697390b 100644
--- a/src/main/java/frc/robot/subsystems/UltraSonic.java
+++ b/src/main/java/frc/robot/subsystems/UltraSonic.java
@@ -21,11 +21,12 @@
 public class UltraSonic extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
-
-  Ultrasonic us = new Ultrasonic (RobotMap.ULTRASONIC, RobotMap.ULTRASONIC_ECHO, Unit.kInches);
+  AnalogInput in = new AnalogInput(RobotMap.ULTRASONIC);
+  //Ultrasonic us = new Ultrasonic (RobotMap.ULTRASONIC, RobotMap.ULTRASONIC_ECHO, Unit.kInches);
 
   public double getDistance(){
-    return us.getRangeInches();
+    return in.getVoltage() * 1024 / 25.4;
+    //return us.getRangeInches();
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/pneumatics.java b/src/main/java/frc/robot/subsystems/pneumatics.java
index cffcddf..5ad6be2 100644
--- a/src/main/java/frc/robot/subsystems/pneumatics.java
+++ b/src/main/java/frc/robot/subsystems/pneumatics.java
@@ -18,6 +18,7 @@
  * Add your docs here.
  */
 public class pneumatics extends Subsystem {
+  boolean out = false;
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
   public static Compressor compressor;
@@ -54,4 +55,10 @@ public void stop() {
     doubleSol.set(DoubleSolenoid.Value.kOff);
     doubleSol2.set(DoubleSolenoid.Value.kOff);
   }
+  public boolean getOut(){
+    return out;
+ }
+ public void toggleOut(){
+    out = !out;   
+ }
 }

From b11a60ef94e2e49776cbe9eaa9f68baf95c6c6e5 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Fri, 22 Mar 2019 19:23:31 -0400
Subject: [PATCH 44/50] Added wrist controller

---
 src/main/java/frc/robot/commands/ArmDrive.java | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/ArmDrive.java b/src/main/java/frc/robot/commands/ArmDrive.java
index 9dd3267..1e5c1ff 100644
--- a/src/main/java/frc/robot/commands/ArmDrive.java
+++ b/src/main/java/frc/robot/commands/ArmDrive.java
@@ -43,19 +43,19 @@ protected void execute() {
       SmartDashboard.putBoolean("Arm moving", false);
       Robot.arm.stop();
     }
-    if(OI.xBoxControl.getY(Hand.kLeft) > 0){
+    if(OI.xBoxControl.getY(Hand.kLeft) > 0 && !OI.xBoxControl.getStickButton(Hand.kLeft)){
       Robot.m_wrist.setTarget(Robot.m_wrist.getAngle() + 1);
       Robot.m_wrist.move();
-    }else if(OI.xBoxControl.getY(Hand.kLeft) < 0){
+    }else if(OI.xBoxControl.getY(Hand.kLeft) < 0 && !OI.xBoxControl.getStickButton(Hand.kLeft)){
       Robot.m_wrist.setTarget(Robot.m_wrist.getAngle() + 1);
       Robot.m_wrist.move();
-    }else{
-      Robot.m_wrist.stop();
-    }
-    if(OI.xBoxControl.getStickButton(Hand.kLeft)){
+    }else if(OI.xBoxControl.getStickButton(Hand.kLeft)){
       Robot.m_wrist.setTarget(WRIST_POSISTION_STRAIGHT);
       Robot.m_wrist.move();
+    }else {
+      Robot.m_wrist.stop();
     }
+    
   }
 
   // Make this return true when this Command no longer needs to run execute()

From bf1ff610c2b4e52d1526a067d7124ccee13bdadd Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 23 Mar 2019 09:15:26 -0400
Subject: [PATCH 45/50] Debugged robot code, all motors work now

---
 src/main/java/frc/robot/commands/ArmDrive.java |  4 ++--
 src/main/java/frc/robot/subsystems/Arm.java    |  1 -
 src/main/java/frc/robot/subsystems/Wrist.java  | 16 +++++++++++-----
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/commands/ArmDrive.java b/src/main/java/frc/robot/commands/ArmDrive.java
index 1e5c1ff..5fa3268 100644
--- a/src/main/java/frc/robot/commands/ArmDrive.java
+++ b/src/main/java/frc/robot/commands/ArmDrive.java
@@ -44,10 +44,10 @@ protected void execute() {
       Robot.arm.stop();
     }
     if(OI.xBoxControl.getY(Hand.kLeft) > 0 && !OI.xBoxControl.getStickButton(Hand.kLeft)){
-      Robot.m_wrist.setTarget(Robot.m_wrist.getAngle() + 1);
+      Robot.m_wrist.raise();
       Robot.m_wrist.move();
     }else if(OI.xBoxControl.getY(Hand.kLeft) < 0 && !OI.xBoxControl.getStickButton(Hand.kLeft)){
-      Robot.m_wrist.setTarget(Robot.m_wrist.getAngle() + 1);
+      Robot.m_wrist.lower();
       Robot.m_wrist.move();
     }else if(OI.xBoxControl.getStickButton(Hand.kLeft)){
       Robot.m_wrist.setTarget(WRIST_POSISTION_STRAIGHT);
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index e6d6a69..77f01f9 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -58,7 +58,6 @@ public void move(){
   
   public int readPos(){
     if(!reedSwitch_1.get()){
-      SmartDashboard.putBoolean("Arm position 1", true);
       position = 1;
     }else if(!reedSwitch_2.get()){
       position = 2;
diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java
index 51818ab..3d09136 100644
--- a/src/main/java/frc/robot/subsystems/Wrist.java
+++ b/src/main/java/frc/robot/subsystems/Wrist.java
@@ -57,7 +57,7 @@ else if(getTop()) {
     }
     //forward = down  backwards = up
     if(down){
-      if(getAngle() < targetAngle && getAngle() >= BOTTOM){
+      if(getAngle() < targetAngle || getAngle() <= BOTTOM){
         finished = true;
         wristTalon.set(0);
       }else{
@@ -65,13 +65,13 @@ else if(getTop()) {
         wristTalon.set(SPEED);
       }
     }else{
-      if(getAngle() < targetAngle && getAngle() <= TOP) {
-        wristTalon.set(-SPEED);
-      }
-      else {
+      if(getAngle() > targetAngle || getAngle() >= TOP) {
         finished = true;
         wristTalon.set(0);
       }
+      else {
+        wristTalon.set(-SPEED);
+      }
       
     }
     
@@ -99,4 +99,10 @@ public double getAngle(){
   public void stop(){
     wristTalon.set(0);
   }
+  public void raise(){
+    wristTalon.set(-SPEED);
+  }
+  public void lower(){
+    wristTalon.set(SPEED);
+  }
 }

From cf19e6fb5414c11e408a78349f40289cf3e97038 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 23 Mar 2019 09:15:37 -0400
Subject: [PATCH 46/50] "

---
 src/main/java/frc/robot/Robot.java            |  1 +
 src/main/java/frc/robot/RobotMap.java         | 16 ++++++------
 .../java/frc/robot/commands/ArmDrive.java     | 14 ++++------
 .../java/frc/robot/commands/ClimbStair.java   |  9 ++++---
 .../frc/robot/commands/JoystickDrive.java     |  3 ++-
 .../frc/robot/commands/PneumaticsDrive.java   |  2 +-
 src/main/java/frc/robot/subsystems/Arm.java   | 24 ++++++-----------
 src/main/java/frc/robot/subsystems/Claw.java  |  8 +++---
 src/main/java/frc/robot/subsystems/Wrist.java | 26 ++++++-------------
 9 files changed, 44 insertions(+), 59 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index ba26fd6..fe5e01b 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -107,6 +107,7 @@ public void robotPeriodic() {
     SmartDashboard.putNumber("Wrist Encoder", m_wrist.getAngle());
     SmartDashboard.putNumber("Reed switch", arm.readPos());
     SmartDashboard.putNumber("String pot", arm.readPos());
+    SmartDashboard.putNumber("UltraSonic", m_ultraSonic.getDistance());
     //--------------------------Do 10 times per Second --------------------------------------------------
     if(m_timer.hasPeriodPassed(.1)){
       CameraI2c.read();
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index baede11..cebc6b1 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -38,19 +38,19 @@ public class RobotMap {
   public final static int ARM_REED_1 = 1; //UNUSED(?)
   //public final static int ULTRASONIC = 4;
   public final static int ULTRASONIC_ECHO = 5;
-  public final static int WRIST_ENCODER_A = 6;
-  public final static int WRIST_ENCODER_B = 7;
+  public final static int WRIST_ENCODER_A = 7;
+  public final static int WRIST_ENCODER_B = 6;
 
   //CAN
   public final static int ARM_L = 4;//
   public final static int ARM_R = 5;//
   public final static int CLAW_1 = 6;//
-  public final static int CLAW_2 = 7;//
+  public final static int CLAW_2 = 8;//
   public final static int LEFTMOTOR_1 = 3;  //
   public final static int LEFTMOTOR_2 = 1;//
   public final static int RIGHTMOTOR_1 = 2;//
   public final static int RIGHTMOTOR_2 = 0; //
-  public final static int WRISTMOTOR = 8;//
+  public final static int WRISTMOTOR = 7;//
 
    //Pneumatics
   public final static int PNEUMATIC_COMPRESSOR = 0;
@@ -58,8 +58,8 @@ public class RobotMap {
   public final static int DOUBLESOL_REVERSE = 1;
   public final static int DOUBLESOL_FORWARD1 = 2;
   public static final int DOUBLESOL_REVERSE1 = 3;
-  public static final int CLIMBSOL_FRONT_FORWARD = 4;
-  public static final int CLIMBSOL_FRONT_REVERSE = 5;
-  public static final int CLIMBSOL_BACK_FORWARD = 6;
-  public static final int CLIMBSOL_BACK_REVERSE =7;
+  public static final int CLIMBSOL_FRONT_FORWARD = 7;
+  public static final int CLIMBSOL_FRONT_REVERSE = 6;
+  public static final int CLIMBSOL_BACK_FORWARD = 4;
+  public static final int CLIMBSOL_BACK_REVERSE =5;
 }
diff --git a/src/main/java/frc/robot/commands/ArmDrive.java b/src/main/java/frc/robot/commands/ArmDrive.java
index 5fa3268..30bda95 100644
--- a/src/main/java/frc/robot/commands/ArmDrive.java
+++ b/src/main/java/frc/robot/commands/ArmDrive.java
@@ -19,6 +19,7 @@ public class ArmDrive extends Command {
   public ArmDrive() {
     // Use requires() here to declare subsystem dependencies
     requires(Robot.arm);
+    requires(Robot.m_wrist);
   }
 
   // Called just before this Command runs the first time
@@ -43,17 +44,12 @@ protected void execute() {
       SmartDashboard.putBoolean("Arm moving", false);
       Robot.arm.stop();
     }
-    if(OI.xBoxControl.getY(Hand.kLeft) > 0 && !OI.xBoxControl.getStickButton(Hand.kLeft)){
-      Robot.m_wrist.raise();
-      Robot.m_wrist.move();
-    }else if(OI.xBoxControl.getY(Hand.kLeft) < 0 && !OI.xBoxControl.getStickButton(Hand.kLeft)){
-      Robot.m_wrist.lower();
-      Robot.m_wrist.move();
-    }else if(OI.xBoxControl.getStickButton(Hand.kLeft)){
+    if(OI.xBoxControl.getY(Hand.kLeft) > .1 || OI.xBoxControl.getY(Hand.kLeft) < .1){
+      Robot.m_wrist.raise(OI.xBoxControl.getY(Hand.kLeft));
+    }
+    if(OI.xBoxControl.getStickButton(Hand.kLeft)){
       Robot.m_wrist.setTarget(WRIST_POSISTION_STRAIGHT);
       Robot.m_wrist.move();
-    }else {
-      Robot.m_wrist.stop();
     }
     
   }
diff --git a/src/main/java/frc/robot/commands/ClimbStair.java b/src/main/java/frc/robot/commands/ClimbStair.java
index 554b3f1..c96bbb2 100644
--- a/src/main/java/frc/robot/commands/ClimbStair.java
+++ b/src/main/java/frc/robot/commands/ClimbStair.java
@@ -8,23 +8,26 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 
 public class ClimbStair extends Command {
   boolean finished = false;
   int step = 0;
-  final static double firstDistance = 100;
-  final static double secondDistance = 50;
+  // Step is 49 in deep
+  final static double firstDistance = 55;
+  final static double secondDistance = 29;
   public ClimbStair() {
     // Use requires() here to declare subsystem dependencies
     requires(Robot.m_climb);
-    requires(Robot.m_driveTrain);
+    //requires(Robot.m_driveTrain);
     requires(Robot.m_ultraSonic);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
+    SmartDashboard.putBoolean("Climb run", true);
     Robot.m_climb.liftFront();
   }
 
diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index ceb7fc1..3654882 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -33,7 +33,7 @@ protected void execute() {
     double rightSpeed = 0.0;
     double leftSpeed = 0.0;
     
-    tankDrive = SmartDashboard.getBoolean("TankDrive", tankDrive);
+
     /*if(tankDrive){
       leftSpeed = OI.xBoxControl.getY(Hand.kLeft);
       rightSpeed = OI.xBoxControl.getY(Hand.kRight);
@@ -42,6 +42,7 @@ protected void execute() {
       leftSpeed = OI.xBoxControl.getX(Hand.kRight);
       rightSpeed = OI.xBoxControl.getY(Hand.kRight);
       Robot.m_driveTrain.drive(rightSpeed, leftSpeed);
+      SmartDashboard.putNumber("Drive train value", rightSpeed);
     //}
     SmartDashboard.putNumber("GyroAngle", Robot.m_driveTrain.getGyroZ());
     //SmartDaShboard.putNumber("Left Speed: ", leftSpeed);
diff --git a/src/main/java/frc/robot/commands/PneumaticsDrive.java b/src/main/java/frc/robot/commands/PneumaticsDrive.java
index 8609018..679ef87 100644
--- a/src/main/java/frc/robot/commands/PneumaticsDrive.java
+++ b/src/main/java/frc/robot/commands/PneumaticsDrive.java
@@ -32,7 +32,7 @@ protected void execute() {
       pneumatics.extend();
     }
     */
-    if(OI.xBoxControl.getYButton()){
+    if(OI.xBoxControl.getYButtonPressed()){
       Robot.m_pneumatics.toggleOut();
       if(Robot.m_pneumatics.getOut()){
         Robot.m_pneumatics.retract();
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 77f01f9..ac494d9 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -48,20 +48,17 @@ public void initDefaultCommand() {
   }
 
   public void move(){
-     if(getSwitch()){
-       motorL.set(speed);
-       motorR.set(speed);
-      } else {
-        stop();
-      }
+
+       motorL.set(speed * leftMod);
+       motorR.set(speed * rightMod);
   }
   
   public int readPos(){
-    if(!reedSwitch_1.get()){
+    if(reedSwitch_1.get()){
       position = 1;
-    }else if(!reedSwitch_2.get()){
+    }else if(reedSwitch_2.get()){
       position = 2;
-    }else if(!reedSwitch_3.get()){
+    }else if(reedSwitch_3.get()){
       position = 3;
     }
     return position;
@@ -71,13 +68,8 @@ public boolean getSwitch(){
     return switchBottom.get();
   }
   public void moveDown(){
-    if(getSwitch()) {
-      stop();
-    }
-    else {
-      motorL.set(speed);
-      motorR.set(speed);
-    }
+      motorL.set(-speed * leftMod);
+      motorR.set(-speed * rightMod);
   }
   public void stop(){
     motorL.set(0);
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index 88a8077..d844092 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -7,6 +7,8 @@
 
 package frc.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.GenericHID.Hand;
@@ -21,14 +23,14 @@
 public class Claw extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
-  public double speed = .5;
+  public double speed = 0;
   @Override
   public void initDefaultCommand() {
     // Set the default command for a subsystem here.
      setDefaultCommand(new ClawDrive()); 
   }
-    static Spark claw1 = new Spark(RobotMap.CLAW_1);
-    static Spark claw2 = new Spark(RobotMap.CLAW_2);
+    static WPI_TalonSRX claw1 = new WPI_TalonSRX(RobotMap.CLAW_1);
+    static WPI_TalonSRX claw2 = new WPI_TalonSRX(RobotMap.CLAW_2);
   public void moveForward(){
     speed = SmartDashboard.getNumber("Claw Speed", speed);
     claw2.set(speed);
diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java
index 3d09136..46d9a73 100644
--- a/src/main/java/frc/robot/subsystems/Wrist.java
+++ b/src/main/java/frc/robot/subsystems/Wrist.java
@@ -20,14 +20,14 @@
 public class Wrist extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
-  private final static double SPEED = .1;
+  private final static double SPEED = .85;
   //num of angles = 1 rotation   is it 360? no one knows
   private final static double ANGLE_TO_ROTATION = 360;
   public static final double TOP = 100;
   public static final double BOTTOM = 0;
   private boolean finished = false;
   private double targetAngle;
-
+  double speed1, speed2, speed3;
   Encoder wristEncoder;
   DigitalInput switchBottom;
   DigitalInput switchTop;
@@ -48,13 +48,6 @@ public Wrist() {
   public void move() {
     finished = false;
     boolean down = getAngle()>targetAngle;
-    if(getBottom()) {
-      wristEncoder.reset();
-      wristTalon.set(0);
-    }
-    else if(getTop()) {
-      wristTalon.set(0);
-    }
     //forward = down  backwards = up
     if(down){
       if(getAngle() < targetAngle || getAngle() <= BOTTOM){
@@ -83,13 +76,6 @@ public boolean atTarget() {
   public void setTarget(double target) {
     targetAngle = target;
   }
-
-  public boolean getTop() {
-    return switchTop.get();
-  }
-  public boolean getBottom() {
-    return switchBottom.get();
-  }
   public boolean getFinished(){
     return finished;
   }
@@ -99,8 +85,12 @@ public double getAngle(){
   public void stop(){
     wristTalon.set(0);
   }
-  public void raise(){
-    wristTalon.set(-SPEED);
+  public void raise(double speed){
+    speed = -speed;
+    speed3 = speed2;
+    speed2 = speed1;
+    speed1 = speed;
+    wristTalon.set((speed /4 + speed1 + speed2 + speed3) * SPEED);
   }
   public void lower(){
     wristTalon.set(SPEED);

From 192768c5c1f2221dbbda53cc68b505ad802553a8 Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 23 Mar 2019 10:40:00 -0400
Subject: [PATCH 47/50] Deleted unused imports

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

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index ac494d9..26c356e 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -9,13 +9,8 @@
 
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-
-import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.RobotMap;
 import frc.robot.commands.ArmDrive;
 
@@ -49,8 +44,8 @@ public void initDefaultCommand() {
 
   public void move(){
 
-       motorL.set(speed * leftMod);
-       motorR.set(speed * rightMod);
+       motorL.set(speed);
+       motorR.set(speed);
   }
   
   public int readPos(){
@@ -68,8 +63,8 @@ public boolean getSwitch(){
     return switchBottom.get();
   }
   public void moveDown(){
-      motorL.set(-speed * leftMod);
-      motorR.set(-speed * rightMod);
+      motorL.set(-speed);
+      motorR.set(-speed);
   }
   public void stop(){
     motorL.set(0);

From 1a157b0d6b1c8d2e869c09723fce8668ec8e512f Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 23 Mar 2019 14:59:14 -0400
Subject: [PATCH 48/50] Did stuff

---
 src/main/java/frc/robot/OI.java                     |  3 ++-
 src/main/java/frc/robot/commands/JoystickDrive.java |  2 +-
 src/main/java/frc/robot/subsystems/Arm.java         | 10 +++++-----
 3 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
index cdc7c3a..68e8a02 100644
--- a/src/main/java/frc/robot/OI.java
+++ b/src/main/java/frc/robot/OI.java
@@ -61,6 +61,7 @@ public class OI {
   Button lowRocket_c = new JoystickButton(xBoxControlArm, 1);
   Button midRocket_c = new JoystickButton(xBoxControlArm, 2);
   Button button = new JoystickButton(xBoxControl, 2);
+  Button climb = new JoystickButton(xBoxControl, 1);
   Button highRocket_c = new JoystickButton(xBoxControlArm, 3);
   Button lowRocket_h = new JoystickButton(xBoxControlArm, 4);
   Button cargoShip_h = new JoystickButton(xBoxControlArm, 6);
@@ -71,7 +72,7 @@ public OI() {
     pistonExtend.whenPressed(new ExtendPiston());
     
     pistonRetract.whenPressed(new RetractPiston());
-    
+    climb.whenPressed(new ClimbStair());
     button.whenPressed(new TurnDegrees());
 
     
diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index 3654882..11de8f6 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -40,7 +40,7 @@ protected void execute() {
       Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed);
     }else{*/
       leftSpeed = OI.xBoxControl.getX(Hand.kRight);
-      rightSpeed = OI.xBoxControl.getY(Hand.kRight);
+      rightSpeed = -OI.xBoxControl.getY(Hand.kRight);
       Robot.m_driveTrain.drive(rightSpeed, leftSpeed);
       SmartDashboard.putNumber("Drive train value", rightSpeed);
     //}
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 26c356e..258a720 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -22,7 +22,7 @@ public class Arm extends Subsystem {
   public int position = 0;
   public static int MARGIN_OF_ERROR = 30;
   //WPI_TalonSRX motor;
-  private static final double leftMod = .95;
+  private static final double leftMod = .88;
   private static final double rightMod = 1;
   private static WPI_TalonSRX motorL = new WPI_TalonSRX(RobotMap.ARM_L);
   private static WPI_TalonSRX motorR = new WPI_TalonSRX(RobotMap.ARM_R);
@@ -44,8 +44,8 @@ public void initDefaultCommand() {
 
   public void move(){
 
-       motorL.set(speed);
-       motorR.set(speed);
+       motorL.set(speed *leftMod);
+       motorR.set(speed * rightMod);
   }
   
   public int readPos(){
@@ -63,8 +63,8 @@ public boolean getSwitch(){
     return switchBottom.get();
   }
   public void moveDown(){
-      motorL.set(-speed);
-      motorR.set(-speed);
+      motorL.set(-speed *leftMod);
+      motorR.set(-speed * rightMod);
   }
   public void stop(){
     motorL.set(0);

From 60e23d3fcce6f275c0f54eb9d351b30b90dc295d Mon Sep 17 00:00:00 2001
From: 2421RTR <47060653+2421RTR@users.noreply.github.com>
Date: Sat, 23 Mar 2019 15:01:42 -0400
Subject: [PATCH 49/50] Joystickdrive updated

---
 src/main/java/frc/robot/commands/JoystickDrive.java | 4 ++--
 src/main/java/frc/robot/subsystems/Wrist.java       | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index 3654882..670e59a 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -39,8 +39,8 @@ protected void execute() {
       rightSpeed = OI.xBoxControl.getY(Hand.kRight);
       Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed);
     }else{*/
-      leftSpeed = OI.xBoxControl.getX(Hand.kRight);
-      rightSpeed = OI.xBoxControl.getY(Hand.kRight);
+      leftSpeed = -OI.xBoxControl.getX(Hand.kRight);
+      rightSpeed = -OI.xBoxControl.getY(Hand.kRight);
       Robot.m_driveTrain.drive(rightSpeed, leftSpeed);
       SmartDashboard.putNumber("Drive train value", rightSpeed);
     //}
diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java
index 46d9a73..f74db51 100644
--- a/src/main/java/frc/robot/subsystems/Wrist.java
+++ b/src/main/java/frc/robot/subsystems/Wrist.java
@@ -90,7 +90,7 @@ public void raise(double speed){
     speed3 = speed2;
     speed2 = speed1;
     speed1 = speed;
-    wristTalon.set((speed /4 + speed1 + speed2 + speed3) * SPEED);
+    wristTalon.set(speed * SPEED);
   }
   public void lower(){
     wristTalon.set(SPEED);

From 7420a96a75de8f33b564061282f2837b30feec8a Mon Sep 17 00:00:00 2001
From: FRC2421 <46624805+FRC2421@users.noreply.github.com>
Date: Sat, 23 Mar 2019 15:36:11 -0400
Subject: [PATCH 50/50] Climb code revised

---
 src/main/java/frc/robot/Robot.java                  | 3 ++-
 src/main/java/frc/robot/RobotMap.java               | 4 ++--
 src/main/java/frc/robot/commands/ClimbStair.java    | 4 ++--
 src/main/java/frc/robot/commands/ExtendPiston.java  | 4 ++--
 src/main/java/frc/robot/commands/JoystickDrive.java | 4 ----
 src/main/java/frc/robot/commands/RetractPiston.java | 4 ++--
 src/main/java/frc/robot/subsystems/Wrist.java       | 5 +++--
 7 files changed, 13 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index fe5e01b..db3dd1f 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -83,7 +83,8 @@ public void robotInit() {
     m_laser = new LaserFinder();
     SmartDashboard.putBoolean("TankDrive", false);
     m_wrist = new Wrist();
-
+    m_climb.dropFront();
+    m_climb.dropBack();
     m_ultraSonic = new UltraSonic();
     
     //Scheduler.getInstance().add(new GetDistance());
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index cebc6b1..4694ef9 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -38,8 +38,8 @@ public class RobotMap {
   public final static int ARM_REED_1 = 1; //UNUSED(?)
   //public final static int ULTRASONIC = 4;
   public final static int ULTRASONIC_ECHO = 5;
-  public final static int WRIST_ENCODER_A = 7;
-  public final static int WRIST_ENCODER_B = 6;
+  public final static int WRIST_ENCODER_A = 6;
+  public final static int WRIST_ENCODER_B = 7;
 
   //CAN
   public final static int ARM_L = 4;//
diff --git a/src/main/java/frc/robot/commands/ClimbStair.java b/src/main/java/frc/robot/commands/ClimbStair.java
index c96bbb2..c5ab40a 100644
--- a/src/main/java/frc/robot/commands/ClimbStair.java
+++ b/src/main/java/frc/robot/commands/ClimbStair.java
@@ -20,7 +20,7 @@ public class ClimbStair extends Command {
   public ClimbStair() {
     // Use requires() here to declare subsystem dependencies
     requires(Robot.m_climb);
-    //requires(Robot.m_driveTrain);
+    requires(Robot.m_driveTrain);
     requires(Robot.m_ultraSonic);
   }
 
@@ -35,7 +35,7 @@ protected void initialize() {
   @Override
   protected void execute() {
     if(step == 0){
-      if(Robot.m_ultraSonic.getDistance() > firstDistance){
+      if(Robot.m_ultraSonic.getDistance() < firstDistance){
         step++;
       }else{
         Robot.m_driveTrain.drive(.75, 0);
diff --git a/src/main/java/frc/robot/commands/ExtendPiston.java b/src/main/java/frc/robot/commands/ExtendPiston.java
index d18f15b..eba89c8 100644
--- a/src/main/java/frc/robot/commands/ExtendPiston.java
+++ b/src/main/java/frc/robot/commands/ExtendPiston.java
@@ -13,13 +13,13 @@
 public class ExtendPiston extends Command {
   public ExtendPiston() {
     // Use requires() here to declare subsystem dependencies
-    requires(Robot.m_pneumatics);
+    requires(Robot.m_climb);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    Robot.m_pneumatics.extend();
+    Robot.m_climb.liftFront();
   }
 
   // Called repeatedly when this Command is scheduled to run
diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java
index 1530b06..670e59a 100644
--- a/src/main/java/frc/robot/commands/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/JoystickDrive.java
@@ -39,11 +39,7 @@ protected void execute() {
       rightSpeed = OI.xBoxControl.getY(Hand.kRight);
       Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed);
     }else{*/
-<<<<<<< HEAD
-      leftSpeed = OI.xBoxControl.getX(Hand.kRight);
-=======
       leftSpeed = -OI.xBoxControl.getX(Hand.kRight);
->>>>>>> 60e23d3fcce6f275c0f54eb9d351b30b90dc295d
       rightSpeed = -OI.xBoxControl.getY(Hand.kRight);
       Robot.m_driveTrain.drive(rightSpeed, leftSpeed);
       SmartDashboard.putNumber("Drive train value", rightSpeed);
diff --git a/src/main/java/frc/robot/commands/RetractPiston.java b/src/main/java/frc/robot/commands/RetractPiston.java
index 3876b73..5a99a1c 100644
--- a/src/main/java/frc/robot/commands/RetractPiston.java
+++ b/src/main/java/frc/robot/commands/RetractPiston.java
@@ -13,13 +13,13 @@
 public class RetractPiston extends Command {
   public RetractPiston() {
     // Use requires() here to declare subsystem dependencies
-    requires(Robot.m_pneumatics);
+    requires(Robot.m_climb);
   }
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    Robot.m_pneumatics.retract();
+    Robot.m_climb.dropFront();
   }
 
   // Called repeatedly when this Command is scheduled to run
diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java
index f74db51..a7cd7e2 100644
--- a/src/main/java/frc/robot/subsystems/Wrist.java
+++ b/src/main/java/frc/robot/subsystems/Wrist.java
@@ -11,6 +11,7 @@
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.CounterBase.EncodingType;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import frc.robot.RobotMap;
 
@@ -41,7 +42,7 @@ public void initDefaultCommand() {
   }
 
   public Wrist() {
-    wristEncoder = new Encoder(RobotMap.WRIST_ENCODER_A, RobotMap.WRIST_ENCODER_B);
+    wristEncoder = new Encoder(RobotMap.WRIST_ENCODER_A, RobotMap.WRIST_ENCODER_B, false, EncodingType.k4X);
     wristTalon = new WPI_TalonSRX(RobotMap.WRISTMOTOR);
   }
 
@@ -80,7 +81,7 @@ public boolean getFinished(){
     return finished;
   }
   public double getAngle(){
-    return wristEncoder.getDistance()*ANGLE_TO_ROTATION;
+    return wristEncoder.getDistance();
   }
   public void stop(){
     wristTalon.set(0);