diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "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 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..b7f47a9 --- /dev/null +++ b/simgui.json @@ -0,0 +1,39 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Autonomous": "String Chooser", + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/Odometry/Field": "Field2d", + "/SmartDashboard/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/Autonomous": { + "window": { + "visible": true + } + }, + "/SmartDashboard/Sim Field": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Sim Field": { + "open": true + }, + "SimDrivetrain": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5c5e3e6..f04c0cc 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -6,18 +6,21 @@ package com.stuypulse.robot; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.drivetrain.DriveDrive; import com.stuypulse.robot.commands.drivetrain.DrivetrainDrive; +import com.stuypulse.robot.commands.drivetrain.DrivetrainDriveForever; import com.stuypulse.robot.commands.launcher.LaunchPrepare; import com.stuypulse.robot.commands.launcher.LauncherIntakeNote; import com.stuypulse.robot.commands.launcher.LauncherStop; import com.stuypulse.robot.commands.odometry.OdometryRealign; import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.subsystems.drivetrain.Drivetrain; +import com.stuypulse.robot.subsystems.drivetrain.AbstractDrivetrain; import com.stuypulse.robot.subsystems.launcher.Launcher; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.vision.AbstractVision; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -29,8 +32,10 @@ public class RobotContainer { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem - public final Drivetrain drivetrain = Drivetrain.getInstance(); - public final Launcher launcher = Launcher.getInstance(); + public final AbstractDrivetrain drivetrain = AbstractDrivetrain.getInstance(); + // public final Launcher launcher = Launcher.getInstance(); + //public final AbstractOdometry odometry = AbstractOdometry.getInstance(); + //public final AbstractVision vision = AbstractVision.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -48,7 +53,7 @@ public RobotContainer() { /****************/ private void configureDefaultCommands() { - drivetrain.setDefaultCommand(new DrivetrainDrive(drivetrain, driver)); + drivetrain.setDefaultCommand(new DrivetrainDrive(driver)); } /***************/ @@ -73,10 +78,12 @@ private void configureDriverBindings() { private void configureOperatorBindings() { // odometry - driver.getDPadUp().onTrue(new OdometryRealign(Rotation2d.fromDegrees(180))); - driver.getDPadLeft().onTrue(new OdometryRealign(Rotation2d.fromDegrees(-90))); - driver.getDPadDown().onTrue(new OdometryRealign(Rotation2d.fromDegrees(0))); - driver.getDPadRight().onTrue(new OdometryRealign(Rotation2d.fromDegrees(90))); + //driver.getDPadUp().onTrue(new OdometryRealign(Rotation2d.fromDegrees(180))); + //driver.getDPadLeft().onTrue(new OdometryRealign(Rotation2d.fromDegrees(-90))); + //driver.getDPadDown().onTrue(new OdometryRealign(Rotation2d.fromDegrees(0))); + //driver.getDPadRight().onTrue(new OdometryRealign(Rotation2d.fromDegrees(90))); + + driver.getLeftButton().onTrue(new DrivetrainDriveForever(2)); } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/AngleAlign.java b/src/main/java/com/stuypulse/robot/commands/AngleAlign.java new file mode 100644 index 0000000..5f6afdc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/AngleAlign.java @@ -0,0 +1,87 @@ +package com.stuypulse.robot.commands; + +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.subsystems.drivetrain.AbstractDrivetrain; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.vision.AbstractVision; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.angles.AFuser; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.filters.IFilter; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; + +import edu.wpi.first.wpilibj2.command.Command; + +public class AngleAlign extends Command { + private final AbstractDrivetrain drivetrain; + private final AbstractOdometry odometry; + private final AbstractVision vision; + + private final AFuser angleError; + private IFilter speedAdjFilter; + + protected final AnglePIDController angleController; + + private final BStream finished; + + public AngleAlign() { + this.drivetrain = AbstractDrivetrain.getInstance(); + this.odometry = Odometry.getInstance(); + this.vision = AbstractVision.getInstance(); + + this.angleError = new AFuser(Alignment.FUSION_FILTER.get(), + () -> Angle.fromRotation2d(vision.getOutput().get(0).getPrimaryTag().getPose().toPose2d().getRotation()), + () -> Angle.fromRotation2d(odometry.getPose().getRotation()) + ); + + this.speedAdjFilter = new LowPassFilter(Alignment.SPEED_ADJ_FILTER.get()); + + this.angleController = new AnglePIDController(Alignment.Rotation.P, Alignment.Rotation.I, Alignment.Rotation.D); + + this.finished = BStream.create(this::isAligned) + //TODO: check to see if it is degrees or radians + .and(() -> angleController.isDoneDegrees(Alignment.ALIGNED_THRESHOLD_ANGLE.get())) + .filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME)); + + addRequirements(drivetrain, odometry, vision); + } + + public boolean isAligned() { + // check to see if the robot is within a threshold of the april tag target + return Math.abs(angleError.get().toDegrees()) < Alignment.ALIGNED_THRESHOLD_ANGLE.get(); + } + + private double getTurn() { + // //calcs with the setpoint angle of line between tag and robot to the current angle of the robot + // return angleController.update( + // /* angle of line between tag and robot*/ + // angleController.getSetpoint() + // , + // odometry.getRotation()); + return 0; + } + + @Override + public void initialize() { + odometry.resetOdometery(odometry.getPose()); + angleController.reset(); + angleError.reset(); + } + + @Override + public void execute() { + } + + @Override + public boolean isFinished() { + return finished.get(); + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/drivetrain/DriveDrive.java b/src/main/java/com/stuypulse/robot/commands/drivetrain/DriveDrive.java new file mode 100644 index 0000000..fce64bc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/drivetrain/DriveDrive.java @@ -0,0 +1,25 @@ +package com.stuypulse.robot.commands.drivetrain; + +import com.stuypulse.robot.subsystems.drivetrain.AbstractDrivetrain; +import com.stuypulse.stuylib.input.Gamepad; + +import edu.wpi.first.wpilibj2.command.Command; + +// DriveCommand.java +public class DriveDrive extends Command { + + private AbstractDrivetrain drivetrain; + private Gamepad gamepad; + + public DriveDrive(AbstractDrivetrain drivetrain, Gamepad gamepad) { + this.drivetrain = drivetrain; + this.gamepad = gamepad; + + addRequirements(drivetrain); + } + + public void execute() { + drivetrain.tankDriveVolts(gamepad.getLeftTrigger(), gamepad.getRightTrigger()); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java index 1d50ffb..b2d2483 100644 --- a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDrive.java @@ -1,6 +1,6 @@ package com.stuypulse.robot.commands.drivetrain; -import com.stuypulse.robot.subsystems.drivetrain.Drivetrain; +import com.stuypulse.robot.subsystems.drivetrain.AbstractDrivetrain; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.streams.numbers.IStream; @@ -8,8 +8,7 @@ import edu.wpi.first.wpilibj2.command.Command; public class DrivetrainDrive extends Command { - public final Drivetrain drivetrain; - private final Gamepad driver; + public final AbstractDrivetrain drivetrain; double rightSpeed; double leftSpeed; @@ -17,9 +16,8 @@ public class DrivetrainDrive extends Command { private final IStream speed; private final IStream angle; - public DrivetrainDrive(Drivetrain drivetrain, Gamepad driver) { - this.drivetrain = drivetrain; - this.driver = driver; + public DrivetrainDrive(Gamepad driver) { + this.drivetrain = AbstractDrivetrain.getInstance(); this.speed = IStream.create( () -> driver.getRightY() - driver.getLeftY()) @@ -41,9 +39,6 @@ public DrivetrainDrive(Drivetrain drivetrain, Gamepad driver) { @Override public void execute() { - if (driver.getRawLeftButton() || driver.getRawRightButton()) { - drivetrain.curvatureDrive(speed.get(), angle.get(), true); - } - else drivetrain.stop(); + drivetrain.arcadeDrive(speed.get(), angle.get()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDriveForever.java b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDriveForever.java index 945a6fc..65888f8 100644 --- a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDriveForever.java +++ b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainDriveForever.java @@ -5,23 +5,28 @@ package com.stuypulse.robot.commands.drivetrain; -import com.stuypulse.robot.subsystems.drivetrain.Drivetrain; +import com.stuypulse.robot.subsystems.drivetrain.AbstractDrivetrain; import edu.wpi.first.wpilibj2.command.Command; public class DrivetrainDriveForever extends Command { - private final Drivetrain drivetrain; + private final AbstractDrivetrain drivetrain; private final double speed; - public DrivetrainDriveForever(Drivetrain drivetrain, double speed) { - this.drivetrain = drivetrain; + public DrivetrainDriveForever(double speed) { + this.drivetrain = AbstractDrivetrain.getInstance(); this.speed = speed; addRequirements(drivetrain); } + @Override + public void initialize() { + drivetrain.tankDriveVolts(speed, speed); + } + @Override public void execute() { - drivetrain.curvatureDrive(speed, 0, false); + drivetrain.tankDriveVolts(speed, speed); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 977b270..9a5dfc4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -2,16 +2,12 @@ import com.stuypulse.stuylib.network.SmartNumber; -import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.util.Fiducial; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation.Alliance; public interface Field { double WIDTH = 16.54; diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index a881379..2c30034 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -8,7 +8,6 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; -import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkMax; import static com.revrobotics.CANSparkMax.IdleMode; diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index a93fc97..efa29a9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -14,14 +14,14 @@ public interface Gamepad { } public interface Drivetrain { - int LEFTREAR = 1; - int LEFTFRONT = 2; - int RIGHTREAR = 3; - int RIGHTFRONT = 4; + int LEFTREAR = 10; + int LEFTFRONT = 11; + int RIGHTFRONT = 12; + int RIGHTREAR = 13; } public interface Launcher { - int FEEDER = 5; - int LAUNCHER = 6; + int FEEDER = 20; + int LAUNCHER = 21; } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 3d91e1d..eea4f2b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -20,6 +20,18 @@ public interface Settings { public interface Drivetrain { //TODO: ask rain for true track width currently using cad with +-0.1 double TRACK_WIDTH = Units.inchesToMeters(23.83); + double WHEEL_RADIUS = Units.inchesToMeters(3.0); + double MASS_Kg = Units.lbsToKilograms(32.5462411); + double GEARING = 8.45; + double J_KG_METER_SQUARED = 7.19537; + + public interface Feedforward { + double kV = 1.6658; + double kA = 0.4515; + + double kVAngular = 6.34 / 5.0; + double kAAngular = 1.35 / 5.0; + } } public interface Launcher { @@ -32,4 +44,30 @@ public interface Launcher { double kLauncherDelay = 1; } + + public interface Alignment { + + SmartNumber SPEED_ADJ_FILTER = new SmartNumber("Alignment/Speed Adj RC", 0.1); + + SmartNumber FUSION_FILTER = new SmartNumber("Alignment/Fusion RC", 0.3); + SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15); + + //TODO: placeholder values for proper thresholds + SmartNumber ALIGNED_THRESHOLD_X = new SmartNumber("Alignment/X Threshold", 0.08); + SmartNumber ALIGNED_THRESHOLD_Y = new SmartNumber("Alignment/Y Threshold", 0.1); + SmartNumber ALIGNED_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Threshold", 8); + + public interface Translation { + SmartNumber P = new SmartNumber("Alignment/Translation/kP", 1); + SmartNumber I = new SmartNumber("Alignment/Translation/kI", 0); + SmartNumber D = new SmartNumber("Alignment/Translation/kD", 0.0); + } + + public interface Rotation { + SmartNumber P = new SmartNumber("Alignment/Rotation/kP", 1); + SmartNumber I = new SmartNumber("Alignment/Rotation/kI", 0); + SmartNumber D = new SmartNumber("Alignment/Rotation/kD", 0); + } + + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java new file mode 100644 index 0000000..7bc74f5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/AbstractDrivetrain.java @@ -0,0 +1,44 @@ +package com.stuypulse.robot.subsystems.drivetrain; + +import com.stuypulse.stuylib.math.Angle; + +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class AbstractDrivetrain extends SubsystemBase { + private static final AbstractDrivetrain instance; + + static { + if (RobotBase.isReal()) { + instance = new Drivetrain(); + } else { + instance = new DrivetrainSim(); + } + } + + public static AbstractDrivetrain getInstance() { + return instance; + } + + + public abstract double getDistance(); + + public abstract double getVelocity(); + + public abstract Angle getAngle(); + + public abstract void tankDriveVolts(double leftVolts, double rightVolts); + public abstract void arcadeDrive(double speed, double angle); + // public abstract void curvatureDrive(double speed, double angle, boolean isQuickTurn); + public abstract void stop(); + + public abstract void periodicChild(); + + @Override + public void periodic() { + periodicChild(); + } + + + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java index 307504c..59f6616 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/Drivetrain.java @@ -4,23 +4,13 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.math.Angle; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Drivetrain extends SubsystemBase { - - private static final Drivetrain instance; - - static { - instance = new Drivetrain(); - } - - public static Drivetrain getInstance() { - return instance; - } +public class Drivetrain extends AbstractDrivetrain { private final DifferentialDrive drivetrain; private final CANSparkMax leftFront; @@ -29,10 +19,11 @@ public static Drivetrain getInstance() { private final CANSparkMax rightBack; public Drivetrain() { - leftFront = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushed); - leftBack = new CANSparkMax(Ports.Drivetrain.LEFTREAR, MotorType.kBrushed); - rightFront = new CANSparkMax(Ports.Drivetrain.RIGHTFRONT, MotorType.kBrushed); - rightBack = new CANSparkMax(Ports.Drivetrain.RIGHTREAR, MotorType.kBrushed); + //TODO: changed to brushless for simulation + leftFront = new CANSparkMax(Ports.Drivetrain.LEFTFRONT, MotorType.kBrushless); + leftBack = new CANSparkMax(Ports.Drivetrain.LEFTREAR, MotorType.kBrushless); + rightFront = new CANSparkMax(Ports.Drivetrain.RIGHTFRONT, MotorType.kBrushless); + rightBack = new CANSparkMax(Ports.Drivetrain.RIGHTREAR, MotorType.kBrushless); Motors.Drivetrain.LEFT.configure(leftFront); Motors.Drivetrain.LEFT.configure(leftBack); @@ -55,6 +46,10 @@ public double getRightSpeed() { return rightFront.getEncoder().getVelocity(); } + public double getVelocity() { + return (getLeftSpeed() + getRightSpeed()) / 2.0; + } + public double getLeftVoltage() { return leftFront.getAppliedOutput(); } @@ -71,7 +66,21 @@ public double getRightDistance() { return rightFront.getEncoder().getPosition(); } + public double getDistance() { + return (getLeftDistance() + getRightDistance()) / 2.0; + } + + public Angle getAngle() { + return Angle.fromDegrees(Math.toDegrees(getLeftDistance() - getRightDistance() / Settings.Drivetrain.TRACK_WIDTH)); + } + //********** Drive Methods **********// + public void tankDriveVolts(double leftVolts, double rightVolts) { + leftFront.setVoltage(leftVolts); + rightFront.setVoltage(-rightVolts); + drivetrain.feed(); + } + public void tankDrive(double leftSpeed, double rightSpeed) { drivetrain.tankDrive(leftSpeed, rightSpeed); } @@ -89,7 +98,7 @@ public void stop() { } @Override - public void periodic() { + public void periodicChild() { SmartDashboard.putNumber("Drivetrain/Left Speed", getLeftSpeed()); SmartDashboard.putNumber("Drivetrain/Right Speed", getRightSpeed()); SmartDashboard.putNumber("Drivetrain/Left Voltage", getLeftVoltage()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/drivetrain/DrivetrainSim.java b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/DrivetrainSim.java new file mode 100644 index 0000000..25a5118 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/drivetrain/DrivetrainSim.java @@ -0,0 +1,106 @@ +package com.stuypulse.robot.subsystems.drivetrain; +import com.stuypulse.robot.constants.Settings.Drivetrain; +import com.stuypulse.robot.constants.Settings.Drivetrain.*; +import com.stuypulse.stuylib.math.Angle; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class DrivetrainSim extends AbstractDrivetrain{ + private final DifferentialDrivetrainSim sim; + private final Field2d field; + + public DrivetrainSim() { + // this.sim = new DifferentialDrivetrainSim( + // DCMotor.getNEO(2), + // Drivetrain.GEARING, + // Drivetrain.J_KG_METER_SQUARED, + // Drivetrain.MASS_Kg, + // Drivetrain.WHEEL_RADIUS, + // Drivetrain.TRACK_WIDTH, + // VecBuilder.fill(0, 0, 0, 0, 0, 0, 0) + // ); + sim = new DifferentialDrivetrainSim( + LinearSystemId.identifyDrivetrainSystem( + Feedforward.kV, // linear velocity gain + Feedforward.kA, // linear acceleration gain + Feedforward.kVAngular, // angular velocity gain + Feedforward.kAAngular, // angular acceleration gain + Drivetrain.TRACK_WIDTH // track width of the drivetrain + ), + DCMotor.getNEO(6), + Drivetrain.GEARING, + Drivetrain.TRACK_WIDTH, + Drivetrain.WHEEL_RADIUS, + + // give the drivetrain measurement noise (none in this example) + VecBuilder.fill(0, 0, 0, 0, 0, 0, 0) + ); + + this.field = new Field2d(); + SmartDashboard.putData("Sim Field", field); + } + + public double getLeftDistance() { + return sim.getLeftPositionMeters(); + } + + public double getRightDistance() { + return sim.getRightPositionMeters(); + } + + public double getDistance() { + return (getLeftDistance() + getRightDistance()) / 2.0; + } + + public double getLeftVelocity() { + return sim.getLeftVelocityMetersPerSecond(); + } + + public double getRightVelocity() { + return sim.getRightVelocityMetersPerSecond(); + } + + public double getVelocity() { + return (getLeftVelocity() + getRightVelocity()) / 2.0; + } + + public Angle getAngle() { + return Angle.fromDegrees(sim.getHeading().getDegrees()); + } + + public void tankDriveVolts(double leftVolts, double rightVolts) { + sim.setInputs(leftVolts * RoboRioSim.getVInVoltage(), rightVolts * RoboRioSim.getVInVoltage()); + } + + public void arcadeDrive(double speed, double angle) { + double leftVolts = speed + angle; + double rightVolts = speed - angle; + tankDriveVolts(leftVolts, rightVolts); + } + + public void curvatureDrive(double speed, double angle, boolean isQuickTurn) { + //XXX: using arcade drive for now + arcadeDrive(speed, angle); + } + + public void stop() { + tankDriveVolts(0, 0); + } + + public void periodicChild() { + sim.update(0.02); + field.setRobotPose(sim.getPose()); + + SmartDashboard.putNumber("SimDrivetrain/Left Distance", getLeftDistance()); + SmartDashboard.putNumber("SimDrivetrain/Right Distance", getRightDistance()); + SmartDashboard.putNumber("SimDrivetrain/Distance", getDistance()); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java b/src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java index 21d6508..2735289 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/com/stuypulse/robot/subsystems/launcher/Launcher.java @@ -25,11 +25,13 @@ public static Launcher getInstance() { private final CANSparkMax launcher; public Launcher() { - feeder = new CANSparkMax(Ports.Launcher.FEEDER, MotorType.kBrushed); - launcher = new CANSparkMax(Ports.Launcher.LAUNCHER, MotorType.kBrushed); + //TODO: changed to brushless for simulation + feeder = new CANSparkMax(Ports.Launcher.FEEDER, MotorType.kBrushless); + launcher = new CANSparkMax(Ports.Launcher.LAUNCHER, MotorType.kBrushless); - Motors.Launcher.LAUNCHER.configure(launcher); Motors.Launcher.FEEDER.configure(feeder); + Motors.Launcher.LAUNCHER.configure(launcher); + } //********** SETTERS ********** diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/AbstractOdometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/AbstractOdometry.java index 9697532..3f0c154 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/AbstractOdometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/AbstractOdometry.java @@ -32,7 +32,5 @@ public final Translation2d getTranslation() { return getPose().getTranslation(); } - public final Rotation2d getRotation() { - return getPose().getRotation(); - } + public abstract Rotation2d getRotation(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java index 14448cc..29924e5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -6,6 +6,7 @@ import com.stuypulse.robot.util.VisionData; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -20,7 +21,7 @@ public class Odometry extends AbstractOdometry { private final FieldObject2d odometryPose2D; public Odometry() { - this.drivetrain = Drivetrain.getInstance(); + this.drivetrain = new Drivetrain(); this.odometry = new DifferentialDriveOdometry(getRotation(), drivetrain.getLeftDistance(), drivetrain.getRightDistance()); resetOdometery(new Pose2d()); @@ -29,6 +30,15 @@ public Odometry() { SmartDashboard.putData("Field", field); } + public final Rotation2d getRotation() { + if (this.odometry == null) { + return new Rotation2d(); + } + else { + return this.odometry.getPoseMeters().getRotation(); + } + } + @Override public Field2d getField() { return field; @@ -42,12 +52,12 @@ public Pose2d getPose() { @Override public void updateOdometry() { - odometry.update(getRotation(), drivetrain.getLeftDistance(), drivetrain.getRightDistance()); + //odometry.update(getRotation(), drivetrain.getLeftDistance(), drivetrain.getRightDistance()); } @Override public void resetOdometery(Pose2d pose2d) { - odometry.resetPosition(getRotation(), drivetrain.getLeftDistance(), drivetrain.getRightDistance(), pose2d); + //odometry.resetPosition(getRotation(), drivetrain.getLeftDistance(), drivetrain.getRightDistance(), pose2d); } @Override