Skip to content

Commit a565950

Browse files
Nummun14Nummun14
committed
Made changes, values don't work yet
1 parent dbd237c commit a565950

File tree

2 files changed

+76
-76
lines changed

2 files changed

+76
-76
lines changed

‎src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package frc.trigon.robot.subsystems.armelevator;
22

3+
import com.ctre.phoenix6.controls.VoltageOut;
34
import edu.wpi.first.math.geometry.Rotation2d;
45
import edu.wpi.first.wpilibj2.command.Command;
56
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
@@ -34,7 +35,8 @@ public static Command getDebuggingCommand(boolean ignoreConstraints) {
3435

3536
public static Command getArmCalibrationCommand() {
3637
return new ArmCalibrationCommand(
37-
ArmElevatorConstants.ARM_MASTER_MOTOR,
38+
() -> RobotContainer.ARM_ELEVATOR.getCurrentArmAngle().getRotations(),
39+
voltage -> ArmElevatorConstants.ARM_MASTER_MOTOR.setControl(new VoltageOut(voltage).withEnableFOC(true)),
3840
RobotContainer.ARM_ELEVATOR
3941
);
4042
}

‎src/main/java/lib/commands/ArmCalibrationCommand.java‎

Lines changed: 73 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -1,110 +1,108 @@
11
package lib.commands;
22

3-
import com.ctre.phoenix6.controls.VoltageOut;
43
import edu.wpi.first.math.geometry.Rotation2d;
54
import edu.wpi.first.wpilibj2.command.Command;
6-
import edu.wpi.first.wpilibj2.command.SubsystemBase;
7-
import lib.hardware.phoenix6.talonfx.TalonFXMotor;
8-
import lib.hardware.phoenix6.talonfx.TalonFXSignal;
5+
import edu.wpi.first.wpilibj2.command.Subsystem;
96
import org.littletonrobotics.junction.Logger;
107

11-
import java.util.HashMap;
8+
import java.util.function.Consumer;
9+
import java.util.function.Supplier;
1210

1311
public class ArmCalibrationCommand extends Command {
14-
//Find the gravity offset, kG, and kS
15-
private final TalonFXMotor motor;
16-
private static final double VOLTAGE_INCREMENT = 0.001;
17-
private Rotation2d gravityOffset;
18-
private double kG, kS, currentVoltage, minimumVoltage, maximumVoltage;
19-
private boolean isCalculationFinished = false;
20-
private double previousVelocity;
21-
HashMap<Double, Rotation2d> name = new HashMap<>();
22-
23-
24-
public ArmCalibrationCommand(TalonFXMotor motor, SubsystemBase... requirements) {
25-
this.motor = motor;
26-
addRequirements(requirements);
12+
private static final double
13+
STARTING_VOLTAGE = 0.002,
14+
VOLTAGE_INCREMENT = 0.0001,
15+
POSITION_DEADBAND_ROTATIONS = 0.00000000001;
16+
private final Supplier<Double> positionSupplier;
17+
private final Consumer<Double> voltageConsumer;
18+
private double
19+
currentVoltage,
20+
provisionalKGMinimum,
21+
kGMinimum,
22+
kGMaximum;
23+
private Rotation2d
24+
previousPosition,
25+
gravityOffset;
26+
private boolean isStationary = true;
27+
28+
public ArmCalibrationCommand(Supplier<Double> positionSupplier, Consumer<Double> voltageConsumer, Subsystem Requirement) {
29+
addRequirements(Requirement);
30+
this.positionSupplier = positionSupplier;
31+
this.voltageConsumer = voltageConsumer;
32+
this.currentVoltage = STARTING_VOLTAGE;
33+
this.provisionalKGMinimum = 0;
34+
this.kGMinimum = 0;
35+
this.kGMaximum = 0;
36+
this.previousPosition = Rotation2d.fromRotations(positionSupplier.get());
37+
this.gravityOffset = Rotation2d.fromRotations(positionSupplier.get());
2738
}
2839

2940
@Override
3041
public void initialize() {
42+
voltageConsumer.accept(STARTING_VOLTAGE);
3143
}
3244

33-
3445
@Override
3546
public void execute() {
36-
runCalculateGravityOffset();
37-
System.out.println(isVelocityIncreasing() + "\ncurrent velocity: " + motor.getSignal(TalonFXSignal.VELOCITY) + " \nprevious velocity: " + previousVelocity);
38-
System.out.println("\nmaximumPosition: " + gravityOffset.getRotations() + "\nminimumPosition: " + name.get(minimumVoltage).getRotations());
39-
}
40-
41-
@Override
42-
public boolean isFinished() {
43-
return isCalculationFinished;
47+
if (!isMoving() && !isStationary) {
48+
provisionalKGMinimum = currentVoltage;
49+
System.out.println(provisionalKGMinimum + " provisionalKGMinimum");
50+
increaseVoltage();
51+
isStationary = true;
52+
gravityOffset = Rotation2d.fromRotations(positionSupplier.get());
53+
} else if (!isMoving()) {
54+
kGMaximum = currentVoltage;
55+
kGMinimum = provisionalKGMinimum;
56+
System.out.println(kGMaximum + " kGMaximum");
57+
increaseVoltage();
58+
} else {
59+
System.out.println(isMoving());
60+
isStationary = false;
61+
}
62+
previousPosition = Rotation2d.fromRotations(positionSupplier.get());
4463
}
4564

4665
@Override
4766
public void end(boolean interrupted) {
48-
System.out.println(interrupted);
49-
calculateKG();
50-
calculateKS();
51-
logValues();
52-
printResults();
67+
final double kG = calculateKG();
68+
final double kS = calculateKS();
69+
printResults(kG, kS);
70+
logResults(kG, kS);
5371
}
5472

55-
private void logValues() {
56-
Logger.recordOutput("/SmartDashboard/ArmCalibrationCommand/GravityOffset", gravityOffset);
57-
Logger.recordOutput("/SmartDashboard/ArmCalibrationCommand/kG", kG);
58-
Logger.recordOutput("/SmartDashboard/ArmCalibrationCommand/kS", kS);
59-
}
60-
61-
private void printResults() {
62-
System.out.println("GravityOffset: " + gravityOffset);
63-
System.out.println("kG: " + kG);
64-
System.out.println("kS: " + kS);
65-
System.out.println("Maximum Voltage: " + maximumVoltage);
66-
System.out.println("Minimum Voltage: " + minimumVoltage);
67-
}
68-
69-
private void runCalculateGravityOffset() {
70-
if (isArmStoppedMoving()) {
71-
currentVoltage += VOLTAGE_INCREMENT;
72-
motor.setControl(new VoltageOut(currentVoltage));
73-
logMotorSignalsToHashmap();
74-
}
75-
if (isVelocityIncreasing()) {
76-
maximumVoltage = currentVoltage - VOLTAGE_INCREMENT;
77-
gravityOffset = name.get(maximumVoltage);
78-
getMinimumVoltage();
79-
isCalculationFinished = true;
80-
}
73+
@Override
74+
public boolean isFinished() {
75+
return Math.abs(positionSupplier.get() - gravityOffset.getRotations()) > Rotation2d.k180deg.getRotations();
8176
}
8277

83-
private void getMinimumVoltage() {
84-
minimumVoltage = maximumVoltage;
85-
while ((Math.abs(gravityOffset.getRotations() - name.get(minimumVoltage).getRotations()) > 0.0001)) {
86-
minimumVoltage -= VOLTAGE_INCREMENT;
87-
}
78+
private void increaseVoltage() {
79+
currentVoltage += VOLTAGE_INCREMENT;
80+
voltageConsumer.accept(currentVoltage);
8881
}
8982

90-
private boolean isVelocityIncreasing() {
91-
return Math.abs(motor.getSignal(TalonFXSignal.VELOCITY) - previousVelocity) > 0.001;
83+
private boolean isMoving() {
84+
return Math.abs(previousPosition.getRotations() - positionSupplier.get()) > POSITION_DEADBAND_ROTATIONS;
9285
}
9386

94-
private boolean isArmStoppedMoving() {
95-
return Math.abs(motor.getSignal(TalonFXSignal.VELOCITY)) < 0.01;
87+
private double calculateKG() {
88+
return (kGMinimum + kGMaximum) / 2;
9689
}
9790

98-
private void logMotorSignalsToHashmap() {
99-
name.put(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE), Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.POSITION)));
100-
previousVelocity = motor.getSignal(TalonFXSignal.VELOCITY);
91+
private double calculateKS() {
92+
return (kGMaximum - kGMinimum) / 2;
10193
}
10294

103-
private void calculateKG() {
104-
kG = (maximumVoltage + minimumVoltage) / 2;
95+
private void printResults(double kG, double kS) {
96+
System.out.println("Gravity Offset (rotations): " + gravityOffset.getRotations());
97+
System.out.println("Minimum kG: " + kGMinimum);
98+
System.out.println("Maximum kG: " + kGMaximum);
99+
System.out.println("kG: " + kG);
100+
System.out.println("kS: " + kS);
105101
}
106102

107-
private void calculateKS() {
108-
kS = (maximumVoltage - minimumVoltage) / 2;
103+
private void logResults(double kG, double kS) {
104+
Logger.recordOutput("ArmCalibrationV2Command/GravityOffset", gravityOffset);
105+
Logger.recordOutput("ArmCalibrationV2Command/kG", kG);
106+
Logger.recordOutput("ArmCalibrationV2Command/kS", kS);
109107
}
110-
}
108+
}

0 commit comments

Comments
 (0)