Skip to content

Commit d5b20cc

Browse files
ori-covalori-coval
committed
Merge branch 'main' of https://github.com/primo4586/RobotCode2024 into Shooter
# Conflicts: # src/main/java/frc/robot/Constants.java
2 parents 4031718 + 0b6c228 commit d5b20cc

File tree

8 files changed

+320
-23
lines changed

8 files changed

+320
-23
lines changed

‎src/main/java/frc/robot/Constants.java

Lines changed: 52 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
import edu.wpi.first.apriltag.AprilTagFields;
1313
import edu.wpi.first.math.Matrix;
1414
import edu.wpi.first.math.VecBuilder;
15+
import edu.wpi.first.math.controller.PIDController;
1516
import edu.wpi.first.math.geometry.Pose2d;
1617
import edu.wpi.first.math.geometry.Rotation2d;
1718
import edu.wpi.first.math.geometry.Rotation3d;
@@ -28,9 +29,39 @@
2829
import edu.wpi.first.math.numbers.N3;
2930

3031
public final class Constants {
31-
public static final double stickDeadband = 0.1;
3232

33+
public static final double stickDeadband = 0.1;
34+
35+
public static final class IntakeArm{ //TODO: This must be tuned to specific robot
36+
// mm
37+
public static final int KMotorID = 0;
38+
public static final double mmVelocity = 5.0;
39+
public static final double mmAcceleration = 10.0;
40+
public static final double mmJerk = 50;
41+
public static final double KP = 24.0;
42+
public static final double KD = 0.1;
43+
public static final double KV = 0.12;
44+
public static final double KS = 0.25;
45+
public static final double PeakForwardVoltage = 11.5;
46+
public static final double PeakReverseVoltage = -11.5;
47+
public static final double SensorToMechanismRatio = 0;
48+
public static final boolean ForwardSoftLimitEnable = true;
49+
public static final double ForwardSoftLimitThreshold = 300;
50+
public static final boolean ReverseSoftLimitEnable = true;
51+
public static final double RevesrseSoftLimitThreshold = 0;
52+
// not mm
53+
public static final double minimumError = 0;
54+
public static final double startingValue = 0;
55+
public static final double zeroEncoderValue = 0;
56+
public static final double intakeArmSpeed = 0;
57+
// switch
58+
public static final int switchID = 0;
59+
60+
}
3361
public static final class Swerve {
62+
public static final double minimumErrorAligning = 0; // TODO: This must be tuned to specific robot
63+
public static final PIDController aligningPID = new PIDController(0, 0, 0);
64+
3465
public static final int pigeonID = 10;
3566
public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW-
3667

@@ -175,26 +206,26 @@ public static final class AutoConstants { //TODO: The below constants are used i
175206
}
176207

177208
public static class Vision {
178-
public static final String kRightCameraName = "Arducam_OV9281_USB_Camera";
179-
public static final String kLeftCameraName = "YOUR CAMERA NAME";
180-
// Cam mounted facing forward, half a meter forward of center, half a meter up
181-
// from center.
182-
public static final Transform3d kRightRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
183-
new Rotation3d(0, 0, 0));
184-
public static final Transform3d kLeftRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
185-
new Rotation3d(0, 0, 0));
186-
187-
// The layout of the AprilTags on the field
188-
public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
189-
190-
// The standard deviations of our vision estimated poses, which affect
191-
// correction rate
192-
// (Fake values. Experiment and determine estimation noise on an actual robot.)
193-
public static final Matrix<N3, N1> kRightSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
194-
public static final Matrix<N3, N1> kRightMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
195-
196-
public static final Matrix<N3, N1> kLeftSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
197-
public static final Matrix<N3, N1> kLeftMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
209+
public static final String kRightCameraName = "Arducam_OV9281_USB_Camera";
210+
public static final String kLeftCameraName = "YOUR CAMERA NAME";
211+
// Cam mounted facing forward, half a meter forward of center, half a meter up
212+
// from center.
213+
public static final Transform3d kRightRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
214+
new Rotation3d(0, 0, 0));
215+
public static final Transform3d kLeftRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
216+
new Rotation3d(0, 0, 0));
217+
218+
// The layout of the AprilTags on the field
219+
public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
220+
221+
// The standard deviations of our vision estimated poses, which affect
222+
// correction rate
223+
// (Fake values. Experiment and determine estimation noise on an actual robot.)
224+
public static final Matrix<N3, N1> kRightSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
225+
public static final Matrix<N3, N1> kRightMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
226+
227+
public static final Matrix<N3, N1> kLeftSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
228+
public static final Matrix<N3, N1> kLeftMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
198229

199230
public static final Pose2d target = new Pose2d(1, 1, new Rotation2d(Units.degreesToRadians(0)));
200231
}

‎src/main/java/frc/robot/RobotContainer.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
import edu.wpi.first.wpilibj2.command.InstantCommand;
99
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
1010
import frc.Utils.PathPlanner.PathPlannerHelper;
11-
import frc.robot.commands.*;
11+
import frc.robot.commands.SwerveCommands.*;
1212
import frc.robot.subsystems.*;
1313

