Competition changes from the end of qualifications

FIRST-Team-2557-The-SOTABots · Apr 22, 2017 · a5e73e6 · a5e73e6
1 parent 8ce62da
commit a5e73e6
Show file tree

Hide file tree

Showing 7 changed files with 69 additions and 55 deletions.
diff --git a/4_20_17.grip b/4_20_17.grip
@@ -1,10 +1,10 @@
 <grip:Pipeline>
   <sources>
     <grip:Camera>
-      <property name="address" value="http://10.25.57.6:5808/?action=stream"/>
+      <property name="address" value="http://10.25.57.127:5809/?action=stream"/>
     </grip:Camera>
     <grip:Camera>
-      <property name="address" value="http://10.25.57.6:5809/?action=stream"/>
+      <property name="address" value="http://10.25.57.127:5808/?action=stream"/>
     </grip:Camera>
   </sources>
   <steps>
@@ -70,16 +70,16 @@
         <value>100.0</value>
       </grip:Input>
       <grip:Input step="4" socket="2">
-        <value>0.0</value>
+        <value>2.0</value>
       </grip:Input>
       <grip:Input step="4" socket="3">
-        <value>0.0</value>
+        <value>2.0</value>
       </grip:Input>
       <grip:Input step="4" socket="4">
         <value>1000.0</value>
       </grip:Input>
       <grip:Input step="4" socket="5">
-        <value>0.0</value>
+        <value>2.0</value>
       </grip:Input>
       <grip:Input step="4" socket="6">
         <value>1000.0</value>
@@ -135,29 +135,29 @@
       <grip:Input step="5" socket="0"/>
     </grip:Connection>
     <grip:Connection>
-      <grip:Output step="3" socket="0" previewed="true"/>
-      <grip:Input step="4" socket="0"/>
+      <grip:Output source="1" socket="0" previewed="true"/>
+      <grip:Input step="1" socket="0"/>
     </grip:Connection>
     <grip:Connection>
       <grip:Output step="2" socket="0" previewed="true"/>
       <grip:Input step="3" socket="0"/>
     </grip:Connection>
+    <grip:Connection>
+      <grip:Output step="3" socket="0" previewed="true"/>
+      <grip:Input step="4" socket="0"/>
+    </grip:Connection>
     <grip:Connection>
       <grip:Output step="0" socket="0" previewed="true"/>
       <grip:Input step="2" socket="0"/>
     </grip:Connection>
     <grip:Connection>
-      <grip:Output source="0" socket="0" previewed="true"/>
-      <grip:Input step="1" socket="0"/>
+      <grip:Output step="1" socket="0" previewed="true"/>
+      <grip:Input step="2" socket="1"/>
     </grip:Connection>
     <grip:Connection>
-      <grip:Output source="0" socket="0" previewed="true"/>
+      <grip:Output source="1" socket="0" previewed="true"/>
       <grip:Input step="0" socket="0"/>
     </grip:Connection>
-    <grip:Connection>
-      <grip:Output step="1" socket="0" previewed="true"/>
-      <grip:Input step="2" socket="1"/>
-    </grip:Connection>
   </connections>
   <settings>
     <teamNumber>2557</teamNumber>

diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/Robot.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/Robot.java
@@ -4,6 +4,7 @@
 import edu.wpi.first.wpilibj.CameraServer;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
