Skip to content

Commit

Permalink
Merge pull request #102 from TitaniumTitans/GearFox-ReleaseCleanup
Browse files Browse the repository at this point in the history 
Gear fox release cleanup
  • Loading branch information
@GearBoxFox
GearBoxFox authored Jun 12, 2023
2 parents ce74f6e + 96a319d commit fe566b6
Show file tree
Hide file tree
Showing 16 changed files with 434 additions and 324 deletions.
82 changes: 13 additions & 69 deletions src/main/deploy/pathplanner/PickUp Left Engage.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@
},
"prevControl": null,
"nextControl": {
"x": 2.9043353876181506,
"y": 4.942159182563979
"x": 2.9199809276811535,
"y": 4.715452588799915
},
"holonomicAngle": 180.0,
"isReversal": false,
Expand All @@ -24,66 +24,16 @@
},
{
"anchorPoint": {
"x": 2.983458773935092,
"y": 4.869613072559479
"x": 6.855607395425317,
"y": 4.370858566278536
},
"prevControl": {
"x": 2.9305153990206114,
"y": 4.8784369683785584
"x": 3.763329456483474,
"y": 4.552223841289788
},
"nextControl": {
"x": 3.3099162689553454,
"y": 4.815203490056104
},
"holonomicAngle": 180.0,
"isReversal": false,
"velOverride": 4.5,
"isLocked": false,
"isStopPoint": false,
"stopEvent": {
"names": [],
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0
}
},
{
"anchorPoint": {
"x": 5.177978601571239,
"y": 4.633838215044851
},
"prevControl": {
"x": 3.841372590307194,
"y": 4.576961363501699
},
"nextControl": {
"x": 5.60418699784768,
"y": 4.651974742545977
},
"holonomicAngle": 180.0,
"isReversal": false,
"velOverride": 5.0,
"isLocked": false,
"isStopPoint": false,
"stopEvent": {
"names": [],
"executionBehavior": "parallel",
"waitBehavior": "none",
"waitTime": 0
}
},
{
"anchorPoint": {
"x": 6.619832537910691,
"y": 4.66104300629654
},
"prevControl": {
"x": 6.030395394124122,
"y": 4.642906478795414
},
"nextControl": {
"x": 6.030395394124122,
"y": 4.642906478795414
"x": 3.763329456483474,
"y": 4.552223841289788
},
"holonomicAngle": 180.0,
"isReversal": true,
Expand Down Expand Up @@ -155,25 +105,19 @@
]
},
{
"position": 4.01,
"position": 0.4,
"names": [
"ClearGround"
]
},
{
"position": 3.7,
"names": [
"RaiseArm"
"LowerIntake"
]
},
{
"position": 1.5272727272727273,
"position": 2.05,
"names": [
"LowerIntake"
"ClearGround"
]
},
{
"position": 3.38181818181818,
"position": 1.390909090909091,
"names": [
"ClearGround"
]
Expand Down
28 changes: 14 additions & 14 deletions src/main/deploy/pathplanner/PickUp Left.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,16 +24,16 @@
},
{
"anchorPoint": {
"x": 6.72,
"y": 4.6
"x": 6.710515175416316,
"y": 4.216698082518972
},
"prevControl": {
"x": 3.0382849172715884,
"y": 4.6725461100045
"x": 3.028800092687905,
"y": 4.289244192523473
},
"nextControl": {
"x": 3.0382849172715884,
"y": 4.6725461100045
"x": 3.028800092687905,
"y": 4.289244192523473
},
"holonomicAngle": 180.0,
"isReversal": true,
Expand All @@ -52,16 +52,16 @@
},
{
"anchorPoint": {
"x": 1.9315401788698316,
"y": 4.316448983775161
"x": 1.9678132338720822,
"y": 4.153220236265034
},
"prevControl": {
"x": 2.475636003903587,
"y": 4.515950786287538
"x": 2.5119090589058377,
"y": 4.3527220387774115
},
"nextControl": {
"x": 1.3874443538360763,
"y": 4.116947181262784
"x": 1.4237174088383269,
"y": 3.953718433752657
},
"holonomicAngle": -180.61,
"isReversal": false,
Expand All @@ -83,8 +83,8 @@
"y": 3.8539675324964686
},
"prevControl": {
"x": 5.549777415344305,
"y": 6.302398745148367
"x": 5.2323881840746145,
"y": 6.03035083263149
},
"nextControl": null,
"holonomicAngle": 178.65211271980135,
Expand Down
82 changes: 67 additions & 15 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,16 +9,19 @@
import com.pathplanner.lib.auto.PIDConstants;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import frc.robot.supersystems.ArmLimits;
import frc.robot.supersystems.ArmPose;
import kotlin.Unit;
import lib.utils.piecewise.PiecewiseInterval;
import lib.utils.piecewise.Range;
import lib.utils.piecewise.RangedPiecewise;
Expand Down Expand Up @@ -65,6 +68,8 @@ private ModuleConstants() {
public static final GosDoubleProperty MAX_SPEED_FPS = new GosDoubleProperty(false, "Max Drive Speed", 17);
public static final double MAX_SPEED_L3_MPS = Units.feetToMeters(MAX_SPEED_FPS.getValue());



/** Constants for the Phoenix Pro Modules using Falcon 500s **/
public static final double L3_GEAR_RATIO = 6.12;

Expand Down Expand Up @@ -119,23 +124,64 @@ private DriveConstants() {
public static final Transform3d FRONT_CAM_POSE = new Transform3d(
new Translation3d(Units.inchesToMeters(12.0), 0.0, Units.inchesToMeters(8.00)),
new Rotation3d(Units.degreesToRadians(-2.2), 0.0, 0.0));
public static final Transform3d LEFT_CAM_POSE = new Transform3d(
new Translation3d(Units.inchesToMeters(2.0), Units.inchesToMeters(6.0), Units.inchesToMeters(24.0)),
new Rotation3d(Units.degreesToRadians(180.0), 0.0, Units.degreesToRadians(90.0)));
public static final String FRONT_CAM_NAME = "FrontPiCam";
public static final String LEFT_CAM_NAME = "LeftWebCam";

public static final GosDoubleProperty CAM_AMBIGUITY_THRESHOLD
= new GosDoubleProperty(false, "Camera ambiguity threshold", 0.2);
public static final GosDoubleProperty CAM_DISTANCE_THRESHOLD
= new GosDoubleProperty(false, "Camera distance threshold", 4);

public static final String LEFT_GLOBAL_CAM = "LeftGlobalCam";
public static final String RIGHT_GLOBAL_CAM = "RightGlobalCam";

public static final Transform3d RIGHT_CAM_POSE = new Transform3d(
new Translation3d(Units.inchesToMeters(10.5), Units.inchesToMeters(8.5), Units.inchesToMeters(6)),
new Rotation3d(0.0, Units.degreesToRadians(50), Units.degreesToRadians(-18))
);

public static final Transform3d LEFT_CAM_POSE = new Transform3d(
new Translation3d(Units.inchesToMeters(10.5), Units.inchesToMeters(-8.5), Units.inchesToMeters(6)),
new Rotation3d(0.0, Units.degreesToRadians(50), Units.degreesToRadians(18))
);

// Tag Follow controllers
public static final ProfiledPIDController FOLLOW_CONTROLLER_X = new ProfiledPIDController(
4.0,
0.0,
0.0,
new TrapezoidProfile.Constraints(
AutoConstants.MAX_VELOCITY_MPS_AUTO,
AutoConstants.MAX_ACCELERATION_MPS_AUTO
)
);

public static final ProfiledPIDController FOLLOW_CONTROLLER_Y = new ProfiledPIDController(
4.0,
0.0,
0.0,
new TrapezoidProfile.Constraints(
AutoConstants.MAX_VELOCITY_MPS_AUTO,
AutoConstants.MAX_ACCELERATION_MPS_AUTO
)
);

public static final ProfiledPIDController FOLLOW_CONTROLLER_THETA = new ProfiledPIDController(
4.0,
0.0,
0.0,
new TrapezoidProfile.Constraints(
AutoConstants.MAX_VELOCITY_MPS_AUTO,
AutoConstants.MAX_ACCELERATION_MPS_AUTO
)
);
}

public static class WristConstants {
private WristConstants() {
throw new IllegalStateException("Utility Class");
}

// Motor and sensor ids
public static final int WRIST_ID = 20;
public static final int INTAKE_ID = 19;

Expand All @@ -149,6 +195,8 @@ private WristConstants() {

public static final double WRIST_UPPER_LIMIT = 125.0;
public static final int TOF_PORT = 23;

// Controller constants
public static final double WRIST_KP = 0.05;
public static final double WRIST_KI = 0.0;
public static final double WRIST_KD = 0.0;
Expand All @@ -165,22 +213,22 @@ private AutoConstants() {
}

//Trajectory following values
public static final double MAX_VELOCITY_MPS_AUTO = Units.feetToMeters(16);
public static final double MAX_VELOCITY_MPS_AUTO = Units.feetToMeters(16.0);
public static final double MAX_ACCELERATION_MPS_AUTO = MAX_VELOCITY_MPS_AUTO / 2.0;

public static final PIDController THETA_CONTROLLER =
new PIDController(3.25, 0.1, 0.3);

public static final PIDController CONTROLLER_X =
new PIDController(3.2, 0.03, 0.3);
new PIDController(4, 0.03, 0.3);
public static final PIDController CONTROLLER_Y =
new PIDController(3.2, 0.03, 0.3);
new PIDController(4, 0.03, 0.3);

public static final PIDConstants CONSTANTS_X =
new PIDConstants(4.0, 0.005, 0.0);
new PIDConstants(6.0, 0.01, 0.0);

public static final PIDConstants THETA_CONSTANTS =
new PIDConstants(3.2, 0.0, 0.0);
new PIDConstants(4.2, 0.0, 0.0);

//Auto balance constants
public static final double BALANCE_P = -0.04;
Expand Down Expand Up @@ -222,27 +270,32 @@ private ArmConstants() {
throw new IllegalStateException("Utility Class");
}

// Motor and sensor constants
public static final int ARM_EXTENSION_ID = 18;
public static final int ARM_ANGLE_ID_MASTER = 16;
public static final int ARM_ANGLE_ID_FOLLOWER = 17;
public static final int LIMIT_SWITCH_PORT = 3;

public static final double KP_ANGLE = CURRENT_MODE == Mode.HELIOS_V1 ? 0.53 : 0.227;
public static final double KI_ANGLE = 0.007;
public static final double KI_ANGLE = 0.0007;
public static final double KD_ANGLE = 0.08;

// Pid constants
public static final GosDoubleProperty ARM_EXT_KP = new GosDoubleProperty(true, "Arm extension kP", 0.5);
public static final GosDoubleProperty ARM_EXT_KI = new GosDoubleProperty(false, "Arm extension kI", 0);
public static final GosDoubleProperty ARM_EXT_KD = new GosDoubleProperty(false, "Arm extension kD", 0);

// Feedforward constants
public static final double ARM_KV = 0.05;
public static final double ARM_KS = 0.0;
public static final double ARM_KG = 0.37;
public static final double ARM_KG = 0.17;

public static final double ARM_OFFSET = CURRENT_MODE == Mode.HELIOS_V1 ? 165.0 : 294;
// offset for the absolute value sensor
public static final double ARM_OFFSET = CURRENT_MODE == Mode.HELIOS_V1 ? 280.0 : 294;

public static final int ENCODER_PORT = 4;

// Physical constantst
public static final double SPROCKET_DIAMETER = 1.99;
public static final double EXTENSION_RATIO = 0.3532;

Expand All @@ -262,8 +315,6 @@ private LimitConstants() {
throw new IllegalStateException("Utility Class");
}



// Arm Extension limits for Piecewise Function
public static final GosDoubleProperty ARM_EXT_STOW =
new GosDoubleProperty(false, "Arm Extension Stow Limit", 0.0);
Expand Down Expand Up @@ -380,6 +431,7 @@ private ArmSetpoints() {
throw new IllegalStateException("Utility Class");
}

// Setpoints that are able to be adjusted mid match
public static final GosDoubleProperty HUMAN_HEIGHT = new GosDoubleProperty(false, "HUMAN HIEGHT", 233.6);
public static final GosDoubleProperty HUMAN_WRIST = new GosDoubleProperty(false, "HUMAN WRIST", 78.0);

Expand All @@ -392,7 +444,7 @@ private ArmSetpoints() {
public static final ArmPose HUMAN_PLAYER_STATION = new ArmPose(0.0, 230.6, 78.0);

public static final ArmPose MIDDLE_GOAL = new ArmPose(0.0, 252.1, 99.7);
public static final ArmPose HIGH_GOAL = new ArmPose(20, 240.0, 95.3);
public static final ArmPose HIGH_GOAL = new ArmPose(20, 245.0, 95.3);
}

public static final int DRIVER_PORT = 0;
Expand Down
14 changes: 14 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,12 @@

package frc.robot;

import com.pathplanner.lib.PathConstraints;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import lib.factories.SparkMaxFactory;
import org.littletonrobotics.junction.*;
import org.littletonrobotics.junction.networktables.NT4Publisher;
Expand Down Expand Up @@ -38,6 +43,15 @@ public void robotInit() {
//Base code written from the AdvantageKit logging framework (6328 Mechanical Advantage)
//Sets up a base logger for non-subsystem inputs

PathPlannerTrajectory traj = PathPlanner.loadPath("Mobility Right", new PathConstraints(1.0, 1.0));
PathPlannerTrajectory ftraj = lib.utils.PathPlannerFlipper.flipTrajectory(traj);

Field2d ffield = new Field2d();
ffield.getObject("Traj").setTrajectory(traj);
ffield.getObject("FTraj").setTrajectory(ftraj);

SmartDashboard.putData("FLIP FIELD", ffield);

Logger logger = Logger.getInstance();
m_pdh = new PowerDistribution(1, PowerDistribution.ModuleType.kRev);
m_pdh.setSwitchableChannel(false);
Expand Down
Loading

0 comments on commit fe566b6

Please sign in to comment.