Skip to content
This repository was archived by the owner on Jan 16, 2025. It is now read-only.

Commit ebaac1a

Browse files
CryptoSOL123CryptoSOL123
committed
Added all the constants
1 parent 1496d71 commit ebaac1a

9 files changed

+246
-8
lines changed

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

+2
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,8 @@
88
import edu.wpi.first.wpilibj2.command.Commands;
99

1010
public class RobotContainer {
11+
// Init all the required stuff
12+
1113
public RobotContainer() {
1214
configureBindings();
1315
}

‎src/main/java/frc/robot/commands/RunSwerveBase.java

+45
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
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;
6+
7+
import edu.wpi.first.wpilibj2.command.Command;
8+
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
9+
import frc.robot.subsystems.SwerveBase;
10+
11+
public class RunSwerveBase extends Command {
12+
/** Creates a new RunSwerveBase. */
13+
private final SwerveBase swerveBase;
14+
private final CommandXboxController joystick;
15+
public RunSwerveBase(SwerveBase swerveBase, CommandXboxController joystick) {
16+
this.swerveBase = swerveBase;
17+
this.joystick = joystick;
18+
addRequirements(swerveBase);
19+
// Use addRequirements() here to declare subsystem dependencies.
20+
}
21+
22+
// Called when the command is initially scheduled.
23+
@Override
24+
public void initialize() {
25+
swerveBase.coast();
26+
}
27+
28+
// Called every time the scheduler runs while the command is scheduled.
29+
@Override
30+
public void execute() {
31+
swerveBase.setSwerveDrive(joystick.getLeftX(), joystick.getLeftY(), joystick.getRightX());
32+
}
33+
34+
// Called once the command ends or is interrupted.
35+
@Override
36+
public void end(boolean interrupted) {
37+
swerveBase.brake();
38+
}
39+
40+
// Returns true when the command should end.
41+
@Override
42+
public boolean isFinished() {
43+
return false;
44+
}
45+
}

‎src/main/java/frc/robot/constants/ConstantsEverything.java

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
package frc.robot.constants;
2+
3+
public class ConstantsEverything {
4+
public static final int handoffTopMotorCanID = 0;
5+
public static final int handoffBottomMotorCanID = 1;
6+
7+
public static final int hangerMotorCanID = 2;
8+
9+
public static final int shooterTopMotorCanID = 3;
10+
public static final int shooterBottomMotorCanID = 4;
11+
12+
public static final int intakeMotorOneCanID = 5;
13+
public static final int intakeMotorTwoCanID = 6;
14+
15+
public static final int rightFrontMoveMotor = 7;
16+
public static final int rightFrontTurnMotor = 8;
17+
public static final int rightFrontCanCoder = 15;
18+
19+
public static final int leftFrontMoveMotor = 9;
20+
public static final int leftFrontTurnMotor = 10;
21+
public static final int leftFrontCanCoder = 16;
22+
23+
public static final int rightBackMoveMotor = 11;
24+
public static final int rightBackTurnMotor = 12;
25+
public static final int rightBackCanCoder = 17;
26+
27+
28+
public static final int leftBackMoveMotor = 13;
29+
public static final int leftBackTurnMotor = 14;
30+
public static final int leftBackCanCoder = 18;
31+
32+
public static final int gyroscopeCanID = 19;
33+
34+
}

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

+3-2
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010

1111
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1212
import edu.wpi.first.wpilibj2.command.SubsystemBase;
13+
import frc.robot.constants.ConstantsEverything;
1314

1415
public class HandoffSubsystem extends SubsystemBase {
1516
/** Creates a new HandoffSubsystem. */
@@ -18,8 +19,8 @@ public class HandoffSubsystem extends SubsystemBase {
1819
public HandoffSubsystem() {
1920

2021
// Add values for the deviceID
21-
topMotor = new CANSparkMax(0, MotorType.kBrushless);
22-
bottomMotor = new CANSparkMax(1, MotorType.kBrushless);
22+
topMotor = new CANSparkMax(ConstantsEverything.handoffTopMotorCanID, MotorType.kBrushless);
23+
bottomMotor = new CANSparkMax(ConstantsEverything.handoffBottomMotorCanID, MotorType.kBrushless);
2324
bottomMotor.setInverted(true);
2425

2526
}

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

+3-2
Original file line numberDiff line numberDiff line change
@@ -11,14 +11,15 @@
1111

1212
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1313
import edu.wpi.first.wpilibj2.command.SubsystemBase;
14+
import frc.robot.constants.ConstantsEverything;
1415

1516
public class HangerSubsystem extends SubsystemBase {
1617
/** Creates a new HangerSubsystem. */
17-
private final TalonFX hangerMotor; // Kraken is powered by the TalonFX software-- Ricky Hu
18+
private final TalonFX hangerMotor; // Kraken is powered by the TalonFX software-- Ricky Hu
1819
public HangerSubsystem() {
1920
// Set values for Talon fx
2021
// Only one Hanger motor
21-
hangerMotor = new TalonFX(2);
22+
hangerMotor = new TalonFX(ConstantsEverything.hangerMotorCanID);
2223
// Might need to make it inverted?
2324
}
2425
// Regular coasting and braking of the motor

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

+3-2
Original file line numberDiff line numberDiff line change
@@ -12,14 +12,15 @@
1212

1313
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1414
import edu.wpi.first.wpilibj2.command.SubsystemBase;
15+
import frc.robot.constants.ConstantsEverything;
1516

1617
public class IntakeSubsystem extends SubsystemBase {
1718
/** Creates a new IntakeSubsystem. */
1819
private final CANSparkMax motorOne;
1920
private final CANSparkMax motorTwo;
2021
public IntakeSubsystem() {
21-
motorOne = new CANSparkMax(5, MotorType.kBrushless);
22-
motorTwo = new CANSparkMax(6, MotorType.kBrushless);
22+
motorOne = new CANSparkMax(ConstantsEverything.intakeMotorOneCanID, MotorType.kBrushless);
23+
motorTwo = new CANSparkMax(ConstantsEverything.intakeMotorTwoCanID, MotorType.kBrushless);
2324
}
2425

2526
public void coast(){

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

+3-2
Original file line numberDiff line numberDiff line change
@@ -9,14 +9,15 @@
99

1010
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1111
import edu.wpi.first.wpilibj2.command.SubsystemBase;
12+
import frc.robot.constants.ConstantsEverything;
1213

1314
public class ShooterSubsystem extends SubsystemBase {
1415
/** Creates a new ShooterSubsystem. */
1516
private final TalonFX topShooterMotor;
1617
private final TalonFX bottomShooterMotor;
1718
public ShooterSubsystem() {
18-
topShooterMotor = new TalonFX(3);
19-
bottomShooterMotor = new TalonFX(4);
19+
topShooterMotor = new TalonFX(ConstantsEverything.shooterTopMotorCanID);
20+
bottomShooterMotor = new TalonFX(ConstantsEverything.shooterBottomMotorCanID);
2021
bottomShooterMotor.setInverted(true);
2122
}
2223

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

+82
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
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 java.lang.reflect.Array;
8+
9+
import com.ctre.phoenix6.hardware.CANcoder;
10+
import com.ctre.phoenix6.hardware.Pigeon2;
11+
import com.ctre.phoenix6.hardware.TalonFX;
12+
13+
import edu.wpi.first.math.kinematics.ChassisSpeeds;
14+
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
15+
import edu.wpi.first.math.kinematics.SwerveModuleState;
16+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
17+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
18+
import frc.robot.constants.ConstantsEverything;
19+
20+
public class SwerveBase extends SubsystemBase {
21+
/** Creates a new SwerveBase. */
22+
private final SwerveModule rightFront;
23+
private final SwerveModule leftFront;
24+
private final SwerveModule rightBack;
25+
private final SwerveModule leftBack;
26+
private final Pigeon2 gyroscope;
27+
28+
public SwerveBase() {
29+
rightFront = new SwerveModule(new TalonFX(ConstantsEverything.rightFrontMoveMotor), new TalonFX(ConstantsEverything.rightFrontTurnMotor), new CANcoder(ConstantsEverything.rightFrontCanCoder), 0);
30+
leftFront = new SwerveModule(new TalonFX(ConstantsEverything.leftFrontMoveMotor), new TalonFX(ConstantsEverything.leftFrontTurnMotor), new CANcoder(ConstantsEverything.leftFrontCanCoder), 0);
31+
rightBack = new SwerveModule(new TalonFX(ConstantsEverything.rightBackMoveMotor), new TalonFX(ConstantsEverything.rightBackTurnMotor), new CANcoder(ConstantsEverything.rightBackCanCoder), 0);
32+
leftBack = new SwerveModule(new TalonFX(ConstantsEverything.leftBackMoveMotor), new TalonFX(ConstantsEverything.leftBackTurnMotor), new CANcoder(ConstantsEverything.leftBackCanCoder), 0);
33+
34+
gyroscope = new Pigeon2(ConstantsEverything.gyroscopeCanID);
35+
36+
}
37+
public void coast(){
38+
rightFront.coast();
39+
leftFront.coast();
40+
rightBack.coast();
41+
leftBack.coast();
42+
}
43+
44+
public void brake(){
45+
rightFront.brake();
46+
leftFront.brake();
47+
rightBack.brake();
48+
leftBack.brake();
49+
}
50+
51+
public void setSwerveModulesStates(SwerveModuleState[] swerveModuleStates){
52+
rightFront.setSwerveState(swerveModuleStates[0]);
53+
leftFront.setSwerveState(swerveModuleStates[1]);
54+
rightBack.setSwerveState(swerveModuleStates[2]);
55+
leftBack.setSwerveState(swerveModuleStates[3]);
56+
}
57+
58+
public void setSwerveBaseSpeed(ChassisSpeeds chassisSpeeds){
59+
setSwerveModulesStates(new SwerveDriveKinematics().toSwerveModuleStates(chassisSpeeds));
60+
}
61+
62+
public void setSwerveDrive(double velocityY, double velocityX, double rX){
63+
setSwerveBaseSpeed(ChassisSpeeds.fromFieldRelativeSpeeds(velocityX, velocityY, rX, gyroscope.getRotation2d()));
64+
}
65+
66+
@Override
67+
public void periodic() {
68+
// This method will be called once per scheduler run
69+
70+
71+
SmartDashboard.putNumber("Right Front Speed", rightFront.getVelocity());
72+
SmartDashboard.putNumber("Left Front Speed", leftFront.getVelocity());
73+
SmartDashboard.putNumber("Right Back Speed", rightBack.getVelocity());
74+
SmartDashboard.putNumber("Left Back Speed", leftBack.getVelocity());
75+
76+
77+
SmartDashboard.putNumber("Right Front Angle", rightFront.getAngle());
78+
SmartDashboard.putNumber("Left Front Angle", leftFront.getAngle());
79+
SmartDashboard.putNumber("Right Back Angle", rightBack.getAngle());
80+
SmartDashboard.putNumber("Left Back Angle", leftBack.getAngle());
81+
}
82+
}

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

+71
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
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.hardware.CANcoder;
8+
import com.ctre.phoenix6.hardware.TalonFX;
9+
import com.ctre.phoenix6.signals.NeutralModeValue;
10+
11+
import edu.wpi.first.math.controller.PIDController;
12+
import edu.wpi.first.math.kinematics.SwerveModuleState;
13+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
14+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
15+
16+
// SwerveModule class that controls each individual motor
17+
public class SwerveModule extends SubsystemBase {
18+
/** Creates a new SwerveModule. */
19+
// Kraken motor that are used --> Talon FX
20+
private final TalonFX moveMotor; // Motor that is used to go forward or backward
21+
private final TalonFX turnMotor; // Motor that turns the move motor
22+
private final CANcoder caNcoder; // Encoder to get the position
23+
private final PIDController pidController; // PID Control
24+
private final double offsetAngle;
25+
private double totalSpeed = 0; // Set to 0 at the beggining
26+
27+
public SwerveModule(TalonFX moveMotor, TalonFX turnMotor, CANcoder caNcoder, double offsetAngle) {
28+
this.turnMotor = turnMotor;
29+
this.moveMotor = moveMotor;
30+
this.caNcoder = caNcoder;
31+
this.pidController = new PIDController(0, 0, 0);
32+
this.pidController.setTolerance(5);
33+
this.offsetAngle = offsetAngle;
34+
35+
}
36+
37+
public void coast(){
38+
moveMotor.setNeutralMode(NeutralModeValue.Coast);
39+
turnMotor.setNeutralMode(NeutralModeValue.Coast);
40+
}
41+
42+
public void brake(){
43+
moveMotor.setNeutralMode(NeutralModeValue.Brake);
44+
turnMotor.setNeutralMode(NeutralModeValue.Brake);
45+
}
46+
47+
public double getVelocity(){
48+
return totalSpeed;
49+
}
50+
public double getAngle(){
51+
double angle = caNcoder.getPosition().getValueAsDouble(); // Easier to read and understand
52+
return angle;
53+
}
54+
public void setSwerveState(SwerveModuleState swerveModuleState){
55+
totalSpeed = swerveModuleState.speedMetersPerSecond; // Put the speed in a global variable --> Used in another function
56+
moveMotor.set(totalSpeed);
57+
double newAngle = swerveModuleState.angle.getDegrees() - offsetAngle;
58+
pidController.setSetpoint(newAngle);
59+
pidController.calculate(turnMotor.getPosition().getValueAsDouble(), newAngle);
60+
61+
}
62+
63+
@Override
64+
public void periodic() {
65+
// This method will be called once per scheduler run
66+
SmartDashboard.putNumber("Move Motor Position", moveMotor.getPosition().getValueAsDouble());
67+
SmartDashboard.putNumber("Turn Motor Angle", caNcoder.getPosition().getValueAsDouble());
68+
SmartDashboard.putNumber("Move Motor Temperature", moveMotor.getDeviceTemp().getValueAsDouble());
69+
SmartDashboard.putNumber("Turn Motor Temperature", turnMotor.getDeviceTemp().getValueAsDouble());
70+
}
71+
}

0 commit comments

Comments
 (0)