Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Adding simulation files #6

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Adding simulation files #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2cd0433
Adding simulation files
ishanvenkat Sep 30, 2024
2135dee
Deleting commented code
ishanvenkat Oct 1, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions 1458TeamCode/build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava
wpi.java.debugJni = false

// Set this to true to enable desktop support.
def includeDesktopSupport = false
def includeDesktopSupport = true

// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
// Also defines JUnit 5.
Expand Down Expand Up @@ -98,4 +98,4 @@ wpi.java.configureTestTasks(test)
// Configure string concat to always inline compile
tasks.withType(JavaCompile) {
options.compilerArgs.add '-XDstringConcat=inline'
}
}
19 changes: 10 additions & 9 deletions 1458TeamCode/src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import frc.robot.autos.*;
import frc.robot.commands.*;
Expand Down Expand Up @@ -33,18 +34,18 @@ public class RobotContainer {
/* Subsystems */
private final Drive s_Swerve = new Drive();


/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
s_Swerve.setDefaultCommand(
new TeleopSwerve(
s_Swerve,
() -> -driver.getRawAxis(translationAxis),
() -> -driver.getRawAxis(strafeAxis),
() -> -driver.getRawAxis(rotationAxis),
() -> robotCentric.getAsBoolean()
)
TeleopSwerve teleopSwerveCommand = new TeleopSwerve(
s_Swerve,
() -> -driver.getRawAxis(translationAxis),
() -> -driver.getRawAxis(strafeAxis),
() -> -driver.getRawAxis(rotationAxis),
() -> robotCentric.getAsBoolean()
);
s_Swerve.setDefaultCommand(teleopSwerveCommand);

SmartDashboard.putData("TeleopSwerveCmd", teleopSwerveCommand);

// Configure the button bindings
configureButtonBindings();
Expand Down
44 changes: 42 additions & 2 deletions 1458TeamCode/src/main/java/frc/robot/subsystems/Drive.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,13 +8,20 @@
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;

import java.util.List;

import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.hardware.Pigeon2;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

Expand All @@ -23,6 +30,9 @@ public class Drive extends SubsystemBase {
public Module[] mSwerveMods;
public Pigeon2 gyro;

private Trajectory trajectory;
private final Field2d field = new Field2d();

public Drive() {
gyro = new Pigeon2(Constants.Swerve.pigeonID, "CV");
gyro.getConfigurator().apply(new Pigeon2Configuration());
Expand All @@ -35,8 +45,23 @@ public Drive() {
new Module(3, Constants.Swerve.BackRightMod.constants)
};


swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions());

// Simulation
// Create and push Field2d to SmartDashboard.
SmartDashboard.putData("Field", field);

// Create the trajectory to follow in autonomous.
trajectory =
TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0))
);

// Push the trajectory to Field2d.
field.getObject("traj").setTrajectory(trajectory);
}

public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) {
Expand Down Expand Up @@ -127,10 +152,25 @@ public void periodic() {
for (Module mod : mSwerveMods) {
SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", (mod.getCANcoder().getDegrees()+180)%360);


// SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", ((mod.mAngleMotor.getPosition().getValue()%22.0)*360/22-180));
SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", ((mod.mAngleMotor.getPosition().getValue() * 360) % 360 + 360) % 360); // This is super specific, don't break this pls
// SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond);
}

field.setRobotPose(swerveOdometry.getPoseMeters());
}

@Override
public void simulationPeriodic() {
// TODO: Sim jointly as one system
for (Module mod : mSwerveMods) {
mod.updateSimPeriodic();

SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", (mod.getCANcoder().getDegrees()+180)%360);
SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", ((mod.mAngleMotor.getPosition().getValue() * 360) % 360 + 360) % 360); // This is super specific, don't break this pls
}

swerveOdometry.update(getGyroYaw(), getModulePositions());
field.setRobotPose(swerveOdometry.getPoseMeters());
}
}
81 changes: 76 additions & 5 deletions 1458TeamCode/src/main/java/frc/robot/subsystems/Module.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,16 +5,24 @@
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.sim.CANcoderSimState;
import com.ctre.phoenix6.sim.TalonFXSimState;

import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;

import frc.robot.lib.math.Conversions;
import frc.robot.lib.util.SwerveModuleConstants;
import frc.robot.Constants;
import frc.robot.CTREConfigs;
import frc.robot.Robot;

