Skip to content

Commit

Permalink
Update Intake class
Browse files Browse the repository at this point in the history
  • Loading branch information
@JackJ09
JackJ09 committed Feb 2, 2024
1 parent e7a5724 commit 715a63f
Show file tree
Hide file tree
Showing 3 changed files with 126 additions and 86 deletions.
106 changes: 74 additions & 32 deletions src/main/java/org/team340/robot/Constants.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package org.team340.robot;

import com.revrobotics.CANSparkBase.ExternalFollower;
import com.revrobotics.CANSparkBase.IdleMode;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.wpilibj.ADIS16470_IMU.CalibrationTime;
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
Expand All @@ -9,7 +11,11 @@
import org.team340.lib.swerve.SwerveBase.SwerveMotorType;
import org.team340.lib.swerve.config.SwerveConfig;
import org.team340.lib.swerve.config.SwerveModuleConfig;
import org.team340.lib.util.Math2;
import org.team340.lib.util.config.PIDConfig;
import org.team340.lib.util.config.rev.AbsoluteEncoderConfig;
import org.team340.lib.util.config.rev.SparkMaxConfig;
import org.team340.lib.util.config.rev.SparkPIDControllerConfig;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
Expand Down Expand Up @@ -72,8 +78,10 @@ public static final class RobotMap {
public static final int FRONT_RIGHT_MOVE = 8;
public static final int FRONT_RIGHT_TURN = 9;

public static final int INTAKE_DEPLOY_MOTOR = 20;
public static final int INTAKE_ROLLER_MOTOR = 21;
public static final int INTAKE_PIVOT_LEFT_MOTOR = 20;
public static final int INTAKE_PIVOT_RIGHT_MOTOR = 21;
public static final int INTAKE_ROLLER_UPPER_MOTOR = 22;
public static final int INTAKE_ROLLER_LOWER_MOTOR = 23;

public static final int SHOOTER_PIVOT_MOTOR = 30;
public static final int SHOOTER_FEED_MOTOR = 31;
Expand All @@ -85,54 +93,88 @@ public static final class RobotMap {

public static final class IntakeConstants {

public static final PIDConfig INTAKE_DEPLOY_STRONG_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0);
public static final PIDConfig INTAKE_DEPLOY_WEAK_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0);

public static final double INTAKE_DEPLOY_POSITION = 0.0;
public static final double INTAKE_DEPLOY_POSITION_TOLERANCE = 0.0;

public static final double INTAKE_DEPLOY_ROLLER_SPEED = 0.0;
public static final double INTAKE_SPIT_ROLLER_SPEED = -0.0;
private static final SparkMaxConfig PIVOT_MOTOR_BASE_CONFIG = new SparkMaxConfig()
.clearFaults()
.restoreFactoryDefaults()
.enableVoltageCompensation(VOLTAGE)
.setSmartCurrentLimit(30)
.setIdleMode(IdleMode.kBrake)
.setClosedLoopRampRate(1.5)
.setOpenLoopRampRate(1.5);
private static final SparkMaxConfig ROLLER_MOTOR_BASE_CONFIG = new SparkMaxConfig()
.clearFaults()
.restoreFactoryDefaults()
.enableVoltageCompensation(VOLTAGE)
.setSmartCurrentLimit(30)
.setIdleMode(IdleMode.kCoast)
.setClosedLoopRampRate(1.5)
.setOpenLoopRampRate(1.5);

public static final SparkMaxConfig PIVOT_LEFT_MOTOR_CONFIG = PIVOT_MOTOR_BASE_CONFIG.clone().setInverted(false);
public static final SparkMaxConfig PIVOT_RIGHT_MOTOR_CONFIG = PIVOT_MOTOR_BASE_CONFIG
.clone()
.follow(ExternalFollower.kFollowerSpark, RobotMap.INTAKE_PIVOT_LEFT_MOTOR, false);
public static final SparkMaxConfig ROLLER_UPPER_MOTOR_CONFIG = ROLLER_MOTOR_BASE_CONFIG.clone().setInverted(false);
public static final SparkMaxConfig ROLLER_LOWER_MOTOR_CONFIG = ROLLER_MOTOR_BASE_CONFIG
.clone()
.follow(ExternalFollower.kFollowerSpark, RobotMap.INTAKE_ROLLER_UPPER_MOTOR, true);

public static final int PIVOT_WEAK_PID_SLOT = 0;
public static final int PIVOT_STRONG_PID_SLOT = 1;

public static final SparkPIDControllerConfig PIVOT_MOTOR_PID_CONFIG = new SparkPIDControllerConfig()
.setPID(0.0, 0.0, 0.0, PIVOT_WEAK_PID_SLOT)
.setPID(0.0, 0.0, 0.0, PIVOT_STRONG_PID_SLOT);

public static final AbsoluteEncoderConfig PIVOT_ENCODER_CONFIG = new AbsoluteEncoderConfig()
.setZeroOffset(0.0)
.setInverted(false)
.setPositionConversionFactor(Math2.TWO_PI)
.setVelocityConversionFactor(Math2.TWO_PI / 60.0);

public static final double DEPLOY_POSITION = 0.0;
public static final double DEPLOY_POSITION_TOLERANCE = 0.0;

public static final double DEPLOY_ROLLER_SPEED = 0.0;
public static final double SPIT_ROLLER_SPEED = -0.0;
}

