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

testing from 2/18

Browse Source
This commit is contained in:
Team 2648 2025-02-18 19:01:11 -05:00
parent f6aeec7c7e
commit 0522f7c579
4 changed files with 56 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -15,6 +15,7 @@ import frc.robot.subsystems.ClimberRollers;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Manipulator;
import frc.robot.subsystems.sysid.ElevatorSysID;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
@ -30,6 +31,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
public class RobotContainer {
private ClimberPivot climberPivot;
@ -108,8 +110,8 @@ public class RobotContainer {
);
elevator.setDefaultCommand(
elevator.runManualElevator(
() -> -operator.getLeftY() * .5
elevator.goToSetpoint(
() -> 0
)
);
@ -144,24 +146,22 @@ 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().onTrue(
elevator.goToSetpoint(() -> 20).until(elevator::eitherAtGoal)
operator.povUp().whileTrue(
elevator.goToSetpoint(() -> 50)
);
operator.povDown().onTrue(
elevator.goToSetpoint(() -> 0).until(elevator::eitherAtGoal)
operator.povDown().whileTrue(
elevator.goToSetpoint(() -> 0)
);
operator.a().whileTrue(elevator.maintainPosition());
/*
operator.a().whileTrue(elevator.runManualElevator(() -> 0.2));
@ -231,7 +231,7 @@ public class RobotContainer {
sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition)
.withSize(2, 1)
.withPosition(0, 0)
.withWidget(BuiltInWidgets.kTextView);
.withWidget(BuiltInWidgets.kGraph);
sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition)
.withSize(2, 1)
@ -258,9 +258,14 @@ public class RobotContainer {
.withPosition(4, 1)
.withWidget(BuiltInWidgets.kBooleanBox);
sensorTab.addDouble("ElevMotor1", elevator::getMotor1);
sensorTab.addDouble("ElevMotor1", elevator::getMotor1)
.withWidget(BuiltInWidgets.kGraph);
sensorTab.addDouble("ElevMotor2", elevator::getMotor2);
sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
.withWidget(BuiltInWidgets.kGraph);
sensorTab.addDouble("PID output", elevator::currentPIDOut)
.withWidget(BuiltInWidgets.kGraph);
}
public Command getAutonomousCommand() {

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

@ -18,23 +18,23 @@ public class ElevatorConstants {
// 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
public static final double kEncoderPositionConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0;
public static final double kEncoderVelocityConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0 / 60;
public static final double kEncoderVelocityConversionFactor = kEncoderPositionConversionFactor / 60;
public static final int kCurrentLimit = 40;
public static final double kUpControllerP = 4;//7; //
public static final double kUpControllerP = 5.6;//7; //
public static final double kUpControllerI = 0;
public static final double kUpControllerD = 0.35;//0.1;//0.35
public static final double kUpControllerD = 0.28;//0.28
public static final double kDownControllerP = 6;//7; //
public static final double kDownControllerP = 5.6;//7; //
public static final double kDownControllerI = 0;
public static final double kDownControllerD = 0.175;//0.1;//0.35
public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
public static final double kAllowedError = 0.75;
public static final double kAllowedError = 1;
public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8; /* kG too high - kG too low / 2 0.95, 0.2 */
public static final double kFeedForwardG = (0.95 + 0.2)/2; /* kG too high + kG too low / 2 */ // calculated value 0.6
public static final double kFeedForwardV = 0.06; // calculated value 0.12
public static final double kFeedForwardV = 0.12; // calculated value 0.12
public static final double kMaxVelocity = 150.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

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

@ -29,6 +29,8 @@ public class Elevator extends SubsystemBase {
private ProfiledPIDController pidControllerDown;
private ElevatorFeedforward feedForward;
private double currentPIDOut;
public Elevator() {
elevatorMotor1 = new SparkMax(
@ -53,8 +55,6 @@ public class Elevator extends SubsystemBase {
PersistMode.kPersistParameters
);
//elevatorClosedLoop = elevatorMotor1.getClosedLoopController();
encoder = elevatorMotor1.getEncoder();
bottomLimitSwitch = new DigitalInput(
@ -170,14 +170,24 @@ public class Elevator extends SubsystemBase {
ElevatorConstants.kMaxHeight
);
return run(() -> {
//pidController.reset(encoder.getPosition(), encoder.getVelocity());
pidControllerDown.setGoal(clampedSetpoint);
elevatorMotor1.setVoltage(
pidControllerUp.calculate(
return run(() -> {
//pidControllerUp.reset(encoder.getPosition(), encoder.getVelocity());
if(!pidControllerDown.atGoal()){
currentPIDOut = pidControllerDown.calculate(
encoder.getPosition(),
clampedSetpoint
) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
);
System.out.println("CALCULATED");
}else{
currentPIDOut = 0;
System.out.println("SET ZERO");
}
elevatorMotor1.setVoltage(
currentPIDOut + feedForward.calculate(pidControllerDown.getSetpoint().velocity)
);
});
@ -237,4 +247,8 @@ public class Elevator extends SubsystemBase {
public double getMotor2() {
return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
}
public double currentPIDOut(){
return currentPIDOut;
}
}

12
src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java
View File

@ -14,17 +14,23 @@ import frc.robot.constants.ElevatorConstants;
import frc.robot.subsystems.Elevator;
public class ElevatorSysID extends Elevator {
private MutVoltage appliedVoltage = Volts.mutable(0);;
private MutVoltage appliedVoltage;
private MutDistance elevatorPosition = Inches.mutable(0);;
private MutDistance elevatorPosition;
private MutLinearVelocity elevatorVelocity = InchesPerSecond.mutable(0);
private MutLinearVelocity elevatorVelocity;
private SysIdRoutine routine;
public ElevatorSysID() {
super();
appliedVoltage = Volts.mutable(0);
elevatorPosition = Inches.mutable(0);
elevatorVelocity = InchesPerSecond.mutable(0);
routine = new SysIdRoutine(
ElevatorConstants.kSysIDConfig,
new SysIdRoutine.Mechanism(