Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/pre-main' into unit_tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@jhhbrown1
jhhbrown1 committed Feb 15, 2025
2 parents 3ef9021 + 7ab4cd2 commit c1149c0
Show file tree
Hide file tree
Showing 13 changed files with 351 additions and 156 deletions.
3 changes: 2 additions & 1 deletion .vscode/settings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,5 +68,6 @@
"editor.detectIndentation": false,
"editor.insertSpaces": false,
"java.format.settings.url": "eclipse-formatter.xml",
"java.sources.organizeImports.staticStarThreshold": 1
"java.sources.organizeImports.staticStarThreshold": 1,
"java.debug.settings.onBuildFailureProceed": true
}
32 changes: 7 additions & 25 deletions simgui-ds.json
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,4 @@
{
"System Joysticks": {
"window": {
"visible": false
}
},
"keyboardJoysticks": [
{
"axisConfig": [
Expand All @@ -16,27 +11,19 @@
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
},
{
"incKey": 69
},
{
"incKey": 82
}
],
"axisCount": 5,
"buttonCount": 8,
"axisCount": 3,
"buttonCount": 4,
"buttonKeys": [
90,
88,
67,
86,
66,
78,
77,
44
86
],
"povConfig": [
{
Expand Down Expand Up @@ -66,8 +53,8 @@
"axisCount": 2,
"buttonCount": 4,
"buttonKeys": [
-1,
-1,
77,
44,
46,
47
],
Expand Down Expand Up @@ -101,10 +88,5 @@
"buttonCount": 0,
"povCount": 0
}
],
"robotJoysticks": [
{
"guid": "Keyboard0"
}
]
}
87 changes: 61 additions & 26 deletions src/main/java/frc/robot/CommandComposer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,46 +3,82 @@
import static edu.wpi.first.wpilibj2.command.Commands.*;
import static frc.robot.Constants.DriveConstants.*;

import java.util.function.Supplier;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.AlgaeGrabberSubsystem;
import frc.robot.subsystems.CheeseStickSubsystem;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.ElevatorSubsystem;
import frc.robot.subsystems.PoseEstimationSubsystem;
import frc.robot.subsystems.WristSubsystem;

public class CommandComposer {
private static DriveSubsystem m_driveSubsystem;
private static AlgaeGrabberSubsystem m_algaeGrabberSubsystem;
private static CheeseStickSubsystem m_cheeseStickSubsystem;
private static ClimberSubsystem m_climberSubsystem;
private static ElevatorSubsystem m_elevatorSubsystem;
private static WristSubsystem m_wristSubsystem;
private static PoseEstimationSubsystem m_poseEstimationSubsystem;

public static void setSubsystems(DriveSubsystem driveSubsystem,
PoseEstimationSubsystem poseEstimationSubsystem) {
AlgaeGrabberSubsystem algaeGrabberSubsystem,
CheeseStickSubsystem cheeseStickSubsystem,
ClimberSubsystem climberSubsystem,
ElevatorSubsystem elevatorSubsystem,
WristSubsystem wristSubsystem, PoseEstimationSubsystem poseEstimationSubsystem) {
m_driveSubsystem = driveSubsystem;
m_algaeGrabberSubsystem = algaeGrabberSubsystem;
m_cheeseStickSubsystem = cheeseStickSubsystem;
m_climberSubsystem = climberSubsystem;
m_elevatorSubsystem = elevatorSubsystem;
m_wristSubsystem = wristSubsystem;
m_poseEstimationSubsystem = poseEstimationSubsystem;
}

/**
* Returns a {@code Command} for testing the rotation capability of the robot.
*
* @return a {@code Command} for testing the rotation capability of the robot
*/
public static Command testRotation() {
double rotionalSpeed = kTurnMaxAngularSpeed * 0.9;
double duration = 2.0;
private static Command scoreLevel(Supplier<Command> levelCommand) {
return sequence(
m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(1),
m_driveSubsystem.run(() -> m_driveSubsystem.drive(.5, 0, rotionalSpeed, true))
.withTimeout(duration),
m_driveSubsystem.run(() -> m_driveSubsystem.drive(-.5, 0, -rotionalSpeed, true))
.withTimeout(duration),
m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, rotionalSpeed, false))
.withTimeout(duration),
m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(1),
m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, -rotionalSpeed, false))
.withTimeout(duration),
m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(1),
m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, rotionalSpeed, false))
.withTimeout(duration),
m_driveSubsystem.run(() -> m_driveSubsystem.setModuleAngles(0)).withTimeout(1),
m_driveSubsystem.run(() -> m_driveSubsystem.drive(0, 0, -rotionalSpeed, false))
.withTimeout(duration));
levelCommand.get(),
m_wristSubsystem.goToAngle(35),
m_elevatorSubsystem.lowerToScore(),
m_cheeseStickSubsystem.release(),
waitSeconds(0), // TODO: find the amount of time needed to retract
levelCommand.get(),
m_cheeseStickSubsystem.grab(),
levelCommand.get(),
m_wristSubsystem.goToAngle(-90));
}

