From c27a3ab78c7e8f63f6332672d2e5c672dc33c1ba Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Thu, 2 Jun 2022 20:37:11 -0700 Subject: [PATCH 01/18] Inputted Parameters for Hood Arm Sim, initial iteration for implementing motor characteristics into motor controllers --- .../common/hardware/motor/NAR_Motor.java | 33 +++++++++++++++++++ .../motorcontroller/NAR_CANSparkMax.java | 14 ++++++-- .../hardware/motorcontroller/NAR_TalonFX.java | 11 +++++-- .../motorcontroller/NAR_TalonSRX.java | 4 ++- .../motorcontroller/NAR_VictorSPX.java | 5 ++- .../java/frc/team3128/subsystems/Hood.java | 30 +++++++++++++++++ 6 files changed, 91 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/team3128/common/hardware/motor/NAR_Motor.java diff --git a/src/main/java/frc/team3128/common/hardware/motor/NAR_Motor.java b/src/main/java/frc/team3128/common/hardware/motor/NAR_Motor.java new file mode 100644 index 00000000..b79d674e --- /dev/null +++ b/src/main/java/frc/team3128/common/hardware/motor/NAR_Motor.java @@ -0,0 +1,33 @@ +package frc.team3128.common.hardware.motor; + +public enum NAR_Motor { + PRO_775(18730, 0.7, 0.71, 134); + + protected double freeSpeedRPM; + protected double freeCurrentAmps; + protected double stallCurrentAmps; + protected double stallTorqueNM; + + NAR_Motor(double freeSpeedRPM, double freeCurrentAmps, double stallCurrentAmps, double stallTorqueNM) { + this.freeSpeedRPM = freeSpeedRPM; + this.freeCurrentAmps = freeCurrentAmps; + this.stallCurrentAmps = stallCurrentAmps; + this.stallTorqueNM = stallTorqueNM; + } + + public double getFreeSpeedRPM() { + return freeSpeedRPM; + } + + public double getFreeCurrentAmps() { + return freeCurrentAmps; + } + + public double getStallCurrentAmps() { + return stallCurrentAmps; + } + + public double getStallTorqueNM() { + return stallTorqueNM; + } +} diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java index fd877a17..ae2024ca 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java @@ -6,7 +6,8 @@ import edu.wpi.first.hal.SimDouble; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SimDeviceSim; - +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.team3128.common.hardware.motor.NAR_Motor; import frc.team3128.common.infrastructure.NAR_EMotor; import net.thefletcher.revrobotics.CANSparkMax; import net.thefletcher.revrobotics.SparkMaxRelativeEncoder; @@ -18,14 +19,18 @@ public class NAR_CANSparkMax extends CANSparkMax implements NAR_EMotor { private SparkMaxRelativeEncoder encoder; private SimDeviceSim encoderSim; private SimDouble encoderSimVel; + private NAR_Motor motor; + + SingleJointedArmSim /** * * @param deviceNumber device id * @param type kBrushed(0) for brushed motor, kBrushless(1) for brushless motor */ - public NAR_CANSparkMax(int deviceNumber, MotorType type) { + public NAR_CANSparkMax(int deviceNumber, MotorType type, NAR_Motor motor) { super(deviceNumber, type); + this.motor = motor; restoreFactoryDefaults(); // Reset config parameters, unfollow other motor controllers @@ -39,6 +44,11 @@ public NAR_CANSparkMax(int deviceNumber, MotorType type) { } } + public NAR_CANSparkMax(int deviceNumber, MotorType type) { + this(deviceNumber, type, null); + } + + @Override public void set(double outputValue) { if (outputValue != prevValue) { diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java index 8a2f3814..45b7dc76 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java @@ -6,6 +6,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.RobotBase; +import frc.team3128.common.hardware.motor.NAR_Motor; import frc.team3128.common.infrastructure.NAR_EMotor; public class NAR_TalonFX extends WPI_TalonFX implements NAR_EMotor { @@ -13,12 +14,14 @@ public class NAR_TalonFX extends WPI_TalonFX implements NAR_EMotor { private double prevValue = 0; private ControlMode prevControlMode = ControlMode.Disabled; private TalonFXSimCollection motorSim; + private NAR_Motor motor; /** * @param deviceNumber device id */ - public NAR_TalonFX(int deviceNumber) { + public NAR_TalonFX(int deviceNumber, NAR_Motor motor) { super(deviceNumber); + this.motor = motor; if(RobotBase.isSimulation()){ motorSim = getTalonFXSimCollection(); @@ -28,6 +31,10 @@ public NAR_TalonFX(int deviceNumber) { enableVoltageCompensation(true); } + public NAR_TalonFX(int deviceNumber) { + + } + public void set(ControlMode controlMode, double outputValue) { if (outputValue != prevValue || controlMode != prevControlMode) { super.set(controlMode, outputValue); @@ -72,6 +79,6 @@ public void follow(NAR_EMotor motor) { if(!(motor instanceof IMotorController)) { throw new RuntimeException("Bad follow: NAR_TalonFX " + getDeviceID() + " attempted to follow non-CTRE motor controller."); } - super.follow((IMotorController)motor); + super.follow((IMotorController) motor); } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java index 46f9753a..42c86234 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java @@ -6,6 +6,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.RobotBase; +import frc.team3128.common.hardware.motor.NAR_Motor; import frc.team3128.common.infrastructure.NAR_EMotor; public class NAR_TalonSRX extends WPI_TalonSRX implements NAR_EMotor { @@ -13,11 +14,12 @@ public class NAR_TalonSRX extends WPI_TalonSRX implements NAR_EMotor { private double prevValue = 0; private ControlMode prevControlMode = ControlMode.Disabled; private TalonSRXSimCollection motorSim; + private NAR_Motor motor; /** * @param deviceNumber device id */ - public NAR_TalonSRX(int deviceNumber) { + public NAR_TalonSRX(int deviceNumber, NAR_Motor motor) { super(deviceNumber); if(RobotBase.isSimulation()) diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_VictorSPX.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_VictorSPX.java index 01706fd0..bb4af1d4 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_VictorSPX.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_VictorSPX.java @@ -5,17 +5,20 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; +import frc.team3128.common.hardware.motor.NAR_Motor; + // To be simmed in the next implementation public class NAR_VictorSPX extends WPI_VictorSPX { private double prevValue = 0; private ControlMode prevControlMode = ControlMode.Disabled; + private NAR_Motor motor; /** * @param deviceNumber device id */ - public NAR_VictorSPX(int deviceNumber){ + public NAR_VictorSPX(int deviceNumber, NAR_Motor motor){ super(deviceNumber); } diff --git a/src/main/java/frc/team3128/subsystems/Hood.java b/src/main/java/frc/team3128/subsystems/Hood.java index 111118bf..c282ed17 100644 --- a/src/main/java/frc/team3128/subsystems/Hood.java +++ b/src/main/java/frc/team3128/subsystems/Hood.java @@ -1,11 +1,16 @@ package frc.team3128.subsystems; +import frc.team3128.Constants; import frc.team3128.ConstantsInt; +import frc.team3128.Robot; + import static frc.team3128.Constants.HoodConstants.*; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax; import frc.team3128.common.hardware.motorcontroller.NAR_TalonFX; @@ -28,6 +33,8 @@ public class Hood extends NAR_PIDSubsystem { private double time; private double prevTime; + private SingleJointedArmSim singleJointedArmSim; + public static synchronized Hood getInstance() { if(instance == null) { instance = new Hood(); @@ -40,6 +47,17 @@ public Hood() { configMotors(); configEncoder(); + + if(Robot.isSimulation()) { + singleJointedArmSim = new SingleJointedArmSim( + DCMotor.getNeo550(1), + HOOD_SHOOTER_GEAR_RATIO, + 0, //TODO Find this (COMMUNISM OVER CAPITALISM) + 0.2400046, + Units.degreesToRadians(3), + Units.degreesToRadians(32), + 0.795601019, true); + } } private void configMotors() { @@ -120,5 +138,17 @@ public double calculateAngleFromDistance(double dist) { return MathUtil.clamp(hoodAngleMap.getInterpolated(new InterpolatingDouble(dist)).value, MIN_ANGLE, MAX_ANGLE); } + + @Override + public void simulationPeriodic() { + singleJointedArmSim.setInputVoltage( + m_hoodMotor.getMotorOutputVoltage() + ); + + singleJointedArmSim.update(0.02); + + m_encoder.setPosition(singleJointedArmSim.getAngleRads()/(2*Math.PI)); + m_hoodMotor.setSimVelocity(singleJointedArmSim.getVelocityRadPerSec() / Constants.ConversionConstants.SPARK_ENCODER_RESOLUTION); + } } From d254f35541a2a13743c150ddab94b75ee059604f Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Thu, 2 Jun 2022 20:37:38 -0700 Subject: [PATCH 02/18] motor characteristics for VictorSPX I forgot to hit save on the file :( --- .../common/hardware/motorcontroller/NAR_VictorSPX.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_VictorSPX.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_VictorSPX.java index bb4af1d4..246e2720 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_VictorSPX.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_VictorSPX.java @@ -20,8 +20,13 @@ public class NAR_VictorSPX extends WPI_VictorSPX { */ public NAR_VictorSPX(int deviceNumber, NAR_Motor motor){ super(deviceNumber); + this.motor = motor; } + public NAR_VictorSPX(int deviceNumber) { + this(deviceNumber, null); + } + @Override public void set(ControlMode controlMode, double outputValue) { if (outputValue != prevValue || controlMode != prevControlMode) { From 4cf70fac04bdb2549a48729ad483f738c97c5025 Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Thu, 2 Jun 2022 20:42:35 -0700 Subject: [PATCH 03/18] motor characteristics for NAR_TalonFX and NAR_TalonSRX I forgot to hit save again --- .../common/hardware/motorcontroller/NAR_TalonFX.java | 3 ++- .../common/hardware/motorcontroller/NAR_TalonSRX.java | 5 +++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java index 45b7dc76..769028b6 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java @@ -32,7 +32,8 @@ public NAR_TalonFX(int deviceNumber, NAR_Motor motor) { } public NAR_TalonFX(int deviceNumber) { - + this(deviceNumber, null); + this.motor = motor; } public void set(ControlMode controlMode, double outputValue) { diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java index 42c86234..7a084321 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java @@ -21,6 +21,7 @@ public class NAR_TalonSRX extends WPI_TalonSRX implements NAR_EMotor { */ public NAR_TalonSRX(int deviceNumber, NAR_Motor motor) { super(deviceNumber); + this.motor = motor; if(RobotBase.isSimulation()) motorSim = getTalonSRXSimCollection(); @@ -29,6 +30,10 @@ public NAR_TalonSRX(int deviceNumber, NAR_Motor motor) { configVoltageCompSaturation(12, 10); } + public NAR_TalonSRX(int deviceNumber) { + this(deviceNumber, null); + } + @Override public void set(ControlMode controlMode, double outputValue) { if (outputValue != prevValue || controlMode != prevControlMode) { From da738867e19eb7502cd8a7a2c4dd4fe317d92764 Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Thu, 9 Jun 2022 22:04:02 -0700 Subject: [PATCH 04/18] Bug Fixes to Hood Simulation Also added Moment of Inertia for Hood (based off of CAD) --- .../common/hardware/motorcontroller/NAR_CANSparkMax.java | 2 -- src/main/java/frc/team3128/subsystems/Hood.java | 9 +++++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java index ae2024ca..514498ce 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java @@ -21,8 +21,6 @@ public class NAR_CANSparkMax extends CANSparkMax implements NAR_EMotor { private SimDouble encoderSimVel; private NAR_Motor motor; - SingleJointedArmSim - /** * * @param deviceNumber device id diff --git a/src/main/java/frc/team3128/subsystems/Hood.java b/src/main/java/frc/team3128/subsystems/Hood.java index c282ed17..751f2d2c 100644 --- a/src/main/java/frc/team3128/subsystems/Hood.java +++ b/src/main/java/frc/team3128/subsystems/Hood.java @@ -52,7 +52,7 @@ public Hood() { singleJointedArmSim = new SingleJointedArmSim( DCMotor.getNeo550(1), HOOD_SHOOTER_GEAR_RATIO, - 0, //TODO Find this (COMMUNISM OVER CAPITALISM) + 0.054195108, //TODO Find this (COMMUNISM OVER CAPITALISM) 0.2400046, Units.degreesToRadians(3), Units.degreesToRadians(32), @@ -147,8 +147,13 @@ public void simulationPeriodic() { singleJointedArmSim.update(0.02); - m_encoder.setPosition(singleJointedArmSim.getAngleRads()/(2*Math.PI)); + double angle = singleJointedArmSim.getAngleRads()/(Math.PI) * 360; + + m_encoder.setPosition(angle); m_hoodMotor.setSimVelocity(singleJointedArmSim.getVelocityRadPerSec() / Constants.ConversionConstants.SPARK_ENCODER_RESOLUTION); + m_hoodMotor.setSimPosition(angle); + + //SmartDashboard.putNumber("Hood Position", m_encoder.getPosition()); } } From f64fbb601a3849062a77a869a2ca861cf3ed2e38 Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Wed, 15 Jun 2022 21:32:34 -0700 Subject: [PATCH 05/18] Hood Fixes 6/14 Updated CmdShootDist to pull limelight distance from a sim-specific constant during simulation, updated Hood constants, fixed angle conversions in Hood's simulationPeriodic --- simgui-window.json | 37 +++++++++++-------- simgui.json | 5 +++ src/main/java/frc/team3128/Constants.java | 9 +++-- .../frc/team3128/commands/CmdShootDist.java | 6 ++- .../java/frc/team3128/subsystems/Hood.java | 15 +++++--- 5 files changed, 46 insertions(+), 26 deletions(-) diff --git a/simgui-window.json b/simgui-window.json index 078db8f4..35160eae 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -1,11 +1,11 @@ { "MainWindow": { "GLOBAL": { - "height": "991", + "height": "1779", "maximized": "1", "style": "0", "userScale": "2", - "width": "1920", + "width": "2880", "xpos": "0", "ypos": "34" } @@ -16,55 +16,60 @@ "Pos": "532,45", "Size": "941,571" }, + "###/SmartDashboard/Hood": { + "Collapsed": "0", + "Pos": "179,255", + "Size": "270,134" + }, "###/SmartDashboard/Hopper": { "Collapsed": "0", "Pos": "255,86", - "Size": "178,98" + "Size": "270,134" }, "###/SmartDashboard/Shooter": { "Collapsed": "0", "Pos": "253,195", - "Size": "178,98" + "Size": "270,134" }, "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "336,164" + "Size": "495,218" }, "###Joysticks": { "Collapsed": "0", "Pos": "119,729", - "Size": "976,179" + "Size": "1516,249" }, "###Keyboard 0 Settings": { "Collapsed": "0", - "Pos": "1533,86", + "Pos": "2214,128", "Size": "300,560" }, "###Keyboard 1 Settings": { "Collapsed": "0", - "Pos": "1210,90", + "Pos": "2042,639", "Size": "300,560" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "1140,696", - "Size": "750,185" + "Pos": "1050,958", + "Size": "1385,804" }, "###Other Devices": { "Collapsed": "0", - "Pos": "1497,31", + "Pos": "1818,24", "Size": "250,695" }, "###System Joysticks": { "Collapsed": "0", - "Pos": "93,294", - "Size": "232,254" + "Pos": "329,1145", + "Size": "354,362" }, "###Timing": { "Collapsed": "0", - "Pos": "5,150", - "Size": "162,142" + "Pos": "-7,239", + "Size": "241,187" }, "Debug##Default": { "Collapsed": "0", @@ -74,7 +79,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "109,114" + "Size": "161,159" } } } diff --git a/simgui.json b/simgui.json index b4a1e0fd..c2883862 100644 --- a/simgui.json +++ b/simgui.json @@ -144,6 +144,11 @@ "visible": true } }, + "/SmartDashboard/Hood": { + "window": { + "visible": true + } + }, "/SmartDashboard/Hopper": { "window": { "visible": true diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index df20bea8..a1de3634 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -233,9 +233,9 @@ public static class HoodConstants { public static final int HOOD_CURRENT_LIMIT = 10; // Amps - public static final double MIN_ANGLE = 3.0; // deg - public static final double MAX_ANGLE = 34.0; // deg - public static final double HOME_ANGLE = 22.0; // deg + public static final double MIN_ANGLE = 9.4; // deg + public static final double MAX_ANGLE = 40.4; // deg + public static final double HOME_ANGLE = 28.4; // deg public static InterpolatingTreeMap hoodAngleMap = new InterpolatingTreeMap(); static { @@ -314,4 +314,7 @@ public static class VisionConstants { public static final double BALL_VEL_THRESHOLD = 2.54; // m/s - 100 in/s public static final int BALL_VEL_PLATEAU_THRESHOLD = 10; } + public static class SimConstants { + public static final double SIM_HUB_DISTANCE = 70; //degrees + } } diff --git a/src/main/java/frc/team3128/commands/CmdShootDist.java b/src/main/java/frc/team3128/commands/CmdShootDist.java index 2fb90299..5c431f49 100644 --- a/src/main/java/frc/team3128/commands/CmdShootDist.java +++ b/src/main/java/frc/team3128/commands/CmdShootDist.java @@ -1,12 +1,15 @@ package frc.team3128.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.team3128.Robot; import frc.team3128.Constants.VisionConstants; import frc.team3128.common.hardware.limelight.Limelight; import frc.team3128.common.utility.Log; import frc.team3128.subsystems.Hood; import frc.team3128.subsystems.LimelightSubsystem; import frc.team3128.subsystems.Shooter; +import static frc.team3128.Constants.SimConstants.*; public class CmdShootDist extends CommandBase { private Shooter shooter; @@ -28,8 +31,9 @@ public void initialize() { @Override public void execute() { - double dist = limelights.calculateDistance("shooter"); + double dist = (Robot.isSimulation()) ? SIM_HUB_DISTANCE : limelights.calculateDistance("shooter"); shooter.beginShoot(shooter.calculateMotorVelocityFromDist(dist)); + hood.startPID(hood.calculateAngleFromDistance(dist)); } diff --git a/src/main/java/frc/team3128/subsystems/Hood.java b/src/main/java/frc/team3128/subsystems/Hood.java index 751f2d2c..6235cf58 100644 --- a/src/main/java/frc/team3128/subsystems/Hood.java +++ b/src/main/java/frc/team3128/subsystems/Hood.java @@ -49,14 +49,17 @@ public Hood() { configEncoder(); if(Robot.isSimulation()) { + m_hoodMotor.setSimPosition(0); + m_encoder.setPosition(0); singleJointedArmSim = new SingleJointedArmSim( DCMotor.getNeo550(1), HOOD_SHOOTER_GEAR_RATIO, 0.054195108, //TODO Find this (COMMUNISM OVER CAPITALISM) 0.2400046, - Units.degreesToRadians(3), - Units.degreesToRadians(32), - 0.795601019, true); + Units.degreesToRadians(MIN_ANGLE), + Units.degreesToRadians(MAX_ANGLE), + 0.795601019, + true); } } @@ -147,11 +150,11 @@ public void simulationPeriodic() { singleJointedArmSim.update(0.02); - double angle = singleJointedArmSim.getAngleRads()/(Math.PI) * 360; + double angle = singleJointedArmSim.getAngleRads()/(2*Math.PI) * 360; - m_encoder.setPosition(angle); + m_encoder.setPosition((angle-MIN_ANGLE) / (2 * Math.PI)); m_hoodMotor.setSimVelocity(singleJointedArmSim.getVelocityRadPerSec() / Constants.ConversionConstants.SPARK_ENCODER_RESOLUTION); - m_hoodMotor.setSimPosition(angle); + m_hoodMotor.setSimPosition(angle - MIN_ANGLE); //SmartDashboard.putNumber("Hood Position", m_encoder.getPosition()); } From 8dbb0dd4dbfc4e233234e621715d77e72d422eb6 Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Fri, 17 Jun 2022 16:35:21 -0700 Subject: [PATCH 06/18] Added Sim GUI Window config to gitignore --- .gitignore | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.gitignore b/.gitignore index 9559b1f6..e1c6c7d3 100644 --- a/.gitignore +++ b/.gitignore @@ -163,3 +163,8 @@ bin/ imgui.ini # End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode + +# Ignore Display Settings on WPILib Simulation GUI +simgui-ds.json +simgui-window.json +simgui.json From cf9a12c64f998ea8fe41bc5ca819e29b6376d077 Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Fri, 17 Jun 2022 16:50:16 -0700 Subject: [PATCH 07/18] Removing simgui jsons from tracked files --- simgui-ds.json | 110 ----------------------------- simgui-window.json | 85 ----------------------- simgui.json | 169 --------------------------------------------- 3 files changed, 364 deletions(-) delete mode 100644 simgui-ds.json delete mode 100644 simgui-window.json delete mode 100644 simgui.json diff --git a/simgui-ds.json b/simgui-ds.json deleted file mode 100644 index a535de67..00000000 --- a/simgui-ds.json +++ /dev/null @@ -1,110 +0,0 @@ -{ - "Keyboard 0 Settings": { - "window": { - "visible": true - } - }, - "Keyboard 1 Settings": { - "window": { - "visible": true - } - }, - "keyboardJoysticks": [ - { - "axisConfig": [ - {}, - { - "decKey": 87, - "decayRate": 1.0, - "incKey": 83, - "keyRate": 1.0 - }, - { - "decKey": 68, - "decayRate": 1.0, - "incKey": 65, - "keyRate": 1.0 - } - ], - "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": 1 - }, - { - "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": "Keyboard1" - }, - { - "guid": "Keyboard0" - } - ], - "useEnableDisableHotkeys": true -} diff --git a/simgui-window.json b/simgui-window.json deleted file mode 100644 index 35160eae..00000000 --- a/simgui-window.json +++ /dev/null @@ -1,85 +0,0 @@ -{ - "MainWindow": { - "GLOBAL": { - "height": "1779", - "maximized": "1", - "style": "0", - "userScale": "2", - "width": "2880", - "xpos": "0", - "ypos": "34" - } - }, - "Window": { - "###/SmartDashboard/Field": { - "Collapsed": "0", - "Pos": "532,45", - "Size": "941,571" - }, - "###/SmartDashboard/Hood": { - "Collapsed": "0", - "Pos": "179,255", - "Size": "270,134" - }, - "###/SmartDashboard/Hopper": { - "Collapsed": "0", - "Pos": "255,86", - "Size": "270,134" - }, - "###/SmartDashboard/Shooter": { - "Collapsed": "0", - "Pos": "253,195", - "Size": "270,134" - }, - "###FMS": { - "Collapsed": "0", - "Pos": "5,540", - "Size": "495,218" - }, - "###Joysticks": { - "Collapsed": "0", - "Pos": "119,729", - "Size": "1516,249" - }, - "###Keyboard 0 Settings": { - "Collapsed": "0", - "Pos": "2214,128", - "Size": "300,560" - }, - "###Keyboard 1 Settings": { - "Collapsed": "0", - "Pos": "2042,639", - "Size": "300,560" - }, - "###NetworkTables": { - "Collapsed": "0", - "Pos": "1050,958", - "Size": "1385,804" - }, - "###Other Devices": { - "Collapsed": "0", - "Pos": "1818,24", - "Size": "250,695" - }, - "###System Joysticks": { - "Collapsed": "0", - "Pos": "329,1145", - "Size": "354,362" - }, - "###Timing": { - "Collapsed": "0", - "Pos": "-7,239", - "Size": "241,187" - }, - "Debug##Default": { - "Collapsed": "0", - "Pos": "60,60", - "Size": "400,400" - }, - "Robot State": { - "Collapsed": "0", - "Pos": "5,20", - "Size": "161,159" - } - } -} diff --git a/simgui.json b/simgui.json deleted file mode 100644 index c2883862..00000000 --- a/simgui.json +++ /dev/null @@ -1,169 +0,0 @@ -{ - "HALProvider": { - "Other Devices": { - "Talon FX[0]": { - "header": { - "open": true - } - }, - "Talon FX[4]": { - "header": { - "open": true - } - }, - "Talon FX[4]/Fwd Limit": { - "header": { - "open": true - } - }, - "Talon FX[4]/Integrated Sensor": { - "header": { - "open": true - } - }, - "Talon FX[4]/Rev Limit": { - "header": { - "open": true - } - }, - "Talon SRX[3128]": { - "header": { - "open": true - } - } - } - }, - "NTProvider": { - "types": { - "/FMSInfo": "FMSInfo", - "/LiveWindow/Climber": "Subsystem", - "/LiveWindow/Hood/PID Controller": "PIDController", - "/LiveWindow/Hopper": "Subsystem", - "/LiveWindow/Intake": "Subsystem", - "/LiveWindow/LimelightSubsystem": "Subsystem", - "/LiveWindow/NAR_Drivetrain": "Subsystem", - "/LiveWindow/Shooter/PID Controller": "PIDController", - "/LiveWindow/Ungrouped/DifferentialDrive[1]": "DifferentialDrive", - "/LiveWindow/Ungrouped/PIDController[10]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[11]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[12]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[13]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[14]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[15]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[16]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[17]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[18]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[19]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[20]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[21]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[22]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[23]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[24]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[25]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[26]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[27]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[28]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[29]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[30]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[31]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[32]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[33]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[34]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[35]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[36]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[37]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[38]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[39]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[40]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[41]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[42]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[43]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[44]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[45]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[46]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[47]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[48]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[49]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[4]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[50]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[51]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[52]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[53]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[54]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[55]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[56]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[57]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[58]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[59]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[5]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[60]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[61]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[62]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[63]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[64]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[65]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[66]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[67]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[68]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[69]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[6]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[70]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[71]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[72]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[73]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[74]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[75]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[7]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[8]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[9]": "PIDController", - "/LiveWindow/Ungrouped/Scheduler": "Scheduler", - "/LiveWindow/Ungrouped/Talon FX [12]": "Motor Controller", - "/LiveWindow/Ungrouped/Talon FX [13]": "Motor Controller", - "/LiveWindow/Ungrouped/Talon FX [4]": "Motor Controller", - "/LiveWindow/Ungrouped/Talon FX [5]": "Motor Controller", - "/LiveWindow/Ungrouped/Talon SRX [6]": "Motor Controller", - "/LiveWindow/Ungrouped/Talon SRX [7]": "Motor Controller", - "/LiveWindow/Ungrouped/Talon SRX [9]": "Motor Controller", - "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro", - "/SmartDashboard/Climber": "Subsystem", - "/SmartDashboard/CommandScheduler": "Scheduler", - "/SmartDashboard/Drivetrain": "Subsystem", - "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/Hood": "Subsystem", - "/SmartDashboard/Hopper": "Subsystem", - "/SmartDashboard/Intake": "Subsystem", - "/SmartDashboard/NAR_Drivetrain": "Subsystem", - "/SmartDashboard/Scheduler": "Scheduler", - "/SmartDashboard/Shooter": "Subsystem" - }, - "windows": { - "/SmartDashboard/Field": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Hood": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Hopper": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Shooter": { - "window": { - "visible": true - } - } - } - }, - "NetworkTables": { - "SmartDashboard": { - "open": true - } - } -} From 1318a661f2ef2a38183951d8d588911f7ab8698d Mon Sep 17 00:00:00 2001 From: Arav Chadha Date: Sat, 18 Jun 2022 12:58:54 -0700 Subject: [PATCH 08/18] Minor fixes Merged AutoPrograms from dev and made other minor changes. --- .../frc/team3128/autonomous/AutoPrograms.java | 255 +++++++++++++----- .../frc/team3128/autonomous/Trajectories.java | 87 ++---- .../frc/team3128/commands/CmdShootDist.java | 8 +- .../common/hardware/motor/NAR_Motor.java | 2 +- .../motorcontroller/NAR_CANSparkMax.java | 8 + .../hardware/motorcontroller/NAR_TalonFX.java | 1 + .../motorcontroller/NAR_TalonSRX.java | 1 + .../java/frc/team3128/subsystems/Hood.java | 5 +- 8 files changed, 219 insertions(+), 148 deletions(-) diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index 52612f1a..c16e3964 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -1,12 +1,21 @@ package frc.team3128.autonomous; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.RamseteController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryUtil; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import static frc.team3128.Constants.DriveConstants.*; import frc.team3128.commands.CmdAlign; import frc.team3128.commands.CmdExtendIntake; import frc.team3128.commands.CmdExtendIntakeAndRun; @@ -25,11 +34,13 @@ import frc.team3128.subsystems.LimelightSubsystem; import frc.team3128.subsystems.NAR_Drivetrain; import frc.team3128.subsystems.Shooter; -import static frc.team3128.autonomous.Trajectories.*; +import java.io.IOException; +import java.nio.file.Path; +import java.util.HashMap; /** * Class to store information about autonomous routines. - * @author Daniel Wang, Mason Lam + * @author Daniel Wang */ public class AutoPrograms { @@ -41,6 +52,8 @@ public class AutoPrograms { private Hood hood; private LimelightSubsystem limelights; + private HashMap trajectories; + public AutoPrograms() { drive = NAR_Drivetrain.getInstance(); shooter = Shooter.getInstance(); @@ -49,9 +62,45 @@ public AutoPrograms() { hood = Hood.getInstance(); limelights = LimelightSubsystem.getInstance(); + loadTrajectories(); initAutoSelector(); } + private void loadTrajectories() { + trajectories = new HashMap(); + + final String[] trajectoryNames = { + "S2H2_i", + "S2H2_ii", + "S2H1", + "S2H2_iii", + "S2H2_iv", + "4Ball_Terminal180_i", + "4Ball_Terminal180_ii", + "Terminal2Tarmac", + "Tarmac2Terminal", + "Billiards_i", + "Billiards_ii", + "3Ballv2_i", + "3Ballv2_ii", + "5Ballv2_i", + "5Ballv2_ii", + "S1H1_i", + "S1H1_ii", + "S1H2_ii", + "S1H2_iii" + }; + + for (String trajectoryName : trajectoryNames) { + Path path = Filesystem.getDeployDirectory().toPath().resolve("paths").resolve(trajectoryName + ".wpilib.json"); + try { + trajectories.put(trajectoryName, TrajectoryUtil.fromPathweaverJson(path)); + } catch (IOException ex) { + DriverStation.reportError("IOException loading trajectory " + trajectoryName, true); + } + } + } + private void initAutoSelector() { String[] autoStrings = new String[] {"1 Ball", "2 Ball", "3 Ball", "4 Ball", "5 Ball", "S2H1", "S2H2", "S1H1", "S1I1", "S1H2", "Billiards"}; NarwhalDashboard.addAutos(autoStrings); @@ -70,18 +119,18 @@ public Command getAutonomousCommand() { switch (selectedAutoName) { case "1 Ball": - initialPose = driveBack30In.getInitialPose(); + initialPose = Trajectories.driveBack30In.getInitialPose(); autoCommand = new SequentialCommandGroup( new CmdShootAlign().withTimeout(2), - trajectoryCmd("driveBack30In")); + trajectoryCmd(Trajectories.driveBack30In)); break; case "2 Ball": - initialPose = twoBallTraj.getInitialPose(); + initialPose = Trajectories.twoBallTraj.getInitialPose(); autoCommand = new SequentialCommandGroup( new InstantCommand(() -> intake.ejectIntake(), intake), new ParallelDeadlineGroup( - trajectoryCmd("twoBallTraj"), + trajectoryCmd(Trajectories.twoBallTraj), new CmdExtendIntakeAndRun() ), @@ -90,30 +139,44 @@ public Command getAutonomousCommand() { new CmdShootAlign().withTimeout(2)); break; case "3 Ball": - initialPose = get("3Ballv2_i").getInitialPose(); + initialPose = trajectories.get("3Ballv2_i").getInitialPose(); autoCommand = new SequentialCommandGroup( - IntakePathCmd("3Ballv2_i"), + + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("3Ballv2_i"), + new CmdExtendIntakeAndRun() + ), new CmdInPlaceTurn(180), new CmdShootAlign().withTimeout(2), - IntakePathCmd("3Ballv2_ii"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("3Ballv2_ii"), + new CmdExtendIntakeAndRun() + ), new CmdShootTurnVision(-170)); break; case "4 Ball": - initialPose = get("4Ball_Terminal180_i").getInitialPose(); + initialPose = trajectories.get("4Ball_Terminal180_i").getInitialPose(); autoCommand = new SequentialCommandGroup( //drive and intake 1 ball - IntakePathCmd("4Ball_Terminal180_i"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("4Ball_Terminal180_i"), + new CmdExtendIntakeAndRun()), //turn and shoot 2 balls new CmdInPlaceTurn(180), new CmdShoot(), //drive to ball and terminal and intake - IntakePathCmd("4Ball_Terminal180_ii"), - + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("4Ball_Terminal180_ii"), + new CmdExtendIntakeAndRun()), new CmdExtendIntakeAndRun().withTimeout(1), //drive to tarmac and shoot @@ -122,20 +185,33 @@ public Command getAutonomousCommand() { new CmdShootAlign().withTimeout(2)); break; case "5 Ball": - initialPose = get("3Ballv2_i").getInitialPose(); + initialPose = trajectories.get("3Ballv2_i").getInitialPose(); autoCommand = new SequentialCommandGroup( - IntakePathCmd("3Ballv2_i"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("3Ballv2_i"), + new CmdExtendIntakeAndRun() + ), new CmdInPlaceTurn(180), new CmdShootAlign().withTimeout(2), - IntakePathCmd("3Ballv2_ii"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("3Ballv2_ii"), + new CmdExtendIntakeAndRun() + ), new CmdShootTurnVision(-170), - IntakePathCmd("5Ballv2_i"), - - new InstantCommand(() -> drive.stop()), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + new SequentialCommandGroup( + trajectoryCmd("5Ballv2_i"), + new InstantCommand(() -> drive.stop())), + // new WaitCommand(0.5)), + new CmdExtendIntakeAndRun() + ), trajectoryCmd("5Ballv2_ii"), @@ -143,11 +219,14 @@ public Command getAutonomousCommand() { ); break; case "S2H1": - initialPose = get("S2H2_i").getInitialPose(); + initialPose = trajectories.get("S2H2_i").getInitialPose(); autoCommand = new SequentialCommandGroup( //drive and intake ball - - IntakePathCmd("S2H2_i"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("S2H2_i"), + new CmdExtendIntakeAndRun() + ), //turn and shoot new CmdInPlaceTurn(180), @@ -155,8 +234,11 @@ public Command getAutonomousCommand() { //turn and hoard first ball new CmdInPlaceTurn(90), - - IntakePathCmd("S2H2_ii"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("S2H2_ii"), + new CmdExtendIntakeAndRun() + ), //drive behind hub new CmdInPlaceTurn(-90), @@ -167,11 +249,15 @@ public Command getAutonomousCommand() { new CmdOuttake(0.5).withTimeout(2)); break; case "S2H2": - initialPose = get("S2H2_i").getInitialPose(); + initialPose = trajectories.get("S2H2_i").getInitialPose(); autoCommand = new SequentialCommandGroup( //drive and intake ball - IntakePathCmd("S2H2_i"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("S2H2_i"), + new CmdExtendIntakeAndRun() + ), //turn and shoot new CmdInPlaceTurn(180), @@ -179,15 +265,20 @@ public Command getAutonomousCommand() { //turn and hoard first ball new CmdInPlaceTurn(90), - - IntakePathCmd("S2H2_ii"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("S2H2_ii"), + new CmdExtendIntakeAndRun() + ), // turn and hoard second ball new CmdInPlaceTurn(180), - - IntakePathCmd("S2H2_iii"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("S2H2_iii"), + new CmdExtendIntakeAndRun()), - //hide ball behind hub + //hide ball behinde hub trajectoryCmd("S2H2_iv"), // new CmdInPlaceTurn(130), @@ -195,13 +286,15 @@ public Command getAutonomousCommand() { new CmdOuttake(0.4).withTimeout(1)); break; case "S1H1": - initialPose = get("S1H1_i").getInitialPose(); - autoCommand = new SequentialCommandGroup( - //drive and intake enemy ball - IntakePathCmd("S1H1_i"), + initialPose = trajectories.get("S1H1_i").getInitialPose(); + autoCommand = new SequentialCommandGroup( + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("S1H1_i"), + new CmdExtendIntakeAndRun() + ), new InstantCommand(() -> drive.stop()), - - //turn and shoot 1 ball + new CmdInPlaceTurn(180), new CmdRetractHopper(), @@ -210,19 +303,20 @@ public Command getAutonomousCommand() { new CmdShootSingleBall() ).withTimeout(2), - //hide enemy ball behind hub trajectoryCmd("S1H1_ii"), new CmdExtendIntake(), new CmdOuttake(0.5).withTimeout(1)); break; case "S1H2": - initialPose = get("S1H1_i").getInitialPose(); + initialPose = trajectories.get("S1H1_i").getInitialPose(); autoCommand = new SequentialCommandGroup( - //drive and intake enemy ball - IntakePathCmd("S1H1_i"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("S1H1_i"), + new CmdExtendIntakeAndRun() + ), new InstantCommand(() -> drive.stop()), - - //turn and shoot 1 ball + new CmdInPlaceTurn(180), new CmdRetractHopper(), @@ -230,11 +324,13 @@ public Command getAutonomousCommand() { new CmdAlign(), new CmdShootSingleBall() ).withTimeout(2), - - //drive and intake enemy ball - IntakePathCmd("S1H2_ii"), - - //hide enemy balls behind hub + + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("S1H2_ii"), + new CmdExtendIntakeAndRun() + ), + new CmdInPlaceTurn(180), trajectoryCmd("S1H2_iii"), @@ -243,13 +339,15 @@ public Command getAutonomousCommand() { new CmdOuttake(0.5).withTimeout(2)); break; case "S1I1": - initialPose = get("S1H1_i").getInitialPose(); + initialPose = trajectories.get("S1H1_i").getInitialPose(); autoCommand = new SequentialCommandGroup( - //drive and intake enemy ball - IntakePathCmd("S1H1_i"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("S1H1_i"), + new CmdExtendIntakeAndRun() + ), new InstantCommand(() -> drive.stop()), - - //turn and shoot 1 ball + new CmdInPlaceTurn(180), new CmdRetractHopper(), @@ -259,30 +357,33 @@ public Command getAutonomousCommand() { ).withTimeout(2)); break; case "Billiards": - // initial position: (6.8, 6.272, 45 deg - should be approx. pointing straight at the ball to knock) initialPose = new Pose2d(6.8, 6.272, Rotation2d.fromDegrees(45)); autoCommand = new SequentialCommandGroup ( - // outtake ball + // initial position: (6.8, 6.272, 45 deg - should be approx. pointing straight at the ball to knock) new SequentialCommandGroup( new CmdExtendIntake(), new CmdOuttake(0.85).withTimeout(1.5) ).withTimeout(2), - //turn, drive, and intake enemy ball new CmdInPlaceTurn(70), + + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("Billiards_i"), + new CmdExtendIntakeAndRun() + ), - IntakePathCmd("Billiards_i"), - - //turn and eject enemy ball new CmdInPlaceTurn(55), new CmdExtendIntake(), new CmdOuttake(0.6).withTimeout(1.5), - //drive and intake ball - IntakePathCmd("Billiards_ii"), + new InstantCommand(() -> intake.ejectIntake(), intake), + new ParallelDeadlineGroup( + trajectoryCmd("Billiards_ii"), + new CmdExtendIntakeAndRun() + ), - //turn and shoot new CmdInPlaceTurn(96), new CmdShootAlign().withTimeout(2)); @@ -295,19 +396,29 @@ public Command getAutonomousCommand() { drive.resetPose(initialPose); return autoCommand; } - - /** - * Follow trajectory and intake balls along the path - */ - private SequentialCommandGroup IntakePathCmd(String trajectory) { - ParallelDeadlineGroup movement = new ParallelDeadlineGroup( - trajectoryCmd(trajectory), - new CmdExtendIntakeAndRun()); - return new SequentialCommandGroup(new InstantCommand(intake::ejectIntake, intake), movement); + + // Helpers to define common commands used in autos + private Command trajectoryCmd(String trajName) { + return trajectoryCmd(trajectories.get(trajName)); + } + + private Command trajectoryCmd(Trajectory traj) { + return new RamseteCommand( + traj, + drive::getPose, + new RamseteController(RAMSETE_B, RAMSETE_ZETA), + new SimpleMotorFeedforward(kS, kV, kA), + DRIVE_KINEMATICS, + drive::getWheelSpeeds, + new PIDController(RAMSETE_KP, 0, 0), + new PIDController(RAMSETE_KP, 0, 0), + drive::tankDriveVolts, + drive) + .andThen(() -> drive.stop()); } /** - * Flip 180 degrees rotation wise but keep same pose translation + * pose is the same translation but flipped 180 rotation */ private Pose2d inverseRotation(Pose2d pose) { return new Pose2d(pose.getTranslation(), pose.getRotation().unaryMinus()); diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index 7a883da9..faf8becd 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -1,27 +1,17 @@ package frc.team3128.autonomous; -import java.io.IOException; -import java.nio.file.Path; -import java.util.HashMap; import java.util.List; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.RamseteController; + import edu.wpi.first.math.controller.SimpleMotorFeedforward; 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.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.TrajectoryUtil; import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj2.command.Command; import frc.team3128.Constants; -import static frc.team3128.Constants.DriveConstants.*; -import frc.team3128.subsystems.NAR_Drivetrain; -import edu.wpi.first.wpilibj2.command.RamseteCommand; /** * Store trajectories for autonomous. Edit points here. @@ -44,6 +34,22 @@ public class Trajectories { .addConstraint(autoVoltageConstraint) .setReversed(true); + public static Trajectory trajectorySimple = TrajectoryGenerator.generateTrajectory( + new Pose2d(Units.inchesToMeters(0), Units.inchesToMeters(0), new Rotation2d(0)), + List.of( + new Translation2d(Units.inchesToMeters(30), Units.inchesToMeters(20)), + new Translation2d(Units.inchesToMeters(60), Units.inchesToMeters(-20)) + ), + new Pose2d(Units.inchesToMeters(90), Units.inchesToMeters(0), new Rotation2d(0)), + forwardTrajConfig); + + public static Trajectory trajectoryLessSimple = TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of( + new Translation2d(Units.inchesToMeters(6), Units.inchesToMeters(-14))), + new Pose2d(Units.inchesToMeters(50), Units.inchesToMeters(-15), new Rotation2d(0)), + forwardTrajConfig); + public static Trajectory driveBack30In = TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, new Rotation2d(0)), List.of(), @@ -61,61 +67,4 @@ public class Trajectories { List.of(), new Pose2d(Units.inchesToMeters(250), 0, new Rotation2d(0)), forwardTrajConfig); - - private static HashMap trajectories = new HashMap(); - - static{ - final String[] trajectoryNames = { - "S2H2_i", - "S2H2_ii", - "S2H1", - "S2H2_iii", - "S2H2_iv", - "4Ball_Terminal180_i", - "4Ball_Terminal180_ii", - "Terminal2Tarmac", - "Tarmac2Terminal", - "Billiards_i", - "Billiards_ii", - "3Ballv2_i", - "3Ballv2_ii", - "5Ballv2_i", - "5Ballv2_ii", - "S1H1_i", - "S1H1_ii", - "S1H2_ii", - "S1H2_iii" - }; - for (String trajectoryName : trajectoryNames) { - Path path = Filesystem.getDeployDirectory().toPath().resolve("paths").resolve(trajectoryName + ".wpilib.json"); - try { - trajectories.put(trajectoryName, TrajectoryUtil.fromPathweaverJson(path)); - } catch (IOException ex) { - DriverStation.reportError("IOException loading trajectory " + trajectoryName, true); - } - } - trajectories.put("driveBack30In", driveBack30In); - trajectories.put("twoBallTraj", twoBallTraj); - trajectories.put("driveForwards500In", driveForwards500In); - } - - public static Trajectory get(String name) { - return trajectories.get(name); - } - - public static Command trajectoryCmd(String traj) { - NAR_Drivetrain drive = NAR_Drivetrain.getInstance(); - return new RamseteCommand( - trajectories.get(traj), - drive::getPose, - new RamseteController(RAMSETE_B, RAMSETE_ZETA), - new SimpleMotorFeedforward(kS, kV, kA), - DRIVE_KINEMATICS, - drive::getWheelSpeeds, - new PIDController(RAMSETE_KP, 0, 0), - new PIDController(RAMSETE_KP, 0, 0), - drive::tankDriveVolts, - drive) - .andThen(() -> drive.stop()); - } } diff --git a/src/main/java/frc/team3128/commands/CmdShootDist.java b/src/main/java/frc/team3128/commands/CmdShootDist.java index 82111301..f48ef9da 100644 --- a/src/main/java/frc/team3128/commands/CmdShootDist.java +++ b/src/main/java/frc/team3128/commands/CmdShootDist.java @@ -1,7 +1,7 @@ package frc.team3128.commands; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.team3128.Robot; import frc.team3128.common.utility.Log; import frc.team3128.subsystems.Hood; import frc.team3128.subsystems.Hopper; @@ -38,9 +38,9 @@ public void initialize() { @Override public void execute() { - double dist = (Robot.isSimulation()) ? SIM_HUB_DISTANCE : limelights.calculateDistance("shooter"); - shooter.beginShoot(shooter.calculateMotorVelocityFromDist(dist)); - hood.startPID(hood.calculateAngleFromDistance(dist)); + double dist = (Robot.isSimulation()) ? SIM_HUB_DISTANCE : limelights.calculateShooterDistance(); + shooter.beginShoot(shooter.calculateRPMFromDist(dist)); + hood.startPID(hood.calculateAngleFromDist(dist)); } @Override diff --git a/src/main/java/frc/team3128/common/hardware/motor/NAR_Motor.java b/src/main/java/frc/team3128/common/hardware/motor/NAR_Motor.java index b79d674e..4b0742ee 100644 --- a/src/main/java/frc/team3128/common/hardware/motor/NAR_Motor.java +++ b/src/main/java/frc/team3128/common/hardware/motor/NAR_Motor.java @@ -30,4 +30,4 @@ public double getStallCurrentAmps() { public double getStallTorqueNM() { return stallTorqueNM; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java index 85ccb917..4bbf59ef 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java @@ -5,6 +5,7 @@ import edu.wpi.first.hal.SimDouble; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SimDeviceSim; +import frc.team3128.common.hardware.motor.NAR_Motor; import net.thefletcher.revrobotics.CANSparkMax; import net.thefletcher.revrobotics.SparkMaxRelativeEncoder; import net.thefletcher.revrobotics.enums.MotorType; @@ -83,4 +84,11 @@ public void setSimVelocity(double vel) { public REVLibError follow(CANSparkMax motor) { return super.follow((CANSparkMax)motor); } + + // public void follow(NAR_EMotor motor) { + // if(!(motor instanceof CANSparkMax)) { + // throw new RuntimeException("Bad follow: NAR_CANSparkMax " + getDeviceId() + " attempted to follow non-CANSparkMax motor controller."); + // } + // super.follow((CANSparkMax)motor); + // } } diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java index 8ae1728a..8490203b 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonFX.java @@ -5,6 +5,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.RobotBase; +import frc.team3128.common.hardware.motor.NAR_Motor; public class NAR_TalonFX extends WPI_TalonFX { diff --git a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java index bf26c977..ee534b4c 100644 --- a/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java +++ b/src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_TalonSRX.java @@ -5,6 +5,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.RobotBase; +import frc.team3128.common.hardware.motor.NAR_Motor; public class NAR_TalonSRX extends WPI_TalonSRX { diff --git a/src/main/java/frc/team3128/subsystems/Hood.java b/src/main/java/frc/team3128/subsystems/Hood.java index d9e4f4c3..f12ae667 100644 --- a/src/main/java/frc/team3128/subsystems/Hood.java +++ b/src/main/java/frc/team3128/subsystems/Hood.java @@ -5,6 +5,7 @@ import frc.team3128.Robot; import static frc.team3128.Constants.HoodConstants.*; +import static frc.team3128.common.hardware.motorcontroller.MotorControllerConstants.*; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -39,7 +40,7 @@ public static synchronized Hood getInstance() { } public Hood() { - super(new PIDController(kP, kI, kD), PLATEAU_COUNT); + super(new PIDController(kP, kI, kD)); configMotors(); configEncoder(); @@ -159,7 +160,7 @@ public void simulationPeriodic() { double angle = singleJointedArmSim.getAngleRads()/(2*Math.PI) * 360; m_encoder.setPosition((angle-MIN_ANGLE) / (2 * Math.PI)); - m_hoodMotor.setSimVelocity(singleJointedArmSim.getVelocityRadPerSec() / Constants.ConversionConstants.SPARK_ENCODER_RESOLUTION); + m_hoodMotor.setSimVelocity(singleJointedArmSim.getVelocityRadPerSec() / SPARKMAX_ENCODER_RESOLUTION); m_hoodMotor.setSimPosition(angle - MIN_ANGLE); //SmartDashboard.putNumber("Hood Position", m_encoder.getPosition()); From 229db4454ebd7d85031fc2948b6f1e2fe53acff4 Mon Sep 17 00:00:00 2001 From: Arav Chadha Date: Sat, 18 Jun 2022 13:35:16 -0700 Subject: [PATCH 09/18] Update Intake.java Added the one-line intake sim (intake motor voltage). --- src/main/java/frc/team3128/subsystems/Intake.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/team3128/subsystems/Intake.java b/src/main/java/frc/team3128/subsystems/Intake.java index e7088d02..8655453f 100644 --- a/src/main/java/frc/team3128/subsystems/Intake.java +++ b/src/main/java/frc/team3128/subsystems/Intake.java @@ -106,6 +106,15 @@ public void ejectIntake(){ public String getSolenoid() { return m_intakeSolenoid.get().toString(); } + + @Override + public void simulationPeriodic() { + + SmartDashboard.putNumber("Intake Motor Voltage", m_intake.getMotorOutputVoltage()); + + } + + } From 9a5882632429f949b1d9ab48ec3fea470873bf5b Mon Sep 17 00:00:00 2001 From: Arav Chadha Date: Sat, 18 Jun 2022 14:03:33 -0700 Subject: [PATCH 10/18] Fix Auto Stuff Accidentally undid the auto and traj changes before, so redid the changes in this commit. --- .../frc/team3128/autonomous/AutoPrograms.java | 255 +++++------------- .../frc/team3128/autonomous/Trajectories.java | 87 ++++-- 2 files changed, 141 insertions(+), 201 deletions(-) diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index c16e3964..52612f1a 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -1,21 +1,12 @@ package frc.team3128.autonomous; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.RamseteController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryUtil; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import static frc.team3128.Constants.DriveConstants.*; import frc.team3128.commands.CmdAlign; import frc.team3128.commands.CmdExtendIntake; import frc.team3128.commands.CmdExtendIntakeAndRun; @@ -34,13 +25,11 @@ import frc.team3128.subsystems.LimelightSubsystem; import frc.team3128.subsystems.NAR_Drivetrain; import frc.team3128.subsystems.Shooter; -import java.io.IOException; -import java.nio.file.Path; -import java.util.HashMap; +import static frc.team3128.autonomous.Trajectories.*; /** * Class to store information about autonomous routines. - * @author Daniel Wang + * @author Daniel Wang, Mason Lam */ public class AutoPrograms { @@ -52,8 +41,6 @@ public class AutoPrograms { private Hood hood; private LimelightSubsystem limelights; - private HashMap trajectories; - public AutoPrograms() { drive = NAR_Drivetrain.getInstance(); shooter = Shooter.getInstance(); @@ -62,45 +49,9 @@ public AutoPrograms() { hood = Hood.getInstance(); limelights = LimelightSubsystem.getInstance(); - loadTrajectories(); initAutoSelector(); } - private void loadTrajectories() { - trajectories = new HashMap(); - - final String[] trajectoryNames = { - "S2H2_i", - "S2H2_ii", - "S2H1", - "S2H2_iii", - "S2H2_iv", - "4Ball_Terminal180_i", - "4Ball_Terminal180_ii", - "Terminal2Tarmac", - "Tarmac2Terminal", - "Billiards_i", - "Billiards_ii", - "3Ballv2_i", - "3Ballv2_ii", - "5Ballv2_i", - "5Ballv2_ii", - "S1H1_i", - "S1H1_ii", - "S1H2_ii", - "S1H2_iii" - }; - - for (String trajectoryName : trajectoryNames) { - Path path = Filesystem.getDeployDirectory().toPath().resolve("paths").resolve(trajectoryName + ".wpilib.json"); - try { - trajectories.put(trajectoryName, TrajectoryUtil.fromPathweaverJson(path)); - } catch (IOException ex) { - DriverStation.reportError("IOException loading trajectory " + trajectoryName, true); - } - } - } - private void initAutoSelector() { String[] autoStrings = new String[] {"1 Ball", "2 Ball", "3 Ball", "4 Ball", "5 Ball", "S2H1", "S2H2", "S1H1", "S1I1", "S1H2", "Billiards"}; NarwhalDashboard.addAutos(autoStrings); @@ -119,18 +70,18 @@ public Command getAutonomousCommand() { switch (selectedAutoName) { case "1 Ball": - initialPose = Trajectories.driveBack30In.getInitialPose(); + initialPose = driveBack30In.getInitialPose(); autoCommand = new SequentialCommandGroup( new CmdShootAlign().withTimeout(2), - trajectoryCmd(Trajectories.driveBack30In)); + trajectoryCmd("driveBack30In")); break; case "2 Ball": - initialPose = Trajectories.twoBallTraj.getInitialPose(); + initialPose = twoBallTraj.getInitialPose(); autoCommand = new SequentialCommandGroup( new InstantCommand(() -> intake.ejectIntake(), intake), new ParallelDeadlineGroup( - trajectoryCmd(Trajectories.twoBallTraj), + trajectoryCmd("twoBallTraj"), new CmdExtendIntakeAndRun() ), @@ -139,44 +90,30 @@ public Command getAutonomousCommand() { new CmdShootAlign().withTimeout(2)); break; case "3 Ball": - initialPose = trajectories.get("3Ballv2_i").getInitialPose(); + initialPose = get("3Ballv2_i").getInitialPose(); autoCommand = new SequentialCommandGroup( - - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("3Ballv2_i"), - new CmdExtendIntakeAndRun() - ), + IntakePathCmd("3Ballv2_i"), new CmdInPlaceTurn(180), new CmdShootAlign().withTimeout(2), - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("3Ballv2_ii"), - new CmdExtendIntakeAndRun() - ), + IntakePathCmd("3Ballv2_ii"), new CmdShootTurnVision(-170)); break; case "4 Ball": - initialPose = trajectories.get("4Ball_Terminal180_i").getInitialPose(); + initialPose = get("4Ball_Terminal180_i").getInitialPose(); autoCommand = new SequentialCommandGroup( //drive and intake 1 ball - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("4Ball_Terminal180_i"), - new CmdExtendIntakeAndRun()), + IntakePathCmd("4Ball_Terminal180_i"), //turn and shoot 2 balls new CmdInPlaceTurn(180), new CmdShoot(), //drive to ball and terminal and intake - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("4Ball_Terminal180_ii"), - new CmdExtendIntakeAndRun()), + IntakePathCmd("4Ball_Terminal180_ii"), + new CmdExtendIntakeAndRun().withTimeout(1), //drive to tarmac and shoot @@ -185,33 +122,20 @@ public Command getAutonomousCommand() { new CmdShootAlign().withTimeout(2)); break; case "5 Ball": - initialPose = trajectories.get("3Ballv2_i").getInitialPose(); + initialPose = get("3Ballv2_i").getInitialPose(); autoCommand = new SequentialCommandGroup( - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("3Ballv2_i"), - new CmdExtendIntakeAndRun() - ), + IntakePathCmd("3Ballv2_i"), new CmdInPlaceTurn(180), new CmdShootAlign().withTimeout(2), - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("3Ballv2_ii"), - new CmdExtendIntakeAndRun() - ), + IntakePathCmd("3Ballv2_ii"), new CmdShootTurnVision(-170), - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - new SequentialCommandGroup( - trajectoryCmd("5Ballv2_i"), - new InstantCommand(() -> drive.stop())), - // new WaitCommand(0.5)), - new CmdExtendIntakeAndRun() - ), + IntakePathCmd("5Ballv2_i"), + + new InstantCommand(() -> drive.stop()), trajectoryCmd("5Ballv2_ii"), @@ -219,14 +143,11 @@ public Command getAutonomousCommand() { ); break; case "S2H1": - initialPose = trajectories.get("S2H2_i").getInitialPose(); + initialPose = get("S2H2_i").getInitialPose(); autoCommand = new SequentialCommandGroup( //drive and intake ball - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("S2H2_i"), - new CmdExtendIntakeAndRun() - ), + + IntakePathCmd("S2H2_i"), //turn and shoot new CmdInPlaceTurn(180), @@ -234,11 +155,8 @@ public Command getAutonomousCommand() { //turn and hoard first ball new CmdInPlaceTurn(90), - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("S2H2_ii"), - new CmdExtendIntakeAndRun() - ), + + IntakePathCmd("S2H2_ii"), //drive behind hub new CmdInPlaceTurn(-90), @@ -249,15 +167,11 @@ public Command getAutonomousCommand() { new CmdOuttake(0.5).withTimeout(2)); break; case "S2H2": - initialPose = trajectories.get("S2H2_i").getInitialPose(); + initialPose = get("S2H2_i").getInitialPose(); autoCommand = new SequentialCommandGroup( //drive and intake ball - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("S2H2_i"), - new CmdExtendIntakeAndRun() - ), + IntakePathCmd("S2H2_i"), //turn and shoot new CmdInPlaceTurn(180), @@ -265,20 +179,15 @@ public Command getAutonomousCommand() { //turn and hoard first ball new CmdInPlaceTurn(90), - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("S2H2_ii"), - new CmdExtendIntakeAndRun() - ), + + IntakePathCmd("S2H2_ii"), // turn and hoard second ball new CmdInPlaceTurn(180), - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("S2H2_iii"), - new CmdExtendIntakeAndRun()), + + IntakePathCmd("S2H2_iii"), - //hide ball behinde hub + //hide ball behind hub trajectoryCmd("S2H2_iv"), // new CmdInPlaceTurn(130), @@ -286,15 +195,13 @@ public Command getAutonomousCommand() { new CmdOuttake(0.4).withTimeout(1)); break; case "S1H1": - initialPose = trajectories.get("S1H1_i").getInitialPose(); - autoCommand = new SequentialCommandGroup( - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("S1H1_i"), - new CmdExtendIntakeAndRun() - ), + initialPose = get("S1H1_i").getInitialPose(); + autoCommand = new SequentialCommandGroup( + //drive and intake enemy ball + IntakePathCmd("S1H1_i"), new InstantCommand(() -> drive.stop()), - + + //turn and shoot 1 ball new CmdInPlaceTurn(180), new CmdRetractHopper(), @@ -303,20 +210,19 @@ public Command getAutonomousCommand() { new CmdShootSingleBall() ).withTimeout(2), + //hide enemy ball behind hub trajectoryCmd("S1H1_ii"), new CmdExtendIntake(), new CmdOuttake(0.5).withTimeout(1)); break; case "S1H2": - initialPose = trajectories.get("S1H1_i").getInitialPose(); + initialPose = get("S1H1_i").getInitialPose(); autoCommand = new SequentialCommandGroup( - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("S1H1_i"), - new CmdExtendIntakeAndRun() - ), + //drive and intake enemy ball + IntakePathCmd("S1H1_i"), new InstantCommand(() -> drive.stop()), - + + //turn and shoot 1 ball new CmdInPlaceTurn(180), new CmdRetractHopper(), @@ -324,13 +230,11 @@ public Command getAutonomousCommand() { new CmdAlign(), new CmdShootSingleBall() ).withTimeout(2), - - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("S1H2_ii"), - new CmdExtendIntakeAndRun() - ), - + + //drive and intake enemy ball + IntakePathCmd("S1H2_ii"), + + //hide enemy balls behind hub new CmdInPlaceTurn(180), trajectoryCmd("S1H2_iii"), @@ -339,15 +243,13 @@ public Command getAutonomousCommand() { new CmdOuttake(0.5).withTimeout(2)); break; case "S1I1": - initialPose = trajectories.get("S1H1_i").getInitialPose(); + initialPose = get("S1H1_i").getInitialPose(); autoCommand = new SequentialCommandGroup( - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("S1H1_i"), - new CmdExtendIntakeAndRun() - ), + //drive and intake enemy ball + IntakePathCmd("S1H1_i"), new InstantCommand(() -> drive.stop()), - + + //turn and shoot 1 ball new CmdInPlaceTurn(180), new CmdRetractHopper(), @@ -357,33 +259,30 @@ public Command getAutonomousCommand() { ).withTimeout(2)); break; case "Billiards": + // initial position: (6.8, 6.272, 45 deg - should be approx. pointing straight at the ball to knock) initialPose = new Pose2d(6.8, 6.272, Rotation2d.fromDegrees(45)); autoCommand = new SequentialCommandGroup ( - // initial position: (6.8, 6.272, 45 deg - should be approx. pointing straight at the ball to knock) + // outtake ball new SequentialCommandGroup( new CmdExtendIntake(), new CmdOuttake(0.85).withTimeout(1.5) ).withTimeout(2), + //turn, drive, and intake enemy ball new CmdInPlaceTurn(70), - - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("Billiards_i"), - new CmdExtendIntakeAndRun() - ), + IntakePathCmd("Billiards_i"), + + //turn and eject enemy ball new CmdInPlaceTurn(55), new CmdExtendIntake(), new CmdOuttake(0.6).withTimeout(1.5), - new InstantCommand(() -> intake.ejectIntake(), intake), - new ParallelDeadlineGroup( - trajectoryCmd("Billiards_ii"), - new CmdExtendIntakeAndRun() - ), + //drive and intake ball + IntakePathCmd("Billiards_ii"), + //turn and shoot new CmdInPlaceTurn(96), new CmdShootAlign().withTimeout(2)); @@ -396,29 +295,19 @@ public Command getAutonomousCommand() { drive.resetPose(initialPose); return autoCommand; } - - // Helpers to define common commands used in autos - private Command trajectoryCmd(String trajName) { - return trajectoryCmd(trajectories.get(trajName)); - } - - private Command trajectoryCmd(Trajectory traj) { - return new RamseteCommand( - traj, - drive::getPose, - new RamseteController(RAMSETE_B, RAMSETE_ZETA), - new SimpleMotorFeedforward(kS, kV, kA), - DRIVE_KINEMATICS, - drive::getWheelSpeeds, - new PIDController(RAMSETE_KP, 0, 0), - new PIDController(RAMSETE_KP, 0, 0), - drive::tankDriveVolts, - drive) - .andThen(() -> drive.stop()); + + /** + * Follow trajectory and intake balls along the path + */ + private SequentialCommandGroup IntakePathCmd(String trajectory) { + ParallelDeadlineGroup movement = new ParallelDeadlineGroup( + trajectoryCmd(trajectory), + new CmdExtendIntakeAndRun()); + return new SequentialCommandGroup(new InstantCommand(intake::ejectIntake, intake), movement); } /** - * pose is the same translation but flipped 180 rotation + * Flip 180 degrees rotation wise but keep same pose translation */ private Pose2d inverseRotation(Pose2d pose) { return new Pose2d(pose.getTranslation(), pose.getRotation().unaryMinus()); diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index faf8becd..7a883da9 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -1,17 +1,27 @@ package frc.team3128.autonomous; +import java.io.IOException; +import java.nio.file.Path; +import java.util.HashMap; import java.util.List; - +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; 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.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.trajectory.TrajectoryUtil; import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.Command; import frc.team3128.Constants; +import static frc.team3128.Constants.DriveConstants.*; +import frc.team3128.subsystems.NAR_Drivetrain; +import edu.wpi.first.wpilibj2.command.RamseteCommand; /** * Store trajectories for autonomous. Edit points here. @@ -34,22 +44,6 @@ public class Trajectories { .addConstraint(autoVoltageConstraint) .setReversed(true); - public static Trajectory trajectorySimple = TrajectoryGenerator.generateTrajectory( - new Pose2d(Units.inchesToMeters(0), Units.inchesToMeters(0), new Rotation2d(0)), - List.of( - new Translation2d(Units.inchesToMeters(30), Units.inchesToMeters(20)), - new Translation2d(Units.inchesToMeters(60), Units.inchesToMeters(-20)) - ), - new Pose2d(Units.inchesToMeters(90), Units.inchesToMeters(0), new Rotation2d(0)), - forwardTrajConfig); - - public static Trajectory trajectoryLessSimple = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(Units.inchesToMeters(6), Units.inchesToMeters(-14))), - new Pose2d(Units.inchesToMeters(50), Units.inchesToMeters(-15), new Rotation2d(0)), - forwardTrajConfig); - public static Trajectory driveBack30In = TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, new Rotation2d(0)), List.of(), @@ -67,4 +61,61 @@ public class Trajectories { List.of(), new Pose2d(Units.inchesToMeters(250), 0, new Rotation2d(0)), forwardTrajConfig); + + private static HashMap trajectories = new HashMap(); + + static{ + final String[] trajectoryNames = { + "S2H2_i", + "S2H2_ii", + "S2H1", + "S2H2_iii", + "S2H2_iv", + "4Ball_Terminal180_i", + "4Ball_Terminal180_ii", + "Terminal2Tarmac", + "Tarmac2Terminal", + "Billiards_i", + "Billiards_ii", + "3Ballv2_i", + "3Ballv2_ii", + "5Ballv2_i", + "5Ballv2_ii", + "S1H1_i", + "S1H1_ii", + "S1H2_ii", + "S1H2_iii" + }; + for (String trajectoryName : trajectoryNames) { + Path path = Filesystem.getDeployDirectory().toPath().resolve("paths").resolve(trajectoryName + ".wpilib.json"); + try { + trajectories.put(trajectoryName, TrajectoryUtil.fromPathweaverJson(path)); + } catch (IOException ex) { + DriverStation.reportError("IOException loading trajectory " + trajectoryName, true); + } + } + trajectories.put("driveBack30In", driveBack30In); + trajectories.put("twoBallTraj", twoBallTraj); + trajectories.put("driveForwards500In", driveForwards500In); + } + + public static Trajectory get(String name) { + return trajectories.get(name); + } + + public static Command trajectoryCmd(String traj) { + NAR_Drivetrain drive = NAR_Drivetrain.getInstance(); + return new RamseteCommand( + trajectories.get(traj), + drive::getPose, + new RamseteController(RAMSETE_B, RAMSETE_ZETA), + new SimpleMotorFeedforward(kS, kV, kA), + DRIVE_KINEMATICS, + drive::getWheelSpeeds, + new PIDController(RAMSETE_KP, 0, 0), + new PIDController(RAMSETE_KP, 0, 0), + drive::tankDriveVolts, + drive) + .andThen(() -> drive.stop()); + } } From e00e4f69ca6dc804bd812adf8d5f4a896eb5fdf6 Mon Sep 17 00:00:00 2001 From: Arav Chadha Date: Wed, 22 Jun 2022 21:47:39 -0700 Subject: [PATCH 11/18] Add hopper sim (Almost working) Mainly working hopper sim, only issue is that ball count does not reset to 0 when robot shoots. --- src/main/java/frc/team3128/Constants.java | 10 +++ .../java/frc/team3128/RobotContainer.java | 14 +++- .../frc/team3128/commands/CmdIntakeCargo.java | 5 ++ .../frc/team3128/commands/CmdOuttake.java | 6 ++ .../frc/team3128/commands/CmdShootDist.java | 15 ++++- .../java/frc/team3128/subsystems/Hood.java | 4 +- .../java/frc/team3128/subsystems/Hopper.java | 66 +++++++++++++++++++ .../java/frc/team3128/subsystems/Shooter.java | 2 +- 8 files changed, 115 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 6635f7f3..2006725f 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -8,6 +8,8 @@ import frc.team3128.common.utility.interpolation.InterpolatingTreeMap; import static frc.team3128.common.hardware.motorcontroller.MotorControllerConstants.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; @@ -37,6 +39,9 @@ public static class DriveConstants { public static final double WHEEL_RADIUS_METERS = Units.inchesToMeters(3.029); public static final double TRACK_WIDTH_METERS = 0.56147; + public static final Pose2d HUB_POS = new Pose2d(Units.inchesToMeters(324), Units.inchesToMeters(162), new Rotation2d(0)); // meters + public static final double HUB_RADIUS = 26.69; + public static final DifferentialDriveKinematics DRIVE_KINEMATICS = new DifferentialDriveKinematics(TRACK_WIDTH_METERS); public static final double ENCODER_DISTANCE_PER_MARK = WHEEL_RADIUS_METERS * 2 * Math.PI / FALCON_ENCODER_RESOLUTION; public static final double DRIVE_NU_TO_METER = ENCODER_DISTANCE_PER_MARK / DRIVE_GEARING; // meters driven per encoder tick @@ -148,6 +153,11 @@ public static class HopperConstants { public static final double HOPPER_MOTOR_2_POWER = 0.7; public static final double REVERSE_HOPPER_MOTOR_POWER = -1; + public static final DCMotor GEARBOX = DCMotor.getVex775Pro(1); + public static final double HOPPER_MOTOR_GEAR_RATIO = 0.25; + public static final double HOPPER_MOMENT_OF_INERTIA = 0.04e-4; + public static final double HOPPER_MOMENT_OF_INERTIA_BALL = 0.16e-4; + } public static class IntakeConstants { diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 2cabce31..823440c7 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -168,8 +168,8 @@ private void configureButtonBindings() { m_leftStick.getButton(13).whenPressed(new InstantCommand(m_climber::bothExtend, m_climber)) .whenReleased(new InstantCommand(m_climber::bothStop, m_climber)); - m_leftStick.getButton(14).whenPressed(new InstantCommand(m_climber::bothRetract, m_climber)) - .whenReleased(new InstantCommand(m_climber::bothStop, m_climber)); + m_leftStick.getButton(14).whenPressed(new InstantCommand(()-> m_hopper.runHopper(.325), m_hopper)) + .whenReleased(new InstantCommand(m_hopper::stopHopper, m_hopper)); m_leftStick.getButton(12).whenPressed(new InstantCommand(m_climber::extendPiston, m_climber)); m_leftStick.getButton(15).whenPressed(new InstantCommand(m_climber::retractPiston, m_climber)); @@ -178,6 +178,16 @@ private void configureButtonBindings() { isShooting.debounce(0.1).whenActive(new InstantCommand(m_hopper::runHopper, m_hopper)) .whenInactive(new InstantCommand(m_hopper::stopHopper, m_hopper)); + + if (Robot.isSimulation()) { + isShooting.debounce(0.1).whenActive(new SequentialCommandGroup + ( + new InstantCommand(m_hopper::runHopper, m_hopper), + new InstantCommand(m_hopper::resetBallCount, m_hopper) + ) + ) + .whenInactive(new InstantCommand(m_hopper::stopHopper, m_hopper)); + } } diff --git a/src/main/java/frc/team3128/commands/CmdIntakeCargo.java b/src/main/java/frc/team3128/commands/CmdIntakeCargo.java index 47a25e44..c7c91895 100644 --- a/src/main/java/frc/team3128/commands/CmdIntakeCargo.java +++ b/src/main/java/frc/team3128/commands/CmdIntakeCargo.java @@ -1,5 +1,6 @@ package frc.team3128.commands; import frc.team3128.subsystems.Intake; +import frc.team3128.Robot; import frc.team3128.subsystems.Hopper; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -23,6 +24,10 @@ public CmdIntakeCargo(){ public void initialize() { m_intake.runIntake(); m_hopper.runHopper(); + + if (Robot.isSimulation()){ + m_hopper.addBall(); + } } @Override diff --git a/src/main/java/frc/team3128/commands/CmdOuttake.java b/src/main/java/frc/team3128/commands/CmdOuttake.java index a5a9cd2d..9be7b0cb 100644 --- a/src/main/java/frc/team3128/commands/CmdOuttake.java +++ b/src/main/java/frc/team3128/commands/CmdOuttake.java @@ -2,6 +2,8 @@ import frc.team3128.subsystems.Intake; import static frc.team3128.Constants.IntakeConstants.*; import static frc.team3128.Constants.HopperConstants.*; + +import frc.team3128.Robot; import frc.team3128.subsystems.Hopper; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -46,6 +48,10 @@ public CmdOuttake(double intakePower, double hopperPower){ public void initialize() { m_intake.runIntake(intakePower); m_hopper.reverseHopper(hopperPower); + + if (Robot.isSimulation()){ + m_hopper.resetBallCount(); + } } @Override diff --git a/src/main/java/frc/team3128/commands/CmdShootDist.java b/src/main/java/frc/team3128/commands/CmdShootDist.java index f48ef9da..72d78824 100644 --- a/src/main/java/frc/team3128/commands/CmdShootDist.java +++ b/src/main/java/frc/team3128/commands/CmdShootDist.java @@ -1,13 +1,18 @@ package frc.team3128.commands; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.team3128.Robot; import frc.team3128.common.utility.Log; import frc.team3128.subsystems.Hood; import frc.team3128.subsystems.Hopper; import frc.team3128.subsystems.LimelightSubsystem; +import frc.team3128.subsystems.NAR_Drivetrain; import frc.team3128.subsystems.Shooter; import static frc.team3128.Constants.SimConstants.*; +import static frc.team3128.Constants.DriveConstants.*; public class CmdShootDist extends CommandBase { private Shooter shooter; @@ -38,7 +43,15 @@ public void initialize() { @Override public void execute() { - double dist = (Robot.isSimulation()) ? SIM_HUB_DISTANCE : limelights.calculateShooterDistance(); + double dist = limelights.calculateShooterDistance(); + if (Robot.isSimulation()) { + + Pose2d pose = NAR_Drivetrain.getInstance().getPose().relativeTo(HUB_POS); + + dist = Units.metersToInches(Math.sqrt(pose.getX() * pose.getX() + pose.getY() * pose.getY())) - HUB_RADIUS; // meters + + SmartDashboard.putNumber("Distance from hub (meters)", Units.inchesToMeters(dist)); + } shooter.beginShoot(shooter.calculateRPMFromDist(dist)); hood.startPID(hood.calculateAngleFromDist(dist)); } diff --git a/src/main/java/frc/team3128/subsystems/Hood.java b/src/main/java/frc/team3128/subsystems/Hood.java index f12ae667..40359e20 100644 --- a/src/main/java/frc/team3128/subsystems/Hood.java +++ b/src/main/java/frc/team3128/subsystems/Hood.java @@ -51,7 +51,7 @@ public Hood() { singleJointedArmSim = new SingleJointedArmSim( DCMotor.getNeo550(1), HOOD_SHOOTER_GEAR_RATIO, - 0.054195108, //TODO Find this (COMMUNISM OVER CAPITALISM) + 0.054195108, 0.2400046, Units.degreesToRadians(MIN_ANGLE), Units.degreesToRadians(MAX_ANGLE), @@ -162,8 +162,6 @@ public void simulationPeriodic() { m_encoder.setPosition((angle-MIN_ANGLE) / (2 * Math.PI)); m_hoodMotor.setSimVelocity(singleJointedArmSim.getVelocityRadPerSec() / SPARKMAX_ENCODER_RESOLUTION); m_hoodMotor.setSimPosition(angle - MIN_ANGLE); - - //SmartDashboard.putNumber("Hood Position", m_encoder.getPosition()); } } diff --git a/src/main/java/frc/team3128/subsystems/Hopper.java b/src/main/java/frc/team3128/subsystems/Hopper.java index 445a5364..b2fc51de 100644 --- a/src/main/java/frc/team3128/subsystems/Hopper.java +++ b/src/main/java/frc/team3128/subsystems/Hopper.java @@ -4,9 +4,14 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team3128.Robot; import frc.team3128.common.hardware.motorcontroller.NAR_TalonSRX; import static frc.team3128.Constants.HopperConstants.*; @@ -23,9 +28,25 @@ public class Hopper extends SubsystemBase { private Encoder m_encoder; + private Timer timer; + + private double prevAngularVelocity; + + private FlywheelSim m_flywheelSim; + + protected int ballCount; + public Hopper() { configMotors(); configEncoders(); + + timer = new Timer(); + + timer.start(); + + if(Robot.isSimulation()) { + m_flywheelSim = new FlywheelSim(GEARBOX, HOPPER_MOTOR_GEAR_RATIO, HOPPER_MOMENT_OF_INERTIA); + } } public static synchronized Hopper getInstance() { @@ -64,6 +85,21 @@ private void configEncoders() { @Override public void periodic() { SmartDashboard.putNumber("Hopper Enc", m_encoder.getDistance()); + + double dt = timer.get(); + + timer.reset(); + + double angularVelocity = m_encoder.getRate() / 4 * 2 * Math.PI; + + double angularAcceleration = (angularVelocity - prevAngularVelocity) / dt; + + double torque = 0.71-(0.71/18734.05)*angularVelocity; + + double momentOfInertia = torque / angularAcceleration; + + SmartDashboard.putNumber("Hopper Moment of Inertia", momentOfInertia * 10000); + } /** @@ -111,4 +147,34 @@ public double getHopperDistance() { return m_encoder.getDistance(); } + public void addBall() { + ballCount = MathUtil.clamp((ballCount + 1), 0, 2); + } + + public void resetBallCount() { + ballCount = 0; + } + + @Override + public void simulationPeriodic() { + + if (ballCount > 0) { + m_flywheelSim = new FlywheelSim(GEARBOX, HOPPER_MOTOR_GEAR_RATIO, HOPPER_MOMENT_OF_INERTIA_BALL); + } + else { + m_flywheelSim = new FlywheelSim(GEARBOX, HOPPER_MOTOR_GEAR_RATIO, HOPPER_MOMENT_OF_INERTIA); + } + + m_flywheelSim.setInputVoltage( + m_hopper1.getMotorOutputVoltage() + ); + + m_flywheelSim.update(0.02); + + SmartDashboard.putNumber("Hopper 1 Motor RPM", m_flywheelSim.getAngularVelocityRPM()); + SmartDashboard.putNumber("Hopper 1 Motor Voltage", m_hopper1.getMotorOutputVoltage()); + SmartDashboard.putNumber("Hopper 2 Motor Voltage", m_hopper2.getMotorOutputVoltage()); + SmartDashboard.putNumber("Hopper Ball Count", ballCount); + } + } \ No newline at end of file diff --git a/src/main/java/frc/team3128/subsystems/Shooter.java b/src/main/java/frc/team3128/subsystems/Shooter.java index 1fc8b7cd..3fe1c007 100644 --- a/src/main/java/frc/team3128/subsystems/Shooter.java +++ b/src/main/java/frc/team3128/subsystems/Shooter.java @@ -163,7 +163,7 @@ public void simulationPeriodic() { // SmartDashboard.putNumber("test", m_leftShooter.getMotorOutputVoltage()); // SmartDashboard.putString("pogger", String.valueOf(m_shooterSim.getAngularVelocityRadPerSec())); - SmartDashboard.putNumber("shooter RPM", m_shooterSim.getAngularVelocityRadPerSec() * 60 / (2*Math.PI)); + SmartDashboard.putNumber("shooter RPMSIM", m_shooterSim.getAngularVelocityRadPerSec() * 60 / (2*Math.PI)); } From 2cbb7b7f62da5ef9c19bf6694cf10a85c6bc4a58 Mon Sep 17 00:00:00 2001 From: Arav Chadha Date: Wed, 22 Jun 2022 22:18:51 -0700 Subject: [PATCH 12/18] Update Hopper.java Create member variables for noBall and ball flywheel sims. --- src/main/java/frc/team3128/subsystems/Hopper.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team3128/subsystems/Hopper.java b/src/main/java/frc/team3128/subsystems/Hopper.java index b2fc51de..99fd3b54 100644 --- a/src/main/java/frc/team3128/subsystems/Hopper.java +++ b/src/main/java/frc/team3128/subsystems/Hopper.java @@ -34,6 +34,10 @@ public class Hopper extends SubsystemBase { private FlywheelSim m_flywheelSim; + private FlywheelSim noBallSim; + + private FlywheelSim ballSim; + protected int ballCount; public Hopper() { @@ -45,7 +49,9 @@ public Hopper() { timer.start(); if(Robot.isSimulation()) { - m_flywheelSim = new FlywheelSim(GEARBOX, HOPPER_MOTOR_GEAR_RATIO, HOPPER_MOMENT_OF_INERTIA); + noBallSim = new FlywheelSim(GEARBOX, HOPPER_MOTOR_GEAR_RATIO, HOPPER_MOMENT_OF_INERTIA); + ballSim = new FlywheelSim(GEARBOX, HOPPER_MOTOR_GEAR_RATIO, HOPPER_MOMENT_OF_INERTIA_BALL); + m_flywheelSim = noBallSim; } } @@ -159,10 +165,10 @@ public void resetBallCount() { public void simulationPeriodic() { if (ballCount > 0) { - m_flywheelSim = new FlywheelSim(GEARBOX, HOPPER_MOTOR_GEAR_RATIO, HOPPER_MOMENT_OF_INERTIA_BALL); + m_flywheelSim = noBallSim; } else { - m_flywheelSim = new FlywheelSim(GEARBOX, HOPPER_MOTOR_GEAR_RATIO, HOPPER_MOMENT_OF_INERTIA); + m_flywheelSim = ballSim; } m_flywheelSim.setInputVoltage( From eea2a146635818c39e594487d5aedf644177409b Mon Sep 17 00:00:00 2001 From: Arav Chadha Date: Mon, 27 Jun 2022 14:12:19 -0700 Subject: [PATCH 13/18] Updated Sim Code Fixed hopper ball count, created constants for hood sim, and updated shooter sim. --- src/main/java/frc/team3128/Constants.java | 13 +++++++--- .../java/frc/team3128/RobotContainer.java | 10 ------- .../frc/team3128/commands/CmdShootDist.java | 4 +++ .../java/frc/team3128/subsystems/Hood.java | 15 ++++++++--- .../java/frc/team3128/subsystems/Hopper.java | 26 +++++++------------ .../java/frc/team3128/subsystems/Shooter.java | 10 +++---- 6 files changed, 36 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 2006725f..1a2c173a 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -90,7 +90,7 @@ public static class ShooterConstants { public static final int LEFT_SHOOTER_ID = 4; public static final int RIGHT_SHOOTER_ID = 5; - public static final double kP = 2e-4; // 3.2e-3; + public static final double kP = 2e-4; // 3.2e-3; public static final double kI = 0; public static final double kD = 0; // 5e-4; @@ -111,6 +111,7 @@ public static class ShooterConstants { public static final double SHOOTER_RADIUS_METERS = 0.0508; public static final DCMotor SHOOTER_GEARBOX = DCMotor.getCIM(2); public static final double SHOOTER_GEARING = 1.5; + public static final double SHOOTER_MOMENT_OF_INERTIA = 0.001271812; public static final InterpolatingTreeMap shooterSpeedsMap = new InterpolatingTreeMap(); static { @@ -155,9 +156,8 @@ public static class HopperConstants { public static final DCMotor GEARBOX = DCMotor.getVex775Pro(1); public static final double HOPPER_MOTOR_GEAR_RATIO = 0.25; - public static final double HOPPER_MOMENT_OF_INERTIA = 0.04e-4; - public static final double HOPPER_MOMENT_OF_INERTIA_BALL = 0.16e-4; - + public static final double HOPPER_MOMENT_OF_INERTIA = 0.04e-4; // 0.04e-4 + public static final double HOPPER_MOMENT_OF_INERTIA_BALL = 0.16e-4; // 0.16e-4 } public static class IntakeConstants { @@ -222,6 +222,11 @@ public static class HoodConstants { public static final double MAX_ANGLE = 40.4; // deg public static final double HOME_ANGLE = 28.4; // deg + public static final DCMotor HOOD_GEARBOX = DCMotor.getNeo550(1); + public static final double HOOD_MOMENT_OF_INERTIA = 0.054195108; + public static final double HOOD_ARM_LENGTH_METERS = 0.2400046; + public static final double HOOD_ARM_MASS_KG = 0.795601019; + public static InterpolatingTreeMap hoodAngleMap = new InterpolatingTreeMap(); static { diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 823440c7..0c305adb 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -178,16 +178,6 @@ private void configureButtonBindings() { isShooting.debounce(0.1).whenActive(new InstantCommand(m_hopper::runHopper, m_hopper)) .whenInactive(new InstantCommand(m_hopper::stopHopper, m_hopper)); - - if (Robot.isSimulation()) { - isShooting.debounce(0.1).whenActive(new SequentialCommandGroup - ( - new InstantCommand(m_hopper::runHopper, m_hopper), - new InstantCommand(m_hopper::resetBallCount, m_hopper) - ) - ) - .whenInactive(new InstantCommand(m_hopper::stopHopper, m_hopper)); - } } diff --git a/src/main/java/frc/team3128/commands/CmdShootDist.java b/src/main/java/frc/team3128/commands/CmdShootDist.java index 72d78824..bb00c2bd 100644 --- a/src/main/java/frc/team3128/commands/CmdShootDist.java +++ b/src/main/java/frc/team3128/commands/CmdShootDist.java @@ -62,6 +62,10 @@ public void end(boolean interrupted) { hopper.stopHopper(); limelights.turnShooterLEDOff(); Log.info("CmdShootDist", "Cancelling shooting"); + + if (Robot.isSimulation()) { + hopper.resetBallCount(); + } } @Override diff --git a/src/main/java/frc/team3128/subsystems/Hood.java b/src/main/java/frc/team3128/subsystems/Hood.java index 40359e20..17f3be9a 100644 --- a/src/main/java/frc/team3128/subsystems/Hood.java +++ b/src/main/java/frc/team3128/subsystems/Hood.java @@ -49,13 +49,13 @@ public Hood() { m_hoodMotor.setSimPosition(0); m_encoder.setPosition(0); singleJointedArmSim = new SingleJointedArmSim( - DCMotor.getNeo550(1), + HOOD_GEARBOX, HOOD_SHOOTER_GEAR_RATIO, - 0.054195108, - 0.2400046, + HOOD_MOMENT_OF_INERTIA, + HOOD_ARM_LENGTH_METERS, Units.degreesToRadians(MIN_ANGLE), Units.degreesToRadians(MAX_ANGLE), - 0.795601019, + HOOD_ARM_MASS_KG, true); } } @@ -151,12 +151,19 @@ public double calculateAngleFromDist(double dist) { @Override public void simulationPeriodic() { + + // Sets input voltage for the sim + singleJointedArmSim.setInputVoltage( m_hoodMotor.getMotorOutputVoltage() ); + // Updates simulation every 20 ms + singleJointedArmSim.update(0.02); + // Updates angle + double angle = singleJointedArmSim.getAngleRads()/(2*Math.PI) * 360; m_encoder.setPosition((angle-MIN_ANGLE) / (2 * Math.PI)); diff --git a/src/main/java/frc/team3128/subsystems/Hopper.java b/src/main/java/frc/team3128/subsystems/Hopper.java index 99fd3b54..044bbdfc 100644 --- a/src/main/java/frc/team3128/subsystems/Hopper.java +++ b/src/main/java/frc/team3128/subsystems/Hopper.java @@ -5,6 +5,7 @@ import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Timer; @@ -91,21 +92,6 @@ private void configEncoders() { @Override public void periodic() { SmartDashboard.putNumber("Hopper Enc", m_encoder.getDistance()); - - double dt = timer.get(); - - timer.reset(); - - double angularVelocity = m_encoder.getRate() / 4 * 2 * Math.PI; - - double angularAcceleration = (angularVelocity - prevAngularVelocity) / dt; - - double torque = 0.71-(0.71/18734.05)*angularVelocity; - - double momentOfInertia = torque / angularAcceleration; - - SmartDashboard.putNumber("Hopper Moment of Inertia", momentOfInertia * 10000); - } /** @@ -164,11 +150,17 @@ public void resetBallCount() { @Override public void simulationPeriodic() { + FlywheelSim prevFlywheelSim = noBallSim; + if (ballCount > 0) { - m_flywheelSim = noBallSim; + m_flywheelSim = ballSim; } else { - m_flywheelSim = ballSim; + m_flywheelSim = noBallSim; + } + + if (m_flywheelSim != prevFlywheelSim) { + m_flywheelSim.setState(VecBuilder.fill(prevFlywheelSim.getAngularVelocityRadPerSec())); } m_flywheelSim.setInputVoltage( diff --git a/src/main/java/frc/team3128/subsystems/Shooter.java b/src/main/java/frc/team3128/subsystems/Shooter.java index 3fe1c007..f7a90226 100644 --- a/src/main/java/frc/team3128/subsystems/Shooter.java +++ b/src/main/java/frc/team3128/subsystems/Shooter.java @@ -42,7 +42,8 @@ public Shooter() { //Robot is a simulation if(RobotBase.isSimulation()){ m_shooterSim = new FlywheelSim( - SHOOTER_CHAR, SHOOTER_GEARBOX, SHOOTER_GEARING + SHOOTER_GEARBOX, SHOOTER_GEARING, SHOOTER_MOMENT_OF_INERTIA + ); } } @@ -158,12 +159,7 @@ public void simulationPeriodic() { ); m_shooterSim.update(0.02); - m_leftShooter.setSimVelocity(m_shooterSim.getAngularVelocityRadPerSec() * SHOOTER_RADIUS_METERS); - //m_rightShooter.setQuadSimVelocity(m_shooterSim.getAngularVelocityRadPerSec() * SHOOTER_RADIUS_METERS); - - // SmartDashboard.putNumber("test", m_leftShooter.getMotorOutputVoltage()); - // SmartDashboard.putString("pogger", String.valueOf(m_shooterSim.getAngularVelocityRadPerSec())); - SmartDashboard.putNumber("shooter RPMSIM", m_shooterSim.getAngularVelocityRadPerSec() * 60 / (2*Math.PI)); + m_leftShooter.setSimVelocity(m_shooterSim.getAngularVelocityRadPerSec() * 60 / (2*Math.PI) / FALCON_NUpS_TO_RPM); } From 73e31af074260c6408f58e3c78e8ae2bd3dd1c46 Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Tue, 4 Oct 2022 22:04:04 -0700 Subject: [PATCH 14/18] Climber Simulation --- src/main/java/frc/team3128/Constants.java | 7 +- .../team3128/commands/CmdClimbEncoder.java | 18 ++ .../commands/CmdClimbTraversalGyro.java | 9 +- .../utility/linear_systems/PendulumSim.java | 53 ++++++ .../java/frc/team3128/subsystems/Climber.java | 167 +++++++++++++++++- .../java/frc/team3128/subsystems/Hopper.java | 2 +- .../team3128/subsystems/NAR_Drivetrain.java | 9 + .../java/frc/team3128/subsystems/Shooter.java | 4 +- 8 files changed, 262 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/team3128/common/utility/linear_systems/PendulumSim.java diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 1a2c173a..f3524b5d 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -156,8 +156,8 @@ public static class HopperConstants { public static final DCMotor GEARBOX = DCMotor.getVex775Pro(1); public static final double HOPPER_MOTOR_GEAR_RATIO = 0.25; - public static final double HOPPER_MOMENT_OF_INERTIA = 0.04e-4; // 0.04e-4 - public static final double HOPPER_MOMENT_OF_INERTIA_BALL = 0.16e-4; // 0.16e-4 + public static final double HOPPER_MOMENT_OF_INERTIA = 0.04; // 0.04e-4 + public static final double HOPPER_MOMENT_OF_INERTIA_BALL = 0.16; // 0.16e-4 } public static class IntakeConstants { @@ -295,5 +295,8 @@ public static class VisionConstants { } public static class SimConstants { public static final double SIM_HUB_DISTANCE = 70; //degrees + public static final double CLIMBER_LONG_ARM_LENGTH = 0.78 * 39.3701; //inches + public static final double CLIMBER_SHORT_ARM_LENGTH = 0.2 * 39.3701; //inches + public static final double GRAVITATIONAL_CONSTANT = 9.8; // m/s^2 } } diff --git a/src/main/java/frc/team3128/commands/CmdClimbEncoder.java b/src/main/java/frc/team3128/commands/CmdClimbEncoder.java index 4cbab551..a46efa9e 100644 --- a/src/main/java/frc/team3128/commands/CmdClimbEncoder.java +++ b/src/main/java/frc/team3128/commands/CmdClimbEncoder.java @@ -1,7 +1,11 @@ package frc.team3128.commands; +import edu.wpi.first.wpilibj.IterativeRobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import static frc.team3128.Constants.ClimberConstants.*; + +import frc.team3128.Robot; import frc.team3128.subsystems.Climber; public class CmdClimbEncoder extends CommandBase{ @@ -23,8 +27,10 @@ public CmdClimbEncoder(double distance) { @Override public void initialize() { + SmartDashboard.putNumber("Climber Tick Goal", this.distance); // want to go higher than current place = go up if (distance > m_climber.getCurrentTicks()) { + System.out.println("Extending"); m_climber.bothExtend(); isGoingDown = false; } @@ -34,9 +40,21 @@ public void initialize() { } } + + + @Override public void end(boolean interrupted) { m_climber.bothStop(); + + if(Robot.isSimulation()) { + if(distance == CLIMB_ENC_DIAG_EXTENSION) { + m_climber.latchLongArm(); + } + else if(distance == m_climber.getDesiredTicks(SMALL_VERTICAL_DISTANCE)) { + m_climber.switchToShortArm(); + } + } } @Override diff --git a/src/main/java/frc/team3128/commands/CmdClimbTraversalGyro.java b/src/main/java/frc/team3128/commands/CmdClimbTraversalGyro.java index 48114969..4b6194dd 100644 --- a/src/main/java/frc/team3128/commands/CmdClimbTraversalGyro.java +++ b/src/main/java/frc/team3128/commands/CmdClimbTraversalGyro.java @@ -1,5 +1,6 @@ package frc.team3128.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -25,16 +26,19 @@ public CmdClimbTraversalGyro() { new WaitCommand(.25), new CmdClimbEncoder(m_climber.getDesiredTicks(SMALL_VERTICAL_DISTANCE)), + //Sim: Latch Short Arm //piston extend new InstantCommand(() -> m_climber.extendPiston()), //elev extend new CmdClimbEncoder(CLIMB_ENC_DIAG_EXTENSION), + //Sim: Latch Long Arm new WaitCommand(0.25), //piston retract + //Sim: Unlatch Short Arm new InstantCommand(() -> m_climber.retractPiston()), //elev retract @@ -43,7 +47,8 @@ public CmdClimbTraversalGyro() { //Climber is manually fully retracted on High Bar new WaitCommand(0.25), - + + //Sim: Latch Short Arm new CmdClimbEncoder(m_climber.getDesiredTicks(SMALL_VERTICAL_DISTANCE)), new InstantCommand(() -> m_climber.extendPiston()), @@ -52,8 +57,10 @@ public CmdClimbTraversalGyro() { //elev extend new CmdClimbEncoder(CLIMB_ENC_DIAG_EXTENSION), + //Sim: Latch Long Arm //piston extend + //Sim: Unlatch Short Arm new InstantCommand(() -> m_climber.retractPiston()), //elev retract diff --git a/src/main/java/frc/team3128/common/utility/linear_systems/PendulumSim.java b/src/main/java/frc/team3128/common/utility/linear_systems/PendulumSim.java new file mode 100644 index 00000000..36188726 --- /dev/null +++ b/src/main/java/frc/team3128/common/utility/linear_systems/PendulumSim.java @@ -0,0 +1,53 @@ +package frc.team3128.common.utility.linear_systems; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; +import edu.wpi.first.math.numbers.N0; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.numbers.N4; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import frc.team3128.Constants; + +/** + * Physics Simulation class to simulate a single pendulum + * + * @author Sohan :o + */ +public class PendulumSim extends LinearSystemSim { + + private static final double G = 9.8; //Gravitational Constant + private static Matrix b = new MatBuilder(Nat.N2(), Nat.N0()).fill(); + private static Matrix c = Matrix.eye(Nat.N2()); + private static Matrix d = new MatBuilder(Nat.N2(), Nat.N0()).fill(); + + /** + * + * @param initialAngle The initial angle that the robot forms with the downward vertical in degrees (counter-clockwise) + * @param armLength Length of the arm (in meters. I like SI Units) + */ + public PendulumSim(double initialAngle, double armLength) { + super(constructSystem(initialAngle, armLength)); + this.setState(new MatBuilder(Nat.N4(), Nat.N1()).fill(initialAngle, armLength, 0, 0)); + } + + private static LinearSystem constructSystem(double initialAngle, double armLength) { + initialAngle = 0; + Matrix aN = new MatBuilder(Nat.N4(), Nat.N4()) + .fill(0,0,1,0, + 0,0,0,1, + (-1*Constants.SimConstants.GRAVITATIONAL_CONSTANT*Math.cos(initialAngle)/armLength), + (Constants.SimConstants.GRAVITATIONAL_CONSTANT*Math.sin(initialAngle)/(armLength*armLength)),0,0, + 0,0,0,0 + ); + + Matrix bN = new MatBuilder(Nat.N4(), Nat.N1()).fill(0,0,0,1); + Matrix cN = Matrix.eye(Nat.N4()); + Matrix dN = new MatBuilder(Nat.N4(), Nat.N1()).fill(0,0,0,0); + return new LinearSystem<>(aN,bN,cN,dN); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/team3128/subsystems/Climber.java b/src/main/java/frc/team3128/subsystems/Climber.java index 7678777e..6501c6da 100644 --- a/src/main/java/frc/team3128/subsystems/Climber.java +++ b/src/main/java/frc/team3128/subsystems/Climber.java @@ -8,13 +8,27 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.*; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import static frc.team3128.Constants.ClimberConstants.*; import static frc.team3128.common.hardware.motorcontroller.MotorControllerConstants.*; + +import java.time.chrono.ThaiBuddhistChronology; +import java.util.ArrayList; +import java.util.List; + +import frc.team3128.Constants; +import frc.team3128.Robot; import frc.team3128.common.hardware.motorcontroller.NAR_TalonFX; +import frc.team3128.common.utility.linear_systems.PendulumSim; /** * Class for the Climber Subsystem @@ -27,12 +41,36 @@ public class Climber extends SubsystemBase { private DoubleSolenoid m_climberSolenoid; private NAR_TalonFX m_leftMotor, m_rightMotor; - public Climber() { + //Simulation + private Timer deltaTimer; + private PendulumSim pendulumSim; + private boolean isConnectedToTwoRods = false; + private boolean currentArmIsShort = false; + private int currentLongRod = 0; + + private static List rodDisplacements = new ArrayList(); + + //Translations in Inches + static { + rodDisplacements.add(new Translation2d(0, 0)); + rodDisplacements.add(new Translation2d(1.07*39.3701, 0.29*39.3701)); + rodDisplacements.add(new Translation2d(1.68*39.3701, 0.68*39.3701)); + rodDisplacements.add(new Translation2d(2.29*39.3701, 1.07*39.3701)); + } + + public Climber() { configMotors(); configPneumatics(); resetLeftEncoder(); + + if(Robot.isSimulation()) { + deltaTimer = new Timer(); + deltaTimer.start(); + + pendulumSim = new PendulumSim(0, Constants.SimConstants.CLIMBER_SHORT_ARM_LENGTH/39.3701); + } } public static synchronized Climber getInstance() { @@ -134,6 +172,11 @@ public void extendPiston(){ */ public void retractPiston(){ m_climberSolenoid.set(kReverse); + + if(Robot.isSimulation()) { + this.unlatchShortArm(); + } + } /** @@ -158,4 +201,126 @@ public double getCurrentTicks() { public void resetLeftEncoder() { m_leftMotor.setEncoderPosition(0); } + + /** + * Run when the robot switches to the short arm after escalating itself towards the rod + * Function will transfer the angle and angular velocity data from the long arm pendulum simulation + * into the short arm. + * + * Assumes that the current PendulumSim is the long arm + */ + public void switchToShortArm() { + pendulumSim.setState(new MatBuilder(Nat.N4(), Nat.N1()).fill( + pendulumSim.getOutput(0), + Constants.SimConstants.CLIMBER_SHORT_ARM_LENGTH, + pendulumSim.getOutput(2), + 0 + )); + currentArmIsShort = true; + } + + /** + * Run when the long arm latches to the next rod (while the short arm is still attached to the last rod) + * Function will set the angular velocity of the short arm pendulum to 0 to simulate the long arm stopping the + * angular motion. + * + * Assumes that the current PendulumSim is the short arm + */ + public void latchLongArm() { + isConnectedToTwoRods = true; + if(currentLongRod < rodDisplacements.size()-1) { + double xDisplacement = rodDisplacements.get(currentLongRod+1).getX()-rodDisplacements.get(currentLongRod).getX(); + double yDisplacement = rodDisplacements.get(currentLongRod+1).getY()-rodDisplacements.get(currentLongRod).getY(); + double displacement = Math.sqrt(Math.pow(Math.abs(xDisplacement), 2)+Math.pow(Math.abs(yDisplacement), 2)); + double newAngle = -1*Math.acos((displacement*displacement+Constants.SimConstants.CLIMBER_LONG_ARM_LENGTH*Constants.SimConstants.CLIMBER_LONG_ARM_LENGTH + -Constants.SimConstants.CLIMBER_SHORT_ARM_LENGTH*Constants.SimConstants.CLIMBER_LONG_ARM_LENGTH)/(2*displacement*Constants.SimConstants.CLIMBER_LONG_ARM_LENGTH)); + + // pendulumSim.setState(new MatBuilder(Nat.N4(), Nat.N1()).fill( + // newAngle, + // Constants.SimConstants.CLIMBER_LONG_ARM_LENGTH, + // 0, + // 0)); + + pendulumSim = new PendulumSim(newAngle, Constants.SimConstants.CLIMBER_LONG_ARM_LENGTH/39.3701); + System.out.println("NEW ANGLE: "+newAngle); + } + } + + /** + * + * Run when the short arm unlatches from the previous rod, allowing the robot to rotate once again. + */ + public void unlatchShortArm() { + //Calculates the angle with the new rod based on the new rod's displacement from the old rod and the robot's angle with the old rod + //If this bugs, the most obvious bug I can think of is that the angle is in radians instead of degrees + + //double newAngle = Math.asin(-1*(Constants.SimConstants.CLIMBER_SHORT_ARM_LENGTH*Math.sin(90-pendulumSim.getOutput(0)))/(Constants.SimConstants.CLIMBER_LONG_ARM_LENGTH)); + isConnectedToTwoRods = false; + currentArmIsShort = false; + } + + public void getSimAngle() { + if(Robot.isSimulation()) { + pendulumSim.getOutput(0); + } + throw new RuntimeException(); + } + + + /* + * How simulation works for climber (I made some assumptions because I could) + * The angle on pendulumSim corresponds to the angle the long arm makes with the + * vertical of the rod it is attached to (the "vertical of a rod" is just the line going from the rod down to the floor) + * + * pendulumSim also simulates the length of the arm (based on the motor output voltage) + * It is assumed that voltage and velocity of the arm changing length is basically linear + * + * When the robot is changing between rods, the boolean isConnectedToTwoRods should be true + * when both the short and long arms are on a rod (and the angle shouldn't change) + * + * It is assumed that the short arm, the long arm, and the line between the previous and new rods form a + * triangle when it is in the both-arms-connected-to-rods state + * + * + */ + @Override + public void simulationPeriodic() { + + + double voltageScaleFactor = 100000; //Proportion between voltage and velocity (assuming proportional movement) + double encoderLengthFactor = 1/1000; + + double dt = deltaTimer.get(); + + //Assuming Linear Movement of the Climber Motors (proportional to voltage) + + double calculatedArmPosition = m_leftMotor.getSelectedSensorPosition()+voltageScaleFactor*m_leftMotor.getMotorOutputVoltage()*dt; + SmartDashboard.putNumber("Maximum Arm Length", this.getDesiredTicks(Constants.SimConstants.CLIMBER_LONG_ARM_LENGTH)); + m_leftMotor.setSelectedSensorPosition(Math.min(calculatedArmPosition, this.getDesiredTicks(Constants.SimConstants.CLIMBER_LONG_ARM_LENGTH))); + if(!currentArmIsShort) { + pendulumSim.setInput(-1*voltageScaleFactor*encoderLengthFactor*m_leftMotor.getMotorOutputVoltage()); + } + else { + pendulumSim.setInput(0); + } + + + + if(!isConnectedToTwoRods) { + pendulumSim.update(dt); + } + + //Should be the angle with the long arm and the corresponding rod + double angle = pendulumSim.getOutput(0); + + + SmartDashboard.putNumber("Angle of Robot with Rod", angle*(360/2*Math.PI)); + SmartDashboard.putNumber("Climber Extension Length", pendulumSim.getOutput(1)); + SmartDashboard.putNumber("Climber Left Motor", m_leftMotor.getMotorOutputVoltage()); + SmartDashboard.putNumber("Climber Right Motor", m_rightMotor.getMotorOutputVoltage()); + SmartDashboard.putString("Climber Solenoid", m_climberSolenoid.get().toString()); + SmartDashboard.putNumber("Climber Ticks", getCurrentTicks()); + + deltaTimer.reset(); + } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/subsystems/Hopper.java b/src/main/java/frc/team3128/subsystems/Hopper.java index 044bbdfc..643861e3 100644 --- a/src/main/java/frc/team3128/subsystems/Hopper.java +++ b/src/main/java/frc/team3128/subsystems/Hopper.java @@ -150,7 +150,7 @@ public void resetBallCount() { @Override public void simulationPeriodic() { - FlywheelSim prevFlywheelSim = noBallSim; + FlywheelSim prevFlywheelSim = m_flywheelSim; if (ballCount > 0) { m_flywheelSim = ballSim; diff --git a/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java b/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java index 54f5e649..b3e670a8 100644 --- a/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java +++ b/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java @@ -21,6 +21,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import static frc.team3128.Constants.DriveConstants.*; + +import frc.team3128.Robot; import frc.team3128.common.hardware.motorcontroller.NAR_TalonFX; /** @@ -66,6 +68,7 @@ public NAR_Drivetrain(){ odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading())); field = new Field2d(); + SmartDashboard.putData("Field", field); resetEncoders(); } @@ -132,6 +135,7 @@ public void periodic() { SmartDashboard.putNumber("Left Encoder Speed (m/s)", getLeftEncoderSpeed()); SmartDashboard.putNumber("Right Encoder (m/s)", getRightEncoderSpeed()); SmartDashboard.putString("getPose()", getPose().toString()); + SmartDashboard.putNumber("Gyro", getHeading()); SmartDashboard.putData("Field", field); @@ -170,6 +174,11 @@ public double getHeading() { } public double getPitch() { + + if(Robot.isSimulation()) { + + } + return gyro.getRoll(); } diff --git a/src/main/java/frc/team3128/subsystems/Shooter.java b/src/main/java/frc/team3128/subsystems/Shooter.java index f7a90226..66766f26 100644 --- a/src/main/java/frc/team3128/subsystems/Shooter.java +++ b/src/main/java/frc/team3128/subsystems/Shooter.java @@ -87,6 +87,7 @@ public void periodic() { SmartDashboard.putNumber("Shooter Setpoint", getSetpoint()); SmartDashboard.putNumber("Shooter RPM", getMeasurement()); SmartDashboard.putBoolean("Shooter isReady", isReady()); + SmartDashboard.putBoolean("atSetpoint", getController().atSetpoint()); } @@ -159,8 +160,7 @@ public void simulationPeriodic() { ); m_shooterSim.update(0.02); - m_leftShooter.setSimVelocity(m_shooterSim.getAngularVelocityRadPerSec() * 60 / (2*Math.PI) / FALCON_NUpS_TO_RPM); - + m_leftShooter.setSimVelocity(m_shooterSim.getAngularVelocityRadPerSec() * 60 / (2*Math.PI) / FALCON_NUpS_TO_RPM); } /** From 219597974214ce73d863293b1040a59af94d768d Mon Sep 17 00:00:00 2001 From: Arav Chadha Date: Fri, 7 Oct 2022 16:46:02 -0700 Subject: [PATCH 15/18] Simulation Code Simulation code worked on over the summer 2022 --- src/main/java/frc/team3128/Constants.java | 2 +- .../frc/team3128/commands/CmdShootDist.java | 2 +- .../java/frc/team3128/subsystems/Hopper.java | 34 +++++++++++++------ .../java/frc/team3128/subsystems/Shooter.java | 7 +++- 4 files changed, 32 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 1a2c173a..65fe1959 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -156,7 +156,7 @@ public static class HopperConstants { public static final DCMotor GEARBOX = DCMotor.getVex775Pro(1); public static final double HOPPER_MOTOR_GEAR_RATIO = 0.25; - public static final double HOPPER_MOMENT_OF_INERTIA = 0.04e-4; // 0.04e-4 + public static final double HOPPER_MOMENT_OF_INERTIA = 0.0006; // 0.04e-4 public static final double HOPPER_MOMENT_OF_INERTIA_BALL = 0.16e-4; // 0.16e-4 } diff --git a/src/main/java/frc/team3128/commands/CmdShootDist.java b/src/main/java/frc/team3128/commands/CmdShootDist.java index bb00c2bd..1db35514 100644 --- a/src/main/java/frc/team3128/commands/CmdShootDist.java +++ b/src/main/java/frc/team3128/commands/CmdShootDist.java @@ -48,7 +48,7 @@ public void execute() { Pose2d pose = NAR_Drivetrain.getInstance().getPose().relativeTo(HUB_POS); - dist = Units.metersToInches(Math.sqrt(pose.getX() * pose.getX() + pose.getY() * pose.getY())) - HUB_RADIUS; // meters + dist = Units.metersToInches(Math.sqrt(pose.getX() * pose.getX() + pose.getY() * pose.getY())) - HUB_RADIUS; // inches SmartDashboard.putNumber("Distance from hub (meters)", Units.inchesToMeters(dist)); } diff --git a/src/main/java/frc/team3128/subsystems/Hopper.java b/src/main/java/frc/team3128/subsystems/Hopper.java index 044bbdfc..0341ca0d 100644 --- a/src/main/java/frc/team3128/subsystems/Hopper.java +++ b/src/main/java/frc/team3128/subsystems/Hopper.java @@ -67,18 +67,18 @@ public static synchronized Hopper getInstance() { */ private void configMotors() { m_hopper1 = new NAR_TalonSRX(HOPPER_MOTOR_ID); - m_hopper2 = new NAR_TalonSRX(HOPPER_MOTOR_2_ID); + // m_hopper2 = new NAR_TalonSRX(HOPPER_MOTOR_2_ID); m_hopper1.setNeutralMode(NeutralMode.Coast); - m_hopper2.setNeutralMode(NeutralMode.Coast); + // m_hopper2.setNeutralMode(NeutralMode.Coast); m_hopper1.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 45); m_hopper1.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 45); m_hopper1.setControlFramePeriod(ControlFrame.Control_3_General, 20); - m_hopper2.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 45); - m_hopper2.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 45); - m_hopper2.setControlFramePeriod(ControlFrame.Control_3_General, 20); + // m_hopper2.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 45); + // m_hopper2.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 45); + // m_hopper2.setControlFramePeriod(ControlFrame.Control_3_General, 20); } /** @@ -92,6 +92,20 @@ private void configEncoders() { @Override public void periodic() { SmartDashboard.putNumber("Hopper Enc", m_encoder.getDistance()); + + double dt = timer.get(); + + timer.reset(); + + double angularVelocity = m_encoder.getRate() / 4 * 2 * Math.PI; + + double angularAcceleration = (angularVelocity - prevAngularVelocity) / dt; + + double torque = 0.71-(0.71/18734.05) * angularVelocity * 30 / Math.PI; + + double momentOfInertia = torque / angularAcceleration; + + SmartDashboard.putNumber("Hopper Moment of Inertia", momentOfInertia * 10000); } /** @@ -99,7 +113,7 @@ public void periodic() { */ public void runHopper() { m_hopper1.set(HOPPER_MOTOR_POWER); - m_hopper2.set(HOPPER_MOTOR_2_POWER); + // m_hopper2.set(HOPPER_MOTOR_2_POWER); } /** @@ -107,7 +121,7 @@ public void runHopper() { */ public void runHopper(double power) { m_hopper1.set(power); - m_hopper2.set(power + 0.1); + // m_hopper2.set(power + 0.1); } /** @@ -122,7 +136,7 @@ public void reverseHopper(double power) { */ public void stopHopper() { m_hopper1.set(0); - m_hopper2.set(0); + // m_hopper2.set(0); } /** @@ -169,9 +183,9 @@ public void simulationPeriodic() { m_flywheelSim.update(0.02); - SmartDashboard.putNumber("Hopper 1 Motor RPM", m_flywheelSim.getAngularVelocityRPM()); + SmartDashboard.putNumber("Hopper 1 Motor RPM", m_flywheelSim.getAngularVelocityRPM()/4); SmartDashboard.putNumber("Hopper 1 Motor Voltage", m_hopper1.getMotorOutputVoltage()); - SmartDashboard.putNumber("Hopper 2 Motor Voltage", m_hopper2.getMotorOutputVoltage()); + // SmartDashboard.putNumber("Hopper 2 Motor Voltage", m_hopper2.getMotorOutputVoltage()); SmartDashboard.putNumber("Hopper Ball Count", ballCount); } diff --git a/src/main/java/frc/team3128/subsystems/Shooter.java b/src/main/java/frc/team3128/subsystems/Shooter.java index f7a90226..d1f8617e 100644 --- a/src/main/java/frc/team3128/subsystems/Shooter.java +++ b/src/main/java/frc/team3128/subsystems/Shooter.java @@ -7,6 +7,8 @@ import static frc.team3128.Constants.ShooterConstants.*; import frc.team3128.ConstantsInt; +import frc.team3128.Robot; + import static frc.team3128.Constants.ConversionConstants.*; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; @@ -124,7 +126,7 @@ public double getMeasurement() { */ @Override protected void useOutput(double output, double setpoint) { - double ff = kF * setpoint; + double ff = Robot.isSimulation() ? kF * setpoint * 1.678 : kF * setpoint; double voltageOutput = output + ff; if (getController().atSetpoint() && (setpoint != 0)) { @@ -157,9 +159,12 @@ public void simulationPeriodic() { m_shooterSim.setInput( m_leftShooter.getMotorOutputVoltage() ); + m_shooterSim.update(0.02); m_leftShooter.setSimVelocity(m_shooterSim.getAngularVelocityRadPerSec() * 60 / (2*Math.PI) / FALCON_NUpS_TO_RPM); + + SmartDashboard.putNumber("Shooter Motor Voltage", m_leftShooter.getMotorOutputVoltage()); } From fae77b0f8edcb10943af5ada553240df80e03bcf Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Mon, 10 Oct 2022 17:37:55 -0700 Subject: [PATCH 16/18] Fixed Drive Train Simulation --- src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java b/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java index b3e670a8..514db8d7 100644 --- a/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java +++ b/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java @@ -159,6 +159,8 @@ public void simulationPeriodic() { rightLeader.setSimPosition(robotDriveSim.getRightPositionMeters() / DRIVE_NU_TO_METER); rightLeader.setSimVelocity(robotDriveSim.getRightVelocityMetersPerSecond() / DRIVE_NU_TO_METER); + //SmartDashboard.putNumber("Left Sim Voltage", leftLeader.getMotorOutputVoltage()); + //SmartDashboard.putNumber("Right Sim Voltage", rightLeader.getMotorOutputVoltage()); SmartDashboard.putNumber("Left Sim Speed", leftLeader.getSelectedSensorVelocity()); SmartDashboard.putNumber("Right Sim Speed", rightLeader.getSelectedSensorVelocity()); From d8e46ddb4e937818f60243946c17c80df842278e Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Fri, 14 Oct 2022 15:54:10 -0700 Subject: [PATCH 17/18] Fixed Robot not turning on Sim Field --- src/main/java/frc/team3128/RobotContainer.java | 4 ++-- .../java/frc/team3128/commands/CmdArcadeDrive.java | 2 ++ .../java/frc/team3128/commands/CmdShootDist.java | 3 +++ .../frc/team3128/subsystems/NAR_Drivetrain.java | 13 ++++++++++--- 4 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 95820086..4d893f08 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -92,8 +92,8 @@ public RobotContainer() { m_commandScheduler.setDefaultCommand(m_drive, new CmdArcadeDrive(m_rightStick::getY, m_rightStick::getTwist, m_rightStick::getThrottle)); initDashboard(); - // configureButtonBindings(); - configureDriverOperator(); + configureButtonBindings(); + // configureDriverOperator(); if(RobotBase.isSimulation()) DriverStation.silenceJoystickConnectionWarning(true); diff --git a/src/main/java/frc/team3128/commands/CmdArcadeDrive.java b/src/main/java/frc/team3128/commands/CmdArcadeDrive.java index 60559b96..2bc39c04 100644 --- a/src/main/java/frc/team3128/commands/CmdArcadeDrive.java +++ b/src/main/java/frc/team3128/commands/CmdArcadeDrive.java @@ -3,6 +3,7 @@ import java.util.function.DoubleSupplier; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import static frc.team3128.Constants.DriveConstants.*; import frc.team3128.subsystems.NAR_Drivetrain; @@ -28,6 +29,7 @@ public CmdArcadeDrive(DoubleSupplier xSpeed, DoubleSupplier turn, DoubleSupplier m_throttle = throttle; addRequirements(m_drivetrain); + } @Override diff --git a/src/main/java/frc/team3128/commands/CmdShootDist.java b/src/main/java/frc/team3128/commands/CmdShootDist.java index a14f5010..6b7b670e 100644 --- a/src/main/java/frc/team3128/commands/CmdShootDist.java +++ b/src/main/java/frc/team3128/commands/CmdShootDist.java @@ -7,6 +7,7 @@ import frc.team3128.Robot; import frc.team3128.common.utility.Log; import frc.team3128.subsystems.Hood; +import frc.team3128.subsystems.Hopper; import frc.team3128.subsystems.LimelightSubsystem; import frc.team3128.subsystems.NAR_Drivetrain; import frc.team3128.subsystems.Shooter; @@ -17,6 +18,7 @@ public class CmdShootDist extends CommandBase { private Shooter shooter; private LimelightSubsystem limelights; private Hood hood; + private Hopper hopper; /** * Shoot through calculating the approximate distance to target via the limelight @@ -27,6 +29,7 @@ public CmdShootDist() { shooter = Shooter.getInstance(); limelights = LimelightSubsystem.getInstance(); hood = Hood.getInstance(); + hopper = Hopper.getInstance(); addRequirements(shooter, hood); } diff --git a/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java b/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java index 5337e57e..5fb3342d 100644 --- a/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java +++ b/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java @@ -168,16 +168,23 @@ public void simulationPeriodic() { //SmartDashboard.putNumber("Right Sim Voltage", rightLeader.getMotorOutputVoltage()); SmartDashboard.putNumber("Left Sim Speed", leftLeader.getSelectedSensorVelocity()); SmartDashboard.putNumber("Right Sim Speed", rightLeader.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Sim X", robotDriveSim.getPose().getX()); + SmartDashboard.putNumber("Sim Y", robotDriveSim.getPose().getY()); - int dev = SimDeviceDataJNI.getSimDeviceHandle("navX-Sensor[0]"); - SimDouble angle = new SimDouble(SimDeviceDataJNI.getSimValueHandle(dev, "Yaw")); + int dev = SimDeviceDataJNI.getSimDeviceHandle("CANGyro:Pigeon 2[0]"); + SimDouble angle = new SimDouble(SimDeviceDataJNI.getSimValueHandle(dev, "yaw")); angle.set(robotDriveSim.getHeading().getDegrees()); // @Nathan: I tested this out, this seems to work. This preserves parity w/ the real robot in angle, odometry - SmartDashboard.putNumber("Sim Gyro", angle.get()); + SmartDashboard.putNumber("Sim Gyro", robotDriveSim.getHeading().getDegrees()); } public double getHeading() { //gyro.getYaw uses CW as positive //WPI_Pigeon2 negates this + + if(Robot.isSimulation()) { + return robotDriveSim.getHeading().getDegrees(); + } + return gyro.getAngle(); } From e6a9f9ffe4d364cdc3deaaf57a1fba04bce0f839 Mon Sep 17 00:00:00 2001 From: sohanagarkar <33811100+FieryXY@users.noreply.github.com> Date: Fri, 14 Oct 2022 16:05:37 -0700 Subject: [PATCH 18/18] Fixed Sim Field Again but with more Abstraction --- src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java b/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java index 5fb3342d..68a0ba8b 100644 --- a/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java +++ b/src/main/java/frc/team3128/subsystems/NAR_Drivetrain.java @@ -172,7 +172,7 @@ public void simulationPeriodic() { SmartDashboard.putNumber("Sim Y", robotDriveSim.getPose().getY()); int dev = SimDeviceDataJNI.getSimDeviceHandle("CANGyro:Pigeon 2[0]"); - SimDouble angle = new SimDouble(SimDeviceDataJNI.getSimValueHandle(dev, "yaw")); + SimDouble angle = new SimDouble(SimDeviceDataJNI.getSimValueHandle(dev, "rawYawInput")); angle.set(robotDriveSim.getHeading().getDegrees()); // @Nathan: I tested this out, this seems to work. This preserves parity w/ the real robot in angle, odometry SmartDashboard.putNumber("Sim Gyro", robotDriveSim.getHeading().getDegrees()); } @@ -181,9 +181,9 @@ public double getHeading() { //gyro.getYaw uses CW as positive //WPI_Pigeon2 negates this - if(Robot.isSimulation()) { - return robotDriveSim.getHeading().getDegrees(); - } + // if(Robot.isSimulation()) { + // return robotDriveSim.getHeading().getDegrees(); + // } return gyro.getAngle(); }