Working servo testing

src/main/java/frc/robot/Constants.java

@@ -14,6 +14,10 @@ public static final class ControllerConstants {
 		public static final double kTriggerDeadzone = .05;
 	}
 
+	public static final class ServoConstants {
+		public static final int kMaxRotation = 270;
+	}
+
 	public static final class DriveConstants {
 		// CAN IDs (updated)
 		public static final int kFrontRightDrivePort = 10;

src/main/java/frc/robot/Robot.java

@@ -18,13 +18,15 @@
 import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import frc.robot.Constants.ControllerConstants;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.ServoSubsystem;
 
 public class Robot extends TimedRobot {
 	private Command m_autonomousCommand;
 	private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
 	private final CommandPS4Controller m_driverController = new CommandPS4Controller(
 			ControllerConstants.kDriverControllerPort);
 	private final PowerDistribution m_pdh = new PowerDistribution();
+	private final ServoSubsystem m_servoSubsystem = new ServoSubsystem();
 
 	public Robot() {
 		SmartDashboard.putData(m_pdh);
@@ -43,6 +45,10 @@ public void bindDriveControls() {
 						() -> -m_driverController.getLeftX(),
 						() -> m_driverController.getR2Axis() - m_driverController.getL2Axis(),
 						m_driverController.getHID()::getSquareButton));
+		m_driverController.L1().onTrue(m_servoSubsystem.rotateCommand(0.0));
+		m_driverController.L2().onTrue(m_servoSubsystem.rotateCommand(null));
+		m_driverController.R1().onTrue(m_servoSubsystem.rotateCommand(270.0));
+		m_driverController.R2().onTrue(m_servoSubsystem.rotateCommand(200.0));
 	}
 
 	@Override

src/main/java/frc/robot/Subsystems/ServoSubsystem.java

@@ -1,8 +1,11 @@
-package frc.robot.Subsystems;
+package frc.robot.subsystems;
+
+import static frc.robot.Constants.ServoConstants.*;
 
 import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class ServoSubsystem extends SubsystemBase {
@@ -12,20 +15,28 @@ public class ServoSubsystem extends SubsystemBase {
 	public ServoSubsystem() {
 		m_motor = new Servo(0);
 		time = new Timer();
+		m_motor.setBoundsMicroseconds(2500, 0, 1500, 0, 500);
 	}
 
 	public void init() {
 		m_motor.setZeroLatch();
 	}
 
-	public void run() {
-		// TODO Auto-generated method stub
-		super.periodic();
-
-		time.start();
+	@Override
+	public void periodic() {
 		SmartDashboard.putNumber("Time", time.get());
 		SmartDashboard.putNumber("Position", m_motor.getPosition());
+	}
 
-		m_motor.set(1.0);
+	/**
+	 * Rotates the servo. Does not wait for it to finish.
+	 * 
+	 * @param angle The angle from 0 in the left to 270 in the right.
+	 * @return The command.
+	 */
+	public Command rotateCommand(Double angle) {
+		if (angle == null)
+			return runOnce(() -> m_motor.set(.5));
+		return runOnce(() -> m_motor.set(angle / kMaxRotation));
 	}
 }