Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Feature/intake phx6 alon #6

Open
wants to merge 15 commits into
base: main
Choose a base branch
from
Open

Feature/intake phx6 alon #6

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
3919d2b
I love pop music
Dec 3, 2023
1e831bc
Salamic dance
Dec 5, 2023
721eaa5
Update common version
vichik123 Dec 7, 2023
a51c911
Tooda uluhim
Dec 8, 2023
1d09951
focking hell ma'e
Dec 12, 2023
903dce0
Digma balls
Dec 12, 2023
48cee81
Change swerve intialization
vichik123 Dec 13, 2023
d6f81c5
Merge branch 'feature/Intake-phx6-Alon' of https://github.com/littleb…
vichik123 Dec 13, 2023
12fe8b5
after Common fixing
Dec 13, 2023
3d809ce
/
Dec 13, 2023
a250571
change logging for position and velocity to type Rotation2d
Dec 15, 2023
3b00257
config gearratio on dcMotor and remove on unnecessary places
Dec 15, 2023
00e3cf8
correct wrong controlRequest usage.
Dec 15, 2023
fda897a
correct wrong controlRequest usage.
Dec 15, 2023
5495eb3
Merge branch 'feature/Intake-phx6-Alon' of https://github.com/Galaxia…
Dec 17, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion .idea/gradle.xml
View file
Open in desktop

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

20 changes: 15 additions & 5 deletions .idea/jarRepositories.xml
View file
Open in desktop

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

2 changes: 1 addition & 1 deletion .idea/misc.xml
View file
Open in desktop

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

2 changes: 1 addition & 1 deletion build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ dependencies {

implementation group: 'com.google.code.gson', name: 'gson', version: '2.8.6'

implementation 'com.github.galaxia5987:common:1181c49'
implementation 'com.github.galaxia5987:common:e2ed90b8f1'
}

