diff --git a/src/com/team3925/frc2018/OI.java b/src/com/team3925/frc2018/OI.java index 0c638be..c393130 100644 --- a/src/com/team3925/frc2018/OI.java +++ b/src/com/team3925/frc2018/OI.java @@ -8,6 +8,8 @@ import com.team3925.frc2018.commands.OpenGrabbers; import com.team3925.frc2018.commands.RunElevator; import com.team3925.frc2018.commands.RunIntakeWheels; +import com.team3925.frc2018.commands.SetLiftBottom; +import com.team3925.frc2018.commands.SetLiftTop; import com.team3925.frc2018.commands.ShiftHigh; import com.team3925.frc2018.commands.ShiftLow; import com.team3925.frc2018.commands.ShootCube; @@ -62,8 +64,8 @@ private OI() { drivetrain_Shift = new JoystickButton(wheel, 5); - openIntake = new JoystickButton(xbox, 6); - intakeCube = new JoystickButton(xbox, 5); + openIntake = new JoystickButton(xbox, 5); + intakeCube = new JoystickButton(xbox, 6); dropCube = new Trigger() { @Override @@ -113,7 +115,9 @@ public boolean get() { openIntake.whenReleased(new CloseGrabbers()); intakeCube.whenPressed(new RunIntakeWheels(1)); + intakeCube.whenPressed(new SetLiftBottom()); intakeCube.whenReleased(new RunIntakeWheels(0)); + intakeCube.whenReleased(new SetLiftTop()); tuneUp.whenActive(new IncrementAdjustElevator()); tuneDown.whenActive(new DecrementAdjustElevator()); @@ -137,7 +141,7 @@ public double getLiftIntake() { } public double getRawElevator() { - return stick.getRawAxis(5); + return xbox.getRawAxis(5); } public boolean getTestButton() { return wheel.getRawButton(4); diff --git a/src/com/team3925/frc2018/Robot.java b/src/com/team3925/frc2018/Robot.java index cd88d8a..20e670f 100644 --- a/src/com/team3925/frc2018/Robot.java +++ b/src/com/team3925/frc2018/Robot.java @@ -1,15 +1,12 @@ package com.team3925.frc2018; -import javax.swing.text.DefaultStyledDocument.ElementBuffer; - -import com.team3925.frc2018.commands.Center_Left_Switch_Auto; import com.team3925.frc2018.commands.DriveManual; import com.team3925.frc2018.commands.RunElevator; import com.team3925.frc2018.commands.RunElevatorRaw; import com.team3925.frc2018.commands.RunIntakeLiftRaw; -import com.team3925.frc2018.commands.ShootCube; +import com.team3925.frc2018.commands.ZeroIntake; import com.team3925.frc2018.subsystems.Elevator; -import com.team3925.frc2018.subsystems.Elevator.ElevatorState; +import com.team3925.frc2018.subsystems.Intake; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.command.Command; @@ -21,28 +18,38 @@ public class Robot extends IterativeRobot { RunElevatorRaw elevateRaw; RunIntakeLiftRaw liftRaw; Command testAuto; + Command zeroIntake; @Override public void robotInit() { drive = new DriveManual(OI.getInstance()); elevateRaw = new RunElevatorRaw(); liftRaw = new RunIntakeLiftRaw(); - testAuto = new Center_Left_Switch_Auto(); + zeroIntake = new ZeroIntake(); } @Override public void autonomousInit() { - Elevator.getInstance().zero(); - testAuto.start(); +// Elevator.getInstance().zero(); +// +// if (DriverStation.getInstance().getGameSpecificMessage().charAt(0) == 'L') { +// testAuto = new CenterLeftSwitchAuto(); +// }else if(DriverStation.getInstance().getGameSpecificMessage().charAt(0) == 'R') { +// testAuto = new CenterRightSwitchAuto(); +// } +// testAuto.start(); } public void teleopInit() { Elevator.getInstance().zero(); drive.start(); + Intake.getInstance().setIntakeRollers(-0.1); + zeroIntake.start(); } @Override public void teleopPeriodic() { + } @Override diff --git a/src/com/team3925/frc2018/RobotMap.java b/src/com/team3925/frc2018/RobotMap.java index cef1b6d..4b0c699 100644 --- a/src/com/team3925/frc2018/RobotMap.java +++ b/src/com/team3925/frc2018/RobotMap.java @@ -32,7 +32,7 @@ public static final class DrivetrainMap { public static final class IntakeMap { public static final WPI_TalonSRX LEFT_INTAKE = CTREControllerFactory.createDefaultTalon(11); - public static final WPI_VictorSPX RIGHT_INTAKE = CTREControllerFactory.createDefaultVictor(12); + public static final WPI_TalonSRX RIGHT_INTAKE = CTREControllerFactory.createDefaultTalon(12); public static final WPI_TalonSRX LIFT_MOTOR = CTREControllerFactory.createDefaultTalon(13); diff --git a/src/com/team3925/frc2018/commands/DropCube.java b/src/com/team3925/frc2018/commands/DropCube.java index 0d7298c..18bfd7b 100644 --- a/src/com/team3925/frc2018/commands/DropCube.java +++ b/src/com/team3925/frc2018/commands/DropCube.java @@ -8,7 +8,7 @@ public class DropCube extends CommandGroup{ public DropCube() { addParallel(new OpenGrabbers()); - addSequential(new RunIntakeWheels(-0.25)); + addSequential(new RunIntakeWheels(-0.40)); addSequential(new WaitCommand(0.5)); addParallel(new RunIntakeWheels(0)); addSequential(new CloseGrabbers()); diff --git a/src/com/team3925/frc2018/commands/RunElevator.java b/src/com/team3925/frc2018/commands/RunElevator.java index 380e596..1243c01 100644 --- a/src/com/team3925/frc2018/commands/RunElevator.java +++ b/src/com/team3925/frc2018/commands/RunElevator.java @@ -2,6 +2,7 @@ import com.team3925.frc2018.OI; import com.team3925.frc2018.subsystems.Elevator; +import com.team3925.frc2018.subsystems.Intake; import com.team3925.frc2018.subsystems.Elevator.ElevatorState; import edu.wpi.first.wpilibj.command.Command; @@ -24,4 +25,9 @@ protected void initialize() { protected boolean isFinished() { return true; } + + @Override + protected void end() { + Intake.getInstance().setAngle(0); + } } \ No newline at end of file diff --git a/src/com/team3925/frc2018/commands/SetLiftBottom.java b/src/com/team3925/frc2018/commands/SetLiftBottom.java new file mode 100644 index 0000000..50bf348 --- /dev/null +++ b/src/com/team3925/frc2018/commands/SetLiftBottom.java @@ -0,0 +1,46 @@ +package com.team3925.frc2018.commands; + +import com.team3925.frc2018.subsystems.Elevator; +import com.team3925.frc2018.subsystems.Elevator.ElevatorState; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class SetLiftBottom extends CommandGroup { + + public SetLiftBottom() { + + addSequential(new Command() { + + @Override + protected void initialize() { + Elevator.getInstance().setPosition(ElevatorState.BOTTOM); + } + + @Override + protected boolean isFinished() { + return Elevator.getInstance().getElevatorHeightPercentage() > 0.10; + } + }); + + addSequential(new Command() { + + @Override + protected boolean isFinished() { + return Elevator.getInstance().getLimitSwitch(); + } + + @Override + protected void initialize() { + Elevator.getInstance().setRaw(-0.2); + } + + @Override + protected void end() { + Elevator.getInstance().setRaw(0); + Elevator.getInstance().zero(); + } + }); + } + +} diff --git a/src/com/team3925/frc2018/commands/SetLiftTop.java b/src/com/team3925/frc2018/commands/SetLiftTop.java new file mode 100644 index 0000000..a652a6f --- /dev/null +++ b/src/com/team3925/frc2018/commands/SetLiftTop.java @@ -0,0 +1,20 @@ +package com.team3925.frc2018.commands; + +import com.team3925.frc2018.subsystems.Intake; + +import edu.wpi.first.wpilibj.command.Command; + +public class SetLiftTop extends Command{ + + @Override + protected void initialize() { + Intake.getInstance().setAngle(85); + } + + @Override + protected boolean isFinished() { + return true; + } + + +} diff --git a/src/com/team3925/frc2018/commands/ZeroIntake.java b/src/com/team3925/frc2018/commands/ZeroIntake.java index d5292a2..1db12c1 100644 --- a/src/com/team3925/frc2018/commands/ZeroIntake.java +++ b/src/com/team3925/frc2018/commands/ZeroIntake.java @@ -13,17 +13,18 @@ protected boolean isFinished() { } @Override - protected void end() { - // stop motor - Intake.getInstance().zeroLift(); - Intake.getInstance().setLiftMotorRaw(0); + protected void execute() { + Intake.getInstance().setLiftMotorRaw(-0.2); } @Override - protected void execute() { - Intake.getInstance().setLiftMotorRaw(-0.2); + protected void end() { + // stop motor + Intake.getInstance().zeroLift(); + Intake.getInstance().setLiftMotorRaw(0); + Intake.getInstance().setAngle(85); } -}// end of class +} diff --git a/src/com/team3925/frc2018/commands/Center_Left_Switch_Auto.java b/src/com/team3925/frc2018/commands/autos/CenterLeftSwitchAuto.java similarity index 57% rename from src/com/team3925/frc2018/commands/Center_Left_Switch_Auto.java rename to src/com/team3925/frc2018/commands/autos/CenterLeftSwitchAuto.java index 4e8b9c2..83dd811 100644 --- a/src/com/team3925/frc2018/commands/Center_Left_Switch_Auto.java +++ b/src/com/team3925/frc2018/commands/autos/CenterLeftSwitchAuto.java @@ -1,14 +1,19 @@ -package com.team3925.frc2018.commands; +package com.team3925.frc2018.commands.autos; +import com.team3925.frc2018.commands.OpenGrabbers; +import com.team3925.frc2018.commands.RunElevator; +import com.team3925.frc2018.commands.RunIntakeWheels; +import com.team3925.frc2018.commands.ZeroIntake; import com.team3925.frc2018.subsystems.Elevator.ElevatorState; import com.team3925.utils.MotionProfileCommand; import edu.wpi.first.wpilibj.command.CommandGroup; import edu.wpi.first.wpilibj.command.WaitCommand; -public class Center_Left_Switch_Auto extends CommandGroup{ +public class CenterLeftSwitchAuto extends CommandGroup{ - public Center_Left_Switch_Auto() { + public CenterLeftSwitchAuto() { + addSequential(new ZeroIntake()); addParallel(new RunElevator(ElevatorState.BOTTOM)); addParallel(new MotionProfileCommand("CENTER_LEFTSWITCH")); addSequential(new WaitCommand(2)); diff --git a/src/com/team3925/frc2018/commands/autos/CenterRightSwitchAuto.java b/src/com/team3925/frc2018/commands/autos/CenterRightSwitchAuto.java new file mode 100644 index 0000000..6dd0364 --- /dev/null +++ b/src/com/team3925/frc2018/commands/autos/CenterRightSwitchAuto.java @@ -0,0 +1,26 @@ +package com.team3925.frc2018.commands.autos; + +import com.team3925.frc2018.commands.OpenGrabbers; +import com.team3925.frc2018.commands.RunElevator; +import com.team3925.frc2018.commands.RunIntakeWheels; +import com.team3925.frc2018.commands.ZeroIntake; +import com.team3925.frc2018.subsystems.Elevator.ElevatorState; +import com.team3925.utils.MotionProfileCommand; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +public class CenterRightSwitchAuto extends CommandGroup{ + + public CenterRightSwitchAuto() { + addSequential(new ZeroIntake()); + addParallel(new RunElevator(ElevatorState.BOTTOM)); + addParallel(new MotionProfileCommand("CENTER_RIGHTSWITCH")); + addSequential(new WaitCommand(2)); + addSequential(new RunElevator(ElevatorState.SWITCH)); + addSequential(new OpenGrabbers()); + addSequential(new RunIntakeWheels(-0.5)); + } + +} + diff --git a/src/com/team3925/frc2018/subsystems/Elevator.java b/src/com/team3925/frc2018/subsystems/Elevator.java index e54b341..d4c05ce 100644 --- a/src/com/team3925/frc2018/subsystems/Elevator.java +++ b/src/com/team3925/frc2018/subsystems/Elevator.java @@ -26,10 +26,12 @@ public enum ElevatorState{ private static final double kP = 0.2; //0.015 | 0.2 private static final double kI = 0; private static final double kD = 0; - private static final double kF = 0.35; + private static final double kF = 0.35; //0.35 private static final int MOTION_MAGIC_ACCELERATION = 9001; - private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; //5467 + private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; +// private static final int MOTION_MAGIC_ACCELERATION = 9001; +// private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; // private static final int MOTION_MAGIC_ACCELERATION = 20594; // private static final int MOTION_MAGIC_CRUISE_VELOCITY = 5467; @@ -60,13 +62,13 @@ public Elevator() { elevatorMaster.configMotionAcceleration(MOTION_MAGIC_ACCELERATION, Constants.TIMEOUT_MS); elevatorMaster.configMotionCruiseVelocity(MOTION_MAGIC_CRUISE_VELOCITY, Constants.TIMEOUT_MS); - elevatorMaster.overrideLimitSwitchesEnable(true); + elevatorMaster.overrideLimitSwitchesEnable(false); elevatorMaster.configContinuousCurrentLimit(200, Constants.TIMEOUT_MS); - elevatorMaster.configNominalOutputForward(1, Constants.TIMEOUT_MS); - elevatorMaster.configNominalOutputReverse(-1, Constants.TIMEOUT_MS); - RobotMap.ElevatorMap.SLAVE.configNominalOutputForward(1, Constants.TIMEOUT_MS); - RobotMap.ElevatorMap.SLAVE.configNominalOutputReverse(-1, Constants.TIMEOUT_MS); + elevatorMaster.configNominalOutputForward(0, Constants.TIMEOUT_MS); + elevatorMaster.configNominalOutputReverse(0, Constants.TIMEOUT_MS); + RobotMap.ElevatorMap.SLAVE.configNominalOutputForward(0, Constants.TIMEOUT_MS); + RobotMap.ElevatorMap.SLAVE.configNominalOutputReverse(0, Constants.TIMEOUT_MS); } @@ -79,6 +81,7 @@ public boolean isElevatorAtSetpoint() { } public void setPosition(double inches){ +// elevatorMaster.set(ControlMode.Position, (inches * K_ELEVATOR)); elevatorMaster.set(ControlMode.MotionMagic, (Math.min(inches, MAX_SCALE_HEIGHT) * K_ELEVATOR)); elevatorHeight = inches; } @@ -89,19 +92,19 @@ public void setPosition(ElevatorState state){ setPosition((12 * 5.5) + 4.667); break; case SCALE_MAX: - setPosition(12 * 5.5); + setPosition(12 * 5); break; case SCALE_MED: - setPosition(12 * 5); + setPosition(12 * 4.5); break; case SCALE_LOW: - setPosition(12 * 4.5); + setPosition(12 * 4); break; case BOTTOM: setPosition(3); break; case SWITCH: - setPosition(12 * 2.5); + setPosition(12 * 2); break; } } diff --git a/src/com/team3925/frc2018/subsystems/Intake.java b/src/com/team3925/frc2018/subsystems/Intake.java index f8773c9..ba22e21 100644 --- a/src/com/team3925/frc2018/subsystems/Intake.java +++ b/src/com/team3925/frc2018/subsystems/Intake.java @@ -3,8 +3,6 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; -import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import com.team3925.frc2018.Constants; import com.team3925.frc2018.RobotMap; @@ -14,12 +12,13 @@ public class Intake extends Subsystem { + private static final double K_INTAKE = 1504; private final int LIFT_BIG_GEAR_TEETH = 60; private final int LIFT_SMALL_GEAR_TEETH = 40; private final TalonSRX leftIntake = RobotMap.IntakeMap.LEFT_INTAKE; - private final VictorSPX rightIntake = RobotMap.IntakeMap.RIGHT_INTAKE; + private final TalonSRX rightIntake = RobotMap.IntakeMap.RIGHT_INTAKE; private final TalonSRX liftMotor = RobotMap.IntakeMap.LIFT_MOTOR; @@ -39,15 +38,19 @@ private Intake() { Constants.TIMEOUT_MS); liftMotor.selectProfileSlot(0, Constants.PID_ID_X); - liftMotor.config_kP(0, 0, Constants.TIMEOUT_MS); - liftMotor.config_kI(0, 0, Constants.TIMEOUT_MS); + liftMotor.config_kP(0, 3, Constants.TIMEOUT_MS); + liftMotor.config_kI(0, 0.01, Constants.TIMEOUT_MS); liftMotor.config_kD(0, 0, Constants.TIMEOUT_MS); - liftMotor.config_kF(0, 0, Constants.TIMEOUT_MS); + liftMotor.config_kF(0, 0.35, Constants.TIMEOUT_MS); - liftMotor.configMotionAcceleration(0, Constants.TIMEOUT_MS); - liftMotor.configMotionCruiseVelocity(0, Constants.TIMEOUT_MS); + liftMotor.configMotionAcceleration(500, Constants.TIMEOUT_MS); + liftMotor.configMotionCruiseVelocity(700, Constants.TIMEOUT_MS); + + liftMotor.setInverted(true); + liftMotor.setSensorPhase(true); leftIntake.setInverted(true); + } private void setPosition(double revolutions) { @@ -55,6 +58,8 @@ private void setPosition(double revolutions) { } public void setIntakeRollers(double speed) { + if (speed <= 0) + speed = 0.1; leftIntake.set(ControlMode.PercentOutput, speed); rightIntake.set(ControlMode.PercentOutput, speed); } @@ -68,11 +73,16 @@ public boolean getLiftMotorLimitSwitch() { } public void setGrabber(boolean grab) { - grabSolenoid.set((!grab) ? Value.kForward : Value.kReverse); + grabSolenoid.set((grab) ? Value.kForward : Value.kReverse); + } + + public double getLiftMotorEncoder() { + return liftMotor.getSelectedSensorPosition(0); } public void setAngle(double degrees) { - setPosition(((degrees / 360) * (LIFT_BIG_GEAR_TEETH / LIFT_SMALL_GEAR_TEETH)) * Constants.CTRE_ENCODER_TICKS_PER_REV); + liftMotor.set(ControlMode.MotionMagic, (degrees/90) * K_INTAKE); +// setPosition(((degrees / 360) * (LIFT_BIG_GEAR_TEETH / LIFT_SMALL_GEAR_TEETH)) * Constants.CTRE_ENCODER_TICKS_PER_REV); } public double getPosition() { diff --git a/src/com/team3925/utils/CTREControllerFactory.java b/src/com/team3925/utils/CTREControllerFactory.java index e5bd1fb..ff6e077 100644 --- a/src/com/team3925/utils/CTREControllerFactory.java +++ b/src/com/team3925/utils/CTREControllerFactory.java @@ -39,11 +39,14 @@ public static WPI_VictorSPX createDefaultVictor(int id) { public static WPI_VictorSPX createPermanentSlaveVictor(int id, IMotorController masterId) { final WPI_VictorSPX victor = createDefaultVictor(id); + applyGenericSettings(victor); victor.follow(masterId); return victor; } private static BaseMotorController applyGenericSettings(BaseMotorController controller) { + controller.setNeutralMode(NeutralMode.Brake); + controller.overrideLimitSwitchesEnable(false); // controller.config_IntegralZone(0, 0, Constants.TIMEOUT_MS); // controller.config_kD(0, 0, Constants.TIMEOUT_MS); // controller.config_kF(0, 0, Constants.TIMEOUT_MS);