From 1b977a14046555f6ae3bb3d09cedf543e049ddc7 Mon Sep 17 00:00:00 2001 From: Harel Dor Date: Fri, 25 Mar 2016 10:13:58 -0700 Subject: [PATCH] Add camera, change to arcade drive, front chooser only, minor driving fixes --- src/org/usfirst/frc/team3501/robot/Robot.java | 96 +++++-------------- .../robot/commands/driving/JoystickDrive.java | 4 +- .../robot/commands/driving/TimeDrive.java | 14 +-- .../team3501/robot/subsystems/DriveTrain.java | 27 +----- 4 files changed, 34 insertions(+), 107 deletions(-) diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 6bf02a4c..2b58d19d 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; @@ -20,9 +19,8 @@ public class Robot extends IterativeRobot { public static IntakeArm intakeArm; // Sendable Choosers send a drop down menu to the Smart Dashboard. - SendableChooser positionChooser; - SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense, - positionFourDefense, positionFiveDefense; + private static SendableChooser frontChooser; + private static CameraServer camera; @Override public void robotInit() { @@ -32,84 +30,38 @@ public class Robot extends IterativeRobot { oi = new OI(); - initializeSendableChoosers(); - addPositionChooserOptions(); - addDefensesToAllDefenseSendableChoosers(); - sendSendableChoosersToSmartDashboard(); - } - - private void initializeSendableChoosers() { - positionChooser = new SendableChooser(); - positionOneDefense = new SendableChooser(); - positionTwoDefense = new SendableChooser(); - positionThreeDefense = new SendableChooser(); - positionFourDefense = new SendableChooser(); - positionFiveDefense = new SendableChooser(); - } + initializeSendableChooser(); + addFrontChooserOptions(); + sendSendableChooserToSmartDashboard(); - 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); + camera = CameraServer.getInstance(); + camera.setQuality(50); + camera.startAutomaticCapture("cam0"); } - private void addDefensesToAllDefenseSendableChoosers() { - addDefenseOptions(positionOneDefense); - addDefenseOptions(positionTwoDefense); - addDefenseOptions(positionThreeDefense); - addDefenseOptions(positionFourDefense); - addDefenseOptions(positionFiveDefense); + private void initializeSendableChooser() { + frontChooser = new SendableChooser(); } - 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 addFrontChooserOptions() { + frontChooser.addDefault("Intake", false); + frontChooser.addObject("Shooter", true); } - 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); - + private void sendSendableChooserToSmartDashboard() { + SmartDashboard.putData("PositionChooser", frontChooser); } @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)); + boolean flip = (boolean) frontChooser.getSelected(); + + if (flip) { + driveTrain.toggleFlipped(); + } + + Scheduler.getInstance().add(new TimeDrive(4, .7)); } @Override 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..c246ccfa 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java @@ -22,8 +22,8 @@ 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 right = -OI.rightJoystick.getX(); + Robot.driveTrain.drive(left, right); } @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..4778da85 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java @@ -20,30 +20,24 @@ public class TimeDrive extends Command { public TimeDrive(double time, double speed) { requires(Robot.driveTrain); + this.setInterruptible(false); - timer = new Timer(); - this.currentTime = 0; - this.targetTime = time; + this.setTimeout(time); this.speed = speed; } @Override protected void initialize() { - timer.start(); } @Override protected void execute() { - currentTime = timer.get(); - - double output = speed * ((targetTime - currentTime) / (targetTime)); - - Robot.driveTrain.setMotorSpeeds(output, output); + Robot.driveTrain.drive(speed, 0); } @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..f1daa637 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -193,36 +193,17 @@ public class DriveTrain extends PIDSubsystem { return getAvgEncoderDistance(); } - public void joystickDrive(double left, double right) { - int type = Constants.DriveTrain.DRIVE_TYPE; - + public void drive(double left, double right) { // Handle flipping of the "front" of the robot double k = (isFlipped() ? -1 : 1); - robotDrive.tankDrive(-left, -right); - - // if (type == Constants.DriveTrain.TANK) { - // // TODO Test this for negation and for voltage vs. [-1,1] outputs - // double leftSpeed = (-frontLeft.get() + -rearLeft.get()) / 2; - // double rightSpeed = (-frontRight.get() + -rearRight.get()) / 2; - // left = ((Constants.DriveTrain.kADJUST - 1) * leftSpeed + left) - // / Constants.DriveTrain.kADJUST; - // right = ((Constants.DriveTrain.kADJUST - 1) * rightSpeed + right) - // / Constants.DriveTrain.kADJUST; - // robotDrive.tankDrive(-left * k, -right * k); - // } else if (type == Constants.DriveTrain.ARCADE) { - // double speed = (-frontLeft.get() + -rearLeft.get() + -frontRight.get() + - // -rearRight - // .get()) / 2; - // left = ((Constants.DriveTrain.kADJUST - 1) * speed + left) - // / Constants.DriveTrain.kADJUST; - // robotDrive.arcadeDrive(left * k, right); - // } + // During teleop, leftY is throttle, rightX is twist. + robotDrive.arcadeDrive(-left * k, -right * k); } public void setMotorSpeeds(double left, double right) { double k = (isFlipped() ? -1 : 1); - robotDrive.tankDrive(-left, -right); + robotDrive.tankDrive(-left * k, -right * k); } /** -- 2.30.2