Tim coral

.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "currentLanguage": "none",
+    "enableCppIntellisense": false,
+    "projectYear": "2025",
+    "teamNumber": 199
+}

src/main/java/org/carlmontrobotics/Constants.java

@@ -1,15 +1,35 @@
 package org.carlmontrobotics;
 
+import edu.wpi.first.wpilibj.StadiaController.Button;
+
 public final class Constants {
-    // public static final class Drivetrain {
-    //     public static final double MAX_SPEED_MPS = 2;
-    // }
     public static final class OI {
         public static final class Driver {
-            public static final int port = 0;
+            public static final int DRIVE_CONTROLLER_PORT = 0;
         }
         public static final class Manipulator {
-            public static final int port = 1;
+            public static final int MANIPULATOR_CONTROLLER_PORT = 1;
+            public static final int OUTTAKE_BUTTON = Button.kA.value;
+            public static final int INTAKE_BUTTON = Button.kB.value;
         }
     }
+    public static final class CoralEffectorc{
+        public final static int CORAL_MOTOR_PORT = 30;
+        public final static int CORAL_LIMIT_SWITCH_PORT = 0;
+        public final static int CORAL_DISTANCE_SENSOR_PORT = 6;
+        public final static int CORAL_DISTANCE_SENSOR_DISTANCE = 150; //mm
+        public final static double CORAL_INTAKE_ERR = .1;//encoder units - rotations
+        public final static double INPUT_FAST_SPEED = 0.2; //TODO: tune this
+        public final static double INPUT_SLOW_SPEED = 0.1; //TODO: tune this
+        public final static double OUTPUT_SPEED = 0.5; //TODO: tune this
+        public final static double CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET = -0.1; //TOD: tune this?
+        public final static double KP = 1.3; //TODO: tune this
+        public final static double KI = 0;
+        public final static double KD = 0;
+        public final static double INTAKE_TIME_OUT = 0.25;
+        public final static double OUTTAKE_TIME_OUT = 10;
+        public final static double MANUAL_INTAKE_TIME_OUT = 1;
+        public final static double COAST_VELOCITY_AT_INPUT = 300;
+        //public final static int LIMIT_SWITCH_PORT = 7; //TODO: Change
+    }
 }

src/main/java/org/carlmontrobotics/Robot.java

@@ -4,6 +4,8 @@
 
 package org.carlmontrobotics;
 
+import org.carlmontrobotics.commands.CoralIntake;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -17,7 +19,6 @@
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
   private RobotContainer m_robotContainer;