@@ -56,7 +57,6 @@ public class Robot extends IterativeRobot {
 	public static final VisionArray_sub		vision			= new VisionArray_sub();
 	public static OI oi;
 	public double x = 45;
-
 	Command driveTo;
 	Command shiftUp;
 	Command shiftDown;
@@ -65,7 +65,10 @@ public class Robot extends IterativeRobot {
 	Command visionUpdate;
 	Command shooterUpdate;
 	Command driveToTarget;
+
 	public static Command leftX_gear;
+	public static Preferences prefs;
+	public int _auto;
 
 	Command autonomousCommand;
 	SendableChooser<Command> chooser = new SendableChooser<>();
@@ -78,12 +81,17 @@ public class Robot extends IterativeRobot {
 	public void robotInit() {
 		RobotMap.init();
 		CameraServer.getInstance().startAutomaticCapture();
+		prefs = Preferences.getInstance();
 		vision.initializer();
 		shooterUpdate = new PsuedoShooter_cmd();
 		visionUpdate = new Vision_cmd();
 		shiftUp = new ShiftToggle_autoCmd(true);
 		shiftDown = new ShiftToggle_autoCmd(false);
 		leftX_gear = new centerX_gear();
+
+
+
+
 //		Main_auto = new Autonomous_GearLeftHopper(); //GearLeft
 //		Main_auto = new Autonomous_GearLeftShoot();
 //		Main_auto = new Autonomous_GearRightHopper(); //GearRight
@@ -100,14 +108,15 @@ public void robotInit() {
 		driveToTarget = new DriveToTarget_cmd();
 		oi = new OI();
 		oi.init();
+
 		/*
-		SmartDashboard.putData("Auto mode", chooser);
 //		chooser.addObject("Autonomous_GearLeftShoot: ", new Autonomous_GearLeftShoot());
 		chooser.addObject("Autonomous_GearLeft (Hopper): ", new Autonomous_GearLeftHopper());
 //		chooser.addObject("Autonomous_GearRightShoot: ", new Autonomous_GearRightShoot());
 		chooser.addObject("Autonomous_GearRight (Hopper): ", new Autonomous_GearRightHopper());
 		chooser.addObject("Autonomous_GearCenter (ShootLeft): ", new Autonomous_GearCenterShootLeft());		
 //		chooser.addObject("Autonomous_GearCenterShootRight: ", new Autonomous_GearCenterShootRight());
+		SmartDashboard.putData("Auto mode", chooser);
 		*/
 	}
 
@@ -150,22 +159,24 @@ public void autonomousInit() {
 		 * = new MyAutoCommand(); break; case "Default Auto": default:
 		 * autonomousCommand = new ExampleCommand(); break; }
 		 */
-
-		//autonomousCommand = (Command) chooser.getSelected();
-		switch(SmartDashboard.getString("Auto", "0")){
-			default:
-			case "L":
-				autonomousCommand = new Autonomous_GearLeftHopper();
-			case "C":
-				autonomousCommand = new Autonomous_GearCenterShootLeft();
-			case "R":
-				autonomousCommand = new Autonomous_GearRightHopper();
-		}
-		autonomousCommand.start();
 
+//		autonomousCommand = (Command) chooser.getSelected();
+//		autonomousCommand.start();
+
+		_auto = prefs.getInt("Auto", 0);
+		if(_auto == 1){
+			autonomousCommand = new Autonomous_GearLeftHopper();
+		}
+		else if(_auto == 2){
+			autonomousCommand = new Autonomous_GearRightHopper();
+		}
+		else{
+			autonomousCommand = new Autonomous_GearCenterShootLeft();
+		}
 		// schedule the autonomous command (example)
 		if (autonomousCommand != null)
 			autonomousCommand.start();
+
 	}
 
 	/**
@@ -209,6 +220,7 @@ public void teleopInit() {
 	@Override
 	public void teleopPeriodic() {
 		Scheduler.getInstance().run();
+//		SmartDashboard.putNumber("Auto", (int) SmartDashboard.getNumber("Auto", 0.0));
 		RobotMap.euler.update();
 		visionUpdate.start();
 		shooterUpdate.start();
@@ -250,12 +262,12 @@ else if(oi.LB1.get()){
 //    		}
 //    		
 //    	}
-		if(oi.a1.get()) {
-    		if(vision.averageInterpretation(2, 0, 0, 217, 0, .02) == false){
-    			if(vision.findCenterXs(0) < 217){
+		if(oi.y1.get()) {
+    		if(vision.averageInterpretation(2, 0, 0, 220, 0, .02) == false){
+    			if(vision.findCenterXs(0) < 221){
     				RobotMap.robotDrive.arcadeDrive(-.8,-.5);
     			}
-    			else if(vision.findCenterXs(0) > 217){
+    			else if(vision.findCenterXs(0) > 221){
     				RobotMap.robotDrive.arcadeDrive(-.8,.5);
     			}
     			else{

diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Autonomous_GearLeftHopper.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Autonomous_GearLeftHopper.java
@@ -8,8 +8,10 @@
 public class Autonomous_GearLeftHopper extends CommandGroup {
 
     public Autonomous_GearLeftHopper() {
+
     	addSequential(new SensorReset_autoCmd(3)); //Reset gyro angle to zero and encoders to zero
     	addSequential(new TimedDrive(.25, false, 0));
+
     	addSequential(new EncoderDrive_cmd(23.7, 23.67 ,true, .85)); //Drive forward
 
     	addSequential(new GyroDrive_cmd(54, .85)); //Turn to the gear peg
@@ -35,6 +37,13 @@ public Autonomous_GearLeftHopper() {
     	addSequential(new ShiftToggle_autoCmd(false)); //Shift into high gear
     	addSequential(new EncoderDrive_cmd(24.25, 23.7, true, 1)); //Drive forward
 
+
+
+
+
+
+
+
     	/*
     	addSequential(new GyroDrive_cmd(90, .775)); //Turn to face directly away from the hopper
     	

diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Autonomous_GearRightHopper.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Autonomous_GearRightHopper.java
@@ -8,6 +8,7 @@
 public class Autonomous_GearRightHopper extends CommandGroup {
 
     public Autonomous_GearRightHopper() {
+
     	addSequential(new SensorReset_autoCmd(2)); //Reset gyro angle to zero and encoders to zero
     	addSequential(new TimedDrive(.25, false, 0)); //Pause to allow encoders to reset
     	addSequential(new EncoderDrive_cmd(22.7, 22.67 ,true, .85)); //Drive forward
@@ -24,18 +25,19 @@ public Autonomous_GearRightHopper() {
     	addParallel(new TimedGear(2, .75)); //Run gear wheels
     	addSequential(new EncoderDrive_cmd(-13, -13 ,false, -.85));// Drive backwards away from the peg
     	addSequential(new GearToggle_autoCmd(true)); //Put the gear mech to the top
+
+    	addSequential(new GyroDrive_cmd(54, .8)); //Turn to face forward again
 
+    	addSequential(new SensorReset_autoCmd(3)); //Reset the encoders to zero
+    	addSequential(new TimedDrive(.25, false, 0)); //Pause to allow encoders to reset
+    	addSequential(new ShiftToggle_autoCmd(false)); //Shift into high gear
+    	addSequential(new EncoderDrive_cmd(20.25, 19.7, true, .85)); //Drive forward to line up perpendicular to hopper
 
 
 
 
 
-    	addSequential(new GyroDrive_cmd(54, .8)); //Turn to face forward again
 
-    	addSequential(new SensorReset_autoCmd(3)); //Reset the encoders to zero
-    	addSequential(new TimedDrive(.25, false, 0)); //Pause to allow encoders to reset
-    	addSequential(new ShiftToggle_autoCmd(false)); //Shift into high gear
-    	addSequential(new EncoderDrive_cmd(20.25, 19.7, true, .85)); //Drive forward to line up perpendicular to hopper
 
 //    	addSequential(new GyroDrive_cmd(-90, -.75)); //Turn right to face away from the hopper
 //    	

diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/TimedVision.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/TimedVision.java
@@ -22,11 +22,11 @@ protected void initialize() {
 
     // Called repeatedly when this Command is scheduled to run
     protected void execute() {
-    	if(Robot.vision.averageInterpretation(2, 0, 0, 217, 0, .02) == false){
-			if(Robot.vision.findCenterXs(0) < 217){
+    	if(Robot.vision.averageInterpretation(2, 0, 0, 221, 0, .02) == false){
+			if(Robot.vision.findCenterXs(0) < 221){
 				RobotMap.robotDrive.arcadeDrive(-.8,-.5);
 			}
-			else if(Robot.vision.findCenterXs(0) > 217){
+			else if(Robot.vision.findCenterXs(0) > 221){
 				RobotMap.robotDrive.arcadeDrive(-.8,.5);
 			}
 			else{

diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/Intake_sub.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/Intake_sub.java
@@ -32,7 +32,7 @@ public void intake(){
 //					RobotMap.agitator.set(-.55);
 //				}
 //			}
-    		if(Robot.oi.RB2.get() && RobotMap.Lshooter.getEncVelocity() > 3500){
+    		if(Robot.oi.RB2.get()){ // && RobotMap.Lshooter.getEncVelocity() > 3500){
     			RobotMap.intake.set(-.75);
     			RobotMap.agitator.set(-.55);
     			RobotMap.copterAgitator.set(.5);

diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/Shooter_sub.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/Shooter_sub.java
@@ -23,26 +23,17 @@ public void initDefaultCommand() {
 
     public void shooting(){
    		if(Robot.oi.gamepad2.getRawAxis(3) > .1){
-   	    	RobotMap.Lshooter.set(-.75); //-.675 at 3ft making shots like a boss, -.69 to -.715 at 4ft
-   	    	RobotMap.Rshooter.set(.75); //.675 at 3ft making shots like a boss, .69 to -.715 at 4ft
-//   	    	RobotMap.copterAgitator.set(1);
+   	    	RobotMap.Lshooter.set(-.775); //-.675 at 3ft making shots like a boss, -.69 to -.715 at 4ft
+   	    	RobotMap.Rshooter.set(.775); //.675 at 3ft making shots like a boss, .69 to -.715 at 4ft
 //   	    	RobotMap.agitator.set(-.6);
    		}
-   		else if(Robot.oi.b2.get()){
-   			psuedo.set();
-//   			RobotMap.copterAgitator.set(1);
-   		}
-   		else if(Robot.oi.gamepad2.getRawAxis(2) > .1){
-//   			RobotMap.copterAgitator.set(-1);
+//   		else if(Robot.oi.gamepad2.getRawAxis(2) > .1){
 //			RobotMap.agitator.set(.8);
-//			Robot.agitator.leftHopper();
-//			Robot.agitator.rightHopper();
-   		}
+//   		}
    		else{
-//   			RobotMap.copterAgitator.set(0);
    	    	RobotMap.Lshooter.set(0);
    	    	RobotMap.Rshooter.set(0);
-//   	    	RobotMap.agitator.set(0);
+   	    	RobotMap.agitator.set(0);
    		}
 
     }