test {
Expand Down
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/subsystems/intake/Intake.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,9 @@ public Command lowerIntake() {
.andThen(new RunCommand(() -> setAngleMotorAngle(-40), this));
}

// You Jerk :) 😎
// I'm gonna kill them all.
// All the jews must die
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

pls no

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK sorry

public Command run(double power) {
return new RunCommand(() -> this.setSpinMotorPower(power));
}
Expand Down
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,4 +29,7 @@ public class IntakeConstants {
public static final double INTAKE_Y = 0.443;
public static final double INTAKE_SIM_ANGLE_OFFSET = -44.8;
public static final Translation2d ROOT_POSITION = new Translation2d(INTAKE_X, INTAKE_Y);
public static final double kP_SIM = 0;
katzuv marked this conversation as resolved.
Show resolved Hide resolved
public static final double kI_SIM = 0;
public static final double kD_SIM = 0;
}
65 changes: 35 additions & 30 deletions src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,44 +1,46 @@
package frc.robot.subsystems.intake;

import com.ctre.phoenix.motorcontrol.ControlMode;
katzuv marked this conversation as resolved.
Show resolved Hide resolved
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel;
import edu.wpi.first.math.util.Units;
import frc.robot.Constants;
import frc.robot.Ports;
import utils.Utils;
import utils.units.UnitModel;


public class IntakeIOReal implements IntakeIO {
private final CANSparkMax spinMotor = new CANSparkMax(Ports.Intake.INTAKE_MOTOR, CANSparkMaxLowLevel.MotorType.kBrushless);
private final TalonFX angleMotor = new TalonFX(Ports.Intake.ANGLE_MOTOR);

private final TalonFX angleMotorPhx = new TalonFX(0);
katzuv marked this conversation as resolved.
Show resolved Hide resolved
private final TalonFXConfigurator angleConfigurator;
private final TalonFXConfiguration angleConfiguration = new TalonFXConfiguration();

private final UnitModel unitModel = new UnitModel(IntakeConstants.TICKS_PER_DEGREE);

private double currentAngle;

public IntakeIOReal() {
angleMotor.configFactoryDefault();
spinMotor.restoreFactoryDefaults();
angleConfigurator = angleMotorPhx.getConfigurator();
angleConfiguration.Slot0.kP = IntakeConstants.kP;
angleConfiguration.Slot0.kI = IntakeConstants.kI;
angleConfiguration.Slot0.kD = IntakeConstants.kD;
angleConfigurator.apply(angleConfiguration.Slot0);

spinMotor.restoreFactoryDefaults();
spinMotor.setIdleMode(CANSparkMax.IdleMode.kCoast);
spinMotor.enableVoltageCompensation(Constants.NOMINAL_VOLTAGE);
spinMotor.setInverted(Ports.Intake.POWER_INVERTED);
for (int i = 1; i <= 6; i++) {
spinMotor.setPeriodicFramePeriod(CANSparkMaxLowLevel.PeriodicFrame.fromId(i), 50000);
}
spinMotor.burnFlash();

angleMotor.setNeutralMode(NeutralMode.Brake);
angleMotor.enableVoltageCompensation(true);
angleMotor.configVoltageCompSaturation(Constants.NOMINAL_VOLTAGE);
angleMotor.setInverted(Ports.Intake.ANGLE_INVERTED);
angleMotor.config_kP(0, IntakeConstants.kP);
angleMotor.config_kI(0, IntakeConstants.kI);
angleMotor.config_kD(0, IntakeConstants.kD);
angleMotor.config_kF(0, IntakeConstants.kF);
angleMotor.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(
true, 30, 0, 0
));
angleMotor.configClosedLoopPeakOutput(0, 0.4);
spinMotor.burnFlash();
}

@Override
Expand All @@ -48,27 +50,30 @@ public void setSpinMotorPower(double power) {

@Override
public void setAngleMotorAngle(double angle) {
angleMotor.set(ControlMode.Position, angle);
var motorRequest = new PositionVoltage(Units.radiansToRotations(angle));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

From the Phoenix 6 documentation:

Java users should reuse control requests to prevent excessive invocation of the Garbage Collector.

You can create the request object as a class member and change the value as needed with withPosition() (this doesn't create a new object but changes the same one).

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not really, you still create the request every time this function is called. You should create it once and only once.

angleMotorPhx.setControl(motorRequest);
}

@Override
public void setAngleMotorPower(double power) {
angleMotor.set(ControlMode.PercentOutput, power);
angleMotorPhx.setVoltage(power*12);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not simply use the DutyCycleOut control request?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nice, but still, create the request once.

}

@Override
public void resetEncoder(double angle) {
angleMotor.setSelectedSensorPosition(
unitModel.toTicks(angle)
);
}
public void resetEncoder(double angle) {}

@Override
public void updateInputs(IntakeLoggedInputs inputs) {
angleConfigurator.apply(angleConfiguration.Slot0);
AlonSchwierz marked this conversation as resolved.
Show resolved Hide resolved
inputs.spinMotorPower = spinMotor.getAppliedOutput();
inputs.angleMotorAngle = unitModel.toUnits(angleMotor.getSelectedSensorPosition());
inputs.angleMotorVelocity = unitModel.toVelocity(angleMotor.getSelectedSensorVelocity());
inputs.angleMotorcurrent = angleMotor.getSupplyCurrent();
inputs.angleMotorPower = angleMotor.getMotorOutputPercent();
//inputs.setpointSpinMotorPower
//inputs.setpointAngleMotorPower
//inputs.setpointAngleMotorAngle
katzuv marked this conversation as resolved.
Show resolved Hide resolved
inputs.angleMotorPower = angleMotorPhx.getMotorVoltage().getValue()/12;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The battery's voltage is not stable (especially during a match) and will not always be 12V. You have two options:

  1. Use PowerDistribution.getVoltage() for the calculation.
  2. Simply log the voltage directly -- I think this option is better.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think I will do both - thanks!

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done.

Copy link
Member

@katzuv katzuv Dec 16, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nice!
However, creating the powerDistribution object every function call is very wasteful. I just remembered that AdvantageKit has a class called LoggedPowerDistribution which logs all of the voltages automatically, So you can remove this line altogether.

inputs.angleMotorAngle = angleMotorPhx.getRotorPosition().getValue();
currentAngle = inputs.angleMotorAngle;
inputs.angleMotorcurrent = angleMotorPhx.getSupplyCurrent().getValue();
inputs.spinMotorCurrent = spinMotor.getOutputCurrent();
inputs.angleMotorVelocity = angleMotorPhx.getRotorVelocity().getValue();
AlonSchwierz marked this conversation as resolved.
Show resolved Hide resolved
}
}
57 changes: 33 additions & 24 deletions src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,51 +1,60 @@
package frc.robot.subsystems.intake;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.sim.TalonFXSimState;
import edu.wpi.first.math.util.Units;
import utils.motors.TalonFXSim;

public class IntakeIOSim implements IntakeIO {
private final FlywheelSim spinMotor;
private final SingleJointedArmSim angleMotor;
private final PIDController angleFeedback;
private final TalonFXSim angleMotorSim;
private final TalonFXSim powerMotorSim;
private final TalonFXConfiguration angleConfiguration;

public IntakeIOSim() {
spinMotor = new FlywheelSim(DCMotor.getNEO(1), IntakeConstants.SPIN_GEAR_RATIO, 1);
angleMotor = new SingleJointedArmSim(LinearSystemId.createSingleJointedArmSystem(
DCMotor.getFalcon500(1), 2, IntakeConstants.ANGLE_GEAR_RATIO
), DCMotor.getFalcon500(1), IntakeConstants.ANGLE_GEAR_RATIO, 0.7, -Math.PI * 2, Math.PI * 2, false, 0); //could be wrong type of motor or max/min angle
angleFeedback = new PIDController(IntakeConstants.kP, IntakeConstants.kI, IntakeConstants.kD);
angleConfiguration = new TalonFXConfiguration();

angleMotorSim = new TalonFXSim(0, IntakeConstants.ANGLE_GEAR_RATIO, 1);
katzuv marked this conversation as resolved.
Show resolved Hide resolved
powerMotorSim = new TalonFXSim(1, IntakeConstants.SPIN_GEAR_RATIO, 1);

angleConfiguration.Slot0.kP = IntakeConstants.kP_SIM;
angleConfiguration.Slot0.kI = IntakeConstants.kI_SIM;
angleConfiguration.Slot0.kD = IntakeConstants.kD_SIM;
angleMotorSim.configure(angleConfiguration);
}

@Override
public void updateInputs(IntakeLoggedInputs inputs) {
katzuv marked this conversation as resolved.
Show resolved Hide resolved
inputs.spinMotorPower = inputs.setpointSpinMotorPower;
inputs.spinMotorCurrent = spinMotor.getCurrentDrawAmps();
inputs.angleMotorAngle = angleMotor.getAngleRads();
inputs.angleMotorcurrent = angleMotor.getCurrentDrawAmps();

spinMotor.update(0.02);
angleMotor.update(0.02);
inputs.angleMotorPower = angleMotorSim.getAppliedVoltage() / 12;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Again, better to log the voltage directly.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You write OK but change nothing, and resolve the thread?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes but this is a simulation and the BatterySim Class is private for some reason.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

WDYM private? It's very much public https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj/simulation/BatterySim.html

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I understand your problem, you're trying to instantiate BatterySim. True, it's constructor is private, but all of its methods are static, so you don't need to instantiate the class.

inputs.angleMotorcurrent = angleMotorSim.getAppliedCurrent();
inputs.angleMotorVelocity = angleMotorSim.getRotorVelocity();
inputs.angleMotorAngle = angleMotorSim.getRotorPosition() * Math.PI * 2 % Math.PI * 2;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

IntakeConstants.ANGLE_GEAR_RATIO should be calculated to take the conversion to rotations.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what do you mean?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Modify this constant so getRotorPosition() will already return a value in rotations and not in meters (I presume). Also, why not use degrees instead of rotations?

inputs.spinMotorCurrent = powerMotorSim.getAppliedVoltage();
inputs.spinMotorPower = powerMotorSim.getAppliedVoltage() / 12;
}

@Override
public void setSpinMotorPower(double power) {
spinMotor.setInputVoltage(power * 12);
var motorRequest = new DutyCycleOut(power);
powerMotorSim.setControl(motorRequest);
}

@Override
public void setAngleMotorAngle(double angle) {
double angleMotorAppliedVoltage = angleFeedback.calculate(angleMotor.getAngleRads(), angle);
angleMotor.setInputVoltage(angleMotorAppliedVoltage);
var motorRequest = new PositionVoltage(Units.radiansToRotations(angle));
angleMotorSim.setControl(motorRequest);
}

@Override
public void setAngleMotorPower(double power) {
angleMotor.setInputVoltage(power * 12);
var motorRequest = new DutyCycleOut(power);
angleMotorSim.setControl(motorRequest);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Again, you should reuse control requests.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done.

Copy link
Member

@katzuv katzuv Dec 16, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nope, you still create it in each function call.

katzuv marked this conversation as resolved.
Show resolved Hide resolved
}


@Override
public void resetEncoder(double angle) {

Expand Down