1414
/**

‎src/main/java/frc/robot/commands/IntakeArmCommands/IntakeZero.java

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.commands.IntakeArmCommands;
6+
7+
import edu.wpi.first.wpilibj2.command.Command;
8+
import frc.robot.subsystems.IntakeArmSubsystem;
9+
import static frc.robot.Constants.IntakeArm.*;
10+
11+
public class IntakeZero extends Command {
12+
/** Creates a new SetSpeed. */
13+
private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
14+
public IntakeZero() {
15+
// Use addRequirements() here to declare subsystem dependencies.
16+
}
17+
18+
// Called when the command is initially scheduled.
19+
@Override
20+
public void initialize() {
21+
intakeArm.setSpeed(intakeArmSpeed);
22+
}
23+
24+
// Called every time the scheduler runs while the command is scheduled.
25+
@Override
26+
public void execute() {
27+
}
28+
29+
// Called once the command ends or is interrupted.
30+
@Override
31+
public void end(boolean interrupted) {
32+
intakeArm.setSpeed(0);
33+
intakeArm.setEncoder(zeroEncoderValue);
34+
}
35+
36+
// Returns true when the command should end.
37+
@Override
38+
public boolean isFinished() {
39+
return intakeArm.getSwitch();
40+
}
41+
}

‎src/main/java/frc/robot/commands/IntakeArmCommands/MoveIntakeArmToDegree.java

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.commands.IntakeArmCommands;
6+
7+
import edu.wpi.first.wpilibj2.command.Command;
8+
import frc.robot.subsystems.IntakeArmSubsystem;
9+
10+
public class MoveIntakeArmToDegree extends Command {
11+
/** Creates a new MoveIntakeArmTo. */
12+
private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
13+
private double degree;
14+
15+
public MoveIntakeArmToDegree(double degree) {
16+
this.degree = degree;
17+
// Use addRequirements() here to declare subsystem dependencies.
18+
}
19+
20+
// Called when the command is initially scheduled.
21+
@Override
22+
public void initialize() {
23+
intakeArm.moveArmTo(degree);
24+
}
25+
26+
// Called every time the scheduler runs while the command is scheduled.
27+
@Override
28+
public void execute() {}
29+
30+
// Called once the command ends or is interrupted.
31+
@Override
32+
public void end(boolean interrupted) {}
33+
34+
// Returns true when the command should end.
35+
@Override
36+
public boolean isFinished() {
37+
return intakeArm.checkIntakeArmPosion();
38+
}
39+
}

‎src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.commands.IntakeArmCommands;
6+
7+
import edu.wpi.first.wpilibj2.command.InstantCommand;
8+
import frc.robot.subsystems.IntakeArmSubsystem;
9+
10+
// NOTE: Consider using this command inline, rather than writing a subclass. For more
11+
// information, see:
12+
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
13+
public class SetSpeed extends InstantCommand {
14+
private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
15+
private double speed;
16+
public SetSpeed(double speed) {
17+
// Use addRequirements() here to declare subsystem dependencies.
18+
this.speed = speed;
19+
}
20+
21+
// Called when the command is initially scheduled.
22+
@Override
23+
public void initialize() {
24+
intakeArm.setSpeed(speed);
25+
}
26+
}

‎src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.commands.SwerveCommands;
6+
7+
import static frc.robot.Constants.Swerve.*;
8+
9+
import edu.wpi.first.math.controller.PIDController;
10+
import edu.wpi.first.math.geometry.Translation2d;
11+
import edu.wpi.first.wpilibj2.command.Command;
12+
import frc.Utils.vision.Vision;
13+
import frc.robot.subsystems.SwerveSubsystem;
14+
15+
public class AlignToSpeaker extends Command {
16+
/** Creates a new TurnToDegree. */
17+
SwerveSubsystem swerve = SwerveSubsystem.getInstance();
18+
private PIDController pid = aligningPID;
19+
private Vision vision = Vision.getInstance();
20+
double angleFromTarget = vision.GetAngleFromTarget().getDegrees();
21+
22+
public AlignToSpeaker() {
23+
24+
}
25+
26+
// Called when the command is initially scheduled.
27+
@Override
28+
public void initialize() {}
29+
30+
// Called every time the scheduler runs while the command is scheduled.
31+
@Override
32+
public void execute() {
33+
angleFromTarget = vision.GetAngleFromTarget().getDegrees();
34+
calculateOptimalRotation();
35+
double rotation = pid.calculate(angleFromTarget, 180);
36+
swerve.drive(new Translation2d(), rotation, true, false, false);
37+
}
38+
39+
// Called once the command ends or is interrupted.
40+
@Override
41+
public void end(boolean interrupted) {}
42+
43+
// Returns true when the command should end.
44+
@Override
45+
public boolean isFinished() {
46+
return Math.abs(vision.GetAngleFromTarget().getDegrees() - 180) < minimumErrorAligning ;
47+
}
48+
49+
public void calculateOptimalRotation() {
50+
double currentRotation = angleFromTarget;
51+
double targetRotation = 180;
52+
53+
// Calculate the difference between the target and current rotation
54+
double rotationDifference = (targetRotation - currentRotation) % 360;
55+
56+
// Choose the optimal direction
57+
double optimalRotation = (rotationDifference <= 360 / 2)
58+
? rotationDifference
59+
: rotationDifference - 360;
60+
61+
angleFromTarget = optimalRotation;
62+
}
63+
}

