From: Kevin Zhang Date: Wed, 22 Jun 2016 17:55:14 +0000 (-0700) Subject: commit X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=67af84eb001ffa2e0f8b0e5139724c71a2ce7a15 commit --- diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 6bf02a4c..be86bc29 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -1,13 +1,12 @@ package org.usfirst.frc.team3501.robot; -import org.usfirst.frc.team3501.robot.Constants.Auton; -import org.usfirst.frc.team3501.robot.Constants.Defense; -import org.usfirst.frc.team3501.robot.commands.auton.ChooseStrategy; import org.usfirst.frc.team3501.robot.commands.driving.SetLowGear; +import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive; import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; import org.usfirst.frc.team3501.robot.subsystems.IntakeArm; import org.usfirst.frc.team3501.robot.subsystems.Shooter; +import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -19,10 +18,10 @@ public class Robot extends IterativeRobot { public static Shooter shooter; public static IntakeArm intakeArm; - // Sendable Choosers send a drop down menu to the Smart Dashboard. - SendableChooser positionChooser; - SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense, - positionFourDefense, positionFiveDefense; + SendableChooser frontChooser; + boolean isFront; + + CameraServer cameraServer; @Override public void robotInit() { @@ -31,94 +30,42 @@ public class Robot extends IterativeRobot { intakeArm = new IntakeArm(); oi = new OI(); + isFront = true; - initializeSendableChoosers(); - addPositionChooserOptions(); - addDefensesToAllDefenseSendableChoosers(); - sendSendableChoosersToSmartDashboard(); - } + frontChooser = new SendableChooser(); + frontChooser.addDefault("Intake is front", false); + frontChooser.addObject("Shooter is front", true); - private void initializeSendableChoosers() { - positionChooser = new SendableChooser(); - positionOneDefense = new SendableChooser(); - positionTwoDefense = new SendableChooser(); - positionThreeDefense = new SendableChooser(); - positionFourDefense = new SendableChooser(); - positionFiveDefense = new SendableChooser(); - } + SmartDashboard.putData("Front chooser", frontChooser); - private void addPositionChooserOptions() { - positionChooser.addDefault("None", 0); - positionChooser.addObject("Position 1", 1); - positionChooser.addObject("Position 2", 2); - positionChooser.addObject("Position 3", 3); - positionChooser.addObject("Position 4", 4); - positionChooser.addObject("Position 5", 5); - } - - private void addDefensesToAllDefenseSendableChoosers() { - addDefenseOptions(positionOneDefense); - addDefenseOptions(positionTwoDefense); - addDefenseOptions(positionThreeDefense); - addDefenseOptions(positionFourDefense); - addDefenseOptions(positionFiveDefense); - } - - private void addDefenseOptions(SendableChooser chooser) { - chooser.addDefault("Portcullis", Defense.PORTCULLIS); - chooser.addObject("Sally Port", Defense.SALLY_PORT); - chooser.addObject("Rough Terrain" + Auton.ROUGH_TERRAIN_SPEED + " " - + Auton.ROUGH_TERRAIN_TIME, Defense.ROUGH_TERRAIN); - chooser.addObject("Low Bar" + " Will probably work...", Defense.LOW_BAR); - chooser.addObject("Chival De Frise", Defense.CHIVAL_DE_FRISE); - chooser.addObject("Drawbridge", Defense.DRAWBRIDGE); - chooser.addObject("Moat" + Auton.MOAT_SPEED + " " + Auton.MOAT_TIME, - Defense.MOAT); - chooser.addObject("Rock Wall" + Auton.ROCK_WALL_SPEED + " " - + Auton.ROCK_WALL_TIME, Defense.ROCK_WALL); - } - - private void sendSendableChoosersToSmartDashboard() { - SmartDashboard.putData("PositionChooser", positionChooser); - SmartDashboard.putData("Position 1 Defense Chooser", positionOneDefense); - SmartDashboard.putData("Position 2 Defense Chooser", positionTwoDefense); - SmartDashboard.putData("Position 3 Defense Chooser", positionThreeDefense); - SmartDashboard.putData("Position 4 Defense Chooser", positionFourDefense); - SmartDashboard.putData("Position 5 Defense Chooser", positionFiveDefense); + cameraServer = CameraServer.getInstance(); + cameraServer.setQuality(50); + cameraServer.startAutomaticCapture(); } @Override public void autonomousInit() { // get options chosen from drop down menu - Integer chosenPosition = (Integer) positionChooser.getSelected(); - Defense chosenDefense = null; - - if (chosenPosition == 1) - chosenDefense = (Defense) positionOneDefense.getSelected(); - else if (chosenPosition == 2) - chosenDefense = (Defense) positionTwoDefense.getSelected(); - else if (chosenPosition == 3) - chosenDefense = (Defense) positionThreeDefense.getSelected(); - else if (chosenPosition == 4) - chosenDefense = (Defense) positionFourDefense.getSelected(); - else if (chosenPosition == 5) - chosenDefense = (Defense) positionFiveDefense.getSelected(); - - if (chosenPosition != 0) - Scheduler.getInstance().add( - new ChooseStrategy(chosenPosition, chosenDefense)); - - // Scheduler.getInstance().add(new TimeDrive(.6, 4)); + + isFront = (boolean) frontChooser.getSelected(); + + if (!isFront) + driveTrain.toggleFlipped(); + + Scheduler.getInstance().add(new TimeDrive(.6, 4)); } @Override public void autonomousPeriodic() { + Scheduler.getInstance().run(); } @Override public void teleopInit() { + if (!isFront) + driveTrain.toggleFlipped(); Scheduler.getInstance().add(new SetLowGear()); } diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java index a3fed004..8f8a4f25 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java @@ -1,6 +1,5 @@ package org.usfirst.frc.team3501.robot.commands.driving; -import org.usfirst.frc.team3501.robot.OI; import org.usfirst.frc.team3501.robot.Robot; import edu.wpi.first.wpilibj.command.Command; @@ -21,9 +20,10 @@ public class JoystickDrive extends Command { @Override protected void execute() { - double left = -OI.leftJoystick.getY(); - double right = -OI.rightJoystick.getY(); - Robot.driveTrain.joystickDrive(left, right); + double thrust = Robot.oi.leftJoystick.getY(); + double twist = Robot.oi.leftJoystick.getTwist(); + + Robot.driveTrain.joystickDrive(thrust, twist); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java index e9a8f9f3..162f1864 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java @@ -3,11 +3,9 @@ package org.usfirst.frc.team3501.robot.commands.driving; import org.usfirst.frc.team3501.robot.Constants.Auton; import org.usfirst.frc.team3501.robot.Robot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; public class TimeDrive extends Command { - Timer timer; double currentTime, targetTime, speed; public TimeDrive() { @@ -21,7 +19,7 @@ public class TimeDrive extends Command { public TimeDrive(double time, double speed) { requires(Robot.driveTrain); - timer = new Timer(); + this.setTimeout(time); this.currentTime = 0; this.targetTime = time; this.speed = speed; @@ -29,21 +27,17 @@ public class TimeDrive extends Command { @Override protected void initialize() { - timer.start(); } @Override protected void execute() { - currentTime = timer.get(); + Robot.driveTrain.joystickDrive(speed, 0); - double output = speed * ((targetTime - currentTime) / (targetTime)); - - Robot.driveTrain.setMotorSpeeds(output, output); } @Override protected boolean isFinished() { - return currentTime >= targetTime; + return this.isTimedOut(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 1a4e6e5b..30b52efa 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -85,7 +85,7 @@ public class DriveTrain extends PIDSubsystem { } public void stop() { - setMotorSpeeds(0, 0); + drive(0, 0); } public void resetEncoders() { @@ -159,10 +159,10 @@ public class DriveTrain extends PIDSubsystem { /* * Method is a required method that the PID Subsystem uses to return the * calculated PID value to the driver - * + * * @param Gives the user the output from the PID algorithm that is calculated * internally - * + * * Body: Uses the output, does some filtering and drives the robot */ @Override @@ -174,7 +174,7 @@ public class DriveTrain extends PIDSubsystem { output = Math.signum(output) * 0.3; left = output; right = output + drift * Constants.DriveTrain.kp / 10; - setMotorSpeeds(left, right); + drive(left, right); pidOutput = output; } @@ -185,7 +185,7 @@ public class DriveTrain extends PIDSubsystem { /* * Checks the drive mode - * + * * @return the current state of the robot in each state Average distance from * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE */ @@ -199,7 +199,7 @@ public class DriveTrain extends PIDSubsystem { // Handle flipping of the "front" of the robot double k = (isFlipped() ? -1 : 1); - robotDrive.tankDrive(-left, -right); + drive(-left * k, -right * k); // if (type == Constants.DriveTrain.TANK) { // // TODO Test this for negation and for voltage vs. [-1,1] outputs @@ -220,9 +220,11 @@ public class DriveTrain extends PIDSubsystem { // } } - public void setMotorSpeeds(double left, double right) { + public void drive(double thrust, double twist) { + // Handle flipping of the "front" of the robot double k = (isFlipped() ? -1 : 1); - robotDrive.tankDrive(-left, -right); + + robotDrive.arcadeDrive(thrust, twist); } /**