Skip to content

Commit

Permalink
Move pivot to separate class
Browse files Browse the repository at this point in the history
  • Loading branch information
@JackJ09
JackJ09 committed Feb 2, 2024
1 parent 715a63f commit 48b0a5d
Show file tree
Hide file tree
Showing 4 changed files with 147 additions and 82 deletions.
114 changes: 73 additions & 41 deletions src/main/java/org/team340/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
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.SparkFlexConfig;
import org.team340.lib.util.config.rev.SparkMaxConfig;
import org.team340.lib.util.config.rev.SparkPIDControllerConfig;

Expand Down Expand Up @@ -78,22 +79,23 @@ 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_PIVOT_LEFT_MOTOR = 20;
public static final int INTAKE_PIVOT_RIGHT_MOTOR = 21;
public static final int INTAKE_ARM_LEFT_MOTOR = 20;
public static final int INTAKE_ARM_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;
public static final int SHOOTER_LEFT_SHOOT_MOTOR = 32;
public static final int SHOOTER_RIGHT_SHOOT_MOTOR = 33;
public static final int SHOOTER_SHOOT_LEFT_MOTOR = 32;
public static final int SHOOTER_SHOOT_RIGHT_MOTOR = 33;

public static final int SHOOTER_NOTE_DETECTOR = 0;
public static final int PIVOT_LOWER_LIMIT = 1;
}