‎src/main/java/frc/robot/commands/TeleopSwerve.java renamed to ‎src/main/java/frc/robot/commands/SwerveCommands/TeleopSwerve.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
package frc.robot.commands;
1+
package frc.robot.commands.SwerveCommands;
22

33
import frc.robot.Constants;
44
import frc.robot.subsystems.SwerveSubsystem;

‎src/main/java/frc/robot/subsystems/IntakeArmSubsystem.java

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.subsystems;
6+
7+
import com.ctre.phoenix6.StatusCode;
8+
import com.ctre.phoenix6.configs.MotionMagicConfigs;
9+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
10+
import com.ctre.phoenix6.controls.MotionMagicVoltage;
11+
import com.ctre.phoenix6.hardware.TalonFX;
12+
13+
import edu.wpi.first.wpilibj.DigitalInput;
14+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
15+
import static frc.robot.Constants.IntakeArm.*;
16+
17+
public class IntakeArmSubsystem extends SubsystemBase {
18+
private TalonFX m_IntakeArmMotor;
19+
private final MotionMagicVoltage motionMagic = new MotionMagicVoltage(0);
20+
private final DigitalInput intakeSwitch = new DigitalInput(switchID);
21+
// singleton
22+
private static IntakeArmSubsystem instance;
23+
24+
public static IntakeArmSubsystem getInstance() {
25+
if (instance == null) {
26+
instance = new IntakeArmSubsystem();
27+
}
28+
return instance;
29+
}
30+
/** Creates a new IntakeArmSubsystem. */
31+
private IntakeArmSubsystem() {
32+
this.m_IntakeArmMotor = new TalonFX(KMotorID);
33+
TalonFXConfiguration configs = new TalonFXConfiguration();
34+
MotionMagicConfigs mm = new MotionMagicConfigs();
35+
36+
mm.MotionMagicCruiseVelocity = mmVelocity;
37+
mm.MotionMagicAcceleration = mmAcceleration;
38+
mm.MotionMagicJerk = mmJerk;
39+
configs.MotionMagic = mm;
40+
41+
configs.Slot0.kP = KP;
42+
configs.Slot0.kD = KD;
43+
configs.Slot0.kV = KV;
44+
configs.Slot0.kS = KS;
45+
46+
configs.Voltage.PeakForwardVoltage = PeakForwardVoltage;
47+
configs.Voltage.PeakReverseVoltage = PeakReverseVoltage;
48+
configs.Feedback.SensorToMechanismRatio = SensorToMechanismRatio;
49+
50+
configs.SoftwareLimitSwitch.ForwardSoftLimitEnable = ForwardSoftLimitEnable;
51+
configs.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ForwardSoftLimitThreshold;
52+
configs.SoftwareLimitSwitch.ReverseSoftLimitEnable = ReverseSoftLimitEnable;
53+
configs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RevesrseSoftLimitThreshold;
54+
55+
56+
// gives code to TalonFX
57+
StatusCode status = StatusCode.StatusCodeNotInitialized;
58+
for (int i = 0; i < 5; i++) {
59+
status = m_IntakeArmMotor.getConfigurator().apply(configs);
60+
if (status.isOK()) {
61+
break;
62+
}
63+
}
64+
if (!status.isOK()) {
65+
System.out.println("Turret could not apply configs, error code: " + status.toString());
66+
}
67+
68+
69+
m_IntakeArmMotor.setPosition(startingValue);
70+
}
71+
72+
// moving the arm
73+
public void moveArmTo(double degrees){
74+
m_IntakeArmMotor.setControl(motionMagic.withPosition(degrees));
75+
}
76+
//set speed
77+
public void setSpeed(double speed){
78+
m_IntakeArmMotor.set(speed);
79+
}
80+
// set incoder idk
81+
public void setEncoder (double encoderValue){
82+
m_IntakeArmMotor.setPosition(encoderValue);
83+
}
84+
// get if a switch is press
85+
public boolean getSwitch(){
86+
return intakeSwitch.get();
87+
}
88+
// check if intake arm is in place
89+
public boolean checkIntakeArmPosion(){
90+
return Math.abs(m_IntakeArmMotor.getClosedLoopError().getValue()) < minimumError;
91+
}
92+
93+
@Override
94+
public void periodic() {
95+
// This method will be called once per scheduler run
96+
}
97+
}

0 commit comments

Comments
 (0)