diff --git a/.idea/gradle.xml b/.idea/gradle.xml
index 08d18989..580e82e1 100644
--- a/.idea/gradle.xml
+++ b/.idea/gradle.xml
@@ -4,8 +4,11 @@
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
index 3ea5c515..66d322ef 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -1,7 +1,7 @@
-
+
diff --git a/build.gradle b/build.gradle
index 33f95ab3..4fec55bc 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,5 +1,3 @@
-import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO;
-
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3"
@@ -100,8 +98,7 @@ dependencies {
implementation 'com.sparkjava:spark-core:2.9.4'
implementation group: 'com.google.code.gson', name: 'gson', version: '2.8.6'
-
- implementation 'com.github.galaxia5987:common:1181c49'
+ implementation files('libs/common.jar')
}
test {
@@ -141,10 +138,10 @@ tasks.withType(JavaCompile) {
// Create version file
project.compileJava.dependsOn(createVersionFile)
gversion {
- srcDir = "src/main/java/"
+ srcDir = "src/main/java/"
classPackage = "frc.robot"
- className = "BuildConstants"
- dateFormat = "yyyy-MM-dd HH:mm:ss z"
- timeZone = "America/New_York"
- indent = " "
+ className = "BuildConstants"
+ dateFormat = "yyyy-MM-dd HH:mm:ss z"
+ timeZone = "America/New_York"
+ indent = " "
}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 9bd027d0..4ddd1c5d 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -9,13 +9,14 @@
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.intake.Intake;
import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.LoggedRobot;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.NT4Publisher;
import org.littletonrobotics.junction.wpilog.WPILOGReader;
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
-import utils.math.differential.BooleanTrigger;
+import lib.math.differential.BooleanTrigger;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a45786eb..9373b6d8 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -3,33 +3,36 @@
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.commandgroups.PickUpCubeTeleop;
-import frc.robot.commandgroups.ReturnIntake;
import frc.robot.subsystems.arm.Arm;
-import frc.robot.subsystems.arm.ArmConstants;
-import frc.robot.subsystems.arm.ArmPosition;
-import frc.robot.subsystems.arm.commands.ArmAxisControl;
-import frc.robot.subsystems.arm.commands.ArmWithSpline;
import frc.robot.subsystems.gripper.Gripper;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.intake.commands.HoldIntakeInPlace;
import frc.robot.subsystems.intake.commands.ProximitySensorDefaultCommand;
import frc.robot.subsystems.intake.commands.Retract;
import frc.robot.subsystems.leds.Leds;
-import swerve.SwerveDrive;
-import swerve.commands.XboxDrive;
-import utils.Utils;
+import frc.robot.swerve.SwerveDrive;
+import frc.robot.swerve.commands.XboxDrive;
+import lib.Utils;
+
public class RobotContainer {
private static RobotContainer INSTANCE = null;
- private final Arm arm = Arm.getINSTANCE();
+
+ static {
+ SwerveDrive.setInstance(Robot.isReal(),
+ Ports.SwerveDrive.DRIVE_IDS,
+ Ports.SwerveDrive.ANGLE_IDS,
+ Ports.SwerveDrive.ENCODER_IDS);
+ }
+
+// private final Arm arm = Arm.getINSTANCE();
private final Leds leds = Leds.getInstance();
- private final SwerveDrive swerveDrive = SwerveDrive.getInstance(Robot.isReal());
+ private final SwerveDrive swerveDrive = SwerveDrive.getInstance();
private final Intake intake = Intake.getInstance();
- private final Gripper gripper = Gripper.getInstance();
+// private final Gripper gripper = Gripper.getInstance();
private final XboxController xboxController = new XboxController(0);
private final Joystick leftJoystick = new Joystick(1);
private final Joystick rightJoystick = new Joystick(2);
@@ -82,30 +85,11 @@ private void configureDefaultCommands() {
}
private void configureButtonBindings() {
- leftJoystickTrigger.onTrue(new InstantCommand(swerveDrive::resetGyro));
- y.whileTrue(new ArmWithSpline(ArmPosition.TOP_SCORING));
- x.whileTrue(new ArmWithSpline(ArmPosition.MIDDLE_SCORING));
- b.whileTrue(new ArmWithSpline(ArmPosition.FEEDER));
- back.whileTrue(new ArmWithSpline(ArmPosition.FEEDER_CONE));
- a.whileTrue(new ArmWithSpline(ArmPosition.NEUTRAL));
-
- lb.onTrue(new InstantCommand(gripper::toggle));
-
- start.onTrue(new InstantCommand(leds::toggle));
-
- leftJoystickTopBottom.toggleOnTrue(
- new InstantCommand(swerveDrive::lock, swerveDrive)
- );
- leftJoystickTopBottom.onTrue(
- new InstantCommand(leds::toggleRainbow)
- );
-
- xboxLeftTrigger.whileTrue(new PickUpCubeTeleop())
- .onFalse(new Retract(Retract.Mode.UP).andThen(new InstantCommand(() -> intake.setSpinMotorPower(0))));
- xboxRightTrigger.whileTrue(new ReturnIntake());
+ a.whileTrue(new RunCommand(()-> intake.setSpinMotorPower(0.5)));
+ b.whileTrue(new RunCommand(()-> intake.setSpinMotorPower(-0.5)));
+ x.whileTrue(new RunCommand(()-> intake.setAnglePower(0.5)));
+ y.whileTrue(new RunCommand(()-> intake.setAnglePower(-0.5)));
- rb.whileTrue(new ArmAxisControl(0.02, 0.02)
- .until(() -> gripper.getDistance() < ArmConstants.FEEDER_DISTANCE));
}
/**
diff --git a/src/main/java/frc/robot/commandgroups/FeedTeleop.java b/src/main/java/frc/robot/commandgroups/FeedTeleop.java
index 633fa490..023fbb85 100644
--- a/src/main/java/frc/robot/commandgroups/FeedTeleop.java
+++ b/src/main/java/frc/robot/commandgroups/FeedTeleop.java
@@ -1,5 +1,6 @@
package frc.robot.commandgroups;
+import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc.robot.subsystems.intake.Intake;
@@ -12,7 +13,7 @@ public FeedTeleop(boolean outtake) {
addCommands(
new Retract(Retract.Mode.DOWN)
- .andThen(new RunCommand(() -> intake.setAngleMotorAngle(IntakeConstants.ANGLE_DOWN))),
+ .andThen(new RunCommand(() -> intake.setAngleMotorAngle(Rotation2d.fromDegrees(IntakeConstants.ANGLE_DOWN)))),
intake.run(outtake ? -IntakeConstants.INTAKE_POWER : IntakeConstants.INTAKE_POWER)
);
}
diff --git a/src/main/java/frc/robot/commandgroups/bits/CheckSwerve.java b/src/main/java/frc/robot/commandgroups/bits/CheckSwerve.java
index f65ebf49..bc1ad389 100644
--- a/src/main/java/frc/robot/commandgroups/bits/CheckSwerve.java
+++ b/src/main/java/frc/robot/commandgroups/bits/CheckSwerve.java
@@ -3,11 +3,11 @@
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.Robot;
-import swerve.SwerveDrive;
+import frc.robot.swerve.SwerveDrive;
public class CheckSwerve extends SequentialCommandGroup {
public CheckSwerve() {
- SwerveDrive swerveDrive = SwerveDrive.getInstance(Robot.isReal());
+ SwerveDrive swerveDrive = SwerveDrive.getInstance();
addCommands(
new RunCommand(swerveDrive::checkSwerve, swerveDrive).withTimeout(5),
new ZeroPositionSwerve().withTimeout(2)
diff --git a/src/main/java/frc/robot/commandgroups/bits/ZeroPositionSwerve.java b/src/main/java/frc/robot/commandgroups/bits/ZeroPositionSwerve.java
index 58c94755..42fc3726 100644
--- a/src/main/java/frc/robot/commandgroups/bits/ZeroPositionSwerve.java
+++ b/src/main/java/frc/robot/commandgroups/bits/ZeroPositionSwerve.java
@@ -4,12 +4,12 @@
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Robot;
-import swerve.SwerveDrive;
+import frc.robot.swerve.SwerveDrive;
import java.util.Arrays;
public class ZeroPositionSwerve extends Command {
- private final SwerveDrive swerve = SwerveDrive.getInstance(Robot.isReal());
+ private final SwerveDrive swerve = SwerveDrive.getInstance();
private final Timer timer = new Timer();
private final SwerveModuleState[] zeroStates = new SwerveModuleState[4];
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmKinematics.java b/src/main/java/frc/robot/subsystems/arm/ArmKinematics.java
index 9b821095..76ae7f98 100644
--- a/src/main/java/frc/robot/subsystems/arm/ArmKinematics.java
+++ b/src/main/java/frc/robot/subsystems/arm/ArmKinematics.java
@@ -1,7 +1,7 @@
package frc.robot.subsystems.arm;
import edu.wpi.first.math.geometry.Translation2d;
-import utils.math.AngleUtil;
+import lib.math.AngleUtil;
/**
* This class contains the kinematics for the arm.
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmPosition.java b/src/main/java/frc/robot/subsystems/arm/ArmPosition.java
index fe16c1a6..ef3b68db 100644
--- a/src/main/java/frc/robot/subsystems/arm/ArmPosition.java
+++ b/src/main/java/frc/robot/subsystems/arm/ArmPosition.java
@@ -2,8 +2,8 @@
import edu.wpi.first.math.geometry.Translation2d;
import frc.robot.subsystems.leds.Leds;
-import utils.math.Vector2;
-import utils.math.spline.QuinticBezierSpline;
+import lib.math.Vector2;
+import lib.math.spline.QuinticBezierSpline;
public enum ArmPosition {
diff --git a/src/main/java/frc/robot/subsystems/arm/commands/ArmWithSpline.java b/src/main/java/frc/robot/subsystems/arm/commands/ArmWithSpline.java
index c67d3e3b..21a8819a 100644
--- a/src/main/java/frc/robot/subsystems/arm/commands/ArmWithSpline.java
+++ b/src/main/java/frc/robot/subsystems/arm/commands/ArmWithSpline.java
@@ -5,8 +5,8 @@
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.arm.Arm;
import frc.robot.subsystems.arm.ArmPosition;
-import utils.math.Vector2;
-import utils.math.spline.QuinticBezierSpline;
+import lib.math.Vector2;
+import lib.math.spline.QuinticBezierSpline;
public class ArmWithSpline extends Command {
diff --git a/src/main/java/frc/robot/subsystems/arm/commands/ArmXboxControl.java b/src/main/java/frc/robot/subsystems/arm/commands/ArmXboxControl.java
index 3a0ce63a..9c7abea3 100644
--- a/src/main/java/frc/robot/subsystems/arm/commands/ArmXboxControl.java
+++ b/src/main/java/frc/robot/subsystems/arm/commands/ArmXboxControl.java
@@ -5,8 +5,8 @@
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Robot;
import frc.robot.subsystems.arm.Arm;
-import utils.Utils;
-import utils.math.differential.BooleanTrigger;
+import lib.Utils;
+import lib.math.differential.BooleanTrigger;
public class ArmXboxControl extends Command {
private final Arm arm = Arm.getINSTANCE();
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index 6281c48d..88d9c57a 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -2,6 +2,7 @@
import com.ctre.phoenix.motorcontrol.ControlMode;
import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
@@ -14,6 +15,7 @@
import frc.robot.Robot;
import frc.robot.subsystems.intake.commands.HoldIntakeInPlace;
import org.littletonrobotics.junction.Logger;
+import lib.Utils;
public class Intake extends SubsystemBase {
private static Intake INSTANCE;
@@ -70,26 +72,26 @@ public void setSpinMotorPower(double power) {
inputs.setpointSpinMotorPower = power;
}
- public double getAngleMotorAngle() {
+ public Rotation2d getAngleMotorAngle() {
return inputs.angleMotorAngle;
}
/**
* Sets the angles position.
*
- * @param angle is the angle of the retractor. [degrees]
+ * @param angleMotorAngle is the angle of the retractor. [degrees]
*/
- public void setAngleMotorAngle(double angle) {
- inputs.setpointAngleMotorAngle = Math.toRadians(angle);
+ public void setAngleMotorAngle(Rotation2d angleMotorAngle) {
+ inputs.setpointAngleMotorAngle = angleMotorAngle;
angleMode = ControlMode.Position;
}
- private double getAngleMotorVelocity() {
+ private Rotation2d getAngleMotorVelocity() {
return inputs.angleMotorVelocity;
}
public double getCurrent() {
- return inputs.angleMotorcurrent;
+ return inputs.angleMotorAppliedCurrent;
}
public void setAnglePower(double power) {
@@ -103,9 +105,12 @@ public void resetEncoder(double angle) {
public Command lowerIntake() {
return new InstantCommand(() -> resetEncoder(IntakeConstants.ANGLE_UP), this)
- .andThen(new RunCommand(() -> setAngleMotorAngle(-40), this));
+ .andThen(new RunCommand(() -> setAngleMotorAngle(Rotation2d.fromDegrees(-40)), this));
}
+ // You Jerk :) 😎
+ // I'm gonna kill them all.
+ // All the juice must die
public Command run(double power) {
return new RunCommand(() -> this.setSpinMotorPower(power));
}
@@ -127,9 +132,11 @@ public void periodic() {
!(lastCommand instanceof HoldIntakeInPlace);
lastCommand = currentCommand;
- intakeP1.setAngle(Math.toDegrees(getAngleMotorAngle()) + IntakeConstants.INTAKE_MECH_OFFSET);
+ intakeP1.setAngle(getAngleMotorAngle().getDegrees() + IntakeConstants.INTAKE_MECH_OFFSET);
- Logger.recordOutput("IntakePose", getPose3d(getAngleMotorAngle() + IntakeConstants.INTAKE_SIM_ANGLE_OFFSET));
+
+ var intake3d = getPose3d(getAngleMotorAngle().getRadians() + Math.toRadians(IntakeConstants.INTAKE_SIM_ANGLE_OFFSET)); //TODO: ??!?!! check
+ Logger.recordOutput("IntakePose", Utils.pose3dToArray(intake3d));
Logger.processInputs("Intake", inputs);
SmartDashboard.putData("IntakeMech", mech);
}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
index ac5b99a2..a4481574 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -1,15 +1,17 @@
package frc.robot.subsystems.intake;
+import com.ctre.phoenix6.configs.Slot0Configs;
import edu.wpi.first.math.geometry.Translation2d;
-import utils.units.Units;
+import lib.units.Units;
public class IntakeConstants {
public static final double ANGLE_GEAR_RATIO = 35.26;
public static final double SPIN_GEAR_RATIO = 10; //Not a real value
- public static final double kP = 2;
+ public static final double kP = 0.1*ANGLE_GEAR_RATIO;
public static final double kI = 0;
- public static final double kD = 0.2;
- public static final double kF = 0;
+ public static final double kD = 0;
+ public static final double kF = 0; // not relevant anymore
+ public static final Slot0Configs PIDGains = new Slot0Configs().withKP(IntakeConstants.kP).withKI(IntakeConstants.kI).withKD(IntakeConstants.kD);
public static final double FALCON_TICKS_PER_ROTATION = 2048;
public static final double TICKS_PER_DEGREE = FALCON_TICKS_PER_ROTATION / 360 * ANGLE_GEAR_RATIO;
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
index 650da484..209e6663 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -1,11 +1,13 @@
package frc.robot.subsystems.intake;
+import edu.wpi.first.math.geometry.Rotation2d;
+
public interface IntakeIO {
void updateInputs(IntakeLoggedInputs inputs);
void setSpinMotorPower(double power);
- void setAngleMotorAngle(double angle);
+ void setAngleMotorAngle(Rotation2d angle);
void setAngleMotorPower(double power);
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
index 99c06690..559e7f30 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
@@ -1,24 +1,40 @@
package frc.robot.subsystems.intake;
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.PowerDistribution;
import frc.robot.Constants;
import frc.robot.Ports;
-import utils.units.UnitModel;
+
+import javax.swing.text.Position;
+
public class IntakeIOReal implements IntakeIO {
+
+
private final CANSparkMax spinMotor = new CANSparkMax(Ports.Intake.INTAKE_MOTOR, CANSparkMaxLowLevel.MotorType.kBrushless);
- private final TalonFX angleMotor = new TalonFX(Ports.Intake.ANGLE_MOTOR);
- private final UnitModel unitModel = new UnitModel(IntakeConstants.TICKS_PER_DEGREE);
+ private final TalonFX angleMotor = new TalonFX(21);
+ private final TalonFXConfigurator angleConfigurator;
+ private TalonFXConfiguration angleConfiguration = new TalonFXConfiguration();
+
+ private final DutyCycleOut dutyCycleRequest = new DutyCycleOut(0);
+ private final PositionVoltage positionRequest = new PositionVoltage(0);
public IntakeIOReal() {
- angleMotor.configFactoryDefault();
- spinMotor.restoreFactoryDefaults();
+ angleConfiguration.Feedback.SensorToMechanismRatio = IntakeConstants.ANGLE_GEAR_RATIO;
+ angleConfiguration.Feedback.RotorToSensorRatio = 1;
+ angleConfigurator = angleMotor.getConfigurator();
+ angleConfigurator.apply(angleConfiguration.Slot0);
+
+ spinMotor.restoreFactoryDefaults();
spinMotor.setIdleMode(CANSparkMax.IdleMode.kCoast);
spinMotor.enableVoltageCompensation(Constants.NOMINAL_VOLTAGE);
spinMotor.setInverted(Ports.Intake.POWER_INVERTED);
@@ -26,49 +42,40 @@ public IntakeIOReal() {
spinMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.fromId(i), 50000);
}
spinMotor.burnFlash();
-
- angleMotor.setNeutralMode(NeutralMode.Brake);
- angleMotor.enableVoltageCompensation(true);
- angleMotor.configVoltageCompSaturation(Constants.NOMINAL_VOLTAGE);
- angleMotor.setInverted(Ports.Intake.ANGLE_INVERTED);
- angleMotor.config_kP(0, IntakeConstants.kP);
- angleMotor.config_kI(0, IntakeConstants.kI);
- angleMotor.config_kD(0, IntakeConstants.kD);
- angleMotor.config_kF(0, IntakeConstants.kF);
- angleMotor.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(
- true, 30, 0, 0
- ));
- angleMotor.configClosedLoopPeakOutput(0, 0.4);
+ System.out.println(angleMotor);
}
+
@Override
public void setSpinMotorPower(double power) {
spinMotor.set(power);
}
@Override
- public void setAngleMotorAngle(double angle) {
- angleMotor.set(ControlMode.Position, angle);
+ public void setAngleMotorAngle(Rotation2d angle) {
+ angleMotor.setControl(positionRequest.withPosition(angle.getRotations()).withEnableFOC(true));
}
@Override
public void setAngleMotorPower(double power) {
- angleMotor.set(ControlMode.PercentOutput, power);
+ angleMotor.setControl(dutyCycleRequest.withOutput(power).withEnableFOC(true));
}
@Override
public void resetEncoder(double angle) {
- angleMotor.setSelectedSensorPosition(
- unitModel.toTicks(angle)
- );
}
@Override
public void updateInputs(IntakeLoggedInputs inputs) {
+ var powerDistribution = new PowerDistribution(0, PowerDistribution.ModuleType.kRev);
+
+ inputs.angleMotorAppliedVoltage = angleMotor.getMotorVoltage().getValue();
+ inputs.angleMotorAppliedCurrent = angleMotor.getSupplyCurrent().getValue();
+ inputs.angleMotorPower = inputs.angleMotorAppliedVoltage / powerDistribution.getVoltage();
+ inputs.angleMotorAngle = Rotation2d.fromRotations(angleMotor.getPosition().getValue());
+ inputs.angleMotorVelocity = Rotation2d.fromRotations(angleMotor.getVelocity().getValue());
inputs.spinMotorPower = spinMotor.getAppliedOutput();
- inputs.angleMotorAngle = unitModel.toUnits(angleMotor.getSelectedSensorPosition());
- inputs.angleMotorVelocity = unitModel.toVelocity(angleMotor.getSelectedSensorVelocity());
- inputs.angleMotorcurrent = angleMotor.getSupplyCurrent();
- inputs.angleMotorPower = angleMotor.getMotorOutputPercent();
+ inputs.spinMotorAppliedCurrent = spinMotor.getOutputCurrent();
+ inputs.spinMotorAppliedVoltage = spinMotor.getVoltageCompensationNominalVoltage();
}
}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
index 1dce2ee9..78a201fc 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
@@ -1,53 +1,66 @@
package frc.robot.subsystems.intake;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.simulation.FlywheelSim;
-import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Timer;
+import lib.motors.TalonFXSim;
public class IntakeIOSim implements IntakeIO {
- private final FlywheelSim spinMotor;
- private final SingleJointedArmSim angleMotor;
- private final PIDController angleFeedback;
+ private final TalonFXSim angleMotorSim;
+ private final TalonFXSim powerMotorSim;
+ private final TalonFXConfiguration angleConfiguration;
+ private final PIDController angleController = new PIDController(IntakeConstants.kP, IntakeConstants.kI, IntakeConstants.kD);
public IntakeIOSim() {
- spinMotor = new FlywheelSim(DCMotor.getNEO(1), IntakeConstants.SPIN_GEAR_RATIO, 1);
- angleMotor = new SingleJointedArmSim(LinearSystemId.createSingleJointedArmSystem(
- DCMotor.getFalcon500(1), 2, IntakeConstants.ANGLE_GEAR_RATIO
- ), DCMotor.getFalcon500(1), IntakeConstants.ANGLE_GEAR_RATIO, 0.7, -Math.PI * 2, Math.PI * 2, false, 0); //could be wrong type of motor or max/min angle
- angleFeedback = new PIDController(IntakeConstants.kP, IntakeConstants.kI, IntakeConstants.kD);
- }
+ angleConfiguration = new TalonFXConfiguration();
- @Override
- public void updateInputs(IntakeLoggedInputs inputs) {
- inputs.spinMotorPower = inputs.setpointSpinMotorPower;
- inputs.spinMotorCurrent = spinMotor.getCurrentDrawAmps();
- inputs.angleMotorAngle = angleMotor.getAngleRads();
- inputs.angleMotorcurrent = angleMotor.getCurrentDrawAmps();
+ angleMotorSim = new TalonFXSim(1, IntakeConstants.ANGLE_GEAR_RATIO, 0.00001);
+ powerMotorSim = new TalonFXSim(1, 1, 0.00001);
+ angleMotorSim.setController(angleController);
- spinMotor.update(0.02);
- angleMotor.update(0.02);
+ angleConfiguration.Slot0 = IntakeConstants.PIDGains;
+// angleMotorSim.configure(angleConfiguration);
}
+
@Override
public void setSpinMotorPower(double power) {
- spinMotor.setInputVoltage(power * 12);
+ powerMotorSim.setControl(new DutyCycleOut(power));
}
@Override
- public void setAngleMotorAngle(double angle) {
- double angleMotorAppliedVoltage = angleFeedback.calculate(angleMotor.getAngleRads(), angle);
- angleMotor.setInputVoltage(angleMotorAppliedVoltage);
+ public void setAngleMotorAngle(Rotation2d angle) {
+ angleMotorSim.setControl(new PositionVoltage(angle.getRotations()));
}
@Override
public void setAngleMotorPower(double power) {
- angleMotor.setInputVoltage(power * 12);
+ angleMotorSim.setControl(new DutyCycleOut(power));
}
+
@Override
public void resetEncoder(double angle) {
}
+
+ @Override
+ public void updateInputs(IntakeLoggedInputs inputs) {
+ //var batterySim = new BatterySim();
+ angleMotorSim.update(Timer.getFPGATimestamp());
+ powerMotorSim.update(Timer.getFPGATimestamp());
+
+ inputs.angleMotorPower = angleMotorSim.getAppliedVoltage() / 12;
+ inputs.angleMotorAppliedCurrent = angleMotorSim.getAppliedCurrent();
+ inputs.angleMotorAppliedVoltage = angleMotorSim.getAppliedVoltage();
+ inputs.angleMotorVelocity = Rotation2d.fromRotations(angleMotorSim.getVelocity());
+ inputs.angleMotorAngle = Rotation2d.fromRotations(angleMotorSim.getPosition());
+ inputs.spinMotorAppliedCurrent = powerMotorSim.getAppliedVoltage();
+ inputs.spinMotorPower = powerMotorSim.getAppliedVoltage() / 12;
+
+ }
+
}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeLoggedInputs.java b/src/main/java/frc/robot/subsystems/intake/IntakeLoggedInputs.java
index 96d11c5f..db006740 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeLoggedInputs.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeLoggedInputs.java
@@ -1,18 +1,21 @@
package frc.robot.subsystems.intake;
+import edu.wpi.first.math.geometry.Rotation2d;
import org.littletonrobotics.junction.LogTable;
import org.littletonrobotics.junction.inputs.LoggableInputs;
public class IntakeLoggedInputs implements LoggableInputs {
- public double spinMotorPower;
public double setpointSpinMotorPower;
public double setpointAngleMotorPower;
- public double setpointAngleMotorAngle;
+ public Rotation2d setpointAngleMotorAngle = new Rotation2d();
+ public double angleMotorAppliedVoltage;
+ public double angleMotorAppliedCurrent;
public double angleMotorPower;
- public double angleMotorAngle;
- public double angleMotorcurrent;
- public double spinMotorCurrent;
- public double angleMotorVelocity;
+ public Rotation2d angleMotorAngle = new Rotation2d();
+ public double spinMotorPower;
+ public double spinMotorAppliedCurrent;
+ public double spinMotorAppliedVoltage;
+ public Rotation2d angleMotorVelocity = new Rotation2d();
/**
@@ -23,10 +26,11 @@ public void toLog(LogTable table) {
table.put("spinMotorPower", spinMotorPower);
table.put("setpointSpinMotorPower", setpointSpinMotorPower);
table.put("setpointAngleMotorPower", setpointAngleMotorPower);
- table.put("setpointAngleMotorAngle", setpointAngleMotorAngle);
- table.put("angleMotorAngle", angleMotorAngle);
- table.put("angleMotorVelocity", angleMotorVelocity);
- table.put("angleMotorCurrent", angleMotorcurrent);
+ table.put("setpointAngleMotorAngle", setpointAngleMotorAngle.getDegrees());
+
+ table.put("angleMotorAngle", angleMotorAngle.getDegrees());
+ table.put("angleMotorVelocity", angleMotorVelocity.getDegrees());
+ table.put("angleMotorCurrent", angleMotorAppliedCurrent);
table.put("angleMotorPower", angleMotorPower);
}
@@ -35,8 +39,8 @@ public void toLog(LogTable table) {
*/
@Override
public void fromLog(LogTable table) {
- spinMotorPower = table.getDouble("power", spinMotorPower);
- angleMotorAngle = table.getDouble("angle", angleMotorAngle);
- angleMotorVelocity = table.getDouble("velocity", angleMotorVelocity);
+ spinMotorPower = table.get("power", spinMotorPower);
+ angleMotorAngle = table.get("angle", angleMotorAngle);
+ angleMotorVelocity = table.get("velocity", angleMotorVelocity);
}
}
diff --git a/src/main/java/frc/robot/subsystems/intake/commands/HoldIntakeInPlace.java b/src/main/java/frc/robot/subsystems/intake/commands/HoldIntakeInPlace.java
index 1ca5bf65..1ba0cf91 100644
--- a/src/main/java/frc/robot/subsystems/intake/commands/HoldIntakeInPlace.java
+++ b/src/main/java/frc/robot/subsystems/intake/commands/HoldIntakeInPlace.java
@@ -1,6 +1,7 @@
package frc.robot.subsystems.intake.commands;
import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Robot;
import frc.robot.subsystems.intake.Intake;
@@ -17,9 +18,9 @@ public HoldIntakeInPlace() {
@Override
public void execute() {
if (intake.switchedToDefaultCommand() || Robot.justEnabled()) {
- angle = intake.getAngleMotorAngle();
+ angle = intake.getAngleMotorAngle().getRadians();
angle = MathUtil.clamp(angle, IntakeConstants.ANGLE_DOWN, IntakeConstants.ANGLE_UP);
}
- intake.setAngleMotorAngle(IntakeConstants.ANGLE_UP);
+ intake.setAngleMotorAngle(Rotation2d.fromDegrees( IntakeConstants.ANGLE_UP));
}
}
diff --git a/src/main/java/frc/robot/subsystems/intake/commands/IntakeKeyboardControl.java b/src/main/java/frc/robot/subsystems/intake/commands/IntakeKeyboardControl.java
index 2aa8afae..f7adb784 100644
--- a/src/main/java/frc/robot/subsystems/intake/commands/IntakeKeyboardControl.java
+++ b/src/main/java/frc/robot/subsystems/intake/commands/IntakeKeyboardControl.java
@@ -1,5 +1,6 @@
package frc.robot.subsystems.intake.commands;
+import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.intake.Intake;
@@ -16,10 +17,12 @@ public IntakeKeyboardControl() {
@Override
public void execute() {
if (controller.getRawButton(3)) {
- intake.setAngleMotorAngle(IntakeConstants.ANGLE_UP);
+ System.out.println(controller.getRawButton(3));
+ intake.setAngleMotorAngle(Rotation2d.fromDegrees(IntakeConstants.ANGLE_UP));
}
if (controller.getRawButton(4)) {
- intake.setAngleMotorAngle(IntakeConstants.ANGLE_DOWN);
+ System.out.println(controller.getRawButton(4));
+ intake.setAngleMotorAngle(Rotation2d.fromDegrees(IntakeConstants.ANGLE_DOWN));
}
}
}
diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
index 247b1ae9..9629e0a4 100644
--- a/src/main/java/frc/robot/subsystems/vision/Vision.java
+++ b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -4,7 +4,7 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Robot;
import org.littletonrobotics.junction.Logger;
-import swerve.SwerveDrive;
+import frc.robot.swerve.SwerveDrive;
public class Vision extends SubsystemBase {
@@ -45,7 +45,7 @@ public void periodic() {
module.io.updateInputs(module.inputs);
Logger.processInputs(module.name, module.inputs);
results[i] = module.io.getLatestResult();
- Logger.recordOutput("RobotToCamLeft", new Pose3d(SwerveDrive.getInstance(Robot.isReal()).getBotPose()).plus(VisionConstants.ROBOT_TO_CAM[0]));
+ Logger.recordOutput("RobotToCamLeft", new Pose3d(SwerveDrive.getInstance().getBotPose()).plus(VisionConstants.ROBOT_TO_CAM[0]));
}
}
}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSimIO.java b/src/main/java/frc/robot/subsystems/vision/VisionSimIO.java
index 79c6fce0..b85d2d35 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSimIO.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSimIO.java
@@ -11,7 +11,7 @@
import org.photonvision.simulation.VisionTargetSim;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
-import swerve.SwerveDrive;
+import frc.robot.swerve.SwerveDrive;
import java.util.ArrayList;
import java.util.List;
@@ -52,7 +52,7 @@ public Result getLatestResult() {
@Override
public void updateInputs(VisionInputs inputs) {
- var pose = SwerveDrive.getInstance(Robot.isReal()).getBotPose();
+ var pose = SwerveDrive.getInstance().getBotPose();
latestResult = cameraSim.process(0, new Pose3d(pose).plus(robotToCam), visionTargetsSim);
inputs.latency = (long) latestResult.getLatencyMillis();
diff --git a/src/test/java/frc/robot/subsystems/example/ExampleSubsystemTest.java b/src/test/java/frc/robot/subsystems/example/ExampleSubsystemTest.java
index d31da986..df412d5e 100644
--- a/src/test/java/frc/robot/subsystems/example/ExampleSubsystemTest.java
+++ b/src/test/java/frc/robot/subsystems/example/ExampleSubsystemTest.java
@@ -1,10 +1,10 @@
-package frc.robot.subsystems.example;
-
-import org.junit.Test;
-
-public class ExampleSubsystemTest {
-
- @Test
- public void setPower() {
- }
-}
\ No newline at end of file
+//package frc.robot.subsystems.example;
+//
+//import org.junit.Test;
+//
+//public class ExampleSubsystemTest {
+//
+// @Test
+// public void setPower() {
+// }
+//}
\ No newline at end of file
diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json
new file mode 100644
index 00000000..67bf3898
--- /dev/null
+++ b/vendordeps/WPILibNewCommands.json
@@ -0,0 +1,38 @@
+{
+ "fileName": "WPILibNewCommands.json",
+ "name": "WPILib-New-Commands",
+ "version": "1.0.0",
+ "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+ "frcYear": "2024",
+ "mavenUrls": [],
+ "jsonUrl": "",
+ "javaDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-java",
+ "version": "wpilib"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-cpp",
+ "version": "wpilib",
+ "libName": "wpilibNewCommands",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64",
+ "windowsx86-64",
+ "windowsx86",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ]
+}