Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Prep for 2/17 meeting. Finished removing TrashMotion.

Browse Source
This commit is contained in:
NoahLacks63 2025-02-17 05:11:38 +00:00
parent 9fc597bd30
commit 2e9f294cdb
6 changed files with 91 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/main/java/frc/robot/RobotContainer.java
View File

@ -9,15 +9,12 @@ import frc.robot.constants.ClimberPivotConstants;
import frc.robot.constants.ElevatorConstants;
import frc.robot.constants.OIConstants;
import frc.robot.subsystems.ManipulatorPivot;
import frc.robot.subsystems.sysid.ElevatorSysID;
import frc.robot.subsystems.ClimberPivot;
import frc.robot.subsystems.ClimberRollers;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Manipulator;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
@ -112,7 +109,7 @@ public class RobotContainer {
manipulatorPivot.setDefaultCommand(
manipulatorPivot.setSpeed(
manipulatorPivot.runManualPivot(
() -> 0
)
);
@ -305,7 +302,7 @@ public class RobotContainer {
// If the target position is behind the brace, and the arm is not behind the brace, move the arm to a safe position first,
// then the elevator, then the arm again
} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition));
// If the target position is behind the brace, and the arm is behind the brace, move the elevator first, then the arm
} else if (!manipulatorPivot.isMotionSafe(armPosition) && manipulatorPivot.isMotionSafe()) {
@ -315,7 +312,7 @@ public class RobotContainer {
return moveManipulatorUtil(elevatorPosition, armPosition, false, true);
// Catch all command that's safe regardless of arm and elevator positions
} else {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition));
}
}
@ -357,13 +354,13 @@ public class RobotContainer {
private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
if(!isL4){
return Commands.sequence(
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
elevator.goToSetpoint(() -> elevatorPosition),
manipulatorPivot.goToSetpoint(armPosition));
}else{
return Commands.sequence(
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kArmSafeStowPosition),
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
}
@ -379,7 +376,7 @@ public class RobotContainer {
*/
@SuppressWarnings("unused")
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kArmSafeStowPosition, false, true)
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition));
}

