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 index ab29fe4..19e580f 100644 --- 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 index 99cfb2b..9d31064 100644 --- 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 new file mode 100644 index 0000000..c188afe --- /dev/null +++ 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 index 4b6919f..c01825c 100644 --- 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 index 2e65926..5008928 100644 --- 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 index c881553..2cc099b 100644 --- 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); }