public static Command scoreLevelFour() {
return scoreLevel(m_elevatorSubsystem::goToLevelFourHeight);
}

public static Command scoreLevelThree() {
return scoreLevel(m_elevatorSubsystem::goToLevelThreeHeight);
}

public static Command scoreLevelTwo() {
return scoreLevel(m_elevatorSubsystem::goToLevelTwoHeight);
}

public static Command scoreLevelOne() {
return scoreLevel(m_elevatorSubsystem::goToLevelOneHeight);
}

public static Command prepareForCoralPickup() {
return sequence(
m_elevatorSubsystem.goToCoralStationHeight(),
m_wristSubsystem.goToAngle(-90));
}

public static Command pickupAtCoralStation() {
return sequence(
m_cheeseStickSubsystem.release(),
m_elevatorSubsystem.goToCoralStationHeight(),
m_cheeseStickSubsystem.grab());
}

/**
Expand All @@ -62,5 +98,4 @@ public static Command turnTowardTag(int tagID) {
false);
}).withTimeout(1);
}

}
79 changes: 55 additions & 24 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package frc.robot;

import static edu.wpi.first.units.Units.*;

import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.NeutralModeValue;

Expand All @@ -10,33 +12,37 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Distance;

public class Constants {
public static final class AlgaeConstants {
// FLYWHEEL IS 550
// OTHER IS NORMAL NEO

public static final int kFlywheelMotorPort = 90;
public static final int kGrabberAnglePort = 3;
public static final boolean kFlywheelInvert = false;
public static final int kFlywheelMotorPort = 55;
public static final int kGrabberAnglePort = 56;
public static final boolean kFlywheelInvert = true;
public static final boolean kGrabberAngleInvert = false;

public static final int k550SmartCurrentLimit = 15;

public static final int kSmartCurrentLimit = 50;
public static final int kPeakCurrentDurationMillis = 100;

public static final int k550SmartCurrentLimit = 15;

public static final double kCurrentToStop = 20;
public static final double kTimeOverCurrentToStop = .01;

public static final double kP = 0.5;
public static final float kDeployGrabberRotations = 2;
public static final double kFlywheelSpeed = .8;

public static final double kP = 0.5; // TODO: Tune
public static final double kI = 0.0;
public static final double kD = 0;
}

public static final class CheeseStickConstants {
public static final int kServoPort = 0;
public static final int kMaxRotation = 270;
public static final int kServoPort = 0; // TODO: CHANGE
/**
* Set this value to how far the cheese stick wheels extend beyond the lexan.
*/
public static final Distance kExtensionLength = Inch.of(.5);
}

