Merge branch 'master' of https://github.com/DeepBlueRobotics/RobotCode2018

Author: kevinzwang

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

@@ -8,9 +8,10 @@
 package org.usfirst.frc.team199.Robot2018;
 
 import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
+import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
 import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
 import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
-import org.usfirst.frc.team199.Robot2018.commands.OutakeCube;
+import org.usfirst.frc.team199.Robot2018.commands.OuttakeCube;
 import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
 import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
 import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
@@ -38,26 +39,28 @@ public class OI {
 	 */
 
 	public Joystick leftJoy;
+	public Joystick rightJoy;
 	private JoystickButton shiftLowGearButton;
 	private JoystickButton shiftHighGearButton;
 	private JoystickButton shiftDriveTypeButton;
-	private JoystickButton PIDMoveButton;
-	private JoystickButton PIDTurnButton;
+	private JoystickButton pIDMoveButton;
+	private JoystickButton pIDTurnButton;
 	private JoystickButton resetEncButton;
-	private JoystickButton MoveLiftUpButton;
-	private JoystickButton MoveLiftDownButton;
-	public Joystick rightJoy;
+	private JoystickButton moveLiftUpButton;
+	private JoystickButton moveLiftDownButton;
+	private JoystickButton findTurnTimeConstantButton;
 	private JoystickButton updatePIDConstantsButton;
 	private JoystickButton updateEncoderDPPButton;
+
 	public Joystick manipulator;
-	private JoystickButton closeIntake;
-	private JoystickButton openIntake;
-	private JoystickButton raiseIntake;
-	private JoystickButton lowerIntake;
-	private JoystickButton intake;
-	private JoystickButton outake;
-	private JoystickButton toggleLeftIntake;
-	private JoystickButton toggleRightIntake;
+	private JoystickButton closeIntakeButton;
+	private JoystickButton openIntakeButton;
+	private JoystickButton raiseIntakeButton;
+	private JoystickButton lowerIntakeButton;
+	private JoystickButton intakeCubeButton;
+	private JoystickButton outakeCubeButton;
+	private JoystickButton toggleLeftIntakeButton;
+	private JoystickButton toggleRightIntakeButton;
 
 	public int getButton(String key, int def) {
 		if (!SmartDashboard.containsKey("Button/" + key)) {
@@ -69,20 +72,24 @@ public int getButton(String key, int def) {
 		return (int) SmartDashboard.getNumber("Button/" + key, def);
 	}
 
-	public OI() {
+	public OI(Robot robot) {
 		leftJoy = new Joystick(0);
 		shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
 		shiftDriveTypeButton.whenPressed(new ShiftDriveType());
-		PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
-		PIDMoveButton
+		pIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
+		pIDMoveButton
 				.whenPressed(new PIDMove(Robot.sd.getConst("Move Targ", 24), Robot.dt, Robot.sd, RobotMap.distEncAvg));
-		PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
+		pIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
 		// PIDTurnButton.whenPressed(new PIDTurn(Robot.getConst("Turn Targ", 90),
 		// Robot.dt, Robot.sd RobotMap.fancyGyro));
-		PIDTurnButton
+		pIDTurnButton
 				.whenReleased(new PIDTurn(Robot.getConst("Turn Targ", 90), Robot.dt, Robot.sd, RobotMap.fancyGyro));
 		resetEncButton = new JoystickButton(leftJoy, getButton("Reset Dist Enc", 10));
 		resetEncButton.whenPressed(new ResetEncoders());
+		findTurnTimeConstantButton = new JoystickButton(leftJoy, getButton("Find Turn Time Constant", 11));
+		// the command will only run in test mode
+		findTurnTimeConstantButton
+				.whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd));
 
 		rightJoy = new Joystick(1);
 		shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 3));
@@ -93,30 +100,34 @@ public OI() {
 		updatePIDConstantsButton.whenPressed(new UpdatePIDConstants());
 		updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9));
 		updateEncoderDPPButton.whenPressed(new SetDistancePerPulse());
-		MoveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
-		MoveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
-		MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
-		MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
+		moveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
+		moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
+		moveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
+		moveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
 
 		manipulator = new Joystick(2);
-		closeIntake = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
-		closeIntake.whenPressed(new CloseIntake());
-		openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
-		openIntake.whenPressed(new OpenIntake());
-		// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
-		// Button", 3));
-		// raiseIntake.whenPressed(new RaiseIntake());
-		// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
-		// Button", 4));
-		// lowerIntake.whenPressed(new LowerIntake());
-		intake = new JoystickButton(manipulator, getButton("Intake Button", 5));
-		intake.whenPressed(new IntakeCube());
-		outake = new JoystickButton(manipulator, getButton("Outake Button", 6));
-		outake.whenPressed(new OutakeCube());
-		toggleLeftIntake = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
-		toggleLeftIntake.whenPressed(new ToggleLeftIntake());
-		toggleRightIntake = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
-		toggleRightIntake.whenPressed(new ToggleRightIntake());
+		if (manipulator.getButtonCount() == 0) {
+			System.out.println("Manipulator not plugged in!");
+		} else {
+			closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
+			closeIntakeButton.whenPressed(new CloseIntake());
+			openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
+			openIntakeButton.whenPressed(new OpenIntake());
+			// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
+			// Button", 3));
+			// raiseIntake.whenPressed(new RaiseIntake());
+			// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
+			// Button", 4));
+			// lowerIntake.whenPressed(new LowerIntake());
+			intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
+			intakeCubeButton.whenPressed(new IntakeCube());
+			outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6));
+			outakeCubeButton.whenPressed(new OuttakeCube());
+			toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
+			toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
+			toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
+			toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
+		}
 	}
 
 	// /**

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

