Team_2648/2024_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Auto heading control inverted HELP

Browse Source
This commit is contained in:
Bradley Bickford 2024-03-09 18:52:33 -05:00
parent d2d88766c1
commit c7ee873b7e
15 changed files with 441 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/main/deploy/pathplanner/autos/He He He Haw.auto Normal file
View File

@ -0,0 +1,25 @@
{
"version": 1.0,
"startingPose": {
"position": {
"x": 0.4585613729632017,
"y": 7.0398809743488435
},
"rotation": 180.0
},
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "He He He Haw"
}
}
]
}
},
"folder": null,
"choreoAuto": false
}

62
src/main/deploy/pathplanner/autos/Right 2S Center.auto Normal file
View File

@ -0,0 +1,62 @@
{
"version": 1.0,
"startingPose": {
"position": {
"x": 0.52,
"y": 4.034483282641779
},
"rotation": 180.0
},
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Right Sub"
}
},
{
"type": "named",
"data": {
"name": "Charge Shooter"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "Speaker Note Shot"
}
},
{
"type": "path",
"data": {
"pathName": "Right Sub to Right Center Note"
}
},
{
"type": "path",
"data": {
"pathName": "Right Sub from Center"
}
},
{
"type": "named",
"data": {
"name": "Speaker Note Shot"
}
}
]
}
},
"folder": null,
"choreoAuto": false
}

74
src/main/deploy/pathplanner/paths/He He He Haw.path Normal file
View File

@ -0,0 +1,74 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.4585613729632017,
"y": 7.0398809743488435
},
"prevControl": null,
"nextControl": {
"x": 1.4585613729632003,
"y": 7.0398809743488435
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.7856981070087494,
"y": 5.98740707452925
},
"prevControl": {
"x": 1.7856981070087494,
"y": 5.98740707452925
},
"nextControl": {
"x": 3.7856981070087494,
"y": 5.98740707452925
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.69184528112646,
"y": 7.051575128791284
},
"prevControl": {
"x": 4.238771694067605,
"y": 6.519491101660266
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 1.0,
"rotationDegrees": 90.0,
"rotateFast": false
}
],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 88.210089391754,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": 0,
"velocity": 0
},
"useDefaultConstraints": true
}

2
src/main/deploy/pathplanner/paths/Left Subwoofer.path
View File

@ -39,7 +39,7 @@
},
"goalEndState": {
"velocity": 0,
"rotation": -146.30993247402017,
"rotation": 149.21037688376774,
"rotateFast": false
},
"reversed": false,

52
src/main/deploy/pathplanner/paths/Right Sub from Center.path Normal file
View File

@ -0,0 +1,52 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 5.335023775460655,
"y": 1.1577212898015548
},
"prevControl": null,
"nextControl": {
"x": 3.803089543501023,
"y": 0.9472265098376356
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.0432690950851984,
"y": 4.478861151454497
},
"prevControl": {
"x": 2.013883913807713,
"y": 1.5436283864020721
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 143.130102354156,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": 180.0,
"velocity": 0
},
"useDefaultConstraints": true
}

76
src/main/deploy/pathplanner/paths/Right Sub to Right Center Note.path Normal file
View File

@ -0,0 +1,76 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 1.335622956146197,
"y": 4.315142989260338
},
"prevControl": null,
"nextControl": {
"x": 1.8735540604984342,
"y": 1.4149926875352343
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.603689737294005,
"y": 0.8068966565283563
},
"prevControl": {
"x": 6.603689737294005,
"y": 0.8068966565283563
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.6,
"rotationDegrees": 180.0,
"rotateFast": false
}
],
"constraintZones": [],
"eventMarkers": [
{
"name": "Note Pickup",
"waypointRelativePos": 0.6,
"command": {
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Note Pickup"
}
}
]
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": 139.21417852273407,
"velocity": 0
},
"useDefaultConstraints": true
}

52
src/main/deploy/pathplanner/paths/Right Sub.path Normal file
View File

@ -0,0 +1,52 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 0.52,
"y": 4.034483282641779
},
"prevControl": null,
"nextControl": {
"x": 1.0783515584179242,
"y": 3.5199404871715854
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.0783515584179242,
"y": 4.53733192366386
},
"prevControl": {
"x": 1.85016575161896,
"y": 4.034483282638943
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 143.6955028774248,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": -178.76146677965983,
"velocity": 0
},
"useDefaultConstraints": true
}

21
src/main/java/frc/robot/RobotContainer.java
View File