public static final class ClimberConstants {
Expand Down Expand Up @@ -87,7 +93,7 @@ public static final class DriveConstants {
public static final int kBackRightCANCoderPort = 22;
public static final int kBackLeftCANCoderPort = 32;

// Make sure these are tuned (can do with SysId)
// TODO: Make sure these are tuned (can do with SysId)
public static final double kP = 0.09;
public static final double kI = 0.0;
public static final double kD = 0;
Expand All @@ -102,14 +108,17 @@ public static final class DriveConstants {
public static final double kTurnMinAngularSpeed = Math.toRadians(5); // 5 degrees per second
public static final double kDriveMaxVoltage = 12;

public static final double kDriveGearRatio = 6.12;
public static final double kTeleopMaxVoltage = 12;
public static final double kTeleopMaxTurnVoltage = 7.2;
public static final double kDriveGearRatio = 6.75;
public static final double kSteerGearRatio = 150.0 / 7;
public static final double kWheelDiameter = Units.inchesToMeters(4);
public static final double kWheelCircumference = Math.PI * kWheelDiameter;

public static final double kMetersPerMotorRotation = kWheelCircumference / kDriveGearRatio;

// https://docs.wpilib.org/en/latest/docs/software/basic-programming/coordinate-system.html
// TODO: CHECK
public static final Translation2d kFrontLeftLocation = new Translation2d(0.381, 0.381);
public static final Translation2d kFrontRightLocation = new Translation2d(0.381, -0.381);
public static final Translation2d kBackLeftLocation = new Translation2d(-0.381, 0.381);
Expand Down Expand Up @@ -152,30 +161,52 @@ public static final class DriveConstants {
}

public static final class ElevatorConstants {
public static final double kFF = 0.00008040000102482736; // Feedforward Constant
public static final int kElevatorMotorPort = 45;
public static final int kElevatorMotorPort = 57; // TODO: Change with real one
public static final int kSmartCurrentLimit = 60;
public static final int kSecondaryCurrentLimit = 70;
public static final double kMinOutput = -1;
public static final double kMaxOutput = 1;
public static final double kP = 5;
public static final double kP = 0; // TODO: tune
public static final double kI = 0;
public static final double kD = 0;
public static final double kS = 0;
public static final double kG = 2;
public static final double kV = 2;
public static final double kA = 0;
public static final double kGearRatio = 10; // TODO: Right gear ratio?
/**
* 24 teeth, 5 mm pitch, one rotation moves 120 mm, 2 stage cascading elevator
* means total height change is 240 mm.
*/
public static final double kMetersPerPulleyRotation = (24.0 * 5 * 2 / 1000);
/**
* <pre>
* 1 pulley rotation pulley circumference
* 1 motor rot * --------------------- * --------------------
* kGearRatio motor rots 1 pulley rotation
* </pre>
*/
public static final double kMetersPerMotorRotation = (1 / kGearRatio)
* (2 * Math.PI * kMetersPerPulleyRotation);
public static final double kMaxVelocity = 2;
public static final double kMaxAccel = 2;
public static final double kTolerance = 1;
public static final int kMaxExtension = 250;
public static final double kLevelOneHeight = Units.inchesToMeters(41 - 24);
// public static final int kMaxExtension = 250; // TODO: Check this? Ask Design
public static final double kLevelOneHeight = Units.inchesToMeters(41 - 24); // TODO: During testing make sure
// these are right
public static final double kLevelTwoHeight = kLevelOneHeight; // same as level 1
public static final double kLevelThreeHeight = Units.inchesToMeters(60 - 24);
public static final double kLevelFourHeight = Units.inchesToMeters(90 - 24);
public static final double kCoralStationHeight = 0;
public static final double kToScoreHeightDecrease = 0; // TODO: The amount that the elevator decreases in order
// to
// score
public static final double kCoralStationHeight = 0; // TODO: Change
}

public static final class WristConstants {
public static final int kWristMotorPort = 1; /* 60 */
public static final int kWristMotorPort = 58; // TODO: Change
public static final int kSmartCurrentLimit = 20;
public static final int kSecondaryCurrentLimit = 20;

// Make sure these are tuned (can do with SysId)
// TODO: Make sure these are tuned (can do with SysId)
public static final double kP = 0.09;
public static final double kI = 0.0;
public static final double kD = 0;
Expand All @@ -184,7 +215,7 @@ public static final class WristConstants {
public static final double kV = 0.11;
public static final double kA = 0.009;

public static final double kTolerance = 0;
public static final double kTolerance = 0; // TODO: Change this
}

/**
Expand Down
5 changes: 1 addition & 4 deletions src/main/java/frc/robot/Main.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,7 @@
import edu.wpi.first.wpilibj.RobotBase;

public final class Main {
private Main() {
}

public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}
}
Loading

0 comments on commit c1149c0

Please sign in to comment.