10
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -27,12 +27,12 @@ public class ElevatorConstants {
public static final double kAllowedError = 0.75;
public static final double kFeedForwardS = 0;
public static final double kFeedForwardG = .7;//1; // calculated value .6
public static final double kFeedForwardV = 0.12; // calculated value .12 INCHES NOT METERS
public static final double kFeedForwardS = 0; /* kG too high - kG too low / 2 */
public static final double kFeedForwardG = .7; /* kG too high + kG too low / 2 */ // calculated value 0.6
public static final double kFeedForwardV = 0.12; // calculated value 0.12
public static final double kMaxVelocity = 120.0; // 120 inches per second (COOKING) calculated max is 184 in/s
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
public static final double kMaxVelocity = 120.0; // 120 inches per second (COOKING) calculated max is 184 in/s
public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
public static final double kCoralIntakePosition = 0;
public static final double kL1Position = 0;

31
src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
View File

@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Seconds;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@ -13,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
public class ManipulatorPivotConstants {
public static final int kArmMotorID = 1;
public static final int kPivotMotorID = 1;
public static final int kMotorCurrentMax = 40;
@ -27,14 +26,16 @@ public class ManipulatorPivotConstants {
public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
public static final double kFeedForwardS = 0;
public static final double kFeedForwardG = 0.41; // calculated value
public static final double kFeedForwardV = 0.68; //calculated value
public static final double kFeedForwardG = 0.41; // calculated value 0.41
public static final double kFeedForwardV = 0.68; //calculated value 0.68
public static final double kFFGravityOffset = Units.degreesToRadians(-135.0);
public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
public static final double kEncoderOffset = 0;
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
public static final double kL1Position = Units.degreesToRadians(0.0);
public static final double kL2Position = Units.degreesToRadians(25.0);
@ -43,17 +44,16 @@ public class ManipulatorPivotConstants {
public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
public static final double kProcesserPosition = Units.degreesToRadians(175.0);
public static final double kNetPosition = Units.degreesToRadians(175.0);
/**The closest position to the elevator brace without hitting it */
public static final double kArmSafeStowPosition = Units.degreesToRadians(60.0);
/**The forward rotation limit of the arm */
public static final double kPivotSafeStowPosition = Units.degreesToRadians(60.0);
/**The forward rotation limit of the pivot */
public static final double kRotationLimit = Units.degreesToRadians(175.0);
public static final double kSysIDRampRate = 1;
public static final double kSysIDStepVolts = 7;
public static final double kSysIDTimeout = 10;
public static final SensorDirectionValue kSensorDirection = SensorDirectionValue.CounterClockwise_Positive;
public static final IdleMode kIdleMode = IdleMode.kBrake;
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIG
@ -67,20 +67,13 @@ public class ManipulatorPivotConstants {
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
static {
motorConfig
.smartCurrentLimit(kMotorCurrentMax)
.idleMode(kIdleMode);
motorConfig.absoluteEncoder.positionConversionFactor(kPivotConversion);
motorConfig.closedLoop
.p(kPositionalP)
.i(kPositionalI)
.d(kPositionalD)
.velocityFF(0.0); // keep at zero for position pid
motorConfig.closedLoop.maxMotion
.maxAcceleration(kMaxAcceleration)
.maxVelocity(kMaxVelocity)
.allowedClosedLoopError(kPositionalTolerance);
motorConfig.absoluteEncoder
.positionConversionFactor(kPivotConversion)
.inverted(false)
.zeroOffset(kEncoderOffset);
}

51
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -3,18 +3,13 @@ package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkBase;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.DigitalInput;
@ -26,8 +21,6 @@ public class Elevator extends SubsystemBase {
protected SparkMax elevatorMotor1;
protected SparkMax elevatorMotor2;
//private SparkClosedLoopController elevatorClosedLoop;
protected RelativeEncoder encoder;
private DigitalInput bottomLimitSwitch;
@ -127,6 +120,12 @@ public class Elevator extends SubsystemBase {
});
}
/**
* A command that will use the feed forward to hold up the elevator.
* Used for feed forward tuning.
*
* @return Sets motor voltage based on feed forward calculation.
*/
public Command maintainPosition() {
return run(() -> {
elevatorMotor1.setVoltage(feedForward.calculate(0));
@ -148,26 +147,14 @@ public class Elevator extends SubsystemBase {
);
return run(() -> {
/*
if(pidController.atGoal()) {
elevatorMotor1.setVoltage(feedForward.calculate(0));
} else {
*/
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(pidController.getSetpoint().velocity)
);
/*elevatorClosedLoop.setReference(
clampedSetpoint,
ControlType.kMAXMotionPositionControl,
ClosedLoopSlot.kSlot0,
feedForward.calculate(0)
);*/
pidController.reset(encoder.getPosition(), encoder.getVelocity());
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(pidController.getSetpoint().velocity)
);
});
}
@ -191,10 +178,20 @@ public class Elevator extends SubsystemBase {
return bottomLimitSwitch.get();
}
/**
* Returns the motor's output current
*
* @return Motor output current
*/
public double getMotor1() {
return elevatorMotor1.getOutputCurrent();
}
/**
* Returns the motor's output current
*
* @return Motor output current
*/
public double getMotor2() {
return elevatorMotor2.getOutputCurrent();
}

66
src/main/java/frc/robot/subsystems/ManipulatorPivot.java
View File

@ -4,42 +4,47 @@ import com.revrobotics.spark.SparkMax;
import java.util.function.DoubleSupplier;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkAbsoluteEncoder;
import com.revrobotics.spark.SparkBase;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ManipulatorPivotConstants;
public class ManipulatorPivot extends SubsystemBase {
protected SparkMax armMotor;
protected SparkMax pivotMotor;
private SparkAbsoluteEncoder encoder;
private ArmFeedforward feedForward;
private SparkClosedLoopController pivotClosedLoopController;
private SparkAbsoluteEncoder absoluteEncoder;
private ProfiledPIDController pidController;
public ManipulatorPivot() {
armMotor = new SparkMax(
ManipulatorPivotConstants.kArmMotorID,
pivotMotor = new SparkMax(
ManipulatorPivotConstants.kPivotMotorID,
MotorType.kBrushless
);
armMotor.configure(
pivotMotor.configure(
ManipulatorPivotConstants.motorConfig,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
encoder = pivotMotor.getAbsoluteEncoder();
pivotClosedLoopController = armMotor.getClosedLoopController();
absoluteEncoder = armMotor.getAbsoluteEncoder();
pidController = new ProfiledPIDController(
getEncoderPosition(),
getEncoderVelocity(),
getEncoderPosition(),
null
);
feedForward = new ArmFeedforward(
ManipulatorPivotConstants.kFeedForwardS,
@ -66,12 +71,18 @@ public class ManipulatorPivot extends SubsystemBase {
* @return Is the motion safe
*/
public boolean isMotionSafe(double motionTarget) {
return motionTarget > ManipulatorPivotConstants.kArmSafeStowPosition;
return motionTarget > ManipulatorPivotConstants.kPivotSafeStowPosition;
}
public Command setSpeed(DoubleSupplier speed) {
/**
* Manual ManipulatorPivot command that sets the motor based on speed
*
* @param speed The speed to set the motor
* @return A command that sets the motor speed
*/
public Command runManualPivot(DoubleSupplier speed) {
return run(() -> {
armMotor.set(speed.getAsDouble());
pivotMotor.set(speed.getAsDouble());
});
}
@ -90,27 +101,34 @@ public class ManipulatorPivot extends SubsystemBase {
);
return run(() -> {
pivotClosedLoopController.setReference(clampedSetpoint,
SparkBase.ControlType.kMAXMotionPositionControl,
ClosedLoopSlot.kSlot1,
feedForward.calculate(absoluteEncoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, absoluteEncoder.getVelocity()));
pidController.reset(encoder.getPosition(), encoder.getVelocity());
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(
encoder.getPosition(),
0
)
);
});
}
/**
* Returns the CANCoder's position in radians
* Returns the encoder's position in radians
*
* @return CANCoder's position in radians
* @return Encoder's position in radians
*/
public double getEncoderPosition() {
return absoluteEncoder.getPosition();
return encoder.getPosition();
}
/**
* Returns the CANCoder's velocity in radians per second
* Returns the encoder's velocity in radians per second
*
* @return CANCoder's velocity in radians per second
* @return Encoder's velocity in radians per second
*/
public double getEncoderVelocity() {
return absoluteEncoder.getVelocity();
return encoder.getVelocity();
}
}

4
src/main/java/frc/robot/subsystems/sysid/ManipulatorPivotSysID.java
View File

@ -34,11 +34,11 @@ public class ManipulatorPivotSysID extends ManipulatorPivot {
routine = new SysIdRoutine(
ManipulatorPivotConstants.kSysIDConfig,
new SysIdRoutine.Mechanism(
armMotor::setVoltage,
pivotMotor::setVoltage,
(log) -> {
log.motor("armMotor")
.voltage(appliedVoltage.mut_replace(
armMotor.get() * RobotController.getBatteryVoltage(), Volts
pivotMotor.get() * RobotController.getBatteryVoltage(), Volts
))
.angularPosition(pivotPosition.mut_replace(
getEncoderPosition(), Radians