diff --git a/4_20_17.grip b/4_20_17.grip index f940b42..ab4bca9 100644 --- a/4_20_17.grip +++ b/4_20_17.grip @@ -1,10 +1,10 @@ - + - + @@ -70,16 +70,16 @@ 100.0 - 0.0 + 2.0 - 0.0 + 2.0 1000.0 - 0.0 + 2.0 1000.0 @@ -135,29 +135,29 @@ - - + + + + + + - - + + - + - - - - 2557 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 706e9e5..ead5cee 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 @@ -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 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 index ce0691a..6d0b57d 100644 --- 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 index 575ee89..be5c381 100644 --- 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 index 60c0606..03fb4e2 100644 --- 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 index 094262b..ccf77bd 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 @@ -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 index c881553..5c98725 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,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); } }