summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/main/java/frc/robot/Constants.java30
-rw-r--r--src/main/java/frc/robot/Robot.java3
-rw-r--r--src/main/java/frc/robot/RobotContainer.java74
-rw-r--r--src/main/java/frc/robot/commands/IntakeIntermediate.java32
-rw-r--r--src/main/java/frc/robot/subsystems/Arm.java67
-rw-r--r--src/main/java/frc/robot/subsystems/Chain.java6
-rw-r--r--src/main/java/frc/robot/subsystems/Drivetrain.java23
-rw-r--r--src/main/java/frc/robot/subsystems/Hooks.java18
-rw-r--r--src/main/java/frc/robot/subsystems/Intake.java12
-rw-r--r--src/main/java/frc/robot/subsystems/MainMech.java84
-rw-r--r--src/main/java/frc/robot/subsystems/Shooter.java1
-rw-r--r--src/main/java/frc/robot/util/MechState.java15
12 files changed, 261 insertions, 104 deletions
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index efff2b8..f6b7a4b 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -1,12 +1,7 @@
package frc.robot;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.I2C.Port;
@@ -14,8 +9,8 @@ import frc.robot.util.IMUGyro;
public class Constants
{
- public static final GenericHID controller = new GenericHID(0);
- public static final GenericHID controller2 = new GenericHID(1);
+ public static final GenericHID controller1 = new GenericHID(1);
+ public static final GenericHID controller2 = new GenericHID(0);
public static final double InputSmoothing = 0.1;
@@ -35,21 +30,23 @@ public class Constants
public static final TrapezoidProfile.Constraints motor_constraints =
new TrapezoidProfile.Constraints(DriveMaxSpeed, DriveMaxAcceleration);
- public static final SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(0.54082, 2.4705);
- public static final CANSparkMax motorL_leader = new CANSparkMax(11, MotorType.kBrushed);
- public static final CANSparkMax motorL_follower = new CANSparkMax(12, MotorType.kBrushed);
- public static final CANSparkMax motorR_leader = new CANSparkMax(13, MotorType.kBrushed);
- public static final CANSparkMax motorR_follower = new CANSparkMax(14, MotorType.kBrushed);
+ public static final double MotorkS = 0.54082;
+ public static final double MotorkV = 2.4705;
+
+ public static final int motorL_leader_ID = 11;
+ public static final int motorL_follower_ID = 12;
+ public static final int motorR_leader_ID = 13;
+ public static final int motorR_follower_ID = 14;
- public static final Encoder encoderL = new Encoder(6, 7);
- public static final Encoder encoderR = new Encoder(4, 5);
+ public static final int[] encoderL_PINS = {6, 7};
+ public static final int[] encoderR_PINS = {4, 5};
// for Intake
public static final int intakeT_ID = 32;
public static final int intakeD_ID = 31;
- // for Shooter CommandScheduler.getInstance().run();
+ // for Shooter
public static final int shooterT_ID = 21;
public static final int shooterD_ID = 22;
@@ -92,4 +89,7 @@ public class Constants
// Hooks
public static final int hookL_ID = 62;
public static final int hookR_ID = 61;
+
+ public static final int[] hookL_PINS = {0, 0};
+ public static final int[] hookR_PINS = {0, 0};
}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 2fc88fa..f7d7c3f 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -22,7 +22,8 @@ public class Robot extends LoggedRobot {
Logger.addDataReceiver(new NT4Publisher());
Logger.start();
- CameraServer.startAutomaticCapture();
+ CameraServer.startAutomaticCapture(0);
+ CameraServer.startAutomaticCapture(1);
m_robotContainer = new RobotContainer();
}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 88e493c..9a6241a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,32 +4,39 @@
package frc.robot;
-import com.fasterxml.jackson.databind.deser.std.ContainerDeserializerBase;
-
import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.subsystems.Arm;
-import frc.robot.subsystems.Chain;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Hooks;
import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.MainMech;
import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.MainMech.MechCmd;
public class RobotContainer
{
private final Drivetrain m_drive = new Drivetrain();
- private final Chain m_chain = new Chain();
- private final Arm m_arm = new Arm();
+ private final MainMech m_mech = new MainMech();
private final Intake m_intake = new Intake();
private final Shooter m_shooter = new Shooter();
private final Hooks m_hooks = new Hooks();
- private final GenericHID m_controller = Constants.controller;
+ private final GenericHID m_controller1 = Constants.controller1;
+ private final GenericHID m_controller2 = Constants.controller2;
+ SendableChooser<Command> m_chooser = new SendableChooser<>();
public RobotContainer()
{
+ m_chooser.setDefaultOption("None", Commands.none());
+ m_chooser.addOption("Shoot",
+ Commands.parallel(
+ m_shooter.run(() -> m_shooter.atPercentage(1)),
+ Commands.none().withTimeout(500).andThen(m_intake.run(() -> m_intake.atPercentage(1)))));
+ SmartDashboard.putData(m_chooser);
configureBindings();
}
@@ -63,21 +70,23 @@ public class RobotContainer
{
m_shooter.setDefaultCommand(m_shooter.run(() -> m_shooter.atPercentage(0.0)));
m_intake.setDefaultCommand(m_intake.run(() -> m_intake.atPercentage(0.0)));
- m_arm.setDefaultCommand(m_arm.run(() -> m_arm.incrementAngleDegrees(
- deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2)))));
m_hooks.setDefaultCommand(m_hooks.run(() -> m_hooks.atPercentage(
- deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(4)),
- deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(5)))));
- m_chain.setDefaultCommand(m_chain.run(() ->
- m_chain.atPercentage(
- Constants.controller2.getRawAxis(0)
- * (-0.5) + 0.5
- )));
+ deadzone(-0.1, 0.1, m_controller2.getRawAxis(4)),
+ deadzone(-0.1, 0.1, m_controller2.getRawAxis(5)))));
+
+ // m_mech.setDefaultCommands(
+ // m_mech.m_arm.run(() -> m_mech.m_arm.incrementAngle(
+ // deadzone(-0.05, 0.05, m_controller2.getRawAxis(2)) * 0.3)) ,
+ // m_mech.m_chain.run(() -> m_mech.m_chain.atPercentage(
+ // m_controller2.getRawAxis(0)
+ // * (-0.5) + 0.5))
+ // );
+ m_mech.setDefaultCommand(m_mech.run(() -> m_mech.toState(MechCmd.MANUAL).execute()));
m_drive.setDefaultCommand(m_drive.run(() -> {
double[] inputs = correctInputs(
- m_controller.getRawAxis(2),
- m_controller.getRawAxis(1));
+ m_controller1.getRawAxis(2),
+ m_controller1.getRawAxis(1));
m_drive.tank(
inputs[0] * Constants.DriveMaxSpeed,
inputs[1] * Constants.DriveMaxSpeed);
@@ -93,21 +102,18 @@ public class RobotContainer
// new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
- new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3)));
- new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(-0.1)));
- new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
- new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(1)));
-
- new Trigger(() -> Constants.controller2.getRawAxis(6) > 0).whileTrue(
- Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(0)),
- Commands.waitUntil(() -> m_arm.hasClearedAngle(-20)).andThen(m_chain.run(() -> m_chain.atPercentage(0.9)))
- .until(() -> m_chain.isAtPosition(0.9)).andThen(Commands.run(() -> m_arm.atAngleDegrees((-40))))));//.whileTrue(
- // Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(-30)), Commands.waitUntil(() -> m_arm.hasClearedAngleDegree(-20))
- // .andThen(() ->
- // m_chain.run(() -> m_chain.atPercentage(0.9)).until(() ->
- // m_chain.atPosition(0)).andThen(m_arm.run(() -> m_arm.atAngleDegrees(-40))))
- // ));
- // Commands.parallel(arm (chain at 0.waitUntil(arm > -30)).andThen());
+ new Trigger(() -> m_controller1.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3)));
+ new Trigger(() -> m_controller1.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.shoot()));
+ new Trigger(() -> m_controller1.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
+ new Trigger(() -> m_controller1.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.6)));
+
+ new Trigger(() -> m_controller2.getRawButton(1)).whileTrue(m_mech.toState(MechCmd.MANUAL));
+ new Trigger(() -> m_controller2.getRawAxis(6) > 0).whileTrue(m_mech.toState(MechCmd.INTAKE));
+ new Trigger(() -> m_controller2.getRawAxis(7) > 0).whileTrue(m_mech.toState(MechCmd.DEFAULT));
+ new Trigger(() -> m_controller2.getRawButton(4)).whileTrue(m_mech.toState(MechCmd.AMP));
+ new Trigger(() -> m_controller2.getRawButton(3)).whileTrue(m_mech.toState(MechCmd.SPEAKER));
+
+
// new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kReverse));
// new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kForward));
// new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kReverse));
@@ -121,6 +127,6 @@ public class RobotContainer
public Command getAutonomousCommand()
{
- return Commands.print("No autonomous command configured");
+ return m_chooser.getSelected();
}
}
diff --git a/src/main/java/frc/robot/commands/IntakeIntermediate.java b/src/main/java/frc/robot/commands/IntakeIntermediate.java
new file mode 100644
index 0000000..e8ea7ee
--- /dev/null
+++ b/src/main/java/frc/robot/commands/IntakeIntermediate.java
@@ -0,0 +1,32 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Chain;
+
+public class IntakeIntermediate extends Command
+{
+ public Arm m_arm;
+ public Chain m_chain;
+
+ public IntakeIntermediate(Arm arm, Chain chain)
+ {
+ m_arm = arm;
+ m_chain = chain;
+
+ addRequirements(arm, chain);
+ }
+
+ @Override
+ public void execute()
+ {
+ m_arm.atAngleDegrees(-10);
+ m_chain.atPercentage(1);
+ }
+
+ @Override
+ public boolean isFinished()
+ {
+ return m_arm.isAtAngleDegree(-10) && m_chain.isAtPosition(1);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 75e8ad2..0b8814d 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -8,7 +8,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
@@ -16,7 +15,6 @@ import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.PIDCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;
@@ -105,15 +103,9 @@ public class Arm extends SubsystemBase
m_pidR.reset(Constants.arm_initial_position);
}
-
- public void incrementAngleDegrees(double amount) {
- incrementAngle(amount * Math.PI / 180.0);
- }
-
- public void incrementAngle(double amount) {
- atAngle(m_setpoint + amount);
+ public double pos() {
+ return Math.min(m_encoderL.getPosition(), m_encoderR.getPosition()) * 180 / Math.PI;
}
-
// in rad
public void atAngle(double angle)
@@ -130,26 +122,36 @@ public class Arm extends SubsystemBase
m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
);
}
+
+ public void voltage(double leftVoltsPID, double leftVoltsFFW, double rightVoltsPID, double rightVoltsFFW)
+ {
+ Logger.recordOutput(getName() + "/leftPID", leftVoltsPID);
+ Logger.recordOutput(getName() + "/rightPID", rightVoltsPID);
+ Logger.recordOutput(getName() + "/leftFFW", leftVoltsFFW);
+ Logger.recordOutput(getName() + "/rightFFW", rightVoltsFFW);
+
+ m_armL.setVoltage(MathUtil.clamp(leftVoltsPID + leftVoltsFFW, -4, 4));
+ m_armR.setVoltage(MathUtil.clamp(rightVoltsPID + rightVoltsFFW, -4, 4));
+ }
+
// in deg
- public void atAngleDegrees(double angle)
+ public void incrementAngleDegrees(double amount)
{
- atAngle(angle * Math.PI / 180.0);
+ incrementAngle(amount * Math.PI / 180.0);
}
// in rad
- public boolean hasClearedAngle(double angle)
+ public void incrementAngle(double amount)
{
- if(m_encoderL.getPosition() > angle
- && m_encoderR.getPosition() > angle)
- return true;
-
- return false;
+ atAngle(m_setpoint + amount);
}
- public boolean hasClearedAngleDegree(double angle)
+ // in deg
+ public void atAngleDegrees(double angle)
{
- return hasClearedAngle(angle * Math.PI/180);
+ atAngle(angle * Math.PI / 180.0);
}
+
// in rad
public boolean isAtAngle(double angle)
{
@@ -160,21 +162,27 @@ public class Arm extends SubsystemBase
return false;
}
+ // in deg
public boolean isAtAngleDegree(double angle)
{
return isAtAngle(angle * Math.PI/180);
}
- public void voltage(double leftVoltsPID, double leftVoltsFFW, double rightVoltsPID, double rightVoltsFFW)
- {
- Logger.recordOutput(getName() + "/leftPID", leftVoltsPID);
- Logger.recordOutput(getName() + "/rightPID", rightVoltsPID);
- Logger.recordOutput(getName() + "/leftFFW", leftVoltsFFW);
- Logger.recordOutput(getName() + "/rightFFW", rightVoltsFFW);
+ // in rad
+ // public boolean hasClearedAngle(double angle)
+ // {
+ // if(m_encoderL.getPosition() > angle
+ // && m_encoderR.getPosition() > angle)
+ // return true;
+
+ // return false;
+ // }
+
+ // public boolean hasClearedAngleDegree(double angle)
+ // {
+ // return hasClearedAngle(angle * Math.PI/180);
+ // }
- m_armL.setVoltage(MathUtil.clamp(leftVoltsPID + leftVoltsFFW, -4, 4));
- m_armR.setVoltage(MathUtil.clamp(rightVoltsPID + rightVoltsFFW, -4, 4));
- }
@Override
public void periodic()
@@ -183,6 +191,7 @@ public class Arm extends SubsystemBase
Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition() * 180/Math.PI);
}
+ // Sysid
public Command sysIdQausistatic(SysIdRoutine.Direction direction)
{
return m_sysIdRoutine.quasistatic(direction);
diff --git a/src/main/java/frc/robot/subsystems/Chain.java b/src/main/java/frc/robot/subsystems/Chain.java
index 40235fe..3ee7108 100644
--- a/src/main/java/frc/robot/subsystems/Chain.java
+++ b/src/main/java/frc/robot/subsystems/Chain.java
@@ -20,10 +20,6 @@ public class Chain extends SubsystemBase
public Chain()
{
- // TODO: figure out invertedness
-
- // m_motor.setInverted(true);
-
m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
m_encoder.reset();
}
@@ -36,9 +32,9 @@ public class Chain extends SubsystemBase
// perc between 0 and 1;
public void atPercentage(double perc)
{
- // System.out.println(perc);
Logger.recordOutput(getName() + "/perc", perc);
double pid_out = m_pid.calculate(m_encoder.getDistance(), perc);
+
Logger.recordOutput(getName() + "/pidOUt", pid_out);
m_motor.setVoltage(MathUtil.clamp(pid_out, -6, 6));
}
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index aee9597..66d1ba6 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -1,6 +1,7 @@
package frc.robot.subsystems;
import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -40,15 +41,15 @@ public class Drivetrain extends SubsystemBase
{
private IMUGyro m_gyro = Constants.gyro;
- private CANSparkMax m_motorL_leader = Constants.motorL_leader;
- private CANSparkMax m_motorL_follower = Constants.motorL_follower;
- private CANSparkMax m_motorR_leader = Constants.motorR_leader;
- private CANSparkMax m_motorR_follower = Constants.motorR_follower;
+ private CANSparkMax m_motorL_leader = new CANSparkMax(Constants.motorL_leader_ID, MotorType.kBrushed);
+ private CANSparkMax m_motorL_follower = new CANSparkMax(Constants.motorL_follower_ID, MotorType.kBrushed);
+ private CANSparkMax m_motorR_leader = new CANSparkMax(Constants.motorR_leader_ID, MotorType.kBrushed);
+ private CANSparkMax m_motorR_follower = new CANSparkMax(Constants.motorR_follower_ID, MotorType.kBrushed);
- private Encoder m_encoderL = Constants.encoderL;
- private Encoder m_encoderR = Constants.encoderR;
+ private Encoder m_encoderL = new Encoder(Constants.encoderL_PINS[0], Constants.encoderL_PINS[1]);
+ private Encoder m_encoderR = new Encoder(Constants.encoderR_PINS[0], Constants.encoderR_PINS[1]);
- private SimpleMotorFeedforward m_feedforward = Constants.motor_feedforward;
+ private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(Constants.MotorkS, Constants.MotorkV);
private final double kP = 0.363;
@@ -116,8 +117,8 @@ public class Drivetrain extends SubsystemBase
m_odometry = new DifferentialDriveOdometry(
m_gyro.toRotation2d(),
- Constants.encoderL.getDistance(),
- Constants.encoderR.getDistance());
+ m_encoderL.getDistance(),
+ m_encoderR.getDistance());
}
// speed - linear velocity in m/s
@@ -168,8 +169,8 @@ public class Drivetrain extends SubsystemBase
m_odometry = new DifferentialDriveOdometry(
m_gyro.toRotation2d(),
- Constants.encoderL.getDistance(),
- Constants.encoderR.getDistance());
+ m_encoderL.getDistance(),
+ m_encoderR.getDistance());
}
@Override
diff --git a/src/main/java/frc/robot/subsystems/Hooks.java b/src/main/java/frc/robot/subsystems/Hooks.java
index b6c26c5..6e42322 100644
--- a/src/main/java/frc/robot/subsystems/Hooks.java
+++ b/src/main/java/frc/robot/subsystems/Hooks.java
@@ -1,25 +1,29 @@
package frc.robot.subsystems;
-import java.lang.constant.Constable;
+import org.littletonrobotics.junction.Logger;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;
import frc.robot.Constants;
-
-
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Hooks extends SubsystemBase
{
+ // initial angle 237
private final CANSparkMax m_hookL = new CANSparkMax(Constants.hookL_ID, MotorType.kBrushed);
private final CANSparkMax m_hookR = new CANSparkMax(Constants.hookR_ID, MotorType.kBrushed);
+ private final Encoder m_hookL_encoder = new Encoder(Constants.hookL_PINS[0], Constants.hookL_PINS[1]);
+ private final Encoder m_hookR_encoder = new Encoder(Constants.hookR_PINS[0], Constants.hookR_PINS[1]);
+
public Hooks()
{
// TODO: possibly invert to move forward upon positive votage
- // TODO: invert one hook
m_hookL.setInverted(true);
+ m_hookL_encoder.setReverseDirection(true);
}
public void atPercentage(double percL, double percR)
@@ -28,4 +32,10 @@ public class Hooks extends SubsystemBase
m_hookR.set(percR);
}
+ @Override
+ public void periodic()
+ {
+ Logger.recordOutput(getName() + "/encoderL", m_hookL_encoder.getDistance());
+ Logger.recordOutput(getName() + "/encoderR", m_hookR_encoder.getDistance());
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
index 41620bb..036aae0 100644
--- a/src/main/java/frc/robot/subsystems/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake.java
@@ -11,8 +11,8 @@ import frc.robot.Constants;
public class Intake extends SubsystemBase
{
- public static final CANSparkMax m_intakeT = new CANSparkMax(Constants.intakeT_ID, MotorType.kBrushless);
- public static final CANSparkMax m_intakeD = new CANSparkMax(Constants.intakeD_ID, MotorType.kBrushless);
+ private final CANSparkMax m_intakeT = new CANSparkMax(Constants.intakeT_ID, MotorType.kBrushless);
+ private final CANSparkMax m_intakeD = new CANSparkMax(Constants.intakeD_ID, MotorType.kBrushless);
private final RelativeEncoder m_intakeD_encoder = m_intakeD.getEncoder();
public Intake()
@@ -26,17 +26,19 @@ public class Intake extends SubsystemBase
atPercentage(perc, perc);
}
+ // perc is between 0.0 and 1.0
public void atPercentage(double upPerc, double downPerc)
{
m_intakeT.set(upPerc);
m_intakeD.set(downPerc);
}
+
public void shoot()
{
- if (m_intakeD_encoder.getVelocity() > 2000) {
- m_intakeT.set(1);
+ if (Math.abs(m_intakeD_encoder.getVelocity()) > 2000) {
+ m_intakeT.set(-0.5);
}
- m_intakeD.set(1);
+ m_intakeD.set(-0.5);
}
@Override
diff --git a/src/main/java/frc/robot/subsystems/MainMech.java b/src/main/java/frc/robot/subsystems/MainMech.java
new file mode 100644
index 0000000..1e31892
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/MainMech.java
@@ -0,0 +1,84 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.commands.IntakeIntermediate;
+import frc.robot.util.MechState;
+
+public class MainMech extends SubsystemBase
+{
+ public final Arm m_arm = new Arm();
+ public final Chain m_chain = new Chain();
+
+ public enum MechCmd {
+ INTAKE,
+ SPEAKER,
+ AMP,
+ DEFAULT,
+ MANUAL
+ }
+
+ private double deadzone(double low, double high, double val)
+ {
+ if(val > low && val < high) return 0.0;
+ return ((val > 0.0) ? val - high : val - low);
+ }
+ public MechCmd current_state = MechCmd.SPEAKER;
+
+ public Command m_manual_cmd = this.run(() -> {
+
+ });
+
+ public MechState states[] = {
+ new MechState(0.95, -40),
+ new MechState(0, -42),
+ new MechState(0.95, 54.5),
+ new MechState(0.5, -20),
+ new MechState(0.5, 20)
+ };
+
+ public MainMech() { }
+
+ public void setDefaultCommands(Command arm_command, Command chain_command)
+ {
+ m_arm.setDefaultCommand(arm_command);
+ m_chain.setDefaultCommand(chain_command);
+ }
+
+ public Command command(MechState state) {
+
+ return Commands.parallel(
+ m_arm.run(() -> m_arm.atAngleDegrees(state.m_arm_pos)),
+ m_chain.run(() -> m_chain.atPercentage(state.m_chain_pos)));
+ }
+
+ public Command toState(MechCmd state)
+ {
+ MechState next = states[state.ordinal()];
+ if (next.isValid()) {
+ Command next_cmd = command(next);
+ if(state == MechCmd.INTAKE || current_state == MechCmd.INTAKE) {
+ current_state = state;
+ return new IntakeIntermediate(m_arm, m_chain).andThen(next_cmd);
+ }
+ if (state == MechCmd.MANUAL) {
+ current_state = state;
+ double arm_pos = m_arm.pos() + deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2));
+ double chain_pos = Constants.controller2.getRawAxis(0) * (-0.5) + 0.5;
+ MechState manual_state = new MechState(chain_pos, arm_pos);
+ if (manual_state.isValid()) {
+ return command(manual_state);
+ }
+ return Commands.none();
+ }
+ current_state = state;
+ return next_cmd;
+ }
+ return Commands.none();
+ }
+
+ @Override
+ public void periodic() { }
+}
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index 677073c..273e5bd 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -15,6 +15,7 @@ public class Shooter extends SubsystemBase
public Shooter() {}
+ // perc is betwen 0.0 and 1.0
public void atPercentage(double perc)
{
atPercentage(perc, perc);
diff --git a/src/main/java/frc/robot/util/MechState.java b/src/main/java/frc/robot/util/MechState.java
new file mode 100644
index 0000000..f0092f0
--- /dev/null
+++ b/src/main/java/frc/robot/util/MechState.java
@@ -0,0 +1,15 @@
+package frc.robot.util;
+
+public class MechState {
+ public double m_chain_pos;
+ public double m_arm_pos;
+ public MechState(double chain_pos, double arm_pos) {
+ m_chain_pos = chain_pos;
+ m_arm_pos = arm_pos;
+ }
+ // TODO
+ public boolean isValid() {
+ return 30 > (Math.cos(m_arm_pos * Math.PI / 180) * 60 - m_arm_pos * 80);
+ // return true;
+ }
+}
generated by cgit v1.2.3 (git 2.25.1) at 2025-10-31 06:53:16 +0000