@ -9,6 +9,8 @@ import java.util.Optional;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.util.PPLibTelemetry;
import com.pathplanner.lib.util.PathPlannerLogging;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
@ -97,6 +99,7 @@ public class RobotContainer {
// ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand());
// TODO Specify a default auto string once we have one
autoChooser = AutoBuilder.buildAutoChooser();
@ -134,8 +137,8 @@ public class RobotContainer {
drivetrain.setDefaultCommand(
drivetrain.teleopCommand(
driver::getLeftY,
driver::getLeftX,
() -> { return -driver.getLeftY(); },
() -> { return -driver.getLeftX(); },
() -> { return -driver.getRightX(); },
OIConstants.kTeleopDriveDeadband
)
@ -154,9 +157,7 @@ public class RobotContainer {
}
},
() -> {
if (driver.getLeftTriggerAxis() > .25) {
return -1.0;
}else if(driver.getRightTriggerAxis() > 0.25){
if(driver.getRightTriggerAxis() > 0.25){
return 1.0;
} else {
return 0;
@ -270,6 +271,11 @@ public class RobotContainer {
driver.povDown().whileTrue(indexer.shootNote(() -> -1.0));
operator.start().onTrue(shooter.zeroEncoder());
operator.povDown().whileTrue(intake.intakeDownCommand());
operator.povUp().whileTrue(intake.intakeUpCommand());
driver.leftTrigger().whileTrue(Commands.parallel( intake.manualPivot(() -> 0.0, () -> 1.0), indexer.advanceNote()));
}
@ -293,7 +299,10 @@ public class RobotContainer {
teleopTab.addBoolean("indexer beam break", indexer::getBeamBreak);
teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak);
teleopTab.addDouble("shooter angle", shooter::getShooterAngle);
teleopTab.addDouble("intake angle", intake::getIntakeAngle);
teleopTab.addDouble("intake angle", intake::getIntakeDegrees);
teleopTab.addDouble("intake pid", intake::getIntakePID);
teleopTab.addDouble("heading swerve", drivetrain::getHeading);
teleopTab.addDouble("steering encoder", drivetrain::getEncoderSteering);
}
public Command getAutonomousCommand() {

6
src/main/java/frc/robot/constants/AutoConstants.java
View File

@ -13,9 +13,9 @@ public final class AutoConstants {
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kPXController = 1.05;
public static final double kPYController = 1.05;
public static final double kPThetaController = 0.95; // needs to be separate from heading control
public static final double kPXController = 5;
public static final double kPYController = 5;
public static final double kPThetaController = 0.5; // needs to be separate from heading control
public static final double kPHeadingController = 0.02; // for heading control NOT PATHING
public static final double kDHeadingController = 0.0025;

6
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@ -26,10 +26,16 @@ public final class DrivetrainConstants {
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
// Angular offsets of the modules relative to the chassis in radians
/*
public static final double kFrontLeftChassisAngularOffset = Math.PI;
public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
public static final double kBackRightChassisAngularOffset = 0;
*/
public static final double kFrontLeftChassisAngularOffset = -2*Math.PI;
public static final double kFrontRightChassisAngularOffset = -2*Math.PI;
public static final double kBackLeftChassisAngularOffset = -2*Math.PI;
public static final double kBackRightChassisAngularOffset = -2*Math.PI-(4/180.0);
// SPARK MAX CAN IDs
public static final int kFrontLeftDrivingCanId = 8;

9
src/main/java/frc/robot/constants/IntakeConstants.java
View File

@ -4,19 +4,20 @@ public class IntakeConstants {
public static final int kIntakePivotID = 10;
public static final int kIntakeRollerID = 12;
public static final double kIntakePivotConversionFactor = 1/(20.0*(28.0/15.0));
public static final double kIntakePivotConversionFactor = (20.0*(28.0/15.0));
public static final int kPivotCurrentLimit = 40;
public static final double kPIntake = 0;
public static final double kPIntake = 2.5;
public static final double kIIntake = 0;
public static final double kDIntake = 0;
public static final double kSFeedForward = 0;
public static final double kGFeedForward = 1.11;
public static final double kVFeedForward = 0.73;
public static final double kGFeedForward = 0;//1.11;
public static final double kVFeedForward = 0;//0.73;
public static final double kStartingAngle = Math.toRadians(105.0);
public static final double kUpAngle = Math.toRadians(90.0);
public static final double kDownAngle = Math.toRadians(-34.0);
}

4
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -451,4 +451,8 @@ public class Drivetrain extends SubsystemBase {
public double getTurnRate() {
return m_gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
}
public double getEncoderSteering(){
return m_frontLeft.steeringEncoder();
}
}

4
src/main/java/frc/robot/subsystems/Indexer.java
View File

@ -41,9 +41,9 @@ public class Indexer extends SubsystemBase{
public Command advanceNote(){
return run(() -> {
if(indexerBeamBreak.get()){
indexerMotor.set(0.75);
indexerMotor.set(1);
}else{
indexerMotor.set(0.75);
indexerMotor.set(0.0);
}
});
}

70
src/main/java/frc/robot/subsystems/Intake.java
View File

@ -1,5 +1,6 @@
package frc.robot.subsystems;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import com.revrobotics.CANSparkMax;
@ -9,6 +10,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.IntakeConstants;
@ -23,7 +25,11 @@ public class Intake extends SubsystemBase{
private ArmFeedforward intakeFeedForward;
private double armOffset;
public Intake(){
armOffset = 0;
intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
intakeRoller.setSmartCurrentLimit(60);
@ -43,8 +49,8 @@ public class Intake extends SubsystemBase{
);
intakeEncoder = intakePivot.getEncoder();
intakeEncoder.setPosition(IntakeConstants.kStartingAngle);
intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
intakeEncoder.setPosition(Units.radiansToRotations( IntakeConstants.kStartingAngle));
// intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
intakeAnglePID = new PIDController(
IntakeConstants.kPIntake,
@ -52,24 +58,54 @@ public class Intake extends SubsystemBase{
IntakeConstants.kDIntake
);
armOffset = getIntakeAngle()-IntakeConstants.kStartingAngle;
}
public Command intakeDownCommand() {
return run(() -> {
intakeRoller.set(1.0);
intakeRoller.set(0.0);
intakePivot.setVoltage(
intakeAnglePID.calculate(
intakeEncoder.getPosition(),
getIntakeAngle(),
IntakeConstants.kDownAngle
) + intakeFeedForward.calculate(
IntakeConstants.kDownAngle,
intakeEncoder.getVelocity()
0.0
)
);
});
}
public Command autoIntaking(BooleanSupplier beamBreak){
return run(() -> {
if(beamBreak.getAsBoolean()){
intakeRoller.set(1.0);
intakePivot.setVoltage(
intakeAnglePID.calculate(
getIntakeAngle(),
IntakeConstants.kUpAngle
) + intakeFeedForward.calculate(
IntakeConstants.kUpAngle,
0.0
)
);
}else{
intakeRoller.set(0);
intakePivot.setVoltage(
intakeAnglePID.calculate(
getIntakeAngle(),
IntakeConstants.kDownAngle
) + intakeFeedForward.calculate(
IntakeConstants.kDownAngle,
0.0
)
);
}
});
}
public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier rollerSpinny){
return run(() ->{
intakePivot.set(pivotPower.getAsDouble());
@ -83,11 +119,11 @@ public class Intake extends SubsystemBase{
intakePivot.setVoltage(
intakeAnglePID.calculate(
intakeEncoder.getPosition(),
getIntakeAngle(),
IntakeConstants.kDownAngle
) + intakeFeedForward.calculate(
IntakeConstants.kDownAngle,
intakeEncoder.getVelocity()
0.0
)
);
});
@ -99,17 +135,31 @@ public class Intake extends SubsystemBase{
intakePivot.setVoltage(
intakeAnglePID.calculate(
intakeEncoder.getPosition(),
getIntakeAngle(),
IntakeConstants.kUpAngle
) + intakeFeedForward.calculate(
IntakeConstants.kUpAngle,
intakeEncoder.getVelocity()
0.0
)
);
});
}
public double getIntakeAngle(){
return intakeEncoder.getPosition();
return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeConstants.kIntakePivotConversionFactor)-armOffset;
}
public double getIntakePID(){
return intakeAnglePID.calculate(
getIntakeAngle(),
IntakeConstants.kDownAngle
) + intakeFeedForward.calculate(
IntakeConstants.kDownAngle,
0.0
);
}
public double getIntakeDegrees(){
return Math.toDegrees(getIntakeAngle());
}
}

4
src/main/java/frc/robot/utilities/MAXSwerveModule.java
View File

@ -157,4 +157,8 @@ public class MAXSwerveModule {
public void resetEncoders() {
m_drivingEncoder.setPosition(0);
}
public double steeringEncoder(){
return m_turningEncoder.getPosition();
}
}