diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 71bd304b..c061de29 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -145,12 +145,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; @@ -309,6 +318,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 75313fad..1be44727 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 dc8ab15c..caa1369d 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -63,6 +63,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; 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); @@ -120,7 +122,8 @@ public RobotContainer() { // Put any configuration overrides to the dashboard and the terminal SmartDashboard.putData("CONFIG overrides", Config.CONFIG); System.out.println(Config.CONFIG); - + 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); @@ -146,6 +149,9 @@ public RobotContainer() { //setBindingsManipulatorENDEFF(); setBindingsManipulatorNEO(); } + + + private void setDefaultCommands() { drivetrain.setDefaultCommand(new TeleopDrive( 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;