Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Adding trench autos and linear model for roller

Browse Source
This commit is contained in:
mimigamin
2026-04-04 10:27:44 -06:00
parent bf12e9cd7c
commit 0a2559bb4b
9 changed files with 443 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto
+1 -1
View File
@@ -39,6 +39,6 @@
}
},
"resetOdom": true,
"folder": "Spring Break Tests",
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/ShootPreloadBump.auto
+1 -1
View File
@@ -38,6 +38,6 @@
}
},
"resetOdom": true,
"folder": "Spring Break Tests",
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto
+55
View File
@@ -0,0 +1,55 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Intake Extended"
}
},
{
"type": "wait",
"data": {
"waitTime": 0
}
},
{
"type": "path",
"data": {
"pathName": "LeftTrench-Neutral-Shoot"
}
},
{
"type": "named",
"data": {
"name": "Robot Rev Up"
}
},
{
"type": "named",
"data": {
"name": "BumpOffsetBackward"
}
},
{
"type": "path",
"data": {
"pathName": "LeftRamp-Shoot"
}
},
{
"type": "named",
"data": {
"name": "Robot Shoot"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto
+49
View File
@@ -0,0 +1,49 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Intake Extended"
}
},
{
"type": "path",
"data": {
"pathName": "RightTrench-Neutral-Shoot"
}
},
{
"type": "named",
"data": {
"name": "Robot Rev Up"
}
},
{
"type": "named",
"data": {
"name": "BumpOffsetBackward"
}
},
{
"type": "path",
"data": {
"pathName": "RightRamp-Shoot"
}
},
{
"type": "named",
"data": {
"name": "Robot Shoot"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path
+152
View File
@@ -0,0 +1,152 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.5373205128205143,
"y": 7.430435897435898
},
"prevControl": null,
"nextControl": {
"x": 3.7870116206619695,
"y": 7.418012103131955
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.642076923076923,
"y": 7.069961538461539
},
"prevControl": {
"x": 7.178228149431453,
"y": 7.5205380189223625
},
"nextControl": {
"x": 8.216115793872147,
"y": 6.51234787481454
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.781615384615385,
"y": 4.837346153846154
},
"prevControl": {
"x": 7.786585532087434,
"y": 5.241663699788379
},
"nextControl": {
"x": 7.778542446002628,
"y": 4.5873650404629975
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.572282051282052,
"y": 5.511782051282052
},
"prevControl": {
"x": 6.957648225000392,
"y": 5.601152104386376
},
"nextControl": {
"x": 6.204516600433754,
"y": 5.426493771942126
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.3977820512820527,
"y": 5.511782051282052
},
"prevControl": {
"x": 3.7826390804558825,
"y": 5.585258005415613
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 3.4736842105263164,
"rotationDegrees": 112.5
}
],
"constraintZones": [
{
"name": "Constraints Zone",
"minWaypointRelativePos": 2.374307862679942,
"maxWaypointRelativePos": 4.0,
"constraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0,
"unlimited": false
}
}
],
"pointTowardsZones": [
{
"fieldPosition": {
"x": 7.0,
"y": 0.0
},
"rotationOffset": 0.0,
"minWaypointRelativePos": 0.9302325581395282,
"maxWaypointRelativePos": 2.1705426356589252,
"name": "Point Towards Zone"
},
{
"fieldPosition": {
"x": 7.0,
"y": 0.0
},
"rotationOffset": 180.0,
"minWaypointRelativePos": 2.4983388704318887,
"maxWaypointRelativePos": 3.2956810631229274,
"name": "Point Towards Zone"
},
{
"fieldPosition": {
"x": 0.0,
"y": 7.0
},
"rotationOffset": 180.0,
"minWaypointRelativePos": 0.0,
"maxWaypointRelativePos": 0.7353266888150656,
"name": "Point Towards Zone"
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.5,
"maxAcceleration": 5.0,
"maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0,
"nominalVoltage": 12.0,
"unlimited": true
},
"goalEndState": {
"velocity": 3.0,
"rotation": 101.70667085951649
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 3.0,
"rotation": 0.0
},
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/RightTrench-Neutral-Shoot.path
+165
View File
@@ -0,0 +1,165 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.6070897435897447,
"y": 0.6046794871794885
},
"prevControl": null,
"nextControl": {
"x": 3.8833980386504074,
"y": 0.5273348158056345
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.595564102564104,
"y": 0.9884102564102579
},
"prevControl": {
"x": 7.151835398129718,
"y": 0.5429530943613806
},
"nextControl": {
"x": 7.93843114627953,
"y": 1.332612871042205
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.793243589743589,
"y": 3.4070769230769233
},
"prevControl": {
"x": 7.74538518514461,
"y": 3.9526634447090423
},
"nextControl": {
"x": 7.8344917146104915,
"y": 2.936847688346859
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.572282051282052,
"y": 2.5119999999999996
},
"prevControl": {
"x": 7.050706524183416,
"y": 2.443033938208306
},
"nextControl": {
"x": 6.24953920807272,
"y": 2.5585241728388555
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.3977820512820527,
"y": 2.512
},
"prevControl": {
"x": 3.841498071206661,
"y": 2.6514755211571583
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 3.4736842105263164,
"rotationDegrees": -112.5
}
],
"constraintZones": [
{
"name": "Constraints Zone",
"minWaypointRelativePos": 2.2059800664451794,
"maxWaypointRelativePos": 4.0,
"constraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0,
"nominalVoltage": 10.0,
"unlimited": false
}
},
{
"name": "Constraints Zone",
"minWaypointRelativePos": 1.771871539313429,
"maxWaypointRelativePos": 2.542635658914713,
"constraints": {
"maxVelocity": 1.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0,
"nominalVoltage": 6.0,
"unlimited": false
}
}
],
"pointTowardsZones": [
{
"fieldPosition": {
"x": 7.0,
"y": 8.0
},
"rotationOffset": 0.0,
"minWaypointRelativePos": 0.859357696566985,
"maxWaypointRelativePos": 2.1705426356589252,
"name": "Point Towards Zone"
},
{
"fieldPosition": {
"x": 7.0,
"y": 8.0
},
"rotationOffset": 180.0,
"minWaypointRelativePos": 2.4983388704318887,
"maxWaypointRelativePos": 3.2956810631229274,
"name": "Point Towards Zone"
},
{
"fieldPosition": {
"x": 0.0,
"y": 0.6
},
"rotationOffset": 180.0,
"minWaypointRelativePos": 0.0,
"maxWaypointRelativePos": 0.6910299003322272,
"name": "Point Towards Zone"
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 600.0,
"maxAngularAcceleration": 750.0,
"nominalVoltage": 12.0,
"unlimited": true
},
"goalEndState": {
"velocity": 3.0,
"rotation": -113.07
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0.3,
"rotation": 0.0
},
"useDefaultConstraints": false
}
src/main/java/frc4388/robot/RobotContainer.java
+3 -3
View File
@@ -81,7 +81,7 @@ public class RobotContainer {
//Testing of Colors
public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO);
public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO, m_robotSwerveDrive);
public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
@@ -109,7 +109,7 @@ public class RobotContainer {
private Command IntakeExtended = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendedREMOVEME), m_robotIntake)
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExtendingRolling), m_robotIntake)
);
// private Command LidarIntake = new SequentialCommandGroup(
@@ -146,7 +146,7 @@ public class RobotContainer {
);
private Command IntakeRetracted = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RetractedREMOVEME), m_robotIntake)
new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RectractTorque), m_robotIntake)
);
private Command RobotShoot = new SequentialCommandGroup(
src/main/java/frc4388/robot/subsystems/intake/Intake.java
+9 -5
View File
@@ -2,7 +2,6 @@ package frc4388.robot.subsystems.intake;
import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import java.util.function.Supplier;
@@ -11,19 +10,23 @@ import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.Utils;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.subsystems.swerve.SwerveDrive;
public class Intake extends SubsystemBase {
public IntakeIO io;
IntakeStateAutoLogged state = new IntakeStateAutoLogged();
SwerveDrive m_SwerveDrive;
Supplier<Pose2d> m_swervePoseSupplier;
public Intake(
IntakeIO io
IntakeIO io,
SwerveDrive m_SwerveDrive
// Supplier<Pose2d> swervePoseSupplier
) {
this.io = io;
this.m_SwerveDrive = m_SwerveDrive;
// this.m_swervePoseSupplier = swervePoseSupplier;
}
@@ -92,8 +95,9 @@ public class Intake extends SubsystemBase {
public void periodic() {
// FaultReporter.register(this); // TODO Implement fault reporter
// System.out.println(m_armLimitSwitch.get());
ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
double ChassisOverallSpeed = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond);
Logger.processInputs("Intake", state);
Logger.recordOutput("Intake/IntakeState", this.mode);
@@ -127,7 +131,7 @@ public class Intake extends SubsystemBase {
case ExtendingRolling:
io.armOutput(IntakeConstants.ARM_EXTEND_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants.ROLLER_PERCENT_OUTPUT.get());
io.setRollerOutput(state, IntakeConstants. ROLLER_PERCENT_OUTPUT.get()); //getTargetRollerSpeed(ChassisOverallSpeed));
break;
case Retracting:
src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java
+8 -2
View File
@@ -6,6 +6,9 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.revrobotics.spark.FeedbackSensor;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.units.measure.AngularVelocity;
import com.revrobotics.spark.config.LimitSwitchConfig.Behavior;
import com.revrobotics.spark.config.LimitSwitchConfig.Type;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -59,8 +62,9 @@ public class IntakeConstants {
public static final ConfigurableDouble ARM_REVERSE_ROLLER_RANGE = new ConfigurableDouble("Arm reverse roller range", 1.17);
public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .80);
public static final ConfigurableDouble ROLLER_PERCENT_OUTPUT = new ConfigurableDouble("Roller Percent Output", .50);
public static final ConfigurableDouble ROLLER_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Roller Retract Output", .40);
public static final ConfigurableDouble ROLLER_MULTIPLIER_CONST = new ConfigurableDouble("Roller Multiplier Constant", 0.4);
// public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25);
@@ -79,7 +83,9 @@ public class IntakeConstants {
// public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0);
// public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.05);
public static double getTargetRollerSpeed(double chassisSpeed) {
return ROLLER_PERCENT_OUTPUT.get() * (1 + ROLLER_MULTIPLIER_CONST.get() * chassisSpeed);
}
// 0 is paralell to the ground, 90 is directly up