Skip to content
This repository has been archived by the owner on Jan 10, 2024. It is now read-only.

Commit

Permalink
a
Browse files Browse the repository at this point in the history
  • Loading branch information
TheGamer1002 committed Apr 26, 2023
1 parent ec91fec commit 6df5dc3
Show file tree
Hide file tree
Showing 13 changed files with 184 additions and 510 deletions.
Binary file added FRC_20230425_143032.wpilog
Binary file not shown.
Binary file added FRC_20230426_150848.wpilog
Binary file not shown.
2 changes: 2 additions & 0 deletions simgui.json
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,8 @@
"/Shuffleboard/Subsystems/SwitchIdleModeCommmand": "Command",
"/Shuffleboard/Subsystems/Tank": "Subsystem",
"/Shuffleboard/Subsystems/Turret": "Subsystem",
"/Shuffleboard/Subsystems/Vision": "Subsystem",
"/Shuffleboard/Vision/Vision": "PIDController",
"/SmartDashboard/Field": "Field2d",
"/SmartDashboard/Scheduler": "Scheduler"
},
Expand Down
40 changes: 23 additions & 17 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
package frc.robot;

import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
Expand Down Expand Up @@ -78,25 +79,30 @@ public RobotContainer() {

// add a few things to the shuffleboard
// joysticks
var tab = Shuffleboard.getTab("Subsystems");
tab.add(m_Arm);
tab.add(m_AutoCommand);
tab.add(m_BalanceCommand);
tab.add(m_DecreaseMaxSpeedCommand);
tab.add(m_DriveCommand);
tab.add(m_FineDriveCommand);
tab.add(m_IncreaseMaxSpeedCommand);
tab.add(m_MoveArmYCommand);
tab.add(m_PDP);
tab.add(m_PlaceCubeSecondLevelCommand);
tab.add(m_Vision);

tab.add(m_PID);
tab.add(m_PlaceConeSecondLevelCommand);
tab.add(m_Tank);
tab.add(m_Claw);
var subsystemsTab = Shuffleboard.getTab("Subsystems");
var mainTab = Shuffleboard.getTab("Main");
subsystemsTab.add(m_Arm);
subsystemsTab.add(m_AutoCommand);
subsystemsTab.add(m_BalanceCommand);
subsystemsTab.add(m_DecreaseMaxSpeedCommand);
subsystemsTab.add(m_DriveCommand);
subsystemsTab.add(m_FineDriveCommand);
subsystemsTab.add(m_IncreaseMaxSpeedCommand);
subsystemsTab.add(m_MoveArmYCommand);
subsystemsTab.add(m_PDP);
subsystemsTab.add(m_PlaceCubeSecondLevelCommand);
subsystemsTab.add(m_Vision);

subsystemsTab.add(m_PID);
subsystemsTab.add(m_PlaceConeSecondLevelCommand);
subsystemsTab.add(m_Tank);
subsystemsTab.add(m_Claw);

Logger.configureLoggingAndConfig(this, false);




}

/**
Expand Down
43 changes: 43 additions & 0 deletions src/main/java/frc/robot/commands/DriveForward.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Tank;

public class DriveForward extends CommandBase {
/** Creates a new DriveForward. */
private Tank m_tank;

public DriveForward(Tank tank) {
// Use addRequirements() here to declare subsystem dependencies.
m_tank = tank;
addRequirements(m_tank);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {

m_tank.arcadeDrive(0.5, 0);
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
m_tank.arcadeDrive(0, 0);
}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;

// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class DriveForwardAndPickUpPieceCommand extends ParallelDeadlineGroup {
/** Creates a new DriveForwardAndPickUpPieceCommand. */
public DriveForwardAndPickUpPieceCommand() {
// Add the deadline command in the super() call. Add other commands using
// addCommands().
super(new InstantCommand());
// addCommands(new FooCommand(), new BarCommand());
}
}
9 changes: 7 additions & 2 deletions src/main/java/frc/robot/commands/FineDriveCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,29 +4,34 @@

