Gyro pid tuning

eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/Robot.java

@@ -12,11 +12,10 @@
 package org.usfirst.frc4905.Galaktika;
 
 import org.usfirst.frc4905.Galaktika.commands.AutonomousCommand;
+import org.usfirst.frc4905.Galaktika.groupcommands.AutoCombinedLeftRight;
 import org.usfirst.frc4905.Galaktika.groupcommands.AutoCrossTheLine;
 import org.usfirst.frc4905.Galaktika.groupcommands.AutoMiddleLoadSwitch;
 import org.usfirst.frc4905.Galaktika.groupcommands.AutoMiddleRightLoadSwitch;
-import org.usfirst.frc4905.Galaktika.groupcommands.AutoPlayoffs;
-import org.usfirst.frc4905.Galaktika.groupcommands.AutoQuals;
 import org.usfirst.frc4905.Galaktika.subsystems.DriveTrain;
 import org.usfirst.frc4905.Galaktika.subsystems.Elevator;
 import org.usfirst.frc4905.Galaktika.subsystems.Intake;
@@ -129,27 +128,34 @@ public void robotInit() {
         chooser.addDefault("Do Nothing", new Pair<>(new AutonomousCommand(), '*'));
 
     // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
-        chooser.addObject("Qualifier Match Starting from Left", new Pair<>(new AutoQuals(false), 'L'));
-        chooser.addObject("Qualifier Match Starting from Right", new Pair<>(new AutoQuals(false), 'R'));
+        Command command;
+        command = new AutoCombinedLeftRight(false, AutoCombinedLeftRight.MatchType.QUALIFIERS);
+        chooser.addObject("Qualifier Match Starting from Left", new Pair<>(command, 'L'));
+        command = new AutoCombinedLeftRight(false, AutoCombinedLeftRight.MatchType.QUALIFIERS);
+        chooser.addObject("Qualifier Match Starting from Right", new Pair<>(command, 'R'));
         chooser.addObject("Load Switch from Middle", new Pair<>(new AutoMiddleLoadSwitch(false), 'M'));
         chooser.addObject("Load Switch from Middle Right", new Pair<>(new AutoMiddleRightLoadSwitch(false), 'M'));
         chooser.addObject("Cross The Line from Left", new Pair<>(new AutoCrossTheLine(false), 'L'));
         chooser.addObject("Cross The Line from Middle", new Pair<>(new AutoCrossTheLine(false), 'M'));
         chooser.addObject("Cross The Line from Right", new Pair<>(new AutoCrossTheLine(false), 'R'));
-        chooser.addObject("Playoff Match Starting from Left", new Pair<>(new AutoPlayoffs(false), 'L'));
-        chooser.addObject("Playoff Match Starting from Middle", new Pair<>(new AutoPlayoffs(false), 'M'));
-        chooser.addObject("Playoff Match Starting from Right", new Pair<>(new AutoPlayoffs(false), 'R'));
-
-        chooser.addObject("Delay and Qualifier Match Starting from Left", new Pair<>(new AutoQuals(true), 'L'));
-        chooser.addObject("Delay and Qualifier Match Starting from Right", new Pair<>(new AutoQuals(true), 'R'));
+        command = new AutoCombinedLeftRight(false, AutoCombinedLeftRight.MatchType.PLAYOFFS);
+        chooser.addObject("Playoff Match Starting from Left", new Pair<>(command, 'L'));
+        command = new AutoCombinedLeftRight(false, AutoCombinedLeftRight.MatchType.PLAYOFFS);
+        chooser.addObject("Playoff Match Starting from Right", new Pair<>(command, 'R'));
+
+        command = new AutoCombinedLeftRight(true, AutoCombinedLeftRight.MatchType.QUALIFIERS);
+        chooser.addObject("Delay and Qualifier Match Starting from Left", new Pair<>(command, 'L'));
+        command = new AutoCombinedLeftRight(true, AutoCombinedLeftRight.MatchType.QUALIFIERS);
+        chooser.addObject("Delay and Qualifier Match Starting from Right", new Pair<>(command, 'R'));
         chooser.addObject("Delay and Load Switch from Middle", new Pair<>(new AutoMiddleLoadSwitch(true), 'M'));
         chooser.addObject("Delay and Load Switch from Middle Right", new Pair<>(new AutoMiddleRightLoadSwitch(true), 'M'));
         chooser.addObject("Delay and Cross The Line from Left", new Pair<>(new AutoCrossTheLine(true), 'L'));
         chooser.addObject("Delay and Cross The Line from Middle", new Pair<>(new AutoCrossTheLine(true), 'M'));
         chooser.addObject("Delay and Cross The Line from Right", new Pair<>(new AutoCrossTheLine(true), 'R'));
-        chooser.addObject("Delay and Playoff Match Starting from Left", new Pair<>(new AutoPlayoffs(true), 'L'));
-        chooser.addObject("Delay and Playoff Match Starting from Middle", new Pair<>(new AutoPlayoffs(true), 'M'));
-        chooser.addObject("Delay and Playoff Match Starting from Right", new Pair<>(new AutoPlayoffs(true), 'R'));
+        command = new AutoCombinedLeftRight(true, AutoCombinedLeftRight.MatchType.PLAYOFFS);
+        chooser.addObject("Delay and Playoff Match Starting from Left", new Pair<>(command, 'L'));
+        command = new AutoCombinedLeftRight(true, AutoCombinedLeftRight.MatchType.PLAYOFFS);
+        chooser.addObject("Delay and Playoff Match Starting from Right", new Pair<>(command, 'R'));
 
         SmartDashboard.putData("Auto mode", chooser);
         SmartDashboard.putNumber("Autonomous Delay", delaySeconds);
@@ -183,7 +189,7 @@ public void robotInit() {
      */
     @Override
     public void disabledInit(){
-    	
+
     	Trace.getInstance().flushTraceFiles();
     	Robot.ramps.lockRampsIn();
 
@@ -192,7 +198,7 @@ public void disabledInit(){
 
     @Override
     public void disabledPeriodic() {
-    	
+
         Scheduler.getInstance().run();
 
     }

eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/RunIntakeIn.java

@@ -22,7 +22,7 @@
 public class RunIntakeIn extends Command {
 	Joystick intakeController;
     private static final double kDeadzone = 0;
-	public static final double kIntakeSpeed = 0.75;
+	public static final double kIntakeSpeed = 1;
 	// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
 
     // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS

eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/SetShouldJawsBeOpenStateCommand.java

@@ -8,9 +8,9 @@
  *
  */
 public class SetShouldJawsBeOpenStateCommand extends Command {
-	
+
 	private boolean m_state;
-	
+
     public SetShouldJawsBeOpenStateCommand(boolean state) {
         // Use requires() here to declare subsystem dependencies
         requires(Robot.jaws);
@@ -23,7 +23,7 @@ protected void initialize() {
 
     // Called repeatedly when this Command is scheduled to run
     protected void execute() {
-    	Robot.jaws.setShouldJawsBeOpenBoolean(m_state);
+    		Robot.jaws.setShouldJawsBeOpenBoolean(m_state);
     }
 
     // Make this return true when this Command no longer needs to run execute()

eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/ShootCubeInAuto.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc4905.Galaktika.commands;
+
+import org.usfirst.frc4905.Galaktika.Robot;
+
+import edu.wpi.first.wpilibj.command.TimedCommand;
+
+/**
+ *
+ */
+public class ShootCubeInAuto extends TimedCommand {
+
+    public ShootCubeInAuto(double timeout) {
+        super(timeout);
+        // Use requires() here to declare subsystem dependencies
+       	requires(Robot.intake);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    	Robot.intake.ejectIntake(1);
+    }
+
+    // Called once after timeout
+    protected void end() {
+    	Robot.intake.stopIntake();
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    	end();
+    }
+}

eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCombinedLeftRight.java

@@ -0,0 +1,191 @@
+package org.usfirst.frc4905.Galaktika.groupcommands;
+
+import org.usfirst.frc4905.Galaktika.Robot;
+import org.usfirst.frc4905.Galaktika.groupcommands.AutoCommand.MatchType;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class AutoCombinedLeftRight extends AutoCommand {
+	private final boolean m_useDelay;
+	protected final MatchType m_matchType;
+	public AutoCombinedLeftRight(boolean useDelay, MatchType matchType) {
+		// Add Commands here:
+        // e.g. addSequential(new Command1());
+        //      addSequential(new Command2());
+        // these will run in order.
+
+        // To run multiple commands at the same time,
+        // use addParallel()
+        // e.g. addParallel(new Command1());
+        //      addSequential(new Command2());
+        // Command1 and Command2 will run in parallel.
+
+        // A command group will require all of the subsystems that each member
+        // would require.
+        // e.g. if Command1 requires chassis, and Command2 requires arm,
+        // a CommandGroup containing them would require both the chassis and the
+        // arm.
+		debug("top of AutoQuals constructor");
+	    m_useDelay = useDelay;
+	    m_matchType = matchType;
+	    debug("bottom of AutoQuals constructor");
+	}
+
+	protected void prepareToStart() {
+    		debug("top of prepareToStart");
+    		if (m_useDelay) {
+    			delay(Robot.getAutonomousDelay());
+    		}
+        addAutoCombinedCommands(m_matchType);
+		debug("bottom of prepareToStart");
+    }
+
+	private void loadNearSwitchPlate(char robotPos) {
+		debug("top of AutoQuals loadNearSwitchPlate");
+        closeJaws(false);
+        parallelJawsOpenClose();
+        lowerIntake();
+        parallelRetractExtendArms();
+
+        moveElevatorToSwitchHeight();
+
+		driveForward(FORWARD_DISTANCE_TO_SWITCH);
+		if (robotPos == 'R') {
+			turnLeft();
+		} else {
+			turnRight();
+		}
+		driveForwardToWall(LATERAL_DISTANCE_TO_SWITCH);
+		openJaws();
+		parallelJawsOpenClose();
+		debug("bottom of AutoQuals loadNearSwitchPlate");
+	}
+
+	private void loadNearScalePlate(char robotPos) {
+		debug("top of AutoQuals loadNearScalePlate");
+
+        closeJaws(false);
+        parallelJawsOpenClose();
+        lowerIntake();
+        parallelRetractExtendArms();
+
+
+		driveForward(240);//empirical measurement subject to change
+
+		if (robotPos == 'R') {
+			turnDeltaAngle(-18.4);
+		} else {
+			turnDeltaAngle(18.4);
+		}
+		moveElevatorToScaleHeight();
+		delay(2);
+		driveForward(45);
+
+		shootCube(2);
+
+		debug("bottom of AutoQuals loadNearScalePlate");
+	}
+
+	protected void addAutoCombinedCommands(MatchType matchType) {
+		char robotPos = Robot.getInitialRobotLocation();
+	    char switchPlatePos = Robot.getSwitchPlatePosition();
+	    char scalePlatePos = Robot.getScalePlatePosition();
+	    if (matchType == MatchType.QUALIFIERS) {
+		    	if (switchPlatePos == robotPos) {
+		    		loadNearSwitchPlate(robotPos);
+		    	} else if (scalePlatePos == robotPos){
+		    		loadNearScalePlate(robotPos);
+		    	} else {
+		    		crossAutoLine(robotPos);
+		    	}
+	    } else {
+		    closeJaws(false);
+		    parallelJawsOpenClose();
+		    lowerIntake();
+		    parallelRetractExtendArms();
+
+			if (robotPos == 'L') {
+				if (scalePlatePos == 'L') {
+					loadNearScalePlate('L');
+
+				} else {
+					if (switchPlatePos == 'L') {
+
+						moveElevatorToSwitchHeight();
+						driveForward(FORWARD_DISTANCE_TO_SWITCH);
+						turnRight();
+						driveForwardToWall(LATERAL_DISTANCE_TO_SWITCH);
+						openJaws();
+						parallelJawsOpenClose();
+
+					} else {
+						driveForward(FORWARD_DISTANCE_TO_AUTO_LINE);
+					}
+				}
+			} else if (robotPos == 'R') {
+				if (scalePlatePos == 'R') {
+					loadNearScalePlate('R');
+				} else {
+					if (switchPlatePos == 'R') {
+						moveElevatorToSwitchHeight();
+						driveForward(FORWARD_DISTANCE_TO_SWITCH);
+						turnLeft();
+						driveForwardToWall(LATERAL_DISTANCE_TO_SWITCH);
+						openJaws();
+						parallelJawsOpenClose();
+
+					} else {
+						//lost the switch and scale, just go forwards
+						driveForward(FORWARD_DISTANCE_TO_AUTO_LINE);
+					}
+				}
+			}
+	    }
+	}
+
+	protected void pickupFirstCubeFromScale(double deltaAngle) {
+		turnDeltaAngle(deltaAngle);
+		moveElevatorToGroundHeight();
+		driveForward(53);
+		turnToCompassHeading(180);
+		driveForwardToWall(40);
+		closeJaws(true);
+	}
+
+	protected void pickupFirstCubeFromLeftSwitchPlate() {
+		driveBackward(CLEARANCE_TO_TURN);
+		turnLeft();
+		driveForward(80);
+		turnRight();
+		driveForward(40 + CLEARANCE_TO_TURN);
+		turnRight();
+		moveElevatorToGroundHeight();
+		driveForward(33);
+		closeJaws(true);
+	}
+
+	protected void pickupFarCubeFromLeftSwitchPlate() {
+		driveBackward(CLEARANCE_TO_TURN);
+		turnLeft();
+		driveForward(80);
+		turnRight();
+		driveForward(LATERAL_DISTANCE_TO_SCALE_PLATES + CLEARANCE_TO_TURN + 40);
+		turnRight();
+		moveElevatorToGroundHeight();
+		driveForward(33);
+		closeJaws(true);
+	}
+
+	protected void pickupFirstCubeFromRightSwitchPlate() {
+		driveBackward(CLEARANCE_TO_TURN);
+		turnRight();
+		driveForward(80);
+		turnLeft();
+		driveForward(40 + CLEARANCE_TO_TURN);
+		turnLeft();
+		moveElevatorToGroundHeight();
+		driveForward(33);
+		closeJaws(true);
+	}
+
+}