From 9ca89e45fa84b2ec93bc6adf60c7dde1e0a7defb Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Sat, 11 Mar 2017 16:36:19 -0800 Subject: [PATCH] competition fixes --- .../usfirst/frc/team3501/robot/Constants.java | 26 ++++++------- src/org/usfirst/frc/team3501/robot/OI.java | 6 +++ src/org/usfirst/frc/team3501/robot/Robot.java | 39 ++++++++++++------- .../robot/commandgroups/AutonMiddleGear.java | 25 ++++-------- .../robot/commandgroups/AutonSideGear.java | 16 ++++++-- .../robot/commands/driving/DriveDistance.java | 2 +- .../commands/driving/ToggleDriveGear.java | 3 +- .../team3501/robot/subsystems/DriveTrain.java | 7 +++- .../team3501/robot/subsystems/Shooter.java | 25 +----------- 9 files changed, 71 insertions(+), 78 deletions(-) diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index f0cc6e2..4efaf21 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -18,18 +18,19 @@ public class Constants { public final static int TOGGLE_GEAR_PORT = 5; public final static int RUN_INTAKE_PORT = 1; - public final static int REVERSE_INTAKE_PORT = 4; + public final static int REVERSE_INTAKE_PORT = 2; public final static int RUN_INDEXWHEEL_PORT = 1; public final static int REVERSE_INDEXWHEEL_PORT = 2; public static final int BRAKE_CANTALONS_PORT = 5; - public static final int COAST_CANTALONS_PORT = 3; - - public final static int TOGGLE_FLYWHEEL_PORT = 4; - public static final int REVERSE_FLYWHEEL_PORT = 1; - public static final int INCREASE_SHOOTER_SPEED_PORT = 6; - public static final int DECREASE_SHOOTER_SPEED_PORT = 2; - public static final int TOGGLE_GEAR_MANIPULATOR_PORT = 3; + public static final int COAST_CANTALONS_PORT = 6; + + public final static int TOGGLE_FLYWHEEL_PORT = 1; + public static final int REVERSE_FLYWHEEL_PORT = 3; + public static final int INCREASE_SHOOTER_SPEED_PORT = 8; + public static final int DECREASE_SHOOTER_SPEED_PORT = 7; + public static final int RESET_SHOOTER_SPEED_PORT = 5; + public static final int TOGGLE_GEAR_MANIPULATOR_PORT = 2; } public static class Shooter { @@ -39,19 +40,14 @@ public class Constants { public static final int INDEX_WHEEL = 7; public final static int HALL_EFFECT_PORT = 9; - - public static final int MODULE_NUMBER = 10, PISTON_FORWARD = 4, - PISTON_REVERSE = 5; - public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward; - public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse; } public static class DriveTrain { public static final int PISTON_MODULE = 10; public static final int GEAR_MANIPULATOR_PISTON_FORWARD = 4, GEAR_MANIPULATOR_PISTON_REVERSE = 5; - public static final int LEFT_GEAR_PISTON_FORWARD = 0, - LEFT_GEAR_PISTON_REVERSE = 1, RIGHT_GEAR_PISTON_FORWARD = 3, + public static final int LEFT_GEAR_PISTON_FORWARD = 1, + LEFT_GEAR_PISTON_REVERSE = 0, RIGHT_GEAR_PISTON_FORWARD = 3, RIGHT_GEAR_PISTON_REVERSE = 2; public static final Value FORWARD_PISTON_VALUE = DoubleSolenoid.Value.kForward; public static final Value REVERSE_PISTON_VALUE = DoubleSolenoid.Value.kReverse; diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index 9f8ea7a..61f39c3 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -8,6 +8,7 @@ import org.usfirst.frc.team3501.robot.commands.intake.ReverseIntakeContinuous; import org.usfirst.frc.team3501.robot.commands.intake.RunIntakeContinuous; import org.usfirst.frc.team3501.robot.commands.shooter.DecreaseShootingSpeed; import org.usfirst.frc.team3501.robot.commands.shooter.IncreaseShootingSpeed; +import org.usfirst.frc.team3501.robot.commands.shooter.ResetShootingSpeed; import org.usfirst.frc.team3501.robot.commands.shooter.ReverseFlyWheelContinuous; import org.usfirst.frc.team3501.robot.commands.shooter.ReverseIndexWheelContinuous; import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous; @@ -36,6 +37,7 @@ public class OI { public static Button increaseShooterSpeed; public static Button decreaseShooterSpeed; + public static Button resetShooterSpeed; public static Button brakeCANTalons; public static Button coastCANTalons; @@ -84,6 +86,10 @@ public class OI { Constants.OI.DECREASE_SHOOTER_SPEED_PORT); decreaseShooterSpeed.whenPressed(new DecreaseShootingSpeed()); + resetShooterSpeed = new JoystickButton(gamePad, + Constants.OI.RESET_SHOOTER_SPEED_PORT); + resetShooterSpeed.whenPressed(new ResetShootingSpeed()); + brakeCANTalons = new JoystickButton(rightJoystick, Constants.OI.BRAKE_CANTALONS_PORT); brakeCANTalons.whenPressed(new BrakeCANTalons()); diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index a9fb3bf..d551429 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -1,5 +1,7 @@ package org.usfirst.frc.team3501.robot; +import org.usfirst.frc.team3501.robot.commandgroups.AutonMiddleGear; +import org.usfirst.frc.team3501.robot.commandgroups.AutonSideGear; import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; import org.usfirst.frc.team3501.robot.subsystems.Intake; import org.usfirst.frc.team3501.robot.subsystems.Shooter; @@ -8,7 +10,9 @@ import edu.wpi.cscore.UsbCamera; import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Robot extends IterativeRobot { @@ -17,6 +21,9 @@ public class Robot extends IterativeRobot { private static OI oi; private static Intake intake; + Command autonCommand; + SendableChooser autonChooser; + @Override public void robotInit() { driveTrain = DriveTrain.getDriveTrain(); @@ -24,6 +31,18 @@ public class Robot extends IterativeRobot { shooter = Shooter.getShooter(); intake = Intake.getIntake(); + autonChooser = new SendableChooser(); + autonChooser.addDefault("Middle Gear", new AutonMiddleGear()); + autonChooser.addObject("Red Boiler Gear", + new AutonSideGear("RED", "BOILER")); + autonChooser.addObject("Red Retrieval Gear", + new AutonSideGear("RED", "RETRIEVAL")); + autonChooser.addObject("Blue Boiler Gear", + new AutonSideGear("BLUE", "BOILER")); + autonChooser.addObject("Blue Retrieval Gear", + new AutonSideGear("BLUE", "RETRIEVAL")); + SmartDashboard.putData("Autonomous Chooser", autonChooser); + CameraServer server = CameraServer.getInstance(); UsbCamera climberCam = server.startAutomaticCapture("climbercam", 0); UsbCamera intakeCam = server.startAutomaticCapture("intakecam", 1); @@ -47,12 +66,14 @@ public class Robot extends IterativeRobot { return Intake.getIntake(); } - // If the gear values do not match in the left and right piston, then they are - // both set to high gear @Override public void autonomousInit() { - driveTrain.setHighGear(); + // driveTrain.setLowGear(); driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE); + + // autonCommand = (Command) autonChooser.getSelected(); + // autonCommand = new TimeDrive(1.5, 0.6); + // Scheduler.getInstance().add(autonCommand); } @Override @@ -62,26 +83,16 @@ public class Robot extends IterativeRobot { @Override public void teleopInit() { + // driveTrain.setHighGear(); driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_COAST_MODE); } @Override public void teleopPeriodic() { - driveTrain.printEncoderOutput(); Scheduler.getInstance().run(); updateSmartDashboard(); } - @Override - public void disabledInit() { - driveTrain.setCANTalonsBrakeMode(driveTrain.DRIVE_BRAKE_MODE); - } - // - // @Override - // public void disabledPeriodic() { - // Scheduler.getInstance().add(new RunFlyWheel(2)); - // } - public void updateSmartDashboard() { SmartDashboard.putNumber("left encode ", driveTrain.getLeftEncoderDistance()); diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java index 913bbd7..4880baf 100644 --- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java +++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java @@ -1,12 +1,9 @@ package org.usfirst.frc.team3501.robot.commandgroups; -import org.usfirst.frc.team3501.robot.Constants.Direction; import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; -import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.wpilibj.command.WaitCommand; /** * @@ -36,20 +33,14 @@ public class AutonMiddleGear extends CommandGroup { * direction to turn after placing gear on peg in order to cross the * baseline. Only Direction.LEFT and Direction.RIGHT will be accepted */ - public AutonMiddleGear(Direction direction) { + public AutonMiddleGear() { addSequential(new DriveDistance(DISTANCE_TO_PEG, maxTimeOut)); - addSequential(new WaitCommand(3)); - addSequential(new DriveDistance(DISTANCE_TO_BACK_OUT, maxTimeOut)); - addSequential(new TurnForAngle(ANGLE_TO_TURN, direction, maxTimeOut)); - addSequential(new DriveDistance(THIRD_DISTANCE_TO_TRAVEL, maxTimeOut)); - addSequential( - new TurnForAngle(ANGLE_TO_TURN, oppositeOf(direction), maxTimeOut)); - addSequential(new DriveDistance(DISTANCE_TO_BASELINE, maxTimeOut)); - } - - private Direction oppositeOf(Direction direction) { - if (direction == Direction.LEFT) - return Direction.RIGHT; - return Direction.LEFT; + // addSequential(new WaitCommand(3)); + // addSequential(new DriveDistance(DISTANCE_TO_BACK_OUT, maxTimeOut)); + // addSequential(new TurnForAngle(ANGLE_TO_TURN, direction, maxTimeOut)); + // addSequential(new DriveDistance(THIRD_DISTANCE_TO_TRAVEL, maxTimeOut)); + // addSequential( + // new TurnForAngle(ANGLE_TO_TURN, oppositeOf(direction), maxTimeOut)); + // addSequential(new DriveDistance(DISTANCE_TO_BASELINE, maxTimeOut)); } } diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonSideGear.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonSideGear.java index 386dfe5..70f7226 100644 --- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonSideGear.java +++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonSideGear.java @@ -34,7 +34,7 @@ public class AutonSideGear extends CommandGroup { * inches and 205.7286 inches from the right side of the arena this program * chooses which peg to go for based on the starting point */ - public AutonSideGear(String side) { + public AutonSideGear(String team, String side) { requires(Robot.getDriveTrain()); if (side.equals("BOILER")) { @@ -42,7 +42,12 @@ public class AutonSideGear extends CommandGroup { 131.6 - (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3) - ROBOT_LENGTH / 2, 5)); - addSequential(new TurnForAngle(60, Direction.RIGHT, 5)); + + if (team.equals("RED")) + addSequential(new TurnForAngle(60, Direction.LEFT, 5)); + else if (team.equals("BLUE")) + addSequential(new TurnForAngle(60, Direction.RIGHT, 5)); + addSequential(new DriveDistance( 2 * (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3) - ROBOT_LENGTH / 2 + 7, @@ -52,7 +57,12 @@ public class AutonSideGear extends CommandGroup { 131.6 - (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3) - ROBOT_LENGTH / 2, 5)); - addSequential(new TurnForAngle(60, Direction.LEFT, 5)); + + if (team.equals("RED")) + addSequential(new TurnForAngle(60, Direction.RIGHT, 5)); + else if (team.equals("BLUE")) + addSequential(new TurnForAngle(60, Direction.LEFT, 5)); + addSequential(new DriveDistance( 2 * (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3) - ROBOT_LENGTH / 2, diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java index 582cd80..7b721e0 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java @@ -52,7 +52,7 @@ public class DriveDistance extends Command { @Override protected void execute() { double xVal = gyroP * (driveTrain.getAngle() - zeroAngle); - double yVal = driveController.calcPID(driveTrain.getAvgEncoderDistance()); + double yVal = driveController.calcPID(driveTrain.getRightEncoderDistance()); double leftDrive = yVal - xVal; double rightDrive = yVal + xVal; diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java index ee15ec8..e3c0dcc 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ToggleDriveGear.java @@ -27,9 +27,8 @@ public class ToggleDriveGear extends Command { @Override protected void execute() { - Value leftPistonValue = driveTrain.getLeftDriveTrainPiston(); Value rightPistonValue = driveTrain.getRightDriveTrainPiston(); - if (leftPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) { + if (rightPistonValue == Constants.DriveTrain.REVERSE_PISTON_VALUE) { driveTrain.setHighGear(); } else { driveTrain.setLowGear(); diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index a6a6f8c..b7264f1 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -14,7 +14,7 @@ import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.command.Subsystem; public class DriveTrain extends Subsystem { - public static double driveP = 0.012, driveI = 0.0011, driveD = -0.002; + public static double driveP = 0.01, driveI = 0.00115, driveD = -0.002; public static double smallTurnP = 0.004, smallTurnI = 0.0013, smallTurnD = 0.005; public static double largeTurnP = .003, largeTurnI = .0012, largeTurnD = .006; @@ -101,7 +101,10 @@ public class DriveTrain extends Subsystem { } public void joystickDrive(final double thrust, final double twist) { - robotDrive.arcadeDrive(thrust, twist, true); + if ((thrust < 0.1 && thrust > -0.1) && (twist < 0.1 && twist > -0.1)) + robotDrive.arcadeDrive(0, 0, true); + else + robotDrive.arcadeDrive(thrust, twist, true); } public void stop() { diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java index 85ecf46..ab36fdd 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java @@ -7,8 +7,6 @@ import org.usfirst.frc.team3501.robot.utils.PIDController; import com.ctre.CANTalon; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.command.Subsystem; public class Shooter extends Subsystem { @@ -28,17 +26,12 @@ public class Shooter extends Subsystem { private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED; private double currentShooterMotorValue = 0; - private final DoubleSolenoid piston; - private Shooter() { flyWheel1 = new CANTalon(Constants.Shooter.FLY_WHEEL1); flyWheel2 = new CANTalon(Constants.Shooter.FLY_WHEEL2); indexWheel = new CANTalon(Constants.Shooter.INDEX_WHEEL); hallEffect = new HallEffectSensor(Constants.Shooter.HALL_EFFECT_PORT, 1); - - piston = new DoubleSolenoid(Constants.Shooter.MODULE_NUMBER, - Constants.Shooter.PISTON_FORWARD, Constants.Shooter.PISTON_REVERSE); } /** @@ -60,7 +53,7 @@ public class Shooter extends Subsystem { * motor value from -1 to 1(fastest forward) */ public void setFlyWheelMotorVal(double val) { - val = MathLib.restrictToRange(val, 0.0, 1.0); + val = MathLib.restrictToRange(val, -1.0, 1.0); flyWheel1.set(val); flyWheel2.set(val); } @@ -149,22 +142,6 @@ public class Shooter extends Subsystem { this.currentShooterMotorValue = 0; } - public Value getPistonValue() { - return piston.get(); - } - - public void setHighGear() { - changeGear(Constants.Shooter.HIGH_GEAR); - } - - public void setLowGear() { - changeGear(Constants.Shooter.LOW_GEAR); - } - - private void changeGear(DoubleSolenoid.Value gear) { - piston.set(gear); - } - public void reverseFlyWheel() { this.setFlyWheelMotorVal(shooter.REVERSE_FLYWHEEL_MOTOR_VALUE); } -- 2.30.2