public class Module {
Expand All @@ -23,6 +31,7 @@ public class Module {

TalonFX mAngleMotor;
private TalonFX mDriveMotor;

private CANcoder angleEncoder;

private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward(Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA);
Expand All @@ -34,10 +43,20 @@ public class Module {
/* angle motor control requests */
private final PositionVoltage anglePosition = new PositionVoltage(0);

/* simulation */
// private final DifferentialDrivetrainSim mDriveSim;
private final FlywheelSim mDriveSim;
private final SingleJointedArmSim mSteeringSim;

// private final EncoderSim encoderSim;
private final TalonFXSimState mDriveMotorSimState;
private final TalonFXSimState mAngleMotorSimState;
private CANcoderSimState angleEncoderSimState;

public Module(int moduleNumber, SwerveModuleConstants moduleConstants){
this.moduleNumber = moduleNumber;
this.angleOffset = moduleConstants.angleOffset;

/* Angle Encoder Config */
angleEncoder = new CANcoder(moduleConstants.cancoderID, "CV");
angleEncoder.getConfigurator().apply(Robot.ctreConfigs.swerveCANcoderConfig);
Expand All @@ -59,6 +78,32 @@ public Module(int moduleNumber, SwerveModuleConstants moduleConstants){
mAngleMotor.setInverted(true);
}
mDriveMotor.getConfigurator().setPosition(0.0);

// For simulation
mDriveMotorSimState = mDriveMotor.getSimState();
mAngleMotorSimState = mAngleMotor.getSimState();
angleEncoderSimState = angleEncoder.getSimState();

final double kEncoderRadiansPerPulse = 2.0 * Math.PI / 2048;

mDriveSim = new FlywheelSim(
DCMotor.getFalcon500(1),
Constants.Swerve.driveGearRatio,
0.01,
VecBuilder.fill(kEncoderRadiansPerPulse) // Add noise with a std-dev of 1 tick
);

mSteeringSim = new SingleJointedArmSim(
LinearSystemId.createSingleJointedArmSystem(DCMotor.getFalcon500(1), 0.001, Constants.Swerve.angleGearRatio),
DCMotor.getFalcon500(1),
Constants.Swerve.angleGearRatio,
0.0, // Length (m)
Double.NEGATIVE_INFINITY, // Min angle
Double.POSITIVE_INFINITY, // Max angle
false, // Simulate gravity
0.0,
VecBuilder.fill(kEncoderRadiansPerPulse) // Add noise with a std-dev of 1 tick
);
}

public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
Expand Down Expand Up @@ -94,15 +139,41 @@ public void resetToAbsolute() {

public SwerveModuleState getState(){
return new SwerveModuleState(
Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference),
Conversions.RPSToMPS(mDriveMotor.getVelocity().getValue(), Constants.Swerve.wheelCircumference),
Rotation2d.fromRotations(mAngleMotor.getPosition().getValue())
);
}

public SwerveModulePosition getPosition(){
return new SwerveModulePosition(
Conversions.rotationsToMeters(mDriveMotor.getPosition().getValue(), Constants.Swerve.wheelCircumference),
Conversions.rotationsToMeters(mDriveMotor.getPosition().getValue(), Constants.Swerve.wheelCircumference),
Rotation2d.fromRotations(mAngleMotor.getPosition().getValue())
);
}

/** Simulate one module with naive physics model. */
public void updateSimPeriodic() {
// Pass the robot battery voltage to the simulated devices
mDriveMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage());
mAngleMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage());
angleEncoderSimState.setSupplyVoltage(RobotController.getBatteryVoltage());

// Simulate drive
mDriveSim.setInput(mDriveMotorSimState.getMotorVoltage() * RobotController.getBatteryVoltage());
// mDriveSim.setInputs(mDriveMotorSimState.getVelocity().getValue() * RobotController.getBatteryVoltage());
mDriveSim.update(TimedRobot.kDefaultPeriod);

double driveVelocity = mDriveSim.getAngularVelocityRadPerSec() * Constants.Swerve.wheelCircumference / (2.0 * Math.PI);
mDriveMotorSimState.setRawRotorPosition(driveVelocity * Constants.Swerve.driveGearRatio);

// Simulate steering
mSteeringSim.setInput(mAngleMotorSimState.getMotorVoltage() * RobotController.getBatteryVoltage());
mSteeringSim.update(TimedRobot.kDefaultPeriod);

double angle = mSteeringSim.getAngleRads() * Constants.Swerve.wheelCircumference / (2.0 * Math.PI);
double steeringVelocity = mSteeringSim.getVelocityRadPerSec() * Constants.Swerve.wheelCircumference / (2.0 * Math.PI);

mAngleMotorSimState.setRawRotorPosition(angle);
mAngleMotorSimState.setRotorVelocity(steeringVelocity * Constants.Swerve.driveGearRatio);
}
}