resolve merge conflicts

Author: DriverStationComputer

Robot2018/lib/.gitignore

@@ -1 +1 @@
-/User_Libraries.properties
+User_Libraries.properties

Robot2018/lib/User_Libraries.properties

@@ -12,8 +12,8 @@
 import java.util.Map;
 
 import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils;
+import org.usfirst.frc.team199.Robot2018.autonomous.Position;
 import org.usfirst.frc.team199.Robot2018.commands.Autonomous;
-import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Position;
 import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Strategy;
 import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
@@ -23,9 +23,9 @@
 import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject;
 import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
 
-import edu.wpi.first.wpilibj.CameraServer;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
@@ -54,7 +54,7 @@ public class Robot extends IterativeRobot {
 	public static Map<String, ArrayList<String[]>> autoScripts;
 
 	Command autonomousCommand;
-	SendableChooser<Position> posChooser = new SendableChooser<Position>();
+	SendableChooser<Autonomous.Position> posChooser = new SendableChooser<Autonomous.Position>();
 	Map<String, SendableChooser<Strategy>> stratChoosers = new HashMap<String, SendableChooser<Strategy>>();
 	String[] fmsPossibilities = { "LL", "LR", "RL", "RR" };
 
@@ -70,6 +70,14 @@ public double getConst(String key, double def) {
 			}
 			return pref.getDouble("Const/" + key, def);
 		}
+
+		public void putData(String string, PIDController controller) {
+			SmartDashboard.putData(string, controller);
+		}
+
+		public void putNumber(String string, double d) {
+			SmartDashboard.putNumber(string, d);
+		}
 		/*
 		 * if (!SmartDashboard.containsKey("Const/" + key)) { if
 		 * (!SmartDashboard.putNumber("Const/" + key, def)) {
@@ -110,7 +118,7 @@ public void robotInit() {
 		oi = new OI();
 
 		// auto position chooser
-		for (Position p : Position.values()) {
+		for (Autonomous.Position p : Autonomous.Position.values()) {
 			posChooser.addObject(p.getSDName(), p);
 		}
 		SmartDashboard.putData("Starting Position", posChooser);
@@ -132,8 +140,8 @@ public void robotInit() {
 		autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", ""));
 
 		listen = new Listener();
-		CameraServer.getInstance().startAutomaticCapture(0);
-		CameraServer.getInstance().startAutomaticCapture(1);
+		// CameraServer.getInstance().startAutomaticCapture(0);
+		// CameraServer.getInstance().startAutomaticCapture(1);
 	}
 
 	/**
@@ -158,10 +166,12 @@ public void disabledPeriodic() {
 	 */
 	@Override
 	public void autonomousInit() {
+		dt.resetAHRS();
+		AutoUtils.position = new Position(0, 0, 0);
 		Scheduler.getInstance().add(new ShiftLowGear());
 		Scheduler.getInstance().add(new CloseIntake());
 		String fmsInput = DriverStation.getInstance().getGameSpecificMessage();
-		Position startPos = posChooser.getSelected();
+		Autonomous.Position startPos = posChooser.getSelected();
 		double autoDelay = SmartDashboard.getNumber("Auto Delay", 0);
 
 		Map<String, Strategy> strategies = new HashMap<String, Strategy>();

Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java

@@ -1,5 +1,11 @@
 package org.usfirst.frc.team199.Robot2018;
 
+import edu.wpi.first.wpilibj.PIDController;
+
 public interface SmartDashboardInterface {
 	public double getConst(String key, double def);
+
+	public void putData(String string, PIDController controller);
+
+	public void putNumber(String string, double d);
 }

Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java

@@ -6,7 +6,7 @@
 
 public class AutoUtils {
 
-	public static Position position = new Position(0, 0, 0);
+	public static Position position;
 
 	/**
 	 * Parses the inputted script file into a map of scripts
@@ -25,6 +25,7 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
 		String currScriptName = "";
 
 		int count = 0;
+		int errorCount = 0;
 		for (String line : lines) {
 			count++;
 			// remove comments
@@ -75,6 +76,8 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
 				if (isValidCommand(instruction, args, count)) {
 					String[] command = { instruction, args };
 					currScript.add(command);
+				} else {
+					errorCount++;
 				}
 			}
 
@@ -86,6 +89,8 @@ public static Map<String, ArrayList<String[]>> parseScriptFile(String scriptFile
 		// remove the stray one in the beginning
 		autoScripts.remove("");
 
+		System.out.println("[INFO] Successfuly parsed " + count + " lines with " + errorCount + " errors.");
+
 		return autoScripts;
 	}
 
@@ -244,7 +249,7 @@ public static double[] parsePoint(String cmdArgs) {
 		double[] point = new double[2];
 		String parentheseless;
 		String[] pointparts;
-		if (AutoUtils.isPoint(cmdArgs)) {
+		if (isPoint(cmdArgs)) {
 			parentheseless = cmdArgs.substring(1, cmdArgs.length() - 1);
 			pointparts = parentheseless.split(",");
 			try {

Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java

@@ -1,6 +1,7 @@
 #nt setup
 from networktables import NetworkTables
 import __future__
+import time
 NetworkTables.initialize(server='10.1.99.2')
 prefs = NetworkTables.getTable("Preferences")
 
@@ -30,6 +31,8 @@
 prefs.putString("autoscripts", oneline)
 print("Uploading %s as a String[] to key \"autoscripts\"" % filename)
 
+time.sleep(0.1)
+
 #checks if the key has been filled
 tester = "" #a variable to check if autoscripts is None
 

Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py

@@ -17,17 +17,18 @@
  */
 public class AutoMoveTo extends CommandGroup {
 
-	public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc) {
+	public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc,
+			PIDSource pidTurnSource) {
 		// requires(Drivetrain);
 		double rotation;
 		double[] point = { 0, 0 };
 		for (String arg : args) {
 			if (AutoUtils.isDouble(arg)) {
 				rotation = Double.valueOf(arg);
-				addSequential(new PIDTurn(rotation, dt, sd, pidMoveSrc, true));
+				addSequential(new PIDTurn(rotation, dt, sd, pidTurnSource, true));
 			} else if (AutoUtils.isPoint(arg)) {
 				point = AutoUtils.parsePoint(arg);
-				addSequential(new PIDTurn(point, dt, sd, pidMoveSrc));
+				addSequential(new PIDTurn(point, dt, sd, pidTurnSource));
 				addSequential(new PIDMove(point, dt, sd, pidMoveSrc));
 			} else {
 				throw new IllegalArgumentException();
@@ -36,6 +37,6 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface
 	}
 
 	public AutoMoveTo(String[] args) {
-		this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg());
+		this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg(), Robot.dt.getGyro());
 	}
 }

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java

@@ -9,6 +9,8 @@
  */
 public class DefaultIntake extends Command {
 
+	private boolean manipulatorPluggedIn = true;
+
 	public DefaultIntake() {
 		// Use requires() here to declare subsystem dependencies
 		// eg. requires(chassis);
@@ -17,13 +19,21 @@ public DefaultIntake() {
 
 	// Called just before this Command runs the first time
 	protected void initialize() {
+		try {
+			Robot.oi.manipulator.getRawAxis(1);
+		} catch (NullPointerException e) {
+			System.err.println("[ERROR] Manipulator not plugged in.");
+			manipulatorPluggedIn = false;
+		}
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	protected void execute() {
 		// 1 and 5 represent the axes' index in driver station
-		Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1));
-		Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5));
+		if (manipulatorPluggedIn) {
+			Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1));
+			Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5));
+		}
 	}
 
 	// Make this return true when this Command no longer needs to run execute()

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java