public static final class ShooterConstants {

public static final double FEED_INTAKE_SPEED = 0.0;

public static final PIDConfig SHOOTER_FEED_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0);
public static final PIDConfig SHOOTER_LEFT_SHOOT_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0);
public static final PIDConfig SHOOTER_RIGHT_SHOOT_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0);
public static final PIDConfig FEED_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0);
public static final PIDConfig LEFT_SHOOT_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0);
public static final PIDConfig RIGHT_SHOOT_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0);

/**
* All distances in inches. All angles in radians.
*/
public static final class PivotConstants {

public static final double PIVOT_REL_ENC_CONVERSION = 0.2;
public static final double REL_ENC_CONVERSION = 0.2;

public static final double PIVOT_SHOOTER_LENGTH = 0.0;
public static final double PIVOT_BASE_LENGTH = 0.0;
public static final double PIVOT_MINIMUM_LENGTH_OF_DART = 0.0;
public static final double SUM_OF_SQUARES_OF_LENGTHS =
PIVOT_SHOOTER_LENGTH * PIVOT_SHOOTER_LENGTH + PIVOT_BASE_LENGTH * PIVOT_BASE_LENGTH;
public static final double SHOOTER_LENGTH = 0.0;
public static final double BASE_LENGTH = 0.0;
public static final double MINIMUM_LENGTH_OF_DART = 0.0;
public static final double SUM_OF_SQUARES_OF_LENGTHS = SHOOTER_LENGTH * SHOOTER_LENGTH + BASE_LENGTH * BASE_LENGTH;

public static final double PIVOT_TWICE_THE_PRODUCT_OF_LENGTHS = 2 * PIVOT_SHOOTER_LENGTH * PIVOT_BASE_LENGTH;
public static final double TWICE_THE_PRODUCT_OF_LENGTHS = 2 * SHOOTER_LENGTH * BASE_LENGTH;

public static final double PIVOT_OFFSET_ANGLE = Math.acos(
(SUM_OF_SQUARES_OF_LENGTHS - PIVOT_MINIMUM_LENGTH_OF_DART * PIVOT_MINIMUM_LENGTH_OF_DART) /
PIVOT_TWICE_THE_PRODUCT_OF_LENGTHS
public static final double OFFSET_ANGLE = Math.acos(
(SUM_OF_SQUARES_OF_LENGTHS - MINIMUM_LENGTH_OF_DART * MINIMUM_LENGTH_OF_DART) / TWICE_THE_PRODUCT_OF_LENGTHS
);

public static final double PIVOT_MINIMUM_ANGLE = 0.0;
public static final double PIVOT_MAXIMUM_ANGLE = 0.0;
public static final double MINIMUM_ANGLE = 0.0;
public static final double MAXIMUM_ANGLE = 0.0;

public static final PIDConfig PIVOT_PID = new PIDConfig(0.0, 0.0, 0.0, 0.0);
public static final double PIVOT_PID_MIN_OUTPUT = 0.0;
public static final double PIVOT_PID_MAX_OUTPUT = 0.0;
public static final double PIVOT_MIN_VEL = 0.0;
public static final double PIVOT_MAX_VEL = 0.0;
public static final double PIVOT_MAX_ACCEL = 0.0;
public static final double PIVOT_CLOSED_LOOP_ERR = 0.0;
public static final PIDConfig PID = new PIDConfig(0.0, 0.0, 0.0, 0.0);
public static final double PID_MIN_OUTPUT = 0.0;
public static final double PID_MAX_OUTPUT = 0.0;
public static final double MIN_VEL = 0.0;
public static final double MAX_VEL = 0.0;
public static final double MAX_ACCEL = 0.0;
public static final double CLOSED_LOOP_ERR = 0.0;
}
}

