Skip to content
This repository was archived by the owner on Nov 8, 2022. It is now read-only.

Fixed some turret stuff and yeah. #3

Open
KrishWu wants to merge 7 commits into
base: main
Choose a base branch
from
Open

Fixed some turret stuff and yeah. #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
ab14d20
Fixed some turret stuff and yeah.
KrishWu Oct 23, 2022
ec2953d
fixed jitter
markzakharyan Oct 29, 2022
8280a7a
fix offset and shooter never ready bug
markzakharyan Nov 3, 2022
cc217af
idk
markzakharyan Nov 5, 2022
2513454
broken
cookieMosnter Nov 5, 2022
4ce3737
mid comp
cookieMosnter Nov 6, 2022
8793634
final commit :(
markzakharyan Nov 7, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ public static final class RobotIds {

public static final int CONVEYOR_MOTOR_TOP = 16;
public static final int CONVEYOR_MOTOR_BOTTOM = 15;
public static final int CONVEYOR_SENSOR = 0;
public static final int CONVEYOR_SENSOR = 1;

public static final int SHOOTER_MOTOR_LEFT = 21;
public static final int SHOOTER_MOTOR_RIGHT = 22;
Expand Down
28 changes: 18 additions & 10 deletions src/main/java/frc/robot/commands/ShooterCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@

import com.revrobotics.ColorMatch;

import edu.wpi.first.math.filter.Debouncer;
import edu.wpi.first.math.filter.Debouncer.DebounceType;
// import edu.wpi.first.math.filter.Debouncer;
// import edu.wpi.first.math.filter.Debouncer.DebounceType;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
Expand Down Expand Up @@ -44,18 +44,20 @@ public static interface Controls {
private final boolean isRedAlliance;
private boolean isAuto = false;

private boolean missBall = false;
private final double ballMissRPM = 300;
// private boolean missBall = false;
// private final double ballMissRPM = 300;

private NetworkTable ntTable;
private NetworkTable ntTableClimb;
private NetworkTableEntry ntTestRPM;
private NetworkTableEntry ntTestHood;

private NetworkTableEntry ntConstantAiming;

private NetworkTableEntry ntUseCalibrationMap;

private NetworkTableEntry hotRPMAddition;
private NetworkTableEntry hotRPMReduction;
// private NetworkTableEntry hotRPMReduction;

private NetworkTableEntry ntTeleopBuff;

Expand All @@ -68,7 +70,7 @@ public static interface Controls {
private int targetBallCount = -1;
private double doneShootingFrames = 0;

private Debouncer debouncer = new Debouncer(2, DebounceType.kFalling);
// private Debouncer debouncer = new Debouncer(2, DebounceType.kFalling);

private double rpmBuff;
private final double rpmBuffZeta = 17;
Expand Down Expand Up @@ -101,7 +103,10 @@ public ShooterCommand(Shooter shooter, Controls controls, Limelight limelight) {
hotRPMAddition.setDouble(35.0);

ntTeleopBuff = ntTable.getEntry("Teleop RPM Buff");
ntTeleopBuff.setDouble(0);
ntTeleopBuff.setDouble(60.0);

ntConstantAiming = ntTable.getEntry("Constant Aiming Override");
ntConstantAiming.setBoolean(true);


isRedAlliance = NetworkTableInstance.getDefault().getTable("FMSInfo").getEntry("IsRedAlliance").getBoolean(false);
Expand Down Expand Up @@ -136,10 +141,13 @@ public void execute() {
// missBall = false;
// }

limelight.setForceOff(!(controls.getAimShooter() || controls.getConstantAiming()));
boolean constantAiming = ntConstantAiming.getBoolean(true);
// if (!constantAiming) constantAiming = controls.getConstantAiming();

limelight.setForceOff(!(controls.getAimShooter() || constantAiming));

double dist = limelight.getDistance();
if(controls.getAimShooter() || controls.getConstantAiming()) {
if(controls.getAimShooter() || constantAiming) {

if (controls.getAimShooter()) {
TeleOpBaseRPMBuff = ntTeleopBuff.getDouble(0.0);
Expand Down Expand Up @@ -180,7 +188,7 @@ public void execute() {
turrTarget = 0;

else if (controls.overrideTurretCenter()) shooter.setTurretPos(0); // override controlled turret pos
else shooter.setTurretDeltaPos(turrTarget); // limelight controlled turret pos;
else if (limelight.hasTarget()) shooter.setTurretDeltaPos(turrTarget); // limelight controlled turret pos;

// ntTestHood.setDouble(targetHoodPos);
}else{
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/controls/manualdrive/ManualControls.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,13 +35,13 @@ public ManualControls(Joystick xbox, Joystick controlStation) {
this.controlStation = controlStation;

this.speed = new PovNumberStepper(
new NumberStepper(0.5, 0.2, PhysicalConstants.MAX_SPEED, 0.1),
new NumberStepper(0.8, 0.2, PhysicalConstants.MAX_SPEED, 0.1),
xbox,
PovNumberStepper.PovDirection.VERTICAL
);

this.turnSpeed = new PovNumberStepper(
new NumberStepper(0.5, 0.2, PhysicalConstants.MAX_TURN_SPEED, 0.05),
new NumberStepper(0.55, 0.2, PhysicalConstants.MAX_TURN_SPEED, 0.05),
xbox,
PovNumberStepper.PovDirection.HORIZONTAL
);
Expand All @@ -64,7 +64,7 @@ public double getY() {

@Override
public double getSpeed() {
return speed.get();
return speed.get() * (xbox.getRawButton(ControllerIds.XBOX_R_BUMPER) ? 1.25 : 1) * (xbox.getRawButton(ControllerIds.XBOX_L_BUMPER) ? 0.75 : 1);
}

@Override
Expand Down
5 changes: 4 additions & 1 deletion src/main/java/frc/robot/subsystems/DriveTrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ public class DriveTrain extends SubsystemBase {
private double kP, kI, kD, kFF;

private NetworkTable ntTable;
private NetworkTableEntry ntPosition, ntspeed, ntP, ntI, ntD, ntFF, ntifTestingVelocity, ntifTestingRotation;
private NetworkTableEntry ntPosition, ntspeed, ntP, ntI, ntD, ntFF, ntifTestingVelocity, ntifTestingRotation, ntLeftSideBuff;

private DifferentialDrivePoseEstimator estimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(),
new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02), // State measurement standard deviations. X, Y, theta.
Expand Down Expand Up @@ -130,6 +130,9 @@ public DriveTrain() {
ntifTestingRotation = ntTable.getEntry("If testing Rotation");
ntifTestingRotation.setBoolean(false);

// ntLeftSideBuff = ntTable.getEntry("Left Side Buff");
// ntLeftSideBuff.setDouble(1.0);

NtValueDisplay.ntDispTab("Drivetrain").add("L Actual Speed", this::getLVelocity).add("R Actual Speed", this::getRVelocity);
NtValueDisplay.ntDispTab("Drivetrain")
.add("Actual FF", () -> leftMotors[0].getPIDController().getFF())
Expand Down
63 changes: 40 additions & 23 deletions src/main/java/frc/robot/subsystems/Shooter.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,13 @@
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.revrobotics.CANSparkMax;
// import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxPIDController;
import com.revrobotics.CANSparkMax.ControlType;

// import edu.wpi.first.math.controller.PIDController;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
// import com.revrobotics.SparkMaxPIDController.ArbFFUnits;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
Expand All @@ -23,9 +29,9 @@

public class Shooter extends SubsystemBase {
private final double RPMAcceptableDiff = 100;
private final double turretAcceptableDiff = 3;
private final double turretAcceptableDiff = 3.0; //+-degs

private final double turretTurnSpeed = 0.35;
// private final double turretTurnSpeed = 0.35;

private final TalonFX shooterMotorL;
private final TalonFX shooterMotorR;
Expand All @@ -45,8 +51,13 @@ public class Shooter extends SubsystemBase {
private NetworkTable ntTableLimelight;
private NetworkTableEntry ntShooterReady;

private SparkMaxPIDController turrPidController;

private int ballShotCount = 0;

private NetworkTableEntry ntIsTestingTurret;
private NetworkTableEntry ntTestTurrestPos;

/** Creates a new Shooter. */
public Shooter() {
targetRPM = 0;
Expand All @@ -60,7 +71,16 @@ public Shooter() {

turretMotor = new CANSparkMax(RobotIds.SHOOTER_TURRET_MOTOR, MotorType.kBrushless);
turretMotor.getEncoder().setPosition(0.0);
turrPidController = turretMotor.getPIDController();

turrPidController.setP(0.075);
turrPidController.setI(0.00005);
turrPidController.setD(0.1);
turrPidController.setIZone(0.25);
turrPidController.setFF(0.01);
turrPidController.setOutputRange(-1.0, 1.0);

turrPidController.setFeedbackDevice(turretMotor.getEncoder());

shooterMotorL.configFactoryDefault();
shooterMotorR.configFactoryDefault();
Expand Down Expand Up @@ -109,7 +129,15 @@ public Shooter() {
.add("Actual Hood", this::getHoodPos)

.add("Num Balls Shot", this::getBallShotCount)
.add("turretOffsetPos", () -> (getTurretPos() - ((targetTurretPos - startAngle) * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2)))
;


ntIsTestingTurret = ntTable.getEntry("isTestingTurret");
ntIsTestingTurret.setBoolean(false);
ntTestTurrestPos = ntTable.getEntry("ntTestTurrestPos");
ntTestTurrestPos.setDouble(0.0);

}

@Override
Expand All @@ -122,28 +150,15 @@ public void periodic() {

shooterMotorL.set(ControlMode.Velocity, targetRPM / Constants.PhysicalConstants.RPM_PER_FALCON_UNIT);
shooterMotorR.set(ControlMode.Velocity, targetRPM / Constants.PhysicalConstants.RPM_PER_FALCON_UNIT);
// shooterMotorL.set(ControlMode.PercentOutput, 0.5);

double turretPosDiff = targetTurretPos - this.getTurretPosDegrees();

if(Math.abs(turretPosDiff) < turretAcceptableDiff){
turretMotor.set(0.0);
}else{
double speed = Math.abs(turretPosDiff) > turretAcceptableDiff * 3 ?
turretTurnSpeed :
Math.abs(turretPosDiff) > turretAcceptableDiff * 2 ?
turretTurnSpeed / (3 * (3 * turretAcceptableDiff - Math.abs(turretPosDiff)) / (turretAcceptableDiff)): // basically a gradient down from 1 to 1/3
turretTurnSpeed / 3
;

speed /= 1.333;
speed *= Math.copySign(1, turretPosDiff);

//changed
turretMotor.set(speed);
// turretMotor.set(0.0);
// shooterMotorL.set(ControlMode.PercentOutput, 0.5);
if (ntIsTestingTurret.getBoolean(false)) {
targetTurretPos = ntTestTurrestPos.getDouble(0.0);
}

if (targetTurretPos < -70) targetTurretPos = -70;
if (targetTurretPos > 90) targetTurretPos = 90;
turrPidController.setReference((targetTurretPos - startAngle) * 0.8888 * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2, ControlType.kPosition);

}

public void setHoodPos(double pos) {
Expand All @@ -158,7 +173,9 @@ public double getTurretSpeed() {
return turretMotor.getEncoder().getVelocity();
}

// private static final double acceptableBullshit = 5.0;
public void setTurretDeltaPos(double delta){
// if (Math.abs(delta) > acceptableBullshit)
setTurretPos(this.getTurretPosDegrees() + delta);
}

Expand Down Expand Up @@ -201,6 +218,6 @@ public boolean isShooterReady(){
ntTableLimelight.getEntry("Has Target").getBoolean(false) &&
Math.abs(getShooterRpm()) > 2000 &&
Math.abs(getShooterRpm() - targetRPM) < RPMAcceptableDiff &&
Math.abs(getTurretPosDegrees() - targetTurretPos) < turretAcceptableDiff;
Math.abs(getTurretPos() - ((targetTurretPos - startAngle) * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2)) < turretAcceptableDiff;
}
}