end of pine tree 2

This commit is contained in:
2026-03-08 19:24:00 -04:00
parent 21c0421a88
commit 5e1eadf887
7 changed files with 167 additions and 11 deletions

View File

@@ -0,0 +1,37 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intake down"
}
},
{
"type": "named",
"data": {
"name": "spinup"
}
},
{
"type": "path",
"data": {
"pathName": "Center to Shoot"
}
},
{
"type": "named",
"data": {
"name": "shoot close"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

View File

@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.622028469750891,
"y": 4.008102016607354
},
"prevControl": null,
"nextControl": {
"x": 3.266975088967973,
"y": 4.0403795966785285
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.1271055753262162,
"y": 4.008102016607354
},
"prevControl": {
"x": 4.0416370106761565,
"y": 4.0403795966785285
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}

View File

@@ -0,0 +1,59 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 2.9872360616844604,
"y": 4.599857651245552
},
"prevControl": null,
"nextControl": {
"x": 2.1695373665480435,
"y": 5.019466192170819
},
"isLocked": false,
"linkedName": "left close"
},
{
"anchor": {
"x": 1.1043772241992889,
"y": 5.815646500593119
},
"prevControl": {
"x": 2.7074970344009497,
"y": 5.998552787663108
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.5,
"rotationDegrees": 180.0
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 178.53119928561412
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -22.619864948040433
},
"useDefaultConstraints": true
}

View File

@@ -25,7 +25,7 @@
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "left close"
}
],
"rotationTargets": [],

View File

@@ -1,6 +1,7 @@
package frc.robot.constants;
import com.ctre.phoenix6.configs.AudioConfigs;
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.FeedbackConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
@@ -51,10 +52,11 @@ public class ModuleConstants {
public static final double kDriveS = 0;
public static final double kDriveV = kDrivingVelocityFeedForward;
public static final double kDriveA = 0;
public static final double kClosedLoopRampRate = .01;
// TODO Hold over from 2025, adjust?
public static final int kDriveMotorStatorCurrentLimit = 100;
public static final int kDriveMotorSupplyCurrentLimit = 65;
public static final int kDriveMotorStatorCurrentLimit = 90;
public static final int kDriveMotorSupplyCurrentLimit = 55;
// TODO Hold over from 2025, adjust?
public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
@@ -88,6 +90,7 @@ public class ModuleConstants {
public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
public static final AudioConfigs kAudioConfig = new AudioConfigs();
public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
public static final ClosedLoopRampsConfigs kDriveClosedLoopRampConfig = new ClosedLoopRampsConfigs();
static {
kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
@@ -109,6 +112,8 @@ public class ModuleConstants {
kDriveSlot0Config.kV = kDriveV;
kDriveSlot0Config.kA = kDriveA;
kDriveClosedLoopRampConfig.withVoltageClosedLoopRampPeriod(kClosedLoopRampRate);
turningConfig
.idleMode(kTurnIdleMode)
.smartCurrentLimit(kTurnMotorCurrentLimit)

View File

@@ -26,10 +26,10 @@ public class Hood extends SubsystemBase {
private SparkClosedLoopController controller;
private Trigger resetTrigger;
private Trigger timerTrigger;
//private Trigger resetTrigger;
//private Trigger timerTrigger;
private Timer resetTimer;
//private Timer resetTimer;
private double currentTargetDegrees;
@@ -47,7 +47,7 @@ public class Hood extends SubsystemBase {
controller = motor.getClosedLoopController();
resetTimer = new Timer();
/*resetTimer = new Timer();
resetTimer.reset();
resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset));
@@ -61,7 +61,7 @@ public class Hood extends SubsystemBase {
timerTrigger.onTrue(new InstantCommand(() -> {
encoder.setPosition(0);
resetTimer.reset();
}));
}));*/
currentTargetDegrees = HoodConstants.kStartupAngle;
}
@@ -96,7 +96,7 @@ public class Hood extends SubsystemBase {
*
* @return A complete Command structure that performs the specified action
*/
public Command automatedRezero() {
/*public Command automatedRezero() {
return manualSpeed(() -> -1)
.until(timerTrigger)
.andThen(
@@ -112,11 +112,11 @@ public class Hood extends SubsystemBase {
*
* @return A complete Command structure that performs the specified action
*/
public Command automatedRezeroNoTimer() {
/*public Command automatedRezeroNoTimer() {
return manualSpeed(() -> -1)
.until(() -> motor.getOutputCurrent() >= HoodConstants.kAmpsToTriggerPositionReset)
.andThen(new InstantCommand(() -> encoder.setPosition(0)));
}
}*/
public Command manualSpeed(DoubleSupplier speed) {
currentTargetDegrees = 0;

View File

@@ -109,6 +109,7 @@ public class SwerveModule {
drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
drive.getConfigurator().apply(ModuleConstants.kDriveClosedLoopRampConfig);
turning.configure(
ModuleConstants.turningConfig,