"pathName": "#2 Left(No SOTM) under the trench"
}
},
+ {
+ "type": "parallel",
+ "data": {
+ "commands": [
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 8.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ ]
+ }
+ },
{
"type": "path",
"data": {
"pathName": "#3(No SOTM) Left under the trench"
}
+ },
+ {
+ "type": "parallel",
+ "data": {
+ "commands": [
+ {
+ "type": "wait",
+ "data": {
+ "waitTime": 15.0
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Start Spindexer"
+ }
+ }
+ ]
+ }
}
]
}
}
],
"pointTowardsZones": [],
- "eventMarkers": [],
+ "eventMarkers": [
+ {
+ "name": "Intake",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "parallel",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ ]
+ }
+ }
+ }
+ ],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
}
],
"pointTowardsZones": [],
- "eventMarkers": [],
+ "eventMarkers": [
+ {
+ "name": "Retract Intake",
+ "waypointRelativePos": 0.65466816647919,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "parallel",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Retract Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Rollers"
+ }
+ }
+ ]
+ }
+ }
+ }
+ ],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
}
],
"pointTowardsZones": [],
- "eventMarkers": [],
+ "eventMarkers": [
+ {
+ "name": "Getting Ready",
+ "waypointRelativePos": 0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "parallel",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Intake Seizure"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Stop Spindexer"
+ }
+ }
+ ]
+ }
+ },
+ {
+ "type": "parallel",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "Extend Intake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Spin Intake Rollers"
+ }
+ }
+ ]
+ }
+ }
+ ]
+ }
+ }
+ }
+ ],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
import frc.robot.commands.drive_comm.DefaultDriveCommand;
import frc.robot.commands.gpm.AutoShootCommand;
import frc.robot.commands.gpm.ClimbDriveCommand;
+import frc.robot.commands.gpm.IntakeMovementCommand;
import frc.robot.commands.gpm.Superstructure;
import frc.robot.commands.vision.ShutdownAllPis;
import frc.robot.constants.AutoConstants;
e.printStackTrace();
}
if(turret != null){
- turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
+ //turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
}
drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
break;
public void registerCommands() {
if (intake != null){
- NamedCommands.registerCommand("Extend intake", new InstantCommand(()-> intake.extend()));
- NamedCommands.registerCommand("Retract intake", new InstantCommand(()-> intake.retract()));
+ NamedCommands.registerCommand("Extend Intake", new InstantCommand(()-> intake.extend()));
+ NamedCommands.registerCommand("Retract Intake", new InstantCommand(()-> intake.retract()));
}
if (intake != null && spindexer != null){
NamedCommands.registerCommand("Stop Intake Rollers", new ParallelCommandGroup(
new InstantCommand(()->intake.spinStop())
));
+ Command intakeMovement = new IntakeMovementCommand(intake);
+ NamedCommands.registerCommand("Start Intake Seizure", new InstantCommand(()-> intakeMovement.schedule()));
+ NamedCommands.registerCommand("Stop Intake Seizure", new InstantCommand(()-> intakeMovement.cancel()));
+
+
}
if (turret != null && drive != null && hood != null && shooter != null && spindexer != null){
NamedCommands.registerCommand("Auto shoot", new AutoShootCommand(turret, drive, hood, shooter, spindexer));
- NamedCommands.registerCommand("Spin Spindexer", new InstantCommand(()-> spindexer.maxSpindexer(), spindexer));
+ NamedCommands.registerCommand("Start Spindexer", new InstantCommand(()-> spindexer.maxSpindexer(), spindexer));
NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(()-> spindexer.stopSpindexer()));
}
import frc.robot.constants.swerve.DriveConstants;
import frc.robot.controls.BaseDriverConfig;
import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.util.TrenchAssist.TrenchAssist2;
+import frc.robot.util.TrenchAssist.TrenchAssist;
import frc.robot.util.TrenchAssist.TrenchAssistConstants;
import frc.robot.util.Vision.DriverAssist;
Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist());
if (swerve.getTrenchAssist()) {
- drive(TrenchAssist2.calculate(swerve, corrected, trenchAssistPid));
+ drive(TrenchAssist.calculate(swerve, corrected, trenchAssistPid));
} else {
trenchAssistPid.reset();
drive(corrected);
public class IntakeConstants {
/** Intake roller motor speed in range [-1, 1] */
- public static final double SPEED = 0.8;
+ public static final double SPEED = 1.0;
/** 12 tooth pinion driving 36 tooth driven gear */
public static final double GEAR_RATIO = 36.0/12.0;
/** radius (inches) of the rack gear which is a 10 tooth pinion at 10 DP */
TalonFXConfiguration config = new TalonFXConfiguration();
config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
- config.Slot0.kP = 10.0;
+ config.Slot0.kP = 12.0;
config.Slot0.kS = 0.1; // Static friction compensation
config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio
- config.Slot0.kD = 0.20; // The "Braking" term to stop overshoot
+ config.Slot0.kD = 0.50; // The "Braking" term to stop overshoot
var mm = config.MotionMagic;
mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO;
crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT);
double turretRot = crt.solve(leftAbs, rightAbs);
- //SmartDashboard.putNumber("Turret Index", turretIndex);
-
- SmartDashboard.putNumber("CRT Position", Units.rotationsToDegrees(turretRot));
-
+
double motorRotations = turretRot * TurretConstants.GEAR_RATIO;
//Sets the initial motor position
SmartDashboard.putNumber("Turret position", Units.radiansToDegrees(getPositionRad()));
SmartDashboard.putNumber("Encoder left position", encoderLeft.getAbsolutePosition().getValueAsDouble());
SmartDashboard.putNumber("Encoder right position", encoderRight.getAbsolutePosition().getValueAsDouble());
+
+
+ double leftPosition = encoderLeft.getAbsolutePosition().getValueAsDouble();
+ double leftAbs = wrapUnit(leftPosition - TurretConstants.LEFT_ENCODER_OFFSET);
+
+ double rightPosition = encoderRight.getAbsolutePosition().getValueAsDouble();
+ double rightAbs = wrapUnit(rightPosition - TurretConstants.RIGHT_ENCODER_OFFSET);
+
+ crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT);
+
+ double turretRot = crt.solve(leftAbs, rightAbs);
+
+ SmartDashboard.putNumber("CRT Position", Units.rotationsToDegrees(turretRot));
}
/* ---------------- Simulation ---------------- */
public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop
public static double MAX_VELOCITY = 600; // rad/s
- public static double MAX_ACCELERATION = 120.0; // rad/s^2
+ // public static double MAX_ACCELERATION = 120.0; // rad/s^2
+ public static double MAX_ACCELERATION = 320.0; // rad/s^2
// Not using this, but just in case
public static double TURRET_WIDTH = Units.inchesToMeters(6.4);