Adding final changes from competition

FIRST-Team-2557-The-SOTABots · Mar 30, 2017 · a1b7de0 · a1b7de0
1 parent 95e4d80
commit a1b7de0
Show file tree

Hide file tree

Showing 6 changed files with 63 additions and 17 deletions.
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
@@ -67,7 +67,7 @@ public class Robot extends IterativeRobot {
 	@Override
 	public void robotInit() {
 		RobotMap.init();
-		CameraServer.getInstance().startAutomaticCapture();
+//		CameraServer.getInstance().startAutomaticCapture();
 		vision.initializer();
 
 		shooterUpdate = new PsuedoShooter_cmd();

diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Autonomous_GearLeft.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Autonomous_GearLeft.java
@@ -28,6 +28,6 @@ public Autonomous_GearLeft() {
     	addSequential(new TimedDrive(.2, false, .85));
     	addSequential(new GyroDrive_cmd(60, .65));
     	addSequential(new DistanceDrive_cmd(1, -.615), 3.5);
-    	addSequential(new Gear_autoCmd());
+//    	addSequential(new Gear_autoCmd());
     }
 }
diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/GyroDrive_cmd.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/GyroDrive_cmd.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc.team2557.robot.autonomous;
+
+import org.usfirst.frc.team2557.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class GyroDrive_cmd extends Command {
+
+	public double _turn, _angle;
+    public GyroDrive_cmd(double angle, double turn) {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+    	_angle = angle;
+    	_turn = turn;
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+//    	RobotMap.navX.reset();
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    	RobotMap.robotDrive.arcadeDrive(0,_turn);
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+        return RobotMap.navX.getAngle() >_angle - 5 && RobotMap.navX.getAngle() < _angle + 5;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    	RobotMap.robotDrive.arcadeDrive(0,0);
+    	RobotMap.euler.reset();
+    	RobotMap.navX.reset();
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}
diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Main_auto.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/autonomous/Main_auto.java
@@ -16,9 +16,9 @@ public Main_auto() {
 //    	addParallel(new Autonomus_Shooter(.5));
 //    	addSequential(new Auto_Shooter2(10));
 
-    	addSequential(new DistanceDrive_cmd(1.8, -.615), 6.5);
-    	addSequential(new TimedDrive(.2, false, .85));
-    	addSequential(new Gear_autoCmd());
+    	addSequential(new DistanceDrive_cmd(1.7, -.7), 6.5);
+    	addSequential(new TimedDrive(.17, false, .85));
+//    	addSequential(new Gear_autoCmd());
 
         // Add Commands here:
         // e.g. addSequential(new Command1());

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
@@ -23,16 +23,16 @@ public void initDefaultCommand() {
 
     public void intake(){
 
-	    	if(Robot.oi.RB2.get() && (Robot.oi.gamepad2.getRawAxis(3) > 0.1 ) || Robot.oi.b2.get()){
-				RobotMap.copterAgitator.set(.5);
-				if((RobotMap.Rshooter.getEncVelocity() > Robot.psuedoShooter.getPrimeR(71) && RobotMap.Rshooter.getEncVelocity() < Robot.psuedoShooter.getPrimeR(79))
-   					|| (RobotMap.Lshooter.getEncVelocity() > Robot.psuedoShooter.getPrimeL(71)  && RobotMap.Lshooter.getEncVelocity() < Robot.psuedoShooter.getPrimeL(79)))
-				{
-					RobotMap.intake.set(-.6);
-					RobotMap.agitator.set(-.55);
-				}
-			}
-    		else if(Robot.oi.RB2.get()){
+//	    	if(Robot.oi.RB2.get() && (Robot.oi.gamepad2.getRawAxis(3) > 0.1 ) || Robot.oi.b2.get()){
+//				RobotMap.copterAgitator.set(.5);
+//				if((RobotMap.Rshooter.getEncVelocity() > Robot.psuedoShooter.getPrimeR(71) && RobotMap.Rshooter.getEncVelocity() < Robot.psuedoShooter.getPrimeR(79))
+//   					|| (RobotMap.Lshooter.getEncVelocity() > Robot.psuedoShooter.getPrimeL(71)  && RobotMap.Lshooter.getEncVelocity() < Robot.psuedoShooter.getPrimeL(79)))
+//				{
+//					RobotMap.intake.set(-.6);
+//					RobotMap.agitator.set(-.55);
+//				}
+//			}
+    		if(Robot.oi.RB2.get()){
     			RobotMap.intake.set(-.6);
     			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,8 +23,8 @@ 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.Lshooter.set(-1); //-.675 at 3ft making shots like a boss, -.69 to -.715 at 4ft
+   	    	RobotMap.Rshooter.set(1); //.675 at 3ft making shots like a boss, .69 to -.715 at 4ft
 //   	    	RobotMap.copterAgitator.set(1);
 //   	    	RobotMap.agitator.set(-.6);
    		}