@@ -71,13 +71,26 @@ public double getConst(String key, double def) {
 			return pref.getDouble("Const/" + key, def);
 		}
 
+		public void putConst(String key, double def) {
+			Preferences pref = Preferences.getInstance();
+			pref.putDouble("Const/" + key, def);
+			if (pref.getDouble("Const/ + key", def) != def) {
+				System.err.println("pref Key" + "Const/" + key + "already taken by a different type");
+			}
+		}
+
 		public void putData(String string, PIDController controller) {
 			SmartDashboard.putData(string, controller);
 		}
 
 		public void putNumber(String string, double d) {
 			SmartDashboard.putNumber(string, d);
 		}
+
+		public void putBoolean(String string, boolean b) {
+			SmartDashboard.putBoolean(string, b);
+		}
+
 		/*
 		 * if (!SmartDashboard.containsKey("Const/" + key)) { if
 		 * (!SmartDashboard.putNumber("Const/" + key, def)) {
@@ -91,6 +104,10 @@ public static double getConst(String key, double def) {
 		return sd.getConst(key, def);
 	}
 
+	public static void putConst(String key, double def) {
+		sd.putConst(key, def);
+	}
+
 	public static boolean getBool(String key, boolean def) {
 		Preferences pref = Preferences.getInstance();
 		if (!pref.containsKey("Bool/" + key)) {
@@ -114,8 +131,8 @@ public void robotInit() {
 		climberAssist = new ClimberAssist();
 		intakeEject = new IntakeEject();
 		lift = new Lift();
-		dt = new Drivetrain();
-		oi = new OI();
+		dt = new Drivetrain(sd);
+		oi = new OI(this);
 
 		// auto position chooser
 		for (Autonomous.Position p : Autonomous.Position.values()) {
@@ -151,7 +168,7 @@ public void robotInit() {
 	 */
 	@Override
 	public void disabledInit() {
-
+		dt.disableVelocityPIDs();
 	}
 
 	@Override
@@ -195,6 +212,9 @@ public void autonomousPeriodic() {
 
 	@Override
 	public void teleopInit() {
+		System.out.println("In teleopInit()");
+		dt.resetAHRS();
+		AutoUtils.state = new State(0, 0, 0);
 		// This makes sure that the autonomous stops running when
 		// teleop starts running. If you want the autonomous to
 		// continue until interrupted by another command, remove
@@ -224,14 +244,21 @@ public void teleopPeriodic() {
 		// SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
 		// SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
 		//
+
+		System.out.printf("Left: %1$5.2f; Right: %2$5.2f\n", RobotMap.dtLeft.get(), RobotMap.dtRight.get());
+
 		SmartDashboard.putNumber("Angle", dt.getAHRSAngle());
 		SmartDashboard.putNumber("Left Current draw", rmap.pdp.getCurrent(4));
 		SmartDashboard.putNumber("Right Current draw", rmap.pdp.getCurrent(11));
 	}
 
 	boolean firstTime = true;
 
+	@Override
 	public void testInit() {
+		System.out.println("In testInit()");
+		dt.resetAHRS();
+		AutoUtils.state = new State(0, 0, 0);
 	}
 
 	/**
@@ -243,26 +270,39 @@ public void testPeriodic() {
 		// Robot.dt.enableVelocityPIDs();
 		// firstTime = false;
 		//// }
-		// Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5));
+		// dt.getLeftVPID().setConsts(getConst("VelocityLeftkI", 0), 0,
+		// getConst("VelocityLeftkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
+		// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityLeftkF",
+		// 1 / Robot.getConst("Max Low Speed", 84)));
+		// dt.getRightVPID().setConsts(getConst("VelocityRightkI", 0), 0,
+		// getConst("VelocityRightkD", rmap.calcDefkD(dt.getCurrentMaxSpeed())),
+		// /* 1 / dt.getCurrentMaxSpeed() */Robot.getConst("VelocityRightkF",
+		// 1 / Robot.getConst("Max Low Speed", 84)));
+		// dt.resetAllVelocityPIDConsts();
+
+		// dt.setVPIDs(getConst("VPID Test Set", 0.5));
 
 		Scheduler.getInstance().run();
 
-		SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint());
-		SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint());
-		SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
-		SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
-		SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
-		SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
+		// System.out.println("Left VPID Targ: " + dt.getLeftVPIDOutput());
+		// System.out.println("Right VPID Targ: " + dt.getRightVPIDOutput());
+		// System.out.println("Left VPID Error: " + dt.getLeftVPIDerror());
+		// System.out.println("Right VPID Error: " + dt.getRightVPIDerror());
+		// System.out.println("Left Enc Rate: " + dt.getLeftEncRate());
+		// System.out.println("Right Enc Rate: " + dt.getRightEncRate());
+		//
+		// System.out.println("Left Talon Speed: " + rmap.dtLeftMaster.get());
+		// System.out.println("Right Talon Speed: " + rmap.dtRightMaster.get());
 
-		SmartDashboard.putNumber("Left Enc Dist", dt.getLeftDist());
-		SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist());
-		SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist());
+		// System.out.println("Left Enc Dist " + dt.getLeftDist());
+		// System.out.println("Right Enc Dist " + dt.getRightDist());
+		// System.out.println("Avg Enc Dist" + dt.getEncAvgDist());
 
 		// dt.dtLeft.set(0.1);
 		// dt.dtRight.set(-oi.rightJoy.getY());
 		// dt.dtLeft.set(-oi.leftJoy.getY());
 		// dt.dtRight.set(-oi.rightJoy.getY());
 
-		dt.putVelocityControllersToDashboard();
+		// dt.putVelocityControllersToDashboard();
 	}
 }