First commit of DavidRobot2018 (#5)

* First commit of DavidRobot2018 adding basic drivetrain functionality

* changed up workspace organization, functionailty should not be changed

* integrated DavidRobot2018 into Robot2018

* fixed project setup and migrated all packages to one standard

* deleted DavidRobot2018 (previously merged manually into Robot2018)

* split drivetrain gear shift into 2 commands (from 1) and added more flexibility

Author: dfang314

Robot2018/.classpath

@@ -7,5 +7,7 @@
 	<classpathentry kind="var" path="opencv" sourcepath="opencv.sources"/>
 	<classpathentry kind="var" path="cscore" sourcepath="cscore.sources"/>
 	<classpathentry kind="var" path="wpiutil" sourcepath="wpiutil.sources"/>
+	<classpathentry kind="lib" path="/Robot2017/libs/jars/navx_frc.jar"/>
+	<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix.jar"/>
 	<classpathentry kind="output" path="bin"/>
 </classpath>

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

@@ -1,6 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
 package org.usfirst.frc.team199.Robot2018;
 
-import edu.wpi.first.wpilibj.buttons.Button;
+import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
+import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -34,4 +46,27 @@ public class OI {
 	// Start the command when the button is released and let it run the command
 	// until it is finished as determined by it's isFinished method.
 	// button.whenReleased(new ExampleCommand());
+	
+	public Joystick leftJoy;
+		public JoystickButton shiftDrive;
+		public JoystickButton shiftDriveType;
+	public Joystick rightJoy;
+	public Joystick manipulator;
+	
+	
+	public int getButton(String key, int def) {
+		if(!SmartDashboard.containsKey(key)) {
+			SmartDashboard.putNumber(key, def);
+		}
+		return (int) SmartDashboard.getNumber(key, def);
+	}
+	public OI() {
+		leftJoy = new Joystick(0);
+			shiftDrive = new JoystickButton(leftJoy, getButton("Button Shift Drive", 1));
+			shiftDrive.whenPressed(new ShiftLowGear());
+			shiftDriveType = new JoystickButton(leftJoy, getButton("Button Shift Drive Type", 2));
+			shiftDriveType.whenPressed(new ShiftDriveType());
+		rightJoy = new Joystick(1);
+		manipulator = new Joystick(2);
+	}
 }

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

@@ -1,8 +1,11 @@
 
 package org.usfirst.frc.team199.Robot2018;
 
+import java.util.ArrayList;
+
 import org.usfirst.frc.team199.Robot2018.subsystems.Climber;
 import org.usfirst.frc.team199.Robot2018.subsystems.ClimberAssist;
+import org.usfirst.frc.team199.Robot2018.subsystems.Drivetrain;
 import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject;
 import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
 
@@ -26,18 +29,48 @@ public class Robot extends IterativeRobot {
 	public static final ClimberAssist climberAssist = new ClimberAssist();
 	public static final IntakeEject intakeEject = new IntakeEject();
 	public static final Lift lift = new Lift();
+	public static Drivetrain dt;
+
 	public static OI oi;
 
 	Command autonomousCommand;
 	SendableChooser<Command> chooser = new SendableChooser<>();
 
+
+	public static double getConst(String key, double def) {
+		if(!SmartDashboard.containsKey(key)) {
+			SmartDashboard.putNumber(key, def);
+		}
+		return SmartDashboard.getNumber(key, def);
+	}
+	public void sendValuesToDashboard() {
+		ArrayList<String> boolKeys = new ArrayList<String>();
+		ArrayList<Boolean> boolDef = new ArrayList<Boolean>();
+			boolKeys.add("Arcade Drive");
+			boolKeys.add("Arcade Drive Default Setup");
+			boolKeys.add("Square Drive Values");
+			boolKeys.add("High Gear");
+			
+			boolDef.add(true);
+			boolDef.add(true);
+			boolDef.add(false);
+			boolDef.add(false);
+		for(int i = 0; i < boolKeys.size(); i++) {
+			if(!SmartDashboard.containsKey(boolKeys.get(i))) {
+				SmartDashboard.putBoolean(boolKeys.get(i), boolDef.get(i));
+			}
+		}
+	}
+
 	/**
 	 * This function is run when the robot is first started up and should be
 	 * used for any initialization code.
 	 */
 	@Override
 	public void robotInit() {
+		dt = new Drivetrain();
 		oi = new OI();
+		sendValuesToDashboard();
 		// chooser.addObject("My Auto", new MyAutoCommand());
 		SmartDashboard.putData("Auto mode", chooser);
 	}

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

@@ -1,5 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
 package org.usfirst.frc.team199.Robot2018;
 
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.kauailabs.navx.frc.AHRS;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 /**
  * The RobotMap is a mapping from the ports sensors and actuators are wired into
  * to a variable name. This provides flexibility changing wiring, makes checking
@@ -16,4 +35,91 @@ 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 Encoder leftEnc;
+	public static WPI_TalonSRX dtLeftDrive;
+		public static WPI_TalonSRX dtLeftSlave;
+	public static SpeedControllerGroup dtLeft;
+		
+	public static Encoder rightEnc;
+	public static WPI_TalonSRX dtRightDrive;
+		public static WPI_TalonSRX dtRightSlave;
+	public static SpeedControllerGroup dtRight;
+	public static DifferentialDrive robotDrive;
+	public static PIDController turnController;
+	public static PIDController moveController;
+//	public static PIDController moveLeftController;
+//	public static PIDController moveRightController;
+	
+	public static AHRS ahrs;
+	public static AnalogGyro dtGyro;
+	public static DoubleSolenoid dtGear;
+	
+	private void configSRX(WPI_TalonSRX mc) {
+		int kTimeout = (int) Robot.getConst("ConstkTimeoutMs", 10);
+		mc.configNominalOutputForward(0, kTimeout);
+        mc.configNominalOutputReverse(0, kTimeout);
+        mc.configPeakOutputForward(1, kTimeout);
+        mc.configPeakOutputReverse(-1, kTimeout);
+	}
+	public RobotMap() {
+		
+		leftEnc = new Encoder(getPort("Port1LeftEnc", 0), getPort("Port2LeftEnc", 1));
+		dtLeftDrive = new WPI_TalonSRX(getPort("PortLeftTalonSRXDrive", 1));
+		configSRX(dtLeftDrive);
+			dtLeftSlave = new WPI_TalonSRX(getPort("PortLeftTalonSRXSlave", 1));
+			configSRX(dtLeftSlave);
+		dtLeft = new SpeedControllerGroup(dtLeftDrive, dtLeftSlave);
+		
+		rightEnc = new Encoder(getPort("Port1RightEnc", 2), getPort("Port2RightEnc", 3));
+		dtRightDrive = new WPI_TalonSRX(getPort("PortRightTalonSRXDrive", 2));
+		configSRX(dtRightDrive);
+			dtRightSlave = new WPI_TalonSRX(getPort("PortRightTalonSRXSlave", 1));
+			configSRX(dtRightSlave);
+		dtRight = new SpeedControllerGroup(dtRightDrive, dtRightSlave);
+		
+		robotDrive = new DifferentialDrive(dtLeft, dtRight);
+		turnController = new PIDController(Robot.getConst("ConstTurnkP", 1), Robot.getConst("ConstTurnkI", 0), Robot.getConst("ConstTurnkD", 0), ahrs, Robot.dt);
+			turnController.disable(); 
+			turnController.setInputRange(-180, 180);
+			turnController.setOutputRange(-1.0, 1.0);
+			turnController.setContinuous();
+			turnController.setAbsoluteTolerance(Robot.getConst("ConstTurnTolerance", 1));
+		moveController = new PIDController(Robot.getConst("ConstMovekP", 1), Robot.getConst("ConstMovekI", 0), Robot.getConst("ConstMovekD", 0), Robot.dt, Robot.dt);
+			turnController.disable(); 
+			turnController.setInputRange(-180, 180);
+			turnController.setOutputRange(-1.0, 1.0);
+			turnController.setContinuous();
+			turnController.setAbsoluteTolerance(Robot.getConst("ConstMoveTolerance", 2));
+//		moveLeftController = new PIDController(Robot.getConst("ConstMoveLeftkP", 1), Robot.getConst("ConstMoveLeftkI", 0), Robot.getConst("ConstMoveLeftkD", 0), Robot.dt.getLeftDrive(), (PIDOutput) Robot.dt.getLeftDrive());
+//			moveLeftController.disable();
+//			moveLeftController.setInputRange(0, Double.MAX_VALUE);
+//			moveLeftController.setOutputRange(-1.0, 1.0);
+//			moveLeftController.setContinuous(false);
+//			moveLeftController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft", 2));
+//		moveRightController = new PIDController(Robot.getConst("ConstMoveRightkP", 1), Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD", 0), Robot.dt.getRightDrive(), (PIDOutput) Robot.dt.getRightDrive());
+//			moveRightController.disable();
+//			moveRightController.setInputRange(0, Double.MAX_VALUE);
+//			moveRightController.setOutputRange(-1.0, 1.0);
+//			moveRightController.setContinuous(false);
+//			moveRightController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight", 2));
+			
+		ahrs = new AHRS(SerialPort.Port.kMXP);
+		dtGyro = new AnalogGyro(getPort("PortGyro", 0));
+		dtGear = new DoubleSolenoid(getPort("Port1dtGearSolenoid", 0), getPort("Port2dtGearSolenoid", 1));
+		
+	}
+	
+	/**
+	 * Used in RobotMap to find ports for robot components, getPort also puts numbers if they don't exist yet.
+	 * @param key The port key
+	 * @param def The default value
+	 * @return returns the Port
+	 */
+	public int getPort(String key, int def) {
+		if(!SmartDashboard.containsKey(key)) {
+			SmartDashboard.putNumber(key, def);
+		}
+		return (int)SmartDashboard.getNumber(key, def);
+	}
 }

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

@@ -0,0 +1,46 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class PIDDrive extends Command {
+
+	double target;
+    public PIDDrive(double targ) {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+    	target = targ;
+    	requires(Robot.dt);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	Robot.dt.enableMovePid();
+    	Robot.dt.setSetMove(target);
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    	Robot.dt.arcadeDrive(Robot.dt.getPidOut(), 0);
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+        return Robot.dt.onDriveTarg();
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    	Robot.dt.stopDrive();
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    	end();
+    }
+}

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

@@ -0,0 +1,48 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.RobotMap;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class PIDTurn extends Command {
+
+	double target;
+    public PIDTurn(double targ) {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+    	target = targ;
+    	requires(Robot.dt);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	Robot.dt.resetAHRS();
+    	Robot.dt.setSetTurn(target);
+    	Robot.dt.enableTurnPid();
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    	Robot.dt.arcadeDrive(0, Robot.dt.getPidOut());
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+    	return Robot.dt.onTurnTarg();
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    	Robot.dt.stopDrive();
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    	end();
+    }
+}

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

@@ -0,0 +1,43 @@
+package org.usfirst.frc.team199.Robot2018.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class ShiftDriveType extends Command {
+
+	Timer tim;
+    public ShiftDriveType() {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	tim = new Timer();
+    	tim.reset();
+    	tim.start();
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    	SmartDashboard.putBoolean("Arcade Drive", !SmartDashboard.getBoolean("Arcade Drive", false));
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+        return tim.get() > 0.125;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}