@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * Drives the robot a certain target distance using PID. Implements PIDOutput in
@@ -21,6 +20,9 @@ public class PIDMove extends Command implements PIDOutput {
 	private DrivetrainInterface dt;
 	private PIDController moveController;
 	private PIDSource avg;
+	private SmartDashboardInterface sd;
+	private double pointX;
+	private double pointY;
 
 	/**
 	 * Constructs this command with a new PIDController. Sets all of the
@@ -43,13 +45,14 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		target = targ;
 		this.dt = dt;
 		this.avg = avg;
+		this.sd = sd;
 		if (Robot.dt != null) {
 			requires(Robot.dt);
 		}
 		double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
 		moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0),
 				sd.getConst("MovekD", 0), kf, avg, this);
-		SmartDashboard.putData("Move PID", moveController);
+		sd.putData("Move PID", moveController);
 	}
 
 	/**
@@ -70,20 +73,19 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 	 *            the PIDSorceAverage of the DT's two Encoders
 	 */
 	public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) {
-		double dx = point[0] - AutoUtils.position.getX();
-		double dy = point[1] - AutoUtils.position.getY();
+		pointX = point[0];
+		pointY = point[1];
 
-		double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance
-
-		this.target = dist;
 		this.dt = dt;
 		this.avg = avg;
+		this.sd = sd;
 		if (Robot.dt != null) {
 			requires(Robot.dt);
 		}
 		double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
 		moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0),
 				kf, avg, this);
+		sd.putData("Move PID", moveController);
 	}
 
 	/**
@@ -92,6 +94,12 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 	 */
 	@Override
 	public void initialize() {
+		double dx = pointX - AutoUtils.position.getX();
+		double dy = pointY - AutoUtils.position.getY();
+
+		double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance
+		this.target = dist;
+
 		dt.resetDistEncs();
 		moveController.disable();
 		// input is in inches
@@ -100,6 +108,7 @@ public void initialize() {
 		moveController.setOutputRange(-1.0, 1.0);
 		moveController.setContinuous(false);
 		moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.1));
+		System.out.println("move target = " + target);
 		moveController.setSetpoint(target);
 
 		moveController.enable();
@@ -114,8 +123,8 @@ public void initialize() {
 	@Override
 	protected void execute() {
 		System.out.println("Enc Avg Dist: " + avg.pidGet());
-		SmartDashboard.putNumber("Move PID Result", moveController.get());
-		SmartDashboard.putNumber("Move PID Error", moveController.getError());
+		sd.putNumber("Move PID Result", moveController.get());
+		sd.putNumber("Move PID Error", moveController.getError());
 	}
 
 	/**
@@ -144,10 +153,13 @@ protected void end() {
 
 		double angle = Math.toRadians(AutoUtils.position.getRot());
 		double dist = avg.pidGet();
-		double x = Math.cos(angle) * dist;
-		double y = Math.sin(angle) * dist;
+		// x and y are switched because we are using bearings
+		double y = Math.cos(angle) * dist;
+		double x = Math.sin(angle) * dist;
 		AutoUtils.position.changeX(x);
 		AutoUtils.position.changeY(y);
+
+		AutoUtils.position.setRot(dt.getAHRSAngle());
 	}
 
 	/**
@@ -173,6 +185,6 @@ protected void interrupted() {
 	@Override
 	public void pidWrite(double output) {
 		dt.arcadeDrive(output, 0);
-		SmartDashboard.putNumber("Move PID Output", output);
+		sd.putNumber("Move PID Output", output);
 	}
 }