From: Cindy Zhang Date: Fri, 16 Jun 2017 19:48:26 +0000 (-0700) Subject: competition fixes X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=commitdiff_plain;h=f74d236db406193b851bff99e4daec7b7abf35e7 competition fixes --- diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index 48ec242..2064424 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -16,11 +16,9 @@ import edu.wpi.first.wpilibj.SPI; public class Constants { public static class OI { public final static int XBOX_CONTROLLER_PORT = 0; - // public final static int RIGHT_STICK_PORT = 1; public static final int GAME_PAD_PORT = 2; // Xbox Controller Ports - // public final static int TOGGLE_GEAR_PORT = 5; public final static int SHIFT_LOW_PORT = 9; public final static int SHIFT_HIGH_PORT = 10; public final static int RUN_INTAKE_PORT = 6; @@ -30,14 +28,21 @@ public class Constants { public static final int BRAKE_CANTALONS_PORT = 1; public static final int COAST_CANTALONS_PORT = 3; public static final int CLIMB_PORT = 4; + public static final int STOP_CLIMB_PORT = 2; // Game Pad Ports - public final static int TOGGLE_FLYWHEEL_PORT = 1; - public static final int REVERSE_FLYWHEEL_PORT = 3; + public final static int RUN_FLYWHEEL_PORT = 1; + public static final int STOP_FLYWHEEL_PORT = 3; + public static final int REVERSE_FLYWHEEL_PORT = 4; 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 final int RESET_SHOOTER_SPEED_PORT = 2; + public static final int SHIFT_GEAR_MANIPULATOR_HIGH_PORT = 6; + public static final int SHIFT_GEAR_MANIPULATOR_LOW_PORT = 5; + + // 5 is piston out (gear manipulator) + // 6 is piston in (gear manipulator) } diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index c53369e..d2ab674 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -2,10 +2,12 @@ package org.usfirst.frc.team3501.robot; import org.usfirst.frc.team3501.robot.commands.climber.BrakeCANTalons; import org.usfirst.frc.team3501.robot.commands.climber.CoastCANTalons; -import org.usfirst.frc.team3501.robot.commands.climber.ToggleWinch; +import org.usfirst.frc.team3501.robot.commands.climber.RunWinch; +import org.usfirst.frc.team3501.robot.commands.climber.StopWinch; import org.usfirst.frc.team3501.robot.commands.driving.ShiftDriveHighGear; import org.usfirst.frc.team3501.robot.commands.driving.ShiftDriveLowGear; -import org.usfirst.frc.team3501.robot.commands.driving.ToggleGearManipulatorPiston; +import org.usfirst.frc.team3501.robot.commands.driving.ShiftGearManipulatorPistonHigh; +import org.usfirst.frc.team3501.robot.commands.driving.ShiftGearManipulatorPistonLow; 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; @@ -15,6 +17,7 @@ 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; import org.usfirst.frc.team3501.robot.commands.shooter.RunIndexWheelContinuous; +import org.usfirst.frc.team3501.robot.commands.shooter.StopFlyWheel; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.Button; @@ -28,14 +31,15 @@ public class OI { public static Button runIndexWheel; public static Button reverseIndexWheel; - public static Button toggleFlyWheel; + public static Button runFlyWheel; + public static Button stopFlyWheel; public static Button reverseFlyWheel; // public static Button toggleGear; public static Button shiftHigh; public static Button shiftLow; - public static Button toggleGearManipulatorPiston; + // public static Button toggleGearManipulatorPiston; public static Button runIntake; public static Button reverseIntake; @@ -47,6 +51,10 @@ public class OI { public static Button brakeCANTalons; public static Button coastCANTalons; public static Button climb; + public static Button stopclimb; + + public static Button shiftGearManipulatorPistonHigh; + public static Button shiftGearManipulatorPistonLow; public OI() { xboxController = new Joystick(Constants.OI.XBOX_CONTROLLER_PORT); @@ -61,18 +69,16 @@ public class OI { Constants.OI.REVERSE_INDEXWHEEL_PORT); reverseIndexWheel.whileHeld(new ReverseIndexWheelContinuous()); - toggleFlyWheel = new JoystickButton(gamePad, - Constants.OI.TOGGLE_FLYWHEEL_PORT); - toggleFlyWheel.toggleWhenPressed(new RunFlyWheelContinuous()); + runFlyWheel = new JoystickButton(gamePad, Constants.OI.RUN_FLYWHEEL_PORT); + runFlyWheel.whenPressed(new RunFlyWheelContinuous()); + + stopFlyWheel = new JoystickButton(gamePad, Constants.OI.STOP_FLYWHEEL_PORT); + stopFlyWheel.whenPressed(new StopFlyWheel()); reverseFlyWheel = new JoystickButton(gamePad, Constants.OI.REVERSE_FLYWHEEL_PORT); reverseFlyWheel.whileHeld(new ReverseFlyWheelContinuous()); - // toggleGear = new JoystickButton(xboxController, - // Constants.OI.TOGGLE_GEAR_PORT); - // toggleGear.whenPressed(new ToggleDriveGear()); - shiftHigh = new JoystickButton(xboxController, Constants.OI.SHIFT_HIGH_PORT); shiftHigh.whenPressed(new ShiftDriveHighGear()); @@ -80,9 +86,22 @@ public class OI { shiftLow = new JoystickButton(xboxController, Constants.OI.SHIFT_LOW_PORT); shiftLow.whenPressed(new ShiftDriveLowGear()); - toggleGearManipulatorPiston = new JoystickButton(gamePad, - Constants.OI.TOGGLE_GEAR_MANIPULATOR_PORT); - toggleGearManipulatorPiston.whenPressed(new ToggleGearManipulatorPiston()); + /* + * toggleGearManipulatorPiston = new JoystickButton(gamePad, + * Constants.OI.TOGGLE_GEAR_MANIPULATOR_PORT); + * toggleGearManipulatorPiston.whenPressed(new + * ToggleGearManipulatorPiston()); + */ + + shiftGearManipulatorPistonHigh = new JoystickButton(gamePad, + Constants.OI.SHIFT_GEAR_MANIPULATOR_HIGH_PORT); + shiftGearManipulatorPistonHigh + .whenPressed(new ShiftGearManipulatorPistonHigh()); + + shiftGearManipulatorPistonLow = new JoystickButton(gamePad, + Constants.OI.SHIFT_GEAR_MANIPULATOR_LOW_PORT); + shiftGearManipulatorPistonLow + .whenPressed(new ShiftGearManipulatorPistonLow()); runIntake = new JoystickButton(xboxController, Constants.OI.RUN_INTAKE_PORT); @@ -113,7 +132,11 @@ public class OI { coastCANTalons.whenPressed(new CoastCANTalons()); climb = new JoystickButton(xboxController, Constants.OI.CLIMB_PORT); - climb.whenPressed(new ToggleWinch()); + climb.whenPressed(new RunWinch()); + + stopclimb = new JoystickButton(xboxController, + Constants.OI.STOP_CLIMB_PORT); + stopclimb.whenPressed(new StopWinch()); } public static OI getOI() { diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 8151cde..c82f09f 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -2,7 +2,6 @@ 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.commands.driving.TimeDrive; import org.usfirst.frc.team3501.robot.subsystems.Climber; import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; import org.usfirst.frc.team3501.robot.subsystems.Intake; @@ -77,7 +76,7 @@ public class Robot extends IterativeRobot { driveTrain.setLowGear(); // autonCommand = (Command) autonChooser.getSelected(); - autonCommand = new TimeDrive(2, 0.6); + autonCommand = new AutonMiddleGear(); Scheduler.getInstance().add(autonCommand); } diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java index 4880baf..2a1a7a6 100644 --- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java +++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java @@ -1,7 +1,8 @@ package org.usfirst.frc.team3501.robot.commandgroups; -import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; +import org.usfirst.frc.team3501.robot.commands.driving.ShiftGearManipulatorPistonLow; +import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -17,12 +18,6 @@ import edu.wpi.first.wpilibj.command.CommandGroup; */ public class AutonMiddleGear extends CommandGroup { private static final double DISTANCE_TO_PEG = 91.3 - 32; - private static final double DISTANCE_TO_BACK_OUT = -29.75; - private static final double THIRD_DISTANCE_TO_TRAVEL = 70; - private static final double DISTANCE_TO_BASELINE = 50.5; - - private static final double ANGLE_TO_TURN = 90; - private static final double maxTimeOut = 7; /*** @@ -34,13 +29,9 @@ public class AutonMiddleGear extends CommandGroup { * baseline. Only Direction.LEFT and Direction.RIGHT will be accepted */ 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)); + // addSequential(new DriveDistance(DISTANCE_TO_PEG, maxTimeOut)); + + addSequential(new ShiftGearManipulatorPistonLow()); + addSequential(new TimeDrive(2, 0.6)); } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinch.java index 5a76b5c..39149ab 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinch.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinch.java @@ -37,7 +37,7 @@ public class RunWinch extends Command { * @param motorVal * value range is from -1 to 1 */ - public RunWinch(double time, double motorVal) { + public RunWinch() { requires(climber); this.time = time; this.motorVal = motorVal; @@ -45,16 +45,18 @@ public class RunWinch extends Command { @Override protected void initialize() { + climber.setCANTalonsBrakeMode(climber.COAST_MODE); } @Override protected void execute() { - climber.setMotorValues(motorVal); + climber.setMotorValues(climber.CLIMBER_SPEED); } @Override protected boolean isFinished() { - return timeSinceInitialized() >= time; + // return timeSinceInitialized() >= time; + return false; } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java index 664b201..0772ffb 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/RunWinchContinuous.java @@ -35,6 +35,7 @@ public class RunWinchContinuous extends Command { @Override protected void initialize() { + climber.setCANTalonsBrakeMode(climber.COAST_MODE); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/StopWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/StopWinch.java index 0a2b5f9..2bb5752 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/StopWinch.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/StopWinch.java @@ -34,6 +34,7 @@ public class StopWinch extends Command { @Override protected void end() { climber.stop(); + climber.setCANTalonsBrakeMode(climber.BRAKE_MODE); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java index 918481c..0db401c 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java +++ b/src/org/usfirst/frc/team3501/robot/commands/climber/ToggleWinch.java @@ -16,6 +16,7 @@ public class ToggleWinch extends Command { @Override protected void initialize() { + System.out.println("toggled"); } @Override @@ -23,9 +24,11 @@ public class ToggleWinch extends Command { if (climber.shouldBeClimbing) { climber.setCANTalonsBrakeMode(climber.COAST_MODE); climber.setMotorValues(climbingSpeed); + System.out.println("climbing"); } else { climber.setCANTalonsBrakeMode(climber.BRAKE_MODE); + System.out.println("braked"); /* Not sure if should have */ climber.stop(); end(); @@ -40,6 +43,7 @@ public class ToggleWinch extends Command { @Override protected void end() { climber.shouldBeClimbing = !climber.shouldBeClimbing; + System.out.println("ended"); } @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 aa9bfaf..006a13c 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java @@ -13,6 +13,9 @@ import edu.wpi.first.wpilibj.command.Command; */ public class JoystickDrive extends Command { + double previousThrust = 0; + double previousTwist = 0; + public JoystickDrive() { requires(Robot.getDriveTrain()); } @@ -23,15 +26,16 @@ public class JoystickDrive extends Command { @Override protected void execute() { - final double thrust = OI.xboxController.getY(); - final double twist = OI.xboxController.getAxis(AxisType.kZ); + double thrust = OI.xboxController.getY(); + double twist = OI.xboxController.getAxis(AxisType.kZ); - Robot.getDriveTrain().joystickDrive(-thrust, -twist); + thrust = (6 * previousThrust + thrust) / 7; + twist = (6 * previousTwist + twist) / 7; - /* - * double left = OI.leftJoystick.getY(); double right = - * OI.rightJoystick.getY(); Robot.getDriveTrain().tankDrive(-left, -right); - */ + previousThrust = thrust; + previousTwist = twist; + + Robot.getDriveTrain().joystickDrive(-thrust, -twist); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ShiftGearManipulatorPistonHigh.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ShiftGearManipulatorPistonHigh.java new file mode 100644 index 0000000..a16d4d7 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ShiftGearManipulatorPistonHigh.java @@ -0,0 +1,40 @@ +package org.usfirst.frc.team3501.robot.commands.driving; + +import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; + +import edu.wpi.first.wpilibj.command.Command; + +public class ShiftGearManipulatorPistonHigh extends Command { + private DriveTrain driveTrain = Robot.getDriveTrain(); + + public ShiftGearManipulatorPistonHigh() { + } + + @Override + protected void initialize() { + + } + + @Override + protected void execute() { + driveTrain.extendGearManipulatorPiston(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/ShiftGearManipulatorPistonLow.java b/src/org/usfirst/frc/team3501/robot/commands/driving/ShiftGearManipulatorPistonLow.java new file mode 100644 index 0000000..0b482d1 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/ShiftGearManipulatorPistonLow.java @@ -0,0 +1,40 @@ +package org.usfirst.frc.team3501.robot.commands.driving; + +import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; + +import edu.wpi.first.wpilibj.command.Command; + +public class ShiftGearManipulatorPistonLow extends Command { + private DriveTrain driveTrain = Robot.getDriveTrain(); + + public ShiftGearManipulatorPistonLow() { + } + + @Override + protected void initialize() { + + } + + @Override + protected void execute() { + driveTrain.retractGearManipulatorPiston(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/intake/ReverseIntakeContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/intake/ReverseIntakeContinuous.java index f446316..699559d 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/intake/ReverseIntakeContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/intake/ReverseIntakeContinuous.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3501.robot.commands.intake; import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.Intake; import edu.wpi.first.wpilibj.command.Command; @@ -11,9 +12,13 @@ import edu.wpi.first.wpilibj.command.Command; * button.whileHeld(...). */ public class ReverseIntakeContinuous extends Command { + private Intake intake = Robot.getIntake(); + + private double previousMotorValue = 0; + private double targetMotorValue = intake.REVERSE_SPEED; public ReverseIntakeContinuous() { - requires(Robot.getIntake()); + requires(intake); } // Called just before this Command runs the first time @@ -24,7 +29,9 @@ public class ReverseIntakeContinuous extends Command { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.getIntake().runReverseIntake(); + double motorValue = (6 * previousMotorValue + targetMotorValue) / 7; + previousMotorValue = motorValue; + intake.setSpeed(motorValue); } // Make this return true when this Command no longer needs to run execute() @@ -36,7 +43,7 @@ public class ReverseIntakeContinuous extends Command { // Called once after isFinished returns true @Override protected void end() { - Robot.getIntake().stopIntake(); + intake.stopIntake(); } // Called when another command which requires one or more of the same diff --git a/src/org/usfirst/frc/team3501/robot/commands/intake/RunIntakeContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/intake/RunIntakeContinuous.java index 3188fe6..5d26e02 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/intake/RunIntakeContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/intake/RunIntakeContinuous.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3501.robot.commands.intake; import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.Intake; import edu.wpi.first.wpilibj.command.Command; @@ -15,11 +16,13 @@ import edu.wpi.first.wpilibj.command.Command; * */ public class RunIntakeContinuous extends Command { - // create setter method for speed, use setSpeed method to do end() by setting - // speed to 0 + private Intake intake = Robot.getIntake(); + + private double previousMotorValue = 0; + private double targetMotorValue = intake.INTAKE_SPEED; public RunIntakeContinuous() { - requires(Robot.getIntake()); + requires(intake); } @Override @@ -30,13 +33,14 @@ public class RunIntakeContinuous extends Command { // Called just before this Command runs the first time @Override protected void initialize() { - } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.getIntake().runIntake(); + double motorValue = (6 * previousMotorValue + targetMotorValue) / 7; + previousMotorValue = motorValue; + intake.setSpeed(motorValue); } // Called once after isFinished returns true diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseIndexWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseIndexWheelContinuous.java index 4cb18f9..6c6400e 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseIndexWheelContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/ReverseIndexWheelContinuous.java @@ -20,6 +20,9 @@ import edu.wpi.first.wpilibj.command.Command; public class ReverseIndexWheelContinuous extends Command { private Shooter shooter = Robot.getShooter(); + private double previousMotorValue = 0; + private double targetMotorValue = -shooter.DEFAULT_INDEXING_MOTOR_VALUE; + /** * See JavaDoc comment in class for details * @@ -27,7 +30,6 @@ public class ReverseIndexWheelContinuous extends Command { * value range from -1 to 1 */ public ReverseIndexWheelContinuous() { - requires(shooter); } // Called just before this Command runs the first time @@ -38,7 +40,9 @@ public class ReverseIndexWheelContinuous extends Command { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - shooter.reverseIndexWheel(); + double motorValue = (6 * previousMotorValue + targetMotorValue) / 7; + previousMotorValue = motorValue; + shooter.setIndexWheelMotorVal(motorValue); } // Called once after isFinished returns true diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheelContinuous.java index 86c31f7..f751f5b 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheelContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunFlyWheelContinuous.java @@ -24,6 +24,7 @@ public class RunFlyWheelContinuous extends Command { private PIDController wheelController; public RunFlyWheelContinuous() { + requires(shooter); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java index 815577e..1cd3e77 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java @@ -20,11 +20,13 @@ import edu.wpi.first.wpilibj.command.Command; public class RunIndexWheelContinuous extends Command { private Shooter shooter = Robot.getShooter(); + private double previousMotorValue = 0; + private double targetMotorValue = shooter.DEFAULT_INDEXING_MOTOR_VALUE; + /** * See JavaDoc comment in class for details */ public RunIndexWheelContinuous() { - requires(shooter); } @Override @@ -36,8 +38,11 @@ public class RunIndexWheelContinuous extends Command { double shooterSpeed = shooter.getShooterRPM(); double targetShooterSpeed = shooter.getTargetShootingSpeed(); double threshold = shooter.getRPMThreshold(); - if (Math.abs(shooterSpeed - targetShooterSpeed) <= threshold) - shooter.runIndexWheel(); + if (Math.abs(shooterSpeed - targetShooterSpeed) <= threshold) { + double motorValue = (6 * previousMotorValue + targetMotorValue) / 7; + previousMotorValue = motorValue; + shooter.setIndexWheelMotorVal(motorValue); + } } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/StopFlyWheel.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/StopFlyWheel.java index cbce005..0d1bbe8 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/StopFlyWheel.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/StopFlyWheel.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3501.robot.commands.shooter; import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.Shooter; import edu.wpi.first.wpilibj.command.Command; @@ -12,11 +13,13 @@ import edu.wpi.first.wpilibj.command.Command; * @author Shaina */ public class StopFlyWheel extends Command { + private Shooter shooter = Robot.getShooter(); + /** * This command stops the fly wheel. */ public StopFlyWheel() { - + requires(shooter); } // Called just before this Command runs the first time @@ -39,6 +42,7 @@ public class StopFlyWheel extends Command { // subsystems is scheduled to run @Override protected void interrupted() { + end(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index f3df881..c189146 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -25,6 +25,9 @@ public class DriveTrain extends Subsystem { public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI / ENCODER_PULSES_PER_REVOLUTION; + public static final boolean BRAKE_MODE = true; + public static final boolean COAST_MODE = false; + private static DriveTrain driveTrain; private final CANTalon frontLeft, frontRight, rearLeft, rearRight; @@ -90,11 +93,19 @@ public class DriveTrain extends Subsystem { rearRight.set(-right); } + public void setCANTalonsBrakeMode(boolean mode) { + frontLeft.enableBrakeMode(mode); + frontRight.enableBrakeMode(mode); + rearLeft.enableBrakeMode(mode); + rearRight.enableBrakeMode(mode); + } + public void joystickDrive(final double thrust, final double twist) { if ((thrust < 0.1 && thrust > -0.1) && (twist < 0.1 && twist > -0.1)) robotDrive.arcadeDrive(0, 0, true); else robotDrive.arcadeDrive(thrust, twist, true); + System.out.println(thrust + " " + twist); } public void tankDrive(double left, double right) { diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Intake.java b/src/org/usfirst/frc/team3501/robot/subsystems/Intake.java index ed47fa7..b6c82d4 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Intake.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Intake.java @@ -44,7 +44,7 @@ public class Intake extends Subsystem { * @param speed * from -1 to 1 */ - private void setSpeed(double speed) { + public void setSpeed(double speed) { speed = MathLib.restrictToRange(speed, -1.0, 1.0); intakeWheel.set(speed); } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java index 5711b26..b12f6ca 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java @@ -18,8 +18,8 @@ public class Shooter extends Subsystem { private PIDController wheelController; private static final double RPM_THRESHOLD = 10; - private static final double DEFAULT_INDEXING_MOTOR_VALUE = 0.75; - private static final double REVERSE_FLYWHEEL_MOTOR_VALUE = -0.75; + public static final double DEFAULT_INDEXING_MOTOR_VALUE = 0.75; + public static final double REVERSE_FLYWHEEL_MOTOR_VALUE = -0.75; private static final double DEFAULT_SHOOTING_SPEED = 2875; // rpm private static final double SHOOTING_SPEED_INCREMENT = 25;