added values from calculations and cad.
This commit is contained in:
@@ -69,7 +69,7 @@
|
|||||||
},
|
},
|
||||||
"prevControl": {
|
"prevControl": {
|
||||||
"x": 8.08578683847011,
|
"x": 8.08578683847011,
|
||||||
"y": 0.4907704016028375
|
"y": 0.49077040160283747
|
||||||
},
|
},
|
||||||
"nextControl": {
|
"nextControl": {
|
||||||
"x": 8.318287488908608,
|
"x": 8.318287488908608,
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
{
|
{
|
||||||
"robotWidth": 0.9,
|
"robotWidth": 0.921,
|
||||||
"robotLength": 0.9,
|
"robotLength": 0.787,
|
||||||
"holonomicMode": true,
|
"holonomicMode": true,
|
||||||
"pathFolders": [],
|
"pathFolders": [],
|
||||||
"autoFolders": [],
|
"autoFolders": [],
|
||||||
@@ -9,24 +9,26 @@
|
|||||||
"defaultMaxAngVel": 540.0,
|
"defaultMaxAngVel": 540.0,
|
||||||
"defaultMaxAngAccel": 720.0,
|
"defaultMaxAngAccel": 720.0,
|
||||||
"defaultNominalVoltage": 12.0,
|
"defaultNominalVoltage": 12.0,
|
||||||
"robotMass": 74.088,
|
"robotMass": 64.864,
|
||||||
"robotMOI": 6.883,
|
"robotMOI": 37.809,
|
||||||
"robotTrackwidth": 0.546,
|
"robotTrackwidth": 0.546,
|
||||||
"driveWheelRadius": 0.048,
|
"driveWheelRadius": 0.051,
|
||||||
"driveGearing": 5.143,
|
"driveGearing": 6.122,
|
||||||
"maxDriveSpeed": 5.45,
|
"maxDriveSpeed": 4.66,
|
||||||
"driveMotorType": "krakenX60",
|
"driveMotorType": "krakenX60",
|
||||||
"driveCurrentLimit": 60.0,
|
"driveCurrentLimit": 65.0,
|
||||||
"wheelCOF": 1.2,
|
"wheelCOF": 1.2,
|
||||||
"flModuleX": 0.273,
|
"flModuleX": 0.238,
|
||||||
"flModuleY": 0.273,
|
"flModuleY": 0.3015,
|
||||||
"frModuleX": 0.273,
|
"frModuleX": 0.238,
|
||||||
"frModuleY": -0.273,
|
"frModuleY": -0.3015,
|
||||||
"blModuleX": -0.273,
|
"blModuleX": -0.238,
|
||||||
"blModuleY": 0.273,
|
"blModuleY": 0.3015,
|
||||||
"brModuleX": -0.273,
|
"brModuleX": -0.238,
|
||||||
"brModuleY": -0.273,
|
"brModuleY": -0.3015,
|
||||||
"bumperOffsetX": 0.0,
|
"bumperOffsetX": 0.0,
|
||||||
"bumperOffsetY": 0.0,
|
"bumperOffsetY": 0.0,
|
||||||
"robotFeatures": []
|
"robotFeatures": [
|
||||||
|
"{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.55,\"y\":0.0},\"size\":{\"width\":0.921,\"length\":0.305},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}"
|
||||||
|
]
|
||||||
}
|
}
|
||||||
@@ -14,7 +14,7 @@ import edu.wpi.first.math.util.Units;
|
|||||||
|
|
||||||
// TODO This is all hold over from 2025, does any of it need to change?
|
// TODO This is all hold over from 2025, does any of it need to change?
|
||||||
public class AutoConstants {
|
public class AutoConstants {
|
||||||
public static final double kMaxSpeedMetersPerSecond = 5;
|
public static final double kMaxSpeedMetersPerSecond = 4;
|
||||||
public static final double kMaxAccelerationMetersPerSecondSquared = 4;
|
public static final double kMaxAccelerationMetersPerSecondSquared = 4;
|
||||||
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
|
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
|
||||||
public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI;
|
public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI;
|
||||||
|
|||||||
@@ -9,8 +9,7 @@ import edu.wpi.first.math.numbers.N3;
|
|||||||
import edu.wpi.first.math.util.Units;
|
import edu.wpi.first.math.util.Units;
|
||||||
|
|
||||||
public class DrivetrainConstants {
|
public class DrivetrainConstants {
|
||||||
// TODO Hold over from 2025, adjust?
|
public static final double kMaxSpeedMetersPerSecond = 4.663;
|
||||||
public static final double kMaxSpeedMetersPerSecond = 4.125;
|
|
||||||
public static final double kMaxAngularSpeed = 2 * Math.PI;
|
public static final double kMaxAngularSpeed = 2 * Math.PI;
|
||||||
|
|
||||||
public static final double kTrackWidth = Units.inchesToMeters(23.75);
|
public static final double kTrackWidth = Units.inchesToMeters(23.75);
|
||||||
|
|||||||
@@ -17,7 +17,7 @@ public class HoodConstants {
|
|||||||
// TODO Real Values
|
// TODO Real Values
|
||||||
public static final int kMotorCANID = 0;
|
public static final int kMotorCANID = 0;
|
||||||
|
|
||||||
public static final double kConversionFactor = 0;
|
public static final double kConversionFactor = 3.0*147.0/8.0;
|
||||||
|
|
||||||
public static final double kP = 0;
|
public static final double kP = 0;
|
||||||
public static final double kI = 0;
|
public static final double kI = 0;
|
||||||
|
|||||||
@@ -7,8 +7,8 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
|||||||
public class IntakePivotConstants {
|
public class IntakePivotConstants {
|
||||||
// TODO Real values
|
// TODO Real values
|
||||||
public enum IntakePivotPosition {
|
public enum IntakePivotPosition {
|
||||||
kUp(0),
|
kUp(Math.toRadians(116.0)),
|
||||||
kDown(0);
|
kDown(Math.toRadians(0.0));
|
||||||
private double positionRadians;
|
private double positionRadians;
|
||||||
|
|
||||||
private IntakePivotPosition(double positionRadians) {
|
private IntakePivotPosition(double positionRadians) {
|
||||||
@@ -23,7 +23,7 @@ public class IntakePivotConstants {
|
|||||||
public static final int kLeftMotorCANID = 0;
|
public static final int kLeftMotorCANID = 0;
|
||||||
public static final int kRightMotorCANID = 0;
|
public static final int kRightMotorCANID = 0;
|
||||||
|
|
||||||
public static final double kConversionFactor = 0;
|
public static final double kConversionFactor = 60.0/11.0*60.0/18.0*38.0/16.0;
|
||||||
|
|
||||||
// Ultra conservative multiplier to prevent 1/8" lexan destruction, modify at your own peril
|
// Ultra conservative multiplier to prevent 1/8" lexan destruction, modify at your own peril
|
||||||
public static final double kMaxManualSpeedMultiplier = .3;
|
public static final double kMaxManualSpeedMultiplier = .3;
|
||||||
@@ -32,8 +32,9 @@ public class IntakePivotConstants {
|
|||||||
public static final double kI = 0;
|
public static final double kI = 0;
|
||||||
public static final double kD = 0;
|
public static final double kD = 0;
|
||||||
public static final double kS = 0;
|
public static final double kS = 0;
|
||||||
public static final double kV = 0;
|
public static final double kV = 5.26;
|
||||||
public static final double kA = 0;
|
public static final double kA = 0.05;
|
||||||
|
public static final double kG = 0.25;
|
||||||
|
|
||||||
public static final boolean kInvertMotors = false;
|
public static final boolean kInvertMotors = false;
|
||||||
|
|
||||||
@@ -51,17 +52,17 @@ public class IntakePivotConstants {
|
|||||||
.idleMode(kIdleMode)
|
.idleMode(kIdleMode)
|
||||||
.smartCurrentLimit(kCurrentLimit)
|
.smartCurrentLimit(kCurrentLimit)
|
||||||
.inverted(kInvertMotors);
|
.inverted(kInvertMotors);
|
||||||
KLeftMotorConfig.absoluteEncoder
|
KLeftMotorConfig.encoder
|
||||||
.positionConversionFactor(kConversionFactor)
|
.positionConversionFactor(kConversionFactor)
|
||||||
.velocityConversionFactor(kConversionFactor / 60);
|
.velocityConversionFactor(kConversionFactor / 60);
|
||||||
KLeftMotorConfig.closedLoop
|
KLeftMotorConfig.closedLoop
|
||||||
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
|
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
|
||||||
.pid(kP, kI, kD)
|
.pid(kP, kI, kD)
|
||||||
.outputRange(-1, 1)
|
.outputRange(-1, 1)
|
||||||
.positionWrappingEnabled(true)
|
.positionWrappingEnabled(true)
|
||||||
.positionWrappingInputRange(0, 2 * Math.PI)
|
.positionWrappingInputRange(0, 2 * Math.PI)
|
||||||
.feedForward
|
.feedForward
|
||||||
.sva(kS, kV, kA);
|
.svag(kS, kV, kA, kG);
|
||||||
|
|
||||||
kRightMotorConfig
|
kRightMotorConfig
|
||||||
.idleMode(kIdleMode)
|
.idleMode(kIdleMode)
|
||||||
|
|||||||
@@ -8,7 +8,7 @@ public class IntakeRollerConstants {
|
|||||||
public static final int kLeftMotorCANID = 0;
|
public static final int kLeftMotorCANID = 0;
|
||||||
public static final int kRightMotorCANID = 0;
|
public static final int kRightMotorCANID = 0;
|
||||||
|
|
||||||
public static final int kCurrentLimit = 30;
|
public static final int kCurrentLimit = 40;
|
||||||
|
|
||||||
public static final boolean kInvertMotors = false;
|
public static final boolean kInvertMotors = false;
|
||||||
|
|
||||||
|
|||||||
@@ -24,7 +24,7 @@ public class ShooterConstants {
|
|||||||
|
|
||||||
// TODO Conversion factor?
|
// TODO Conversion factor?
|
||||||
|
|
||||||
public static final double kWheelDiameter = Units.inchesToMeters(6);
|
public static final double kWheelDiameter = Units.inchesToMeters(4);
|
||||||
|
|
||||||
// TODO Real values
|
// TODO Real values
|
||||||
public static final int kLeftShooterMotor1CANID = 0;
|
public static final int kLeftShooterMotor1CANID = 0;
|
||||||
@@ -41,20 +41,20 @@ public class ShooterConstants {
|
|||||||
public static final double kLeftI = 0;
|
public static final double kLeftI = 0;
|
||||||
public static final double kLeftD = 0;
|
public static final double kLeftD = 0;
|
||||||
public static final double kLeftS = 0;
|
public static final double kLeftS = 0;
|
||||||
public static final double kLeftV = 0;
|
public static final double kLeftV = 0.21;
|
||||||
public static final double kLeftA = 0;
|
public static final double kLeftA = 0;
|
||||||
|
|
||||||
public static final double kRightP = 0;
|
public static final double kRightP = 0;
|
||||||
public static final double kRightI = 0;
|
public static final double kRightI = 0;
|
||||||
public static final double kRightD = 0;
|
public static final double kRightD = 0;
|
||||||
public static final double kRightS = 0;
|
public static final double kRightS = 0;
|
||||||
public static final double kRightV = 0;
|
public static final double kRightV = 0.21;
|
||||||
public static final double kRightA = 0;
|
public static final double kRightA = 0;
|
||||||
|
|
||||||
public static final double kShooterHeightMeters = 0;
|
public static final double kShooterHeightMeters = 0;
|
||||||
|
|
||||||
// TODO Is this value sane?
|
// TODO Is this value sane?
|
||||||
public static final int kCurrentLimit = 30;
|
public static final int kCurrentLimit = 50;
|
||||||
|
|
||||||
public static final IdleMode kShooterIdleMode = IdleMode.kCoast;
|
public static final IdleMode kShooterIdleMode = IdleMode.kCoast;
|
||||||
|
|
||||||
|
|||||||
@@ -12,9 +12,9 @@ public class SpindexerConstants {
|
|||||||
public static final int kSpindexerMotorCANID = 0;
|
public static final int kSpindexerMotorCANID = 0;
|
||||||
public static final int kFeederMotorCANID = 0;
|
public static final int kFeederMotorCANID = 0;
|
||||||
|
|
||||||
public static final int kSpindexerStatorCurrentLimit = 80;
|
public static final int kSpindexerStatorCurrentLimit = 90;
|
||||||
public static final int kSpindexerSupplyCurrentLimit = 30;
|
public static final int kSpindexerSupplyCurrentLimit = 50;
|
||||||
public static final int kFeederCurrentLimit = 30;
|
public static final int kFeederCurrentLimit = 40;
|
||||||
|
|
||||||
public static final boolean kFeederMotorInverted = false;
|
public static final boolean kFeederMotorInverted = false;
|
||||||
|
|
||||||
|
|||||||
@@ -5,8 +5,8 @@ import java.util.function.DoubleSupplier;
|
|||||||
|
|
||||||
import org.littletonrobotics.junction.Logger;
|
import org.littletonrobotics.junction.Logger;
|
||||||
|
|
||||||
import com.revrobotics.AbsoluteEncoder;
|
|
||||||
import com.revrobotics.PersistMode;
|
import com.revrobotics.PersistMode;
|
||||||
|
import com.revrobotics.RelativeEncoder;
|
||||||
import com.revrobotics.ResetMode;
|
import com.revrobotics.ResetMode;
|
||||||
import com.revrobotics.spark.SparkClosedLoopController;
|
import com.revrobotics.spark.SparkClosedLoopController;
|
||||||
import com.revrobotics.spark.SparkMax;
|
import com.revrobotics.spark.SparkMax;
|
||||||
@@ -22,7 +22,7 @@ public class IntakePivot extends SubsystemBase {
|
|||||||
private SparkMax leftMotor;
|
private SparkMax leftMotor;
|
||||||
private SparkMax rightMotor;
|
private SparkMax rightMotor;
|
||||||
|
|
||||||
private AbsoluteEncoder encoder;
|
private RelativeEncoder encoder;
|
||||||
|
|
||||||
private SparkClosedLoopController controller;
|
private SparkClosedLoopController controller;
|
||||||
|
|
||||||
@@ -46,7 +46,8 @@ public class IntakePivot extends SubsystemBase {
|
|||||||
|
|
||||||
controller = leftMotor.getClosedLoopController();
|
controller = leftMotor.getClosedLoopController();
|
||||||
|
|
||||||
encoder = leftMotor.getAbsoluteEncoder();
|
encoder = leftMotor.getEncoder();
|
||||||
|
encoder.setPosition(IntakePivotConstants.IntakePivotPosition.kUp.getPositionRadians());
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
Reference in New Issue
Block a user