public static final class IntakeConstants {

private static final SparkMaxConfig PIVOT_MOTOR_BASE_CONFIG = new SparkMaxConfig()
private static final SparkMaxConfig ARM_MOTOR_BASE_CONFIG = new SparkMaxConfig()
.clearFaults()
.restoreFactoryDefaults()
.enableVoltageCompensation(VOLTAGE)
Expand All @@ -110,23 +112,23 @@ public static final class IntakeConstants {
.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
public static final SparkMaxConfig ARM_LEFT_MOTOR_CONFIG = ARM_MOTOR_BASE_CONFIG.clone().setInverted(false);
public static final SparkMaxConfig ARM_RIGHT_MOTOR_CONFIG = ARM_MOTOR_BASE_CONFIG
.clone()
.follow(ExternalFollower.kFollowerSpark, RobotMap.INTAKE_PIVOT_LEFT_MOTOR, false);
.follow(ExternalFollower.kFollowerSpark, RobotMap.INTAKE_ARM_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 int ARM_WEAK_PID_SLOT = 0;
public static final int ARM_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 SparkPIDControllerConfig ARM_MOTOR_PID_CONFIG = new SparkPIDControllerConfig()
.setPID(0.0, 0.0, 0.0, ARM_WEAK_PID_SLOT)
.setPID(0.0, 0.0, 0.0, ARM_STRONG_PID_SLOT);

public static final AbsoluteEncoderConfig PIVOT_ENCODER_CONFIG = new AbsoluteEncoderConfig()
public static final AbsoluteEncoderConfig ARM_ENCODER_CONFIG = new AbsoluteEncoderConfig()
.setZeroOffset(0.0)
.setInverted(false)
.setPositionConversionFactor(Math2.TWO_PI)
Expand All @@ -140,42 +142,72 @@ public static final class IntakeConstants {
}

public static final class ShooterConstants {
private static final SparkFlexConfig SHOOT_MOTOR_BASE_CONFIG = new SparkFlexConfig()
.clearFaults()
.restoreFactoryDefaults()
.enableVoltageCompensation(VOLTAGE)
.setSmartCurrentLimit(60, 30)
.setIdleMode(IdleMode.kCoast)
.setClosedLoopRampRate(1.5)
.setOpenLoopRampRate(1.5);

public static final double FEED_INTAKE_SPEED = 0.0;
public static final SparkFlexConfig SHOOT_LEFT_MOTOR_CONFIG = SHOOT_MOTOR_BASE_CONFIG.clone().setInverted(false);
public static final SparkFlexConfig SHOOT_RIGHT_MOTOR_CONFIG = SHOOT_MOTOR_BASE_CONFIG.clone().setInverted(true);

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);
public static final SparkMaxConfig FEED_MOTOR_CONFIG = new SparkMaxConfig()
.clearFaults()
.restoreFactoryDefaults()
.enableVoltageCompensation(VOLTAGE)
.setSmartCurrentLimit(30)
.setIdleMode(IdleMode.kCoast)
.setClosedLoopRampRate(1.5)
.setOpenLoopRampRate(1.5);

/**
* All distances in inches. All angles in radians.
*/
public static final class PivotConstants {
public static final SparkPIDControllerConfig FEED_PID_CONFIG = new SparkPIDControllerConfig().setPID(0.0, 0.0, 0.0, 0);
public static final SparkPIDControllerConfig SHOOT_PID_CONFIG = new SparkPIDControllerConfig().setPID(0.0, 0.0, 0.0, 0);

public static final double REL_ENC_CONVERSION = 0.2;
public static final double FEED_INTAKE_SPEED = 0.0;
}

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;
/**
* All distances in inches. All angles in radians.
*/
public static final class PivotConstants {
public static final SparkMaxConfig PIVOT_MOTOR_CONFIG = new SparkMaxConfig()
.clearFaults()
.restoreFactoryDefaults()
.enableVoltageCompensation(VOLTAGE)
.setSmartCurrentLimit(30)
.setIdleMode(IdleMode.kCoast)
.setClosedLoopRampRate(1.5)
.setOpenLoopRampRate(1.5);

public static final SparkPIDControllerConfig PIVOT_PID_CONFIG = new SparkPIDControllerConfig().setPID(0.0, 0.0, 0.0, 0);

public static final double TWICE_THE_PRODUCT_OF_LENGTHS = 2 * SHOOTER_LENGTH * BASE_LENGTH;
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;
public static final double REL_ENC_CONVERSION = 0.2;

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 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 MINIMUM_ANGLE = 0.0;
public static final double MAXIMUM_ANGLE = 0.0;
public static final double TWICE_THE_PRODUCT_OF_LENGTHS = 2 * SHOOTER_LENGTH * BASE_LENGTH;

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 MINIMUM_ANGLE = 0.0;
public static final double MAXIMUM_ANGLE = 0.0;

public static final double HOMING_SPEED = 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
30 changes: 15 additions & 15 deletions src/main/java/org/team340/robot/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,24 +18,24 @@
*/
public class Intake extends GRRSubsystem {

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 armLeftMotor = createSparkMax("Deploy Motor", RobotMap.INTAKE_ARM_LEFT_MOTOR, MotorType.kBrushless);
private final CANSparkMax armRightMotor = createSparkMax("Deploy Motor", RobotMap.INTAKE_ARM_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 pivotPID = pivotLeftMotor.getPIDController();
private final SparkAbsoluteEncoder pivotEncoder = createSparkMaxAbsoluteEncoder("Pivot Encoder", pivotLeftMotor, Type.kDutyCycle);
private final SparkPIDController armPID = armLeftMotor.getPIDController();
private final SparkAbsoluteEncoder armEncoder = createSparkMaxAbsoluteEncoder("Arm Encoder", armLeftMotor, Type.kDutyCycle);

public Intake() {
super("Intake");
IntakeConstants.PIVOT_LEFT_MOTOR_CONFIG.apply(pivotLeftMotor);
IntakeConstants.PIVOT_RIGHT_MOTOR_CONFIG.apply(pivotRightMotor);
IntakeConstants.ARM_LEFT_MOTOR_CONFIG.apply(armLeftMotor);
IntakeConstants.ARM_RIGHT_MOTOR_CONFIG.apply(armRightMotor);
IntakeConstants.ROLLER_UPPER_MOTOR_CONFIG.apply(rollerUpperMotor);
IntakeConstants.ROLLER_LOWER_MOTOR_CONFIG.apply(rollerLowerMotor);

IntakeConstants.PIVOT_MOTOR_PID_CONFIG.apply(pivotLeftMotor, pivotPID);
IntakeConstants.ARM_MOTOR_PID_CONFIG.apply(armLeftMotor, armPID);

IntakeConstants.PIVOT_ENCODER_CONFIG.apply(pivotLeftMotor, pivotEncoder);
IntakeConstants.ARM_ENCODER_CONFIG.apply(armLeftMotor, armEncoder);
}

public Command deploy() {
Expand All @@ -47,23 +47,23 @@ public Command deploy() {
if (
Math2.epsilonEquals(
IntakeConstants.DEPLOY_POSITION,
pivotLeftMotor.getEncoder().getPosition(),
armLeftMotor.getEncoder().getPosition(),
IntakeConstants.DEPLOY_POSITION_TOLERANCE
)
) weakPID.set(true);
pivotPID.setReference(IntakeConstants.DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0);
armPID.setReference(IntakeConstants.DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0);
})
.onEnd(() -> {
rollerUpperMotor.stopMotor();
pivotLeftMotor.stopMotor();
armLeftMotor.stopMotor();
});
}

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

Expand All @@ -76,16 +76,16 @@ public Command spit() {
if (
Math2.epsilonEquals(
IntakeConstants.DEPLOY_POSITION,
pivotLeftMotor.getEncoder().getPosition(),
armLeftMotor.getEncoder().getPosition(),
IntakeConstants.DEPLOY_POSITION_TOLERANCE
)
) weakPID.set(true);

pivotPID.setReference(IntakeConstants.DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0);
armPID.setReference(IntakeConstants.DEPLOY_POSITION, ControlType.kPosition, weakPID.get() ? 1 : 0);
})
.onEnd(() -> {
rollerUpperMotor.stopMotor();
pivotLeftMotor.stopMotor();
armLeftMotor.stopMotor();
});
}
}
57 changes: 57 additions & 0 deletions src/main/java/org/team340/robot/subsystems/Pivot.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,57 @@
package org.team340.robot.subsystems;

import org.team340.lib.GRRSubsystem;
import org.team340.robot.Constants.PivotConstants;
import org.team340.robot.Constants.RobotMap;

import com.revrobotics.CANSparkLowLevel.MotorType;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.Command;

import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkPIDController;
import com.revrobotics.CANSparkBase.ControlType;

public class Pivot extends GRRSubsystem {
private final CANSparkMax pivotMotor = createSparkMax("Pivot Motor", RobotMap.SHOOTER_PIVOT_MOTOR, MotorType.kBrushless);
private final SparkPIDController pivotPID = pivotMotor.getPIDController();
private final DigitalInput lowerLimit = new DigitalInput(RobotMap.PIVOT_LOWER_LIMIT);

private boolean hasBeenHomed;

public Pivot() {
super("Pivot");

PivotConstants.PIVOT_MOTOR_CONFIG.apply(pivotMotor);

PivotConstants.PIVOT_PID_CONFIG.apply(pivotMotor, pivotPID);
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);

hasBeenHomed = false;
}

/**
* Homes the pivot by driving slowly down until reaching limit.
* @param withOverride If true will ignore {@code hasBeenHomed}.
*/
public Command home(boolean withOverride) {
return commandBuilder("pivot.home()")
.onExecute(() -> {
if(!hasBeenHomed || withOverride) {
pivotMotor.set(PivotConstants.HOMING_SPEED);
} else {
pivotMotor.stopMotor();
}
})
.isFinished(() -> lowerLimit.get() || (hasBeenHomed && !withOverride))
.onEnd((interrupted) -> {
pivotMotor.stopMotor();
hasBeenHomed = !interrupted;
});
}
}
28 changes: 2 additions & 26 deletions src/main/java/org/team340/robot/subsystems/Shooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,52 +19,28 @@
import org.team340.lib.util.config.rev.SparkPIDControllerConfig;
import org.team340.robot.Constants.RobotMap;
import org.team340.robot.Constants.ShooterConstants;
import org.team340.robot.Constants.ShooterConstants.PivotConstants;

public class Shooter extends GRRSubsystem {

private final CANSparkMax pivotMotor = createSparkMax("Pivot Motor", RobotMap.SHOOTER_PIVOT_MOTOR, MotorType.kBrushless);
private final CANSparkMax feedMotor = createSparkMax("Feeder Motor", RobotMap.SHOOTER_FEED_MOTOR, MotorType.kBrushless);
private final CANSparkFlex leftShootMotor = createSparkFlex(
"Shooter Motor Left",
RobotMap.SHOOTER_LEFT_SHOOT_MOTOR,
RobotMap.SHOOTER_SHOOT_LEFT_MOTOR,
MotorType.kBrushless
);
private final CANSparkFlex rightShootMotor = createSparkFlex(
"Shooter Motor Right",
RobotMap.SHOOTER_RIGHT_SHOOT_MOTOR,
RobotMap.SHOOTER_SHOOT_RIGHT_MOTOR,
MotorType.kBrushless
);

private final SparkPIDController pivotPID = pivotMotor.getPIDController();
private final SparkPIDController feedPID = feedMotor.getPIDController();
private final SparkPIDController leftShootPID = leftShootMotor.getPIDController();
private final SparkPIDController rightShootPID = rightShootMotor.getPIDController();

private final AbsoluteEncoder pivotAbsoluteEncoder = createSparkMaxAbsoluteEncoder(
"Pivot Absolute Encoder",
pivotMotor,
Type.kDutyCycle
);
private final RelativeEncoder pivotRelativeEncoder = pivotMotor.getEncoder();

private final DigitalInput noteDetector = createDigitalInput("Note Detector", RobotMap.SHOOTER_NOTE_DETECTOR);

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

new SparkPIDControllerConfig()
.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.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.FEED_PID.p(), ShooterConstants.FEED_PID.i(), ShooterConstants.FEED_PID.d())
Expand Down

0 comments on commit 48b0a5d

Please sign in to comment.