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

stuff works more tuning

Browse Source
This commit is contained in:
Team 2648 2025-02-20 17:40:16 -05:00
parent 858c897aad
commit c48a53a0a5
4 changed files with 102 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -19,6 +19,7 @@ import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@ -102,17 +103,17 @@ public class RobotContainer {
() -> true
)
);
/*
elevator.setDefaultCommand(
elevator.goToSetpoint(
() -> 0
)
);
*/
manipulatorPivot.setDefaultCommand(
manipulatorPivot.runManualPivot(
() -> 0
() -> operator.getRightY() * 0.5
)
);
@ -140,28 +141,28 @@ public class RobotContainer {
driver.start().and(driver.back()).onTrue(
startingConfig()
);
*/
driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
driver.povUp().whileTrue(climberPivot.runPivot(0.5));
driver.povLeft().whileTrue(climberRollers.runRoller(0.5));
driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
*/
operator.povUp().whileTrue(
elevator.goToSetpoint(() -> 50)
manipulatorPivot.goToSetpoint(() -> 0)
);
operator.povDown().whileTrue(
elevator.goToSetpoint(() -> 0)
);
manipulatorPivot.goToSetpoint(() -> (Units.degreesToRadians(110))
));
/*
operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
operator.b().whileTrue(elevator.runManualElevator(() -> -0.2));
//Operator inputs
operator.povUp().onTrue(
moveManipulator(
@ -267,6 +268,12 @@ public class RobotContainer {
.withSize(1, 1)
.withPosition(5, 1)
.withWidget(BuiltInWidgets.kTextView);
sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
}
public Command getAutonomousCommand() {
@ -317,7 +324,7 @@ public class RobotContainer {
// then the elevator, then the arm again
} else if (!manipulatorPivot.isMotionSafe(armPosition) && !manipulatorPivot.isMotionSafe()) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition));
.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()) {
return moveManipulatorUtil(elevatorPosition, armPosition, true, true);
@ -327,7 +334,7 @@ public class RobotContainer {
// Catch all command that's safe regardless of arm and elevator positions
} else {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition));
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
}
}
@ -341,23 +348,24 @@ public class RobotContainer {
* @return Moves the elevator and arm to the setpoints
*/
private Command moveManipulatorUtil(double elevatorPosition, double armPosition, boolean elevatorFirst, boolean sequential) {
if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
/*if (elevatorPosition <= ElevatorConstants.kBracePosition || elevatorPosition == 0) {
armPosition = MathUtil.clamp(
armPosition,
0,
ManipulatorPivotConstants.kRotationLimit
);
}
}*/
return Commands.either(
Commands.either(
elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(armPosition)),
elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(armPosition)),
elevator.goToSetpoint(() -> elevatorPosition).andThen(manipulatorPivot.goToSetpoint(() -> armPosition)),
elevator.goToSetpoint(() -> elevatorPosition).alongWith(manipulatorPivot.goToSetpoint(() -> armPosition)),
() -> sequential
),
Commands.either(
manipulatorPivot.goToSetpoint(armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)),
manipulatorPivot.goToSetpoint(armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)),
manipulatorPivot.goToSetpoint(() -> armPosition).andThen(elevator.goToSetpoint(() -> elevatorPosition)),
manipulatorPivot.goToSetpoint(() -> armPosition).alongWith(elevator.goToSetpoint(() -> elevatorPosition)),
() -> sequential
),
() -> elevatorFirst
@ -368,15 +376,15 @@ public class RobotContainer {
private Command manipulatorSafeTravel(double elevatorPosition, double armPosition, boolean isL4){
if(!isL4){
return Commands.sequence(
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
elevator.goToSetpoint(() -> elevatorPosition),
manipulatorPivot.goToSetpoint(armPosition));
manipulatorPivot.goToSetpoint(() -> armPosition));
}else{
return Commands.sequence(
manipulatorPivot.goToSetpoint(ManipulatorPivotConstants.kPivotSafeStowPosition),
manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition),
elevator.goToSetpoint(() -> elevatorPosition).until(() -> elevator.getEncoderPosition() > ElevatorConstants.kL4TransitionPosition),
Commands.parallel( manipulatorPivot.goToSetpoint(armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
Commands.parallel( manipulatorPivot.goToSetpoint(() -> armPosition)), elevator.goToSetpoint(() -> elevatorPosition));
}
}
@ -391,7 +399,7 @@ public class RobotContainer {
@SuppressWarnings("unused")
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
.andThen(manipulatorPivot.goToSetpoint(armPosition));
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
}
@SuppressWarnings("unused")

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

@ -16,25 +16,25 @@ public class ManipulatorPivotConstants {
public static final int kMotorCurrentMax = 40;
public static final double kPivotConversion = 12.0/60.0 * 20.0/60.0 * 12.0/28.0;
public static final double kPivotConversion = 2 * Math.PI;
public static final double kPivotMaxVelocity = 0;
public static final double kPivotMaxVelocity = 2 * Math.PI;
public static final double kPositionalP = 0;
public static final double kPositionalP = 2.5;
public static final double kPositionalI = 0;
public static final double kPositionalD = 0;
public static final double kPositionalTolerance = Units.degreesToRadians(3.0);
public static final double kFeedForwardS = 0;
public static final double kFeedForwardG = 0.41; // calculated value 0.41
public static final double kFeedForwardS = (0.3-0.19) / 2 * 0.8; //upper: 0.3 lower: 0.19
public static final double kFeedForwardG = (0.3+0.19) / 2; // 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 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 kEncoderOffset = 0.7815;
public static final double kCoralIntakePosition = Units.degreesToRadians(175.0);
public static final double kL1Position = Units.degreesToRadians(0.0);
@ -69,7 +69,8 @@ public class ManipulatorPivotConstants {
static {
motorConfig
.smartCurrentLimit(kMotorCurrentMax)
.idleMode(kIdleMode);
.idleMode(kIdleMode)
.inverted(true);
motorConfig.absoluteEncoder
.positionConversionFactor(kPivotConversion)
.inverted(false)

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

@ -141,23 +141,40 @@ public class Elevator extends SubsystemBase {
* @return Sets motor voltage to achieve the target destination
*/
public Command goToSetpoint(DoubleSupplier setpoint) {
double clampedSetpoint = MathUtil.clamp(
setpoint.getAsDouble(),
0,
ElevatorConstants.kMaxHeight
);
return startRun(() -> {
pidController.setSetpoint(setpoint.getAsDouble());
pidController.reset();
return run(() -> {
},
() -> {
if (!pidController.atSetpoint()) {
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) + feedForward.calculate(0)
);
} else {
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
}
});
/*
elevatorMotor1.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(0)
);
/*
*/
/*
if (!pidController.atSetpoint()) {
elevatorMotor1.setVoltage(
pidController.calculate(
@ -170,8 +187,8 @@ public class Elevator extends SubsystemBase {
feedForward.calculate(0)
);
}
*/
});
});*/
}
/*

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

@ -12,6 +12,7 @@ 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.PIDController;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.ManipulatorPivotConstants;
@ -44,6 +45,8 @@ public class ManipulatorPivot extends SubsystemBase {
ManipulatorPivotConstants.kPositionalI,
ManipulatorPivotConstants.kPositionalD
);
pidController.disableContinuousInput();
feedForward = new ArmFeedforward(
ManipulatorPivotConstants.kFeedForwardS,
@ -92,24 +95,27 @@ public class ManipulatorPivot extends SubsystemBase {
* @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination
*/
public Command goToSetpoint(double setpoint) {
double clampedSetpoint = MathUtil.clamp(
setpoint,
0,
ManipulatorPivotConstants.kRotationLimit
);
return run(() -> {
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(
encoder.getPosition(),
0
)
);
});
public Command goToSetpoint(DoubleSupplier setpoint) {
return startRun(() -> {
pidController.setSetpoint(setpoint.getAsDouble());
pidController.reset();
},
() -> {
if (!pidController.atSetpoint()) {
pivotMotor.setVoltage(
pidController.calculate(
encoder.getPosition(),
setpoint.getAsDouble()
) - feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
} else {
pivotMotor.setVoltage(
-feedForward.calculate(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset, 0)
);
}
});
}
/**
@ -118,7 +124,7 @@ public class ManipulatorPivot extends SubsystemBase {
* @return Encoder's position in radians
*/
public double getEncoderPosition() {
return encoder.getPosition();
return Units.radiansToDegrees( encoder.getPosition());
}
/**
* Returns the encoder's velocity in radians per second
@ -126,6 +132,14 @@ public class ManipulatorPivot extends SubsystemBase {
* @return Encoder's velocity in radians per second
*/
public double getEncoderVelocity() {
return encoder.getVelocity();
return Units.radiansToDegrees(encoder.getVelocity());
}
public double getCGPosition(){
return Units.radiansToDegrees(-encoder.getPosition() + ManipulatorPivotConstants.kFFGravityOffset);
}
public double getPivotOutput(){
return pivotMotor.getAppliedOutput() * pivotMotor.getBusVoltage();
}
}