-
   @Override
   public void robotInit() {
     m_robotContainer = new RobotContainer();

src/main/java/org/carlmontrobotics/RobotContainer.java

@@ -9,10 +9,24 @@
 // import org.carlmontrobotics.commands.*;
 import static org.carlmontrobotics.Constants.OI;
 
+import java.util.function.BooleanSupplier;
+
+import org.carlmontrobotics.Constants.OI;
+import org.carlmontrobotics.Constants.OI.Manipulator;
+import org.carlmontrobotics.commands.CoralIntake;
+import org.carlmontrobotics.commands.CoralOuttake;
+import org.carlmontrobotics.commands.StopCoralMotor;
+import org.carlmontrobotics.commands.TapCoralIn;
+import org.carlmontrobotics.commands.CoralIntakeManual;
+import org.carlmontrobotics.subsystems.CoralEffector;
+
+//limit switch
+import edu.wpi.first.wpilibj.DigitalInput;
 //controllers
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController.Axis;
-
+import edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 //commands
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -25,15 +39,24 @@
 import edu.wpi.first.wpilibj2.command.button.POVButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
+//constats
+import static org.carlmontrobotics.Constants.CoralEffectorc.*;
+import static org.carlmontrobotics.Constants.OI.Driver.*;
+import static org.carlmontrobotics.Constants.OI.Manipulator.*;
+
 public class RobotContainer {
 
   //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(OI.Driver.port);
-  public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port);
+  public final GenericHID driverController = new GenericHID(DRIVE_CONTROLLER_PORT);
+  public final GenericHID manipulatorController = new GenericHID(MANIPULATOR_CONTROLLER_PORT);
+  public final CoralEffector coralEffector = new CoralEffector();
 
-  public RobotContainer() {
+  //public final DigitalInput limitSwitch = new DigitalInput(LIMIT_SWITCH_PORT);
 
+  public RobotContainer() {
+    SmartDashboard.putData("Coral Intake", new CoralIntake(coralEffector));
+    SmartDashboard.putData("coral out", new CoralOuttake(coralEffector));
     setDefaultCommands();
     setBindingsDriver();
     setBindingsManipulator();
@@ -49,7 +72,26 @@ private void setDefaultCommands() {
     // ));
   }
   private void setBindingsDriver() {}
-  private void setBindingsManipulator() {}
+  private void setBindingsManipulator() {
+    // new JoystickButton(manipulatorController, OI.Manipulator.OUTAKE_BUTTON)
+    //   .whileTrue(new CoralOutake(coralEffector))
+    //   .whileFalse(new CoralIntake(coralEffector));
+    // new JoystickButton(manipulatorController, OI.Manipulator.INTAKE_BUTTON)
+    //   .whileTrue(new ManualCoralIntake());
+
+    axisTrigger(manipulatorController, Axis.kLeftTrigger)
+    .whileTrue(new CoralIntake(coralEffector))
+    .onFalse(new StopCoralMotor(coralEffector));
+
+
+    axisTrigger(manipulatorController, Axis.kRightTrigger)
+      .whileTrue(new CoralIntakeManual(coralEffector))
+      .onFalse(new StopCoralMotor(coralEffector));
+
+    new JoystickButton(manipulatorController, (Button.kA.value)).onTrue(new TapCoralIn(coralEffector));
+  }
+
+
 
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
@@ -89,4 +131,8 @@ private double inputProcessing(double value) {
   private double ProcessedAxisValue(GenericHID hid, Axis axis){
     return inputProcessing(getStickValue(hid, axis));
   }
+
+  private Trigger axisTrigger(GenericHID controller, Axis axis) {
+    return new Trigger( (BooleanSupplier)(() -> Math.abs(getStickValue(controller, axis)) > 0.2) );
+  }
 }

src/main/java/org/carlmontrobotics/commands/CoralIntake.java

@@ -0,0 +1,105 @@
+package org.carlmontrobotics.commands;
+
+import static org.carlmontrobotics.Constants.CoralEffectorc.*;
+import org.carlmontrobotics.subsystems.CoralEffector;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class CoralIntake extends Command {
+    // public static double spin;
+    // public static boolean fast2 = false;
+    Timer timer = new Timer();
+    // Timer timer2 = new Timer();
+    // Timer coralInTimer = new Timer();
+    public static double coralMotorPosition;
+    private CoralEffector coralEffector;
+    private boolean isSeen = false;
+    private boolean isTouching = false;
+    private boolean firstTime = true;
+    private boolean bruh = false;
+    public CoralIntake(CoralEffector coralEffector) {
+        // Use addRequirements() here to declare subsystem dependencies.
+        // addRequirements(this.CoralEffector = CoralEffector);
+        addRequirements(this.coralEffector = coralEffector);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+        coralEffector.setMotorSpeed(INPUT_FAST_SPEED);
+        timer.stop();
+        timer.reset();
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        if(firstTime) {
+        if(coralEffector.distanceSensorSeesCoral()) {
+            firstTime = false;
+            bruh = coralEffector.distanceSensorSeesCoral();
+        }
+
+        }
+        // if(coralEffector.getVel() <  COAST_VELOCITY_AT_INPUT && isSeen) {
+        //     isTouching=true;
+        // } else {
+        //     isTouching = false;
+        //}
+        // if (CoralEffector.distanceSensorSees) {
+        //     coralIn = true;
+        //     if (CoralEffector.limitSwitchSees) {
+        //         CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorSlowSpeed);
+        //         spin = CoralEffectorConstants.coralEffectorMotorSlowSpeed;
+        //     } else {
+        //         if (timer.get() < 0.15) {
+        //             CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorFastSpeed);
+        //             spin = CoralEffectorConstants.coralEffectorMotorFastSpeed;
+        //         } else {
+        //             CoralEffector.coralMotor.set(CoralEffectorConstants.coralEffectorMotorFastSpeed2);
+        //             spin = CoralEffectorConstants.coralEffectorMotorFastSpeed2;
+        //         }
+        //     }
+        // } else {
+        //     CoralEffector.coralMotor.set(0);
+        //     spin = 0;
+        //     timer.restart();
+        // }
+        // CoralEffector.coralMotor.set(0.1);
+        // spin = 10;
+        // SmartDashboard.putNumber("spin", spin);
+        //SmartDashboard.putNumber("timer", timer.get());
+        // SmartDashboard.getBoolean("outtakeGet", coralIn);
+
+        // if (coralEffector.coralIn){
+        //     coralEffector.setReferencePosition(coralMotorPosition + CORAL_EFFECTOR_DISTANCE_SENSOR_OFFSET); //rotations
+        // }
+        if (coralEffector.distanceSensorSeesCoral()){
+            coralEffector.setMotorSpeed(INPUT_SLOW_SPEED/2);
+            coralMotorPosition = coralEffector.getEncoderPos(); //mark the position in rotations
+            coralEffector.coralIn=true;
+            timer.start();
+        } else {
+            coralEffector.setMotorSpeed(INPUT_FAST_SPEED);
+        }
+
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        coralEffector.setMotorSpeed(0);
+        bruh = false;
+        if(interrupted != true) {
+            firstTime = true;
+        }
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return !coralEffector.distanceSensorSeesCoral() && bruh;
+    }
+}

src/main/java/org/carlmontrobotics/commands/CoralIntakeManual.java

@@ -0,0 +1,41 @@
+package org.carlmontrobotics.commands;
+
+import static org.carlmontrobotics.Constants.CoralEffectorc.*;
+import org.carlmontrobotics.subsystems.CoralEffector;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import org.carlmontrobotics.subsystems.CoralEffector;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class CoralIntakeManual extends Command {
+  /** Creates a new CoralIntakeManual. */
+  private CoralEffector coralEffector;
+  Timer timer = new Timer();
+  public CoralIntakeManual(CoralEffector coralEffector) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(this.coralEffector = coralEffector);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    coralEffector.setMotorSpeed(INPUT_FAST_SPEED);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return timer.get() > MANUAL_INTAKE_TIME_OUT;
+  }
+}

src/main/java/org/carlmontrobotics/commands/CoralOuttake.java

@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.carlmontrobotics.commands;
+
+import static org.carlmontrobotics.Constants.CoralEffectorc.*;
+import org.carlmontrobotics.subsystems.CoralEffector;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class CoralOuttake extends Command {
+  /** Creates a new SetCoralOut. */
+  private CoralEffector coralEffector;
+  Timer timer;
+  public CoralOuttake(CoralEffector coralEffector) {
+    addRequirements(this.coralEffector = coralEffector);
+    // Use addRequirements() here to declare subsystem dependencies.
+    timer = new Timer();
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    timer.reset();
+    timer.start();
+coralEffector.setMotorSpeed(-OUTPUT_SPEED);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+/*    if (coralEffector.coralDetected()) {
+      coralEffector.setMotorSpeed(CoralEffectorConstants.coralEffectorMotorOutputSpeed);
+      timer.reset();
+      timer.start();
+    }
+    else */
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    coralEffector.setMotorSpeed(0);
+    coralEffector.coralIn = false;
+    coralEffector.setIn(false);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return timer.get() > OUTTAKE_TIME_OUT || !coralEffector.distanceSensorSeesCoral();
+  }
+}

src/main/java/org/carlmontrobotics/commands/StopCoralMotor.java

@@ -0,0 +1,16 @@
+package org.carlmontrobotics.commands;
+
+import org.carlmontrobotics.subsystems.CoralEffector;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class StopCoralMotor extends Command {
+    CoralEffector coralEffector;
+    public StopCoralMotor(CoralEffector coralEffector) {
+        this.coralEffector=coralEffector;
+    }
+    @Override
+    public void initialize() {
+        coralEffector.setMotorSpeed(0);
+    }
+}