package frc.robot.commands;

import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.RebindHat;
import frc.robot.subsystems.Tank;

public class FineDriveCommand extends CommandBase {
@SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
private final Tank m_drivebase;
private double speedMult = 0.43;
private GenericEntry multWidget;

public FineDriveCommand(Tank subsystem) {
m_drivebase = subsystem;
addRequirements(m_drivebase);
multWidget = Shuffleboard.getTab("Fine Drive").add("Speed", speedMult).getEntry();
}

@Override
public void initialize() {
// nothing to do here

}

@Override
public void execute() {
// double check getMaxSpeed(), might be wrong
m_drivebase.arcadeDrive(
RebindHat.ControllerToYAxis() * 0.43, RebindHat.ControllerToXAxis() * 0.43);
RebindHat.ControllerToYAxis() * multWidget.getDouble(0), RebindHat.ControllerToXAxis() * multWidget.getDouble(0));
}

@Override
Expand Down
36 changes: 36 additions & 0 deletions src/main/java/frc/robot/commands/SpinIntake.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Claw;

public class SpinIntake extends CommandBase {
private Claw m_claw;
/** Creates a new SpinIntake. */
public SpinIntake(Claw claw) {
// Use addRequirements() here to declare subsystem dependencies.
m_claw = claw;
addRequirements(m_claw);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
5 changes: 4 additions & 1 deletion src/main/java/frc/robot/commands/TrackPiece.java
Original file line number Diff line number Diff line change
Expand Up @@ -23,12 +23,15 @@ public TrackPiece(Vision vision, Tank tank) {
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_Tank.arcadeDrive(0.05, m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint()));
//m_Tank.arcadeDrive(0.05, m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint()));
m_Vision.enable();
SmartDashboard.putString("a", "pid on");

}

@Override
public void execute() {

SmartDashboard.putNumber(getName(), m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint()));
}
// Called once the command ends or is interrupted.
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/Tank.java
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ public Tank() {
drive.setSafetyEnabled(false);
drive.setExpiration(0.1);

drive.setMaxOutput(1.0);
drive.setMaxOutput(0.8);

brakeMode = true;
SmartDashboard.putBoolean("brakeMode", brakeMode);
Expand Down
30 changes: 23 additions & 7 deletions src/main/java/frc/robot/subsystems/Vision.java
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,9 @@

public class Vision extends PIDSubsystem {
/** Creates a new Vision. */
private static double kP = 0.18;
private static double kI = 0.025;
private static double kD = 0.3;
private static double kP = 0.02;
private static double kI = 0.05;
private static double kD = 0.0007;

public PIDController pidController;
public Tank m_tank;
Expand All @@ -26,17 +26,33 @@ public Vision(Tank tank) {

addChild(getName(), m_controller);
m_tank = tank;
m_controller.setSetpoint(0);
m_controller.setTolerance(2);
m_controller.setIntegratorRange(-0.43, 0.43);

Shuffleboard.getTab(getName()).add(m_controller);

}

@Override
public void useOutput(double output, double setpoint) {
// Use the output here
m_tank.arcadeDrive(0.05, output);

if (0.40 < output) {
output = 0.40;
} else if (-0.40 > output) {
output = -0.40;
} else if (0 < output && output < 0.40) {
output = 0.40;
} else if (-0.40 < output && output < 0) {
output = -0.40;
}


m_tank.arcadeDrive(0, -output * 1);
SmartDashboard.putNumber("will this work?", output);
SmartDashboard.putNumber("setpoint", setpoint);

}

@Override
Expand Down Expand Up @@ -65,8 +81,8 @@ public void setTolerance(double tolerance) {
@Override
public void periodic() {
super.periodic();
SmartDashboard.putNumber("stuff again", m_controller.calculate(getMeasurement(), m_controller.getSetpoint()));
SmartDashboard.putNumber("calculated turning output from vision",
m_controller.calculate(getMeasurement(), m_controller.getSetpoint()));
}


}
Loading

0 comments on commit 6df5dc3

Please sign in to comment.