diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 48dea5de..05f1a331 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -146,12 +146,21 @@ public static final class Armc { // fine for now, change it later before use - ("Incorect use of setIZone()" // Issue #22) // public static final double MAX_FF_VEL_RAD_P_S = 0.2; //rad/s + public static final double MAX_FF_VEL_RAD_P_S = Math.PI * .5; public static final double MAX_FF_ACCEL_RAD_P_S = 53.728 / 4; // rad / s^2 // ((.89*2)/(1.477/(61.875^2))/61.875)-20.84 + + public static final double MAX_FF_VEL_RAD_P_S_BABY = 0; + public static final double MAX_FF_ACCEL_RAD_P_S_BABY = 0; + //TODO: determine these values^ + + public static final double SOFT_LIMIT_LOCATION_IN_RADIANS = 0; public static final double CLIMB_POS = 1.701; //RADIANS public static final double MIN_VOLTAGE = -0.5; // -((kS + kG + 1)/12); public static final double MAX_VOLTAGE = 0.5; // (kS + kG + 1)/12; + public static final double MIN_VOLTAGE_BABY = MIN_VOLTAGE/12 *0.7; + public static final double MAX_VOLTAGE_BABY = MAX_VOLTAGE/12*0.7; public static final double CLIMB_FINISH_POS = -0.38; // if needed public static final double COM_ARM_LENGTH_METERS = 0.381; @@ -310,6 +319,10 @@ public static final class Drivetrainc { public static double kNormalDriveRotation = 0.5; // Percent Multiplier public static double kSlowDriveSpeed = 0.4; // Percent Multiplier public static double kSlowDriveRotation = 0.250; // Percent Multiplier + + //baby speed values, i just guessed the percent multiplier. TODO: find actual ones we wana use + public static double kBabyDriveSpeed = 0.3; + public static double kBabyDriveRotation = 0.2; public static double kAlignMultiplier = 1D / 3D; public static final double kAlignForward = 0.6; diff --git a/src/main/java/org/carlmontrobotics/Robot.java b/src/main/java/org/carlmontrobotics/Robot.java index 0a584cda..46720cba 100644 --- a/src/main/java/org/carlmontrobotics/Robot.java +++ b/src/main/java/org/carlmontrobotics/Robot.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -30,11 +31,13 @@ public void robotInit() { DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); FollowPathCommand.warmupCommand().schedule(); + } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + } @Override diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 6fa49af8..3100f19c 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -66,6 +66,8 @@ import org.carlmontrobotics.subsystems.Led; public class RobotContainer { + private static boolean babyMode = false; + // 1. using GenericHID allows us to use different kinds of controllers // 2. Use absolute paths from constants to reduce confusion public final GenericHID driverController = new GenericHID(Driver.port); @@ -79,71 +81,75 @@ public class RobotContainer { private final Limelight limelight = new Limelight(drivetrain); /* These are assumed to be equal to the AUTO ames in pathplanner */ - /* These must be equal to the pathPlanner path names from the GUI! */ - // Order matters - but the first one is index 1 on the physical selector - index 0 is reserved for null command. - //the last auto is hard-coded to go straight. since we have __3__ Autos, port 4 is simple straight + /* These must be equal to the pathPlanner path names from the GUI! */ + // Order matters - but the first one is index 1 on the physical selector - index + // 0 is reserved for null command. + // the last auto is hard-coded to go straight. since we have __3__ Autos, port 4 + // is simple straight private List autoCommands = new ArrayList(); private SendableChooser autoSelector = new SendableChooser(); private boolean hasSetupAutos = false; private final String[] autoNames = new String[] { - /* These are assumed to be equal to the AUTO ames in pathplanner */ - "Left-Straight", - "Center-Straight", - "Right-Straight", - "Middle-Ram", + /* These are assumed to be equal to the AUTO ames in pathplanner */ + "Left-Straight", + "Center-Straight", + "Right-Straight", + "Middle-Ram", + + "Left-Auto Ruiner", + "Center-Auto Ruiner", + "Right-Auto Ruiner", - "Left-Auto Ruiner", - "Center-Auto Ruiner", - "Right-Auto Ruiner", + "Preloaded Left Shooting", + "Preloaded Right Shooting", - "Preloaded Left Shooting", - "Preloaded Right Shooting", + "Left Shoot", + "Center Shoot", + "Right Shoot", - "Left Shoot", - "Center Shoot", - "Right Shoot", + "Left-Shoot For Left Subwoofer", + "Right-Shoot For Right Subwoofer", - "Left-Shoot For Left Subwoofer", - "Right-Shoot For Right Subwoofer", + // "Preloaded Left-Pickup Subwoofer", + // "Preloaded Right-Pickup Subwoofer", - // "Preloaded Left-Pickup Subwoofer", - // "Preloaded Right-Pickup Subwoofer", + "Left-Amp", + "Shoot on Right", + "Shoot Center" - "Left-Amp", - "Shoot on Right", - "Shoot Center" - }; DigitalInput[] autoSelectors = new DigitalInput[Math.min(autoNames.length, 10)]; - public RobotContainer() { { // SensorFactory.configureCamera(); - //safe auto setup... stuff in setupAutos() is not safe to run here - will break robot + SmartDashboard.setDefaultBoolean("babymode", babyMode); + SmartDashboard.setPersistent("babymode"); + // safe auto setup... stuff in setupAutos() is not safe to run here - will break + // robot registerAutoCommands(); SmartDashboard.putData(autoSelector); SmartDashboard.setPersistent("SendableChooser[0]"); autoSelector.addOption("Nothing", 0); autoSelector.addOption("Raw Forward", 1); - autoSelector.addOption("PP Simple Forward", 2);//index corresponds to index in autoCommands[] + autoSelector.addOption("PP Simple Forward", 2);// index corresponds to index in autoCommands[] - int i=3; - for (String n:autoNames){ + int i = 3; + for (String n : autoNames) { autoSelector.addOption(n, i); i++; } ShuffleboardTab autoSelectorTab = Shuffleboard.getTab("Auto Chooser Tab"); autoSelectorTab.add(autoSelector) - .withSize(2, 1); + .withSize(2, 1); } setDefaultCommands(); setBindingsDriver(); - //setBindingsManipulatorENDEFF(); + // setBindingsManipulatorENDEFF(); setBindingsManipulatorNEO(); } @@ -156,14 +162,12 @@ private void setDefaultCommands() { () -> driverController.getRawButton(Driver.slowDriveButton))); // TODO: Are we going to use default command for intakeshooter? intakeShooter.setDefaultCommand(new TeleopEffector( - intakeShooter, - () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY), - manipulatorController, driverController)); + intakeShooter, + () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY), + manipulatorController, driverController)); arm.setDefaultCommand(new TeleopArm( - arm, - () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY) - )); - + arm, + () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY))); } @@ -171,13 +175,13 @@ private void setBindingsDriver() { new JoystickButton(driverController, Driver.resetFieldOrientationButton) .onTrue(new InstantCommand(drivetrain::resetFieldOrientation)); // axisTrigger(driverController, Axis.kRightTrigger) - // .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"), - // new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight))); + // .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"), + // new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight))); new POVButton(driverController, 0).whileTrue(new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight)); axisTrigger(driverController, Axis.kLeftTrigger) - //.onTrue(new AlignToApriltag(drivetrain, limelight)); - .onTrue(new InstantCommand(()->drivetrain.setFieldOriented(false))) - .onFalse(new InstantCommand(()->drivetrain.setFieldOriented(true))); + // .onTrue(new AlignToApriltag(drivetrain, limelight)); + .onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(false))) + .onFalse(new InstantCommand(() -> drivetrain.setFieldOriented(true))); axisTrigger(driverController, Manipulator.SHOOTER_BUTTON) .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"), @@ -191,12 +195,12 @@ private void setBindingsDriver() { new JoystickButton(driverController, Driver.rotateFieldRelative270Deg) .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), drivetrain)); } + private void setBindingsManipulatorNEO() { new JoystickButton(manipulatorController, EJECT_BUTTON).onTrue(new Eject(intakeShooter)); - new JoystickButton(manipulatorController, Button.kB.value).whileTrue(new RampMaxRPM(intakeShooter)); - new JoystickButton(manipulatorController, AMP_BUTTON).whileTrue(new EjectOuttakeSide(intakeShooter)); + new JoystickButton(manipulatorController, AMP_BUTTON).whileTrue(new EjectOuttakeSide(intakeShooter)); axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) .onTrue( @@ -211,24 +215,25 @@ private void setBindingsManipulatorNEO() { axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON) .onFalse( new InstantCommand(intakeShooter::stopIntake, intakeShooter)); - new JoystickButton(manipulatorController, Button.kY.value).onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR,0)); - new JoystickButton(manipulatorController, Button.kA.value).onTrue(new ArmToPos(arm, GROUND_INTAKE_POS,1)); + new JoystickButton(manipulatorController, Button.kY.value).onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR, 0)); + new JoystickButton(manipulatorController, Button.kA.value).onTrue(new ArmToPos(arm, GROUND_INTAKE_POS, 1)); new JoystickButton(manipulatorController, Button.kLeftStick.value).onTrue(new PassToOuttake(intakeShooter)); new JoystickButton(manipulatorController, Button.kRightStick.value).whileTrue(new PassToIntake(intakeShooter)); - new JoystickButton(manipulatorController, Button.kX.value).onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD,1)); - //TODO: test angles for pov button BEFORE climbing + new JoystickButton(manipulatorController, Button.kX.value).onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD, 1)); + // TODO: test angles for pov button BEFORE climbing new POVButton(manipulatorController, 0).onTrue(new ArmToPos(arm, CLIMB_POS, 0)); new POVButton(manipulatorController, 180).onTrue(new Climb(arm)); new POVButton(manipulatorController, 270).onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD, 2)); } + private void setBindingsManipulatorENDEFF() { /* /Eject also for AMP/ */ new JoystickButton(manipulatorController, EJECT_BUTTON).onTrue(new Eject(intakeShooter)); - new JoystickButton(manipulatorController, Button.kB.value).onTrue(new RampMaxRPM(intakeShooter)); - new JoystickButton(manipulatorController, Button.kB.value).onFalse(new InstantCommand(intakeShooter::stopOutake,intakeShooter)); - new JoystickButton(manipulatorController, AMP_BUTTON).onTrue(new EjectOuttakeSide(intakeShooter)); + new JoystickButton(manipulatorController, Button.kB.value) + .onFalse(new InstantCommand(intakeShooter::stopOutake, intakeShooter)); + new JoystickButton(manipulatorController, AMP_BUTTON).onTrue(new EjectOuttakeSide(intakeShooter)); axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) .onTrue( @@ -243,20 +248,17 @@ private void setBindingsManipulatorENDEFF() { axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON) .onFalse( new InstantCommand(intakeShooter::stopIntake, intakeShooter)); - new JoystickButton(manipulatorController, Button.kY.value).onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR,0)); - new JoystickButton(manipulatorController, Button.kA.value).onTrue(new ArmToPos(arm, GROUND_INTAKE_POS,1)); + new JoystickButton(manipulatorController, Button.kY.value).onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR, 0)); + new JoystickButton(manipulatorController, Button.kA.value).onTrue(new ArmToPos(arm, GROUND_INTAKE_POS, 1)); new JoystickButton(manipulatorController, Button.kLeftStick.value).onTrue(new PassToOuttake(intakeShooter)); - new JoystickButton(manipulatorController, Button.kX.value).onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD,1)); - //TODO: test angles for pov button BEFORE climbing + new JoystickButton(manipulatorController, Button.kX.value).onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD, 1)); + // TODO: test angles for pov button BEFORE climbing new POVButton(manipulatorController, 0).onTrue(new ArmToPos(arm, CLIMB_POS, 0)); new POVButton(manipulatorController, 180).onTrue(new Climb(arm)); new POVButton(manipulatorController, 270).onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD, 2)); - } - - /** * Flips an axis' Y coordinates upside down, but only if the select axis is a * joystick axis @@ -319,24 +321,24 @@ private double DeadzonedAxis(double axOut) { * @param axis The Axis object to retrieve value from the Joystick. * @return A new instance of Trigger based on the given Joystick and Axis * objects. -* * @throws NullPointerException if either stick or axis is null. + * * @throws NullPointerException if either stick or axis is null. */ private Trigger axisTrigger(GenericHID controller, Axis axis) { return new Trigger(() -> Math.abs(getStickValue(controller, axis)) > OI.MIN_AXIS_TRIGGER_VALUE); } - private void registerAutoCommands(){ - ////AUTO-USABLE COMMANDS + private void registerAutoCommands() { + //// AUTO-USABLE COMMANDS NamedCommands.registerCommand("Intake", new IntakeNEO(intakeShooter)); NamedCommands.registerCommand("Eject", new Eject(intakeShooter)); - // NamedCommands.registerCommand("ArmToSpeaker", new MoveToPos(arm, Armc.SPEAKER_ANGLE_RAD, 0)); - NamedCommands.registerCommand("ArmToAmp", new ArmToPos(arm, Armc.AMP_ANGLE_RAD,0)); - NamedCommands.registerCommand("ArmToSubwoofer", new ArmToPos(arm, Armc.SUBWOOFER_ANGLE_RAD,1)); + // NamedCommands.registerCommand("ArmToSpeaker", new MoveToPos(arm, + // Armc.SPEAKER_ANGLE_RAD, 0)); + NamedCommands.registerCommand("ArmToAmp", new ArmToPos(arm, Armc.AMP_ANGLE_RAD, 0)); + NamedCommands.registerCommand("ArmToSubwoofer", new ArmToPos(arm, Armc.SUBWOOFER_ANGLE_RAD, 1)); NamedCommands.registerCommand("ArmToPodium", new ArmToPos(arm, Armc.PODIUM_ANGLE_RAD, 2)); NamedCommands.registerCommand("ArmToGround", new ArmToPos(arm, GROUND_INTAKE_POS, 1)); - NamedCommands.registerCommand("SwitchRPMShoot", new SwitchRPMShootNEO(intakeShooter)); NamedCommands.registerCommand("PassToOuttake", new PassToOuttake(intakeShooter)); @@ -344,39 +346,42 @@ private void registerAutoCommands(){ NamedCommands.registerCommand("StopIntake", new InstantCommand(intakeShooter::stopIntake)); NamedCommands.registerCommand("StopOutake", new InstantCommand(intakeShooter::stopOutake)); NamedCommands.registerCommand("StopBoth", new ParallelCommandGroup( - new InstantCommand(intakeShooter::stopIntake), - new InstantCommand(intakeShooter::stopOutake) - )); + new InstantCommand(intakeShooter::stopIntake), + new InstantCommand(intakeShooter::stopOutake))); } + private void setupAutos() { - ////CREATING PATHS from files + //// CREATING PATHS from files { - for (int i=0;i bezierPoints = PathPlannerPath.bezierFromPoses( - currPos, - currPos.plus(new Transform2d(0,1,new Rotation2d(0))) - ); + currPos, + currPos.plus(new Transform2d(0, 1, new Rotation2d(0)))); /** * PATHPLANNER SETTINGS * Robot Width (m): .91 @@ -388,42 +393,40 @@ private void setupAutos() { */ // Create the path using the bezier points created above PathPlannerPath path = new PathPlannerPath( - bezierPoints, - /*m/s, m/s^2, rad/s, rad/s^2 */ - Autoc.pathConstraints, - new GoalEndState(0, currPos.getRotation()) - ); + bezierPoints, + /* m/s, m/s^2, rad/s, rad/s^2 */ + Autoc.pathConstraints, + new GoalEndState(0, currPos.getRotation())); // Prevent the path from being flipped if the coordinates are already correct path.preventFlipping = DriverStation.getAlliance().get() == DriverStation.Alliance.Blue; if (DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { - path.flipPath(); + path.flipPath(); } - //NOTHING + // NOTHING autoCommands.add(0, new PrintCommand("Running NULL Auto!")); - //RAW FORWARD command + // RAW FORWARD command autoCommands.add(1, new LastResortAuto(drivetrain)); - //smart forward command + // smart forward command autoCommands.add(2, new SequentialCommandGroup( - AutoBuilder.followPath(path) - )); - //no events so just use path instead of auto + AutoBuilder.followPath(path))); + // no events so just use path instead of auto } } public Command getAutonomousCommand() { - if (!hasSetupAutos){ + if (!hasSetupAutos) { setupAutos(); - hasSetupAutos=true; + hasSetupAutos = true; } Integer autoIndex = autoSelector.getSelected(); - if (autoIndex!=null && autoIndex!=0){ - new PrintCommand("Running selected auto: "+autoSelector.toString()); + if (autoIndex != null && autoIndex != 0) { + new PrintCommand("Running selected auto: " + autoSelector.toString()); return autoCommands.get(autoIndex.intValue()); } return new PrintCommand("No auto :("); - } + } } diff --git a/src/main/java/org/carlmontrobotics/commands/Climb.java b/src/main/java/org/carlmontrobotics/commands/Climb.java index fda313de..bb1682a1 100644 --- a/src/main/java/org/carlmontrobotics/commands/Climb.java +++ b/src/main/java/org/carlmontrobotics/commands/Climb.java @@ -14,6 +14,7 @@ public class Climb extends Command { // correctly private Arm arm; private Timer timer = new Timer(); + private boolean babyMode; public Climb(Arm arm) { this.arm = arm; addRequirements(arm); @@ -22,15 +23,17 @@ public Climb(Arm arm) { @Override public void initialize() { + babyMode = SmartDashboard.getBoolean("babymode", false); arm.setLimitsForClimbOn(); arm.setBooleanDrive(false); } @Override public void execute() { + if(!babyMode) { arm.driveArm(-12); timer.start(); - + } } @Override @@ -48,6 +51,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { // TODO: Figure out the actual climb position - return arm.getArmPos() - (CLIMB_FINISH_POS) < Units.degreesToRadians(0.1) || timer.get() >= 5; + return arm.getArmPos() - (CLIMB_FINISH_POS) < Units.degreesToRadians(0.1) || timer.get() >= 10 || babyMode; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/RampMaxRPM.java b/src/main/java/org/carlmontrobotics/commands/RampMaxRPM.java index 8ae9f319..0312fc6c 100644 --- a/src/main/java/org/carlmontrobotics/commands/RampMaxRPM.java +++ b/src/main/java/org/carlmontrobotics/commands/RampMaxRPM.java @@ -6,12 +6,14 @@ import org.carlmontrobotics.subsystems.IntakeShooter; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; // TODO: where do we use this command? public class RampMaxRPM extends Command { // intake until sees game peice or 4sec has passed private final IntakeShooter intake; + private boolean babyMode; private Timer timer; public RampMaxRPM(IntakeShooter intake) { @@ -20,6 +22,7 @@ public RampMaxRPM(IntakeShooter intake) { @Override public void initialize() { + babyMode = SmartDashboard.getBoolean("babymode", false); intake.setMaxOutake(1); } @@ -39,6 +42,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return false || babyMode; } } diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java index c773171e..db127767 100644 --- a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -8,6 +8,7 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.Robot; import org.carlmontrobotics.subsystems.Drivetrain; +import static org.carlmontrobotics.RobotContainer.*; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -26,7 +27,7 @@ public class TeleopDrive extends Command { private double currentForwardVel = 0; private double currentStrafeVel = 0; private double prevTimestamp; - + private boolean babyMode; /** * Creates a new TeleopDrive. */ @@ -57,6 +58,7 @@ public void execute() { double[] speeds = getRequestedSpeeds(); // SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp); prevTimestamp = currentTime; + babyMode = SmartDashboard.getBoolean("babymode", false); // kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation); // kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed); // kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation); @@ -94,6 +96,10 @@ public double[] getRequestedSpeeds() { double driveMultiplier = slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed; double rotationMultiplier = slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation; + if(babyMode == true){ + driveMultiplier = kBabyDriveSpeed; + rotationMultiplier = kBabyDriveRotation; + } // double driveMultiplier = kNormalDriveSpeed; // double rotationMultiplier = kNormalDriveRotation; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index dd5f255c..b93d30bf 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -11,6 +11,9 @@ import org.carlmontrobotics.commands.TeleopArm; import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; +import static org.carlmontrobotics.RobotContainer.*; + +import org.carlmontrobotics.RobotContainer; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkBase.IdleMode; @@ -46,6 +49,7 @@ // Arm angle is measured from horizontal on the intake side of the robot and bounded between -3π/2 and π/2 public class Arm extends SubsystemBase { + private boolean callDrive = true; private final CANSparkMax armMotorMaster/* left */ = MotorControllerFactory.createSparkMax(ARM_MOTOR_PORT_MASTER, MotorConfig.NEO); @@ -76,7 +80,7 @@ public class Arm extends SubsystemBase { private final MutableMeasure voltage = mutable(Volts.of(0)); private final MutableMeasure> velocity = mutable(RadiansPerSecond.of(0)); private final MutableMeasure distance = mutable(Radians.of(0)); - + private static boolean babyMode; private ShuffleboardTab sysIdTab = Shuffleboard.getTab("arm SysID"); private boolean setPIDOff; @@ -118,12 +122,7 @@ public Arm() { armPIDMaster.setPositionPIDWrappingMaxInput((3 * Math.PI) / 2); armPIDMaster.setIZone(IZONE_RAD); - TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints( - Math.PI * .5, - MAX_FF_ACCEL_RAD_P_S); - // ^ worst case scenario - // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S) - armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS); + SmartDashboard.putData("Arm", this); @@ -149,12 +148,23 @@ public Arm() { // SmartDashboard.putNumber("soft limit pos (rad)", SOFT_LIMIT_LOCATION_IN_RADIANS); armMotorMaster.setSmartCurrentLimit(80); armMotorFollower.setSmartCurrentLimit(80); - + if(SmartDashboard.getBoolean("babymode", babyMode) == true){ + armPIDMaster.setOutputRange(-0.3/12, 0.3/12); + } + else{ + armPIDMaster.setOutputRange(MIN_VOLTAGE/12, MAX_VOLTAGE/12); + } + TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints( + (MAX_FF_VEL_RAD_P_S), + (MAX_FF_ACCEL_RAD_P_S)); + armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS); SmartDashboard.putBoolean("arm is at pos", false); if (RobotBase.isSimulation()) { rotationsSim = new SimDeviceSim("AbsoluteEncoder", ARM_MOTOR_PORT_MASTER).getDouble("rotations"); } + } + public void setBooleanDrive(boolean climb) { callDrive = climb; @@ -162,6 +172,13 @@ public void setBooleanDrive(boolean climb) { @Override public void periodic() { + babyMode = SmartDashboard.getBoolean("babymode", false); + + + //Aaron was here + // ^ worst case scenario + // armFeed.maxAchievableVelocity(12, 0, MAX_FF_ACCEL_RAD_P_S) + SmartDashboard.putData(this); // armMotorMaster.setSmartCurrentLimit(50); // armMotorFollower.setSmartCurrentLimit(50); @@ -237,9 +254,13 @@ public void periodic() { armMotorMaster.set(0); armMotorFollower.set(0); } + + + autoCancelArmCommand(); + } public static void setSelector(int num) { numSelector = num;