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
Download Patch File Download Diff File Expand all files Collapse all files

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();
}
}