Skip to content

Commit

Permalink
Merge pull request #2 from SaintsRobotics/swerve
Browse files Browse the repository at this point in the history
Swerve Code
  • Loading branch information
ArnoUUU authored Feb 13, 2024
2 parents 99504a2 + 8656df3 commit 48c628a
Show file tree
Hide file tree
Showing 14 changed files with 1,228 additions and 25 deletions.
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ dependencies {
simulationRelease wpi.sim.enableRelease()

testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
//testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
}

test {
Expand Down
1 change: 1 addition & 0 deletions networktables.json
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
[]
97 changes: 97 additions & 0 deletions simgui-ds.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,97 @@
{
"keyboardJoysticks": [
{
"axisConfig": [
{
"decKey": 65,
"incKey": 68
},
{
"decKey": 87,
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
}
],
"axisCount": 3,
"buttonCount": 4,
"buttonKeys": [
90,
88,
67,
86
],
"povConfig": [
{
"key0": 328,
"key135": 323,
"key180": 322,
"key225": 321,
"key270": 324,
"key315": 327,
"key45": 329,
"key90": 326
}
],
"povCount": 1
},
{
"axisConfig": [
{
"decKey": 74,
"incKey": 76
},
{
"decKey": 73,
"incKey": 75
}
],
"axisCount": 2,
"buttonCount": 4,
"buttonKeys": [
77,
44,
46,
47
],
"povCount": 0
},
{
"axisConfig": [
{
"decKey": 263,
"incKey": 262
},
{
"decKey": 265,
"incKey": 264
}
],
"axisCount": 2,
"buttonCount": 6,
"buttonKeys": [
260,
268,
266,
261,
269,
267
],
"povCount": 0
},
{
"axisCount": 0,
"buttonCount": 0,
"povCount": 0
}
],
"robotJoysticks": [
{
"guid": "Keyboard0"
}
]
}
18 changes: 18 additions & 0 deletions simgui.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
{
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
"/SmartDashboard/Field": "Field2d"
},
"windows": {
"/SmartDashboard/Field": {
"window": {
"visible": true
}
}
}
},
"NetworkTables Info": {
"visible": true
}
}
126 changes: 126 additions & 0 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,126 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.numbers.N3;

/**
* The Constants class provides a convenient place for teams to hold robot-wide
* numerical or boolean
* constants. This class should not be used for any other purpose. All constants
* should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>
* It is advised to statically import this class (or one of its inner classes)
* wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {

/**
* Input/Output constants
*/
public static final class IOConstants {
public static final int kDriverControllerPort = 0;

public static final double kControllerDeadband = 0.2;
public static final double kSlowModeScalar = 0.8;
}

public static final class DriveConstants {
// TODO: set motor and encoder constants
public static final int kFrontLeftDriveMotorPort = 32;
public static final int kRearLeftDriveMotorPort = 29;
public static final int kFrontRightDriveMotorPort = 38;
public static final int kRearRightDriveMotorPort = 34;

public static final int kFrontLeftTurningMotorPort = 28;
public static final int kRearLeftTurningMotorPort = 22;
public static final int kFrontRightTurningMotorPort = 37;
public static final int kRearRightTurningMotorPort = 26;

public static final int kFrontLeftTurningEncoderPort = 19;
public static final int kRearLeftTurningEncoderPort = 20;
public static final int kFrontRightTurningEncoderPort = 18;
public static final int kRearRightTurningEncoderPort = 17;

public static final double kFrontLeftTurningEncoderOffset = 0;
public static final double kRearLeftTurningEncoderOffset = 0;
public static final double kFrontRightTurningEncoderOffset = 0;
public static final double kRearRightTurningEncoderOffset = 0;

// TODO: Test motor orientations before driving on an actual robot
public static final boolean kFrontLeftDriveMotorReversed = false;
public static final boolean kRearLeftDriveMotorReversed = false;
public static final boolean kFrontRightDriveMotorReversed = true;
public static final boolean kRearRightDriveMotorReversed = true;

/** Distance between centers of right and left wheels on robot (in meters). */
public static final double kTrackWidth = 0.57785;

/** Distance between front and back wheels on robot (in meters). */
public static final double kWheelBase = 0.57785;

/** Diameter of each wheel in the SDS MK4i swerve module (in meters) */
public static final double kWheelDiameterMeters = 0.1;

/** Gear ratio between the motor and the wheel. */
public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration
// public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration

// TODO: Tune this PID before running on a robot on the ground
public static final double kPModuleTurningController = -0.3;

public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));

/** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
public static final double kMaxSpeedMetersPerSecond = 4.4196;
/** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164;
// ^^ Calculated using the method taken from the old SDS github example

/** Heading Correction */
public static final double kHeadingCorrectionTurningStopTime = 0.2;
// TODO: Tune this PID before running on a robot on the ground
public static final double kPHeadingCorrectionController = 5;
}

public static final class ShooterConstants {
public static final int kTopShooterMotorPort = 35;
public static final int kBottomShooterMotorPort = 20;
}

public static final class VisionConstants {
// TODO: Update cam pose relative to center of bot
public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));
public static final double[] kLimelightCamPose = {
kCamPose.getX(),
kCamPose.getY(),
kCamPose.getZ(),
kCamPose.getRotation().getX(),
kCamPose.getRotation().getY(),
kCamPose.getRotation().getZ() };

// TODO: Experiment with different std devs in the pose estimator
public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9);

// Field size in meters
public static final double kFieldWidth = 8.21055;
public static final double kFieldLength = 16.54175;
}

}
10 changes: 10 additions & 0 deletions src/main/java/frc/robot/Main.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,19 @@

import edu.wpi.first.wpilibj.RobotBase;

/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}

/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
Expand Down
50 changes: 40 additions & 10 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,66 +8,96 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;

/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;

private RobotContainer m_robotContainer;

/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}

/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}

/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}

@Override
public void disabledPeriodic() {}

@Override
public void disabledExit() {}

/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();

// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}

/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}

@Override
public void autonomousExit() {}

@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}

/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}

@Override
public void teleopExit() {}

@Override
public void testInit() {
// Cancels all running commands at the start of test mode.
CommandScheduler.getInstance().cancelAll();
}

/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}

/** This function is called once when the robot is first started up. */
@Override
public void simulationInit() {}

/** This function is called periodically whilst in simulation. */
@Override
public void testExit() {}
public void simulationPeriodic() {}
}
Loading

0 comments on commit 48c628a

Please sign in to comment.