Skip to content

Commit

Permalink
remove crap to get it working
Browse files Browse the repository at this point in the history
  • Loading branch information
@superdf407
superdf407 committed Aug 17, 2024
1 parent fcaa557 commit 4cfc3bd
Show file tree
Hide file tree
Showing 28 changed files with 501 additions and 1,288 deletions.
2 changes: 1 addition & 1 deletion .wpilib/wpilib_preferences.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,5 +2,5 @@
"enableCppIntellisense": false,
"currentLanguage": "java",
"projectYear": "2023",
"teamNumber": 4467
"teamNumber": 666
}
13 changes: 6 additions & 7 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@

import com.ctre.phoenixpro.signals.InvertedValue;
import com.gos.lib.properties.GosDoubleProperty;
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;
Expand Down Expand Up @@ -225,11 +224,11 @@ private AutoConstants() {
public static final PIDController CONTROLLER_Y =
new PIDController(4, 0.03, 0.3);

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

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

//Auto balance constants
public static final double BALANCE_P = -0.04;
Expand Down Expand Up @@ -277,7 +276,7 @@ private ArmConstants() {
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 KP_ANGLE = CURRENT_MODE == Mode.HELIOS_V2 ? 0.53 : 0.227;
public static final double KI_ANGLE = 0.0007;
public static final double KD_ANGLE = 0.08;

Expand All @@ -292,7 +291,7 @@ private ArmConstants() {
public static final double ARM_KG = 0.17;

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

public static final int ENCODER_PORT = 4;

Expand Down
14 changes: 7 additions & 7 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,9 @@

package frc.robot;

import com.pathplanner.lib.PathConstraints;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
//import com.pathplanner.lib.PathConstraints;
//import com.pathplanner.lib.PathPlanner;
//import com.pathplanner.lib.PathPlannerTrajectory;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj.PowerDistribution;
Expand Down Expand Up @@ -46,12 +46,12 @@ 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);
// 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);
// ffield.getObject("Traj").setTrajectory(traj);
// ffield.getObject("FTraj").setTrajectory(ftraj);

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

Expand Down
31 changes: 16 additions & 15 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,10 @@
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import frc.robot.commands.*;
import frc.robot.commands.autonomous.AutoFactory;
import frc.robot.commands.autonomous.Balance;
//import frc.robot.commands.autonomous..AutoFactory;
//import frc.robot.commands.autonomous.Balance;
import frc.robot.subsystems.arm.ArmExtSubsystem;
import frc.robot.subsystems.vision.CameraSubsystem;
//import frc.robot.subsystems.vision.CameraSubsystem;
import frc.robot.supersystems.ArmPose;
import frc.robot.supersystems.ArmSupersystem;
import lib.controllers.FootPedal;
Expand Down Expand Up @@ -50,14 +50,14 @@ public class RobotContainer {
private LedSubsystem m_led;

// Camera testing crap
private CameraSubsystem m_testingCamera;
// private CameraSubsystem m_testingCamera;

//Controllers
private final CommandXboxController m_driveController = new CommandXboxController(Constants.DRIVER_PORT);
// private final CommandXboxController m_testController = new CommandXboxController(2);

//Logged chooser for auto
private final AutoFactory m_autoFactory;
// private final AutoFactory m_autoFactory;

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
Expand All @@ -77,7 +77,7 @@ public RobotContainer() {
m_super = new ArmSupersystem(m_arm, m_ext, m_wrist, m_drive);
m_foot = new FootPedal(1);

m_testingCamera = new CameraSubsystem("TestCamera", new Transform3d());
// m_testingCamera = new CameraSubsystem("TestCamera", new Transform3d());
break;

case SIM:
Expand All @@ -89,7 +89,7 @@ public RobotContainer() {

LiveWindow.disableAllTelemetry();

m_autoFactory = new AutoFactory(m_super, m_drive, m_wrist, m_ext);
// m_autoFactory = new AutoFactory(m_super, m_drive, m_wrist, m_ext);


// Configure the button bindings
Expand All @@ -115,14 +115,15 @@ private void configureButtonBindings() {
m_driveController.leftTrigger().whileTrue(new IntakeControlCommand(m_wrist, -0.5));
m_driveController.rightTrigger().whileTrue(new IntakeControlCommand(m_wrist, 1.0));

if (Constants.CURRENT_MODE != Constants.Mode.HELIOS_V1) {
if (Constants.CURRENT_MODE != Constants.Mode.HELIOS_V1 && false) {
m_driveController.x().whileTrue(
new SupersystemToPoseCommand(m_super, Constants.ArmSetpoints.INTAKE_BATTERY)
.alongWith(new IntakeControlCommand(m_wrist, 1.0, m_driveController.getHID())));
}
m_driveController.y().whileTrue(
new SupersystemToPoseCommand(m_super, Constants.ArmSetpoints.INTAKE_BATTERY)
.alongWith(new IntakeControlCommand(m_wrist, 1.0, m_driveController.getHID())));

// m_driveController.y().whileTrue(
// new SupersystemToPoseCommand(m_super, Constants.ArmSetpoints.INTAKE_BATTERY)
// .alongWith(new IntakeControlCommand(m_wrist, 1.0, m_driveController.getHID())));

m_driveController.a().whileTrue(new SupersystemToPoseCommand(m_super, Constants.ArmSetpoints.STOW_POSITION));
m_driveController.b().whileTrue(
Expand Down Expand Up @@ -153,7 +154,7 @@ public void configDashboard() {
SmartDashboard.putBoolean("Stella Mode", true);
ShuffleboardTab testCommands = Shuffleboard.getTab("Commands");

testCommands.add("balance", new Balance(m_drive));
// testCommands.add("balance", new Balance(m_drive));

testCommands.add("Toggle Angle Brake Mode", new ToggleArmBrakeModeCommand(m_arm)).withSize(2, 1);
testCommands.add("Toggle Wrist Brake Mode", new InstantCommand(() -> m_wrist.toggleBrakeMode()).runsWhenDisabled());
Expand Down Expand Up @@ -190,7 +191,7 @@ public void configDashboard() {
testCommands.add("Auto Balance", new AutoBalanceTransCommand(m_drive));
testCommands.add("Reset Pose", new InstantCommand(() -> m_drive.resetPoseBase())).withSize(2, 1);

testCommands.add("Align to Zero Degrees", m_drive.alignToAngle(0).asProxy());
// testCommands.add("Align to Zero Degrees", m_drive.alignToAngle(0).asProxy());

}

Expand All @@ -201,8 +202,8 @@ public void configDashboard() {
*/
public Command getAutonomousCommand() {
return new InstantCommand(() ->
m_drive.resetGyro(180))
.andThen(m_autoFactory.getAutoRoutine());
m_drive.resetGyro(180));
// .andThen(m_autoFactory.getAutoRoutine());
}

public ArmSupersystem getArmSupersystem() {
Expand Down
111 changes: 0 additions & 111 deletions src/main/java/frc/robot/commands/autonomous/AutoFactory.java
View file

This file was deleted.

77 changes: 0 additions & 77 deletions src/main/java/frc/robot/commands/autonomous/AutoUtils.java
View file

This file was deleted.

26 changes: 0 additions & 26 deletions src/main/java/frc/robot/commands/autonomous/Balance.java
View file

This file was deleted.

Loading

0 comments on commit 4cfc3bd

Please sign in to comment.