Expand Down
56 changes: 31 additions & 25 deletions src/main/java/org/team340/robot/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkAbsoluteEncoder;
import com.revrobotics.SparkAbsoluteEncoder.Type;
import com.revrobotics.SparkPIDController;
import edu.wpi.first.wpilibj2.command.Command;
import org.team340.lib.GRRSubsystem;
Expand All @@ -16,70 +18,74 @@
*/
public class Intake extends GRRSubsystem {

private final CANSparkMax deployMotor = createSparkMax("Deploy Motor", RobotMap.INTAKE_DEPLOY_MOTOR, MotorType.kBrushless);
private final CANSparkMax rollerMotor = createSparkMax("Roller Motor", RobotMap.INTAKE_ROLLER_MOTOR, MotorType.kBrushless);
private final CANSparkMax pivotLeftMotor = createSparkMax("Deploy Motor", RobotMap.INTAKE_PIVOT_LEFT_MOTOR, MotorType.kBrushless);
private final CANSparkMax pivotRightMotor = createSparkMax("Deploy Motor", RobotMap.INTAKE_PIVOT_RIGHT_MOTOR, MotorType.kBrushless);
private final CANSparkMax rollerUpperMotor = createSparkMax("Roller Motor", RobotMap.INTAKE_ROLLER_UPPER_MOTOR, MotorType.kBrushless);
private final CANSparkMax rollerLowerMotor = createSparkMax("Roller Motor", RobotMap.INTAKE_ROLLER_LOWER_MOTOR, MotorType.kBrushless);

private final SparkPIDController deployPID = deployMotor.getPIDController();
private final SparkPIDController pivotPID = pivotLeftMotor.getPIDController();
private final SparkAbsoluteEncoder pivotEncoder = createSparkMaxAbsoluteEncoder("Pivot Encoder", pivotLeftMotor, Type.kDutyCycle);

public Intake() {
super("Intake");
deployPID.setP(IntakeConstants.INTAKE_DEPLOY_STRONG_PID.p(), 0);
deployPID.setI(IntakeConstants.INTAKE_DEPLOY_STRONG_PID.i(), 0);
deployPID.setD(IntakeConstants.INTAKE_DEPLOY_STRONG_PID.d(), 0);
IntakeConstants.PIVOT_LEFT_MOTOR_CONFIG.apply(pivotLeftMotor);
IntakeConstants.PIVOT_RIGHT_MOTOR_CONFIG.apply(pivotRightMotor);
IntakeConstants.ROLLER_UPPER_MOTOR_CONFIG.apply(rollerUpperMotor);
IntakeConstants.ROLLER_LOWER_MOTOR_CONFIG.apply(rollerLowerMotor);

deployPID.setP(IntakeConstants.INTAKE_DEPLOY_WEAK_PID.p(), 1);
deployPID.setI(IntakeConstants.INTAKE_DEPLOY_WEAK_PID.i(), 1);
deployPID.setD(IntakeConstants.INTAKE_DEPLOY_WEAK_PID.d(), 1);
IntakeConstants.PIVOT_MOTOR_PID_CONFIG.apply(pivotLeftMotor, pivotPID);

IntakeConstants.PIVOT_ENCODER_CONFIG.apply(pivotLeftMotor, pivotEncoder);
}

public Command deploy() {
Mutable<Boolean> weakPID = new Mutable<>(false);

return commandBuilder("intake.deploy()")
.onInitialize(() -> rollerMotor.set(IntakeConstants.INTAKE_DEPLOY_ROLLER_SPEED))
.onInitialize(() -> rollerUpperMotor.set(IntakeConstants.DEPLOY_ROLLER_SPEED))
.onExecute(() -> {
if (
Math2.epsilonEquals(
IntakeConstants.INTAKE_DEPLOY_POSITION,
deployMotor.getEncoder().getPosition(),
IntakeConstants.INTAKE_DEPLOY_POSITION_TOLERANCE
IntakeConstants.DEPLOY_POSITION,
pivotLeftMotor.getEncoder().getPosition(),
IntakeConstants.DEPLOY_POSITION_TOLERANCE
)
) weakPID.set(true);
deployPID.setReference(IntakeConstants.INTAKE_DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0);
pivotPID.setReference(IntakeConstants.DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0);
})
.onEnd(() -> {
rollerMotor.stopMotor();
deployMotor.stopMotor();
rollerUpperMotor.stopMotor();
pivotLeftMotor.stopMotor();
});
}

public Command retract() {
return commandBuilder("intake.retract()")
.onInitialize(() -> rollerMotor.stopMotor())
.onInitialize(() -> rollerUpperMotor.stopMotor())
.onExecute(() -> {
deployPID.setReference(0, ControlType.kPosition, 0);
pivotPID.setReference(0, ControlType.kPosition, 0);
});
}

public Command spit() {
Mutable<Boolean> weakPID = new Mutable<>(false);

return commandBuilder("intake.spit()")
.onInitialize(() -> rollerMotor.set(IntakeConstants.INTAKE_SPIT_ROLLER_SPEED))
.onInitialize(() -> rollerUpperMotor.set(IntakeConstants.SPIT_ROLLER_SPEED))
.onExecute(() -> {
if (
Math2.epsilonEquals(
IntakeConstants.INTAKE_DEPLOY_POSITION,
deployMotor.getEncoder().getPosition(),
IntakeConstants.INTAKE_DEPLOY_POSITION_TOLERANCE
IntakeConstants.DEPLOY_POSITION,
pivotLeftMotor.getEncoder().getPosition(),
IntakeConstants.DEPLOY_POSITION_TOLERANCE
)
) weakPID.set(true);

deployPID.setReference(IntakeConstants.INTAKE_DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0);
pivotPID.setReference(IntakeConstants.DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0);
})
.onEnd(() -> {
rollerMotor.stopMotor();
deployMotor.stopMotor();
rollerUpperMotor.stopMotor();
pivotLeftMotor.stopMotor();
});
}
}
50 changes: 21 additions & 29 deletions src/main/java/org/team340/robot/subsystems/Shooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,41 +52,33 @@ public class Shooter extends GRRSubsystem {

public Shooter() {
super("Shooter");
pivotRelativeEncoder.setPositionConversionFactor(PivotConstants.PIVOT_REL_ENC_CONVERSION);
pivotRelativeEncoder.setPositionConversionFactor(PivotConstants.REL_ENC_CONVERSION);
pivotAbsoluteEncoder.setPositionConversionFactor(Math2.TWO_PI);

new SparkPIDControllerConfig()
.setPID(PivotConstants.PIVOT_PID.p(), PivotConstants.PIVOT_PID.i(), PivotConstants.PIVOT_PID.d())
.setIZone(PivotConstants.PIVOT_PID.iZone())
.setOutputRange(PivotConstants.PIVOT_PID_MIN_OUTPUT, PivotConstants.PIVOT_PID_MAX_OUTPUT)
.setPID(PivotConstants.PID.p(), PivotConstants.PID.i(), PivotConstants.PID.d())
.setIZone(PivotConstants.PID.iZone())
.setOutputRange(PivotConstants.PID_MIN_OUTPUT, PivotConstants.PID_MAX_OUTPUT)
.apply(pivotMotor, pivotPID);
pivotPID.setSmartMotionMaxVelocity(PivotConstants.PIVOT_MAX_VEL, 0);
pivotPID.setSmartMotionMaxVelocity(PivotConstants.PIVOT_MAX_VEL, 0);
pivotPID.setSmartMotionMinOutputVelocity(PivotConstants.PIVOT_MAX_VEL, 0);
pivotPID.setSmartMotionMaxAccel(PivotConstants.PIVOT_MAX_ACCEL, 0);
pivotPID.setSmartMotionAllowedClosedLoopError(PivotConstants.PIVOT_CLOSED_LOOP_ERR, 0);
pivotPID.setSmartMotionMaxVelocity(PivotConstants.MAX_VEL, 0);
pivotPID.setSmartMotionMaxVelocity(PivotConstants.MAX_VEL, 0);
pivotPID.setSmartMotionMinOutputVelocity(PivotConstants.MAX_VEL, 0);
pivotPID.setSmartMotionMaxAccel(PivotConstants.MAX_ACCEL, 0);
pivotPID.setSmartMotionAllowedClosedLoopError(PivotConstants.CLOSED_LOOP_ERR, 0);

new SparkPIDControllerConfig()
.setPID(ShooterConstants.SHOOTER_FEED_PID.p(), ShooterConstants.SHOOTER_FEED_PID.i(), ShooterConstants.SHOOTER_FEED_PID.d())
.setIZone(ShooterConstants.SHOOTER_FEED_PID.iZone())
.setPID(ShooterConstants.FEED_PID.p(), ShooterConstants.FEED_PID.i(), ShooterConstants.FEED_PID.d())
.setIZone(ShooterConstants.FEED_PID.iZone())
.apply(feedMotor, feedPID);

new SparkPIDControllerConfig()
.setPID(
ShooterConstants.SHOOTER_LEFT_SHOOT_PID.p(),
ShooterConstants.SHOOTER_LEFT_SHOOT_PID.i(),
ShooterConstants.SHOOTER_LEFT_SHOOT_PID.d()
)
.setIZone(ShooterConstants.SHOOTER_LEFT_SHOOT_PID.iZone())
.setPID(ShooterConstants.LEFT_SHOOT_PID.p(), ShooterConstants.LEFT_SHOOT_PID.i(), ShooterConstants.LEFT_SHOOT_PID.d())
.setIZone(ShooterConstants.LEFT_SHOOT_PID.iZone())
.apply(leftShootMotor, leftShootPID);

new SparkPIDControllerConfig()
.setPID(
ShooterConstants.SHOOTER_RIGHT_SHOOT_PID.p(),
ShooterConstants.SHOOTER_RIGHT_SHOOT_PID.i(),
ShooterConstants.SHOOTER_RIGHT_SHOOT_PID.d()
)
.setIZone(ShooterConstants.SHOOTER_RIGHT_SHOOT_PID.iZone())
.setPID(ShooterConstants.RIGHT_SHOOT_PID.p(), ShooterConstants.RIGHT_SHOOT_PID.i(), ShooterConstants.RIGHT_SHOOT_PID.d())
.setIZone(ShooterConstants.RIGHT_SHOOT_PID.iZone())
.apply(rightShootMotor, rightShootPID);
}

Expand All @@ -95,14 +87,14 @@ public Shooter() {
* @param angleToShootAt This is the angle that will be used.
*/
private void setShooterToAngle(double angleToShootAt) {
if (angleToShootAt < PivotConstants.PIVOT_MINIMUM_ANGLE || angleToShootAt > PivotConstants.PIVOT_MAXIMUM_ANGLE) {
if (angleToShootAt < PivotConstants.MINIMUM_ANGLE || angleToShootAt > PivotConstants.MAXIMUM_ANGLE) {
DriverStation.reportWarning(
"Invalid shooter pivot angle. " +
angleToShootAt +
" is not between " +
PivotConstants.PIVOT_MINIMUM_ANGLE +
PivotConstants.MINIMUM_ANGLE +
" and " +
PivotConstants.PIVOT_MAXIMUM_ANGLE,
PivotConstants.MAXIMUM_ANGLE,
false
);
return;
Expand All @@ -118,10 +110,10 @@ private void setShooterToAngle(double angleToShootAt) {
private double dartExtensionFromAngle(double sensorAngle) {
double totalLengthOfDart = Math.sqrt(
PivotConstants.SUM_OF_SQUARES_OF_LENGTHS -
PivotConstants.PIVOT_TWICE_THE_PRODUCT_OF_LENGTHS *
Math.cos(sensorAngle + PivotConstants.PIVOT_OFFSET_ANGLE)
PivotConstants.TWICE_THE_PRODUCT_OF_LENGTHS *
Math.cos(sensorAngle + PivotConstants.OFFSET_ANGLE)
);
return (totalLengthOfDart - PivotConstants.PIVOT_MINIMUM_LENGTH_OF_DART);
return (totalLengthOfDart - PivotConstants.MINIMUM_LENGTH_OF_DART);
}

/**
Expand Down

0 comments on commit 715a63f

Please sign in to comment.