Skip to content

Commit

Permalink
Update logging and status frames
Browse files Browse the repository at this point in the history
  • Loading branch information
@TylerSeiford
TylerSeiford committed Nov 24, 2023
1 parent c7d1324 commit a0c9ef1
Show file tree
Hide file tree
Showing 3 changed files with 130 additions and 47 deletions.
60 changes: 52 additions & 8 deletions src/main/java/frc/robot/subsystems/Chassis.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import org.littletonrobotics.junction.AutoLogOutput;

import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.math.kinematics.ChassisSpeeds;
Expand All @@ -29,8 +30,8 @@ private static final class Constants {
private final MotorControllerGroup left, right;
private final DifferentialDrive drive;

@AutoLogOutput
private ChassisSpeeds targetSpeeds = new ChassisSpeeds();
@AutoLogOutput(key = "Chassis/Target")
private ChassisSpeeds targetSpeeds;

public Chassis() {
frontLeft = new WPI_TalonSRX(Constants.FRONT_LEFT_CAN_ID);
Expand Down Expand Up @@ -68,15 +69,58 @@ public Chassis() {
left = new MotorControllerGroup(frontLeft, rearLeft);
right = new MotorControllerGroup(frontRight, rearRight);
drive = new DifferentialDrive(left, right);
}

private void drive(ChassisSpeeds speeds) {
targetSpeeds = speeds;

drive.arcadeDrive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond, false);
// https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods
frontLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 20);
frontLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 20);
frontLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 1000);
frontLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, 20);
frontLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 1000);
frontLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 1000);
frontLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 1000);
frontLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 1000);
frontLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 1000);
frontLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, 1000);
rearLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 20);
rearLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 20);
rearLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 1000);
rearLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, 20);
rearLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 1000);
rearLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 1000);
rearLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 1000);
rearLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 1000);
rearLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 1000);
rearLeft.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, 1000);
frontRight.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 20);
frontRight.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 20);
frontRight.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 1000);
frontRight.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, 20);
frontRight.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 1000);
frontRight.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 1000);
frontRight.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 1000);
frontRight.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 1000);
frontRight.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 1000);
frontRight.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, 1000);
rearRight.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 20);
rearRight.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 20);
rearRight.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 1000);
rearRight.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, 20);
rearRight.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 1000);
rearRight.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 1000);
rearRight.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 1000);
rearRight.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 1000);
rearRight.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 1000);
rearRight.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, 1000);

targetSpeeds = new ChassisSpeeds();
}

public Command defaultCommand(Supplier<ChassisSpeeds> supplier) {
return run(() -> drive(supplier.get()));
return run(() -> targetSpeeds = supplier.get());
}

@Override
public void periodic() {
drive.arcadeDrive(targetSpeeds.vxMetersPerSecond, targetSpeeds.omegaRadiansPerSecond, false);
}
}
66 changes: 39 additions & 27 deletions src/main/java/frc/robot/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import org.littletonrobotics.junction.AutoLogOutput;

import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.DoubleSolenoid;
Expand Down Expand Up @@ -46,8 +47,10 @@ private enum IntakeState {
private final DoubleSolenoid leftRaise, rightRaise, leftClose, rightClose;
private final DoubleSolenoidGroup raise, close;

@AutoLogOutput
private IntakeState state = IntakeState.STOP;
@AutoLogOutput(key = "Intake/State")
private IntakeState state;
@AutoLogOutput(key = "Intake/Speed")
private double speed;

public Intake(PneumaticsControlModule pcm_0, PneumaticsControlModule pcm_1) {
leftMotor = new WPI_TalonSRX(Constants.LEFT_CAN_ID);
Expand Down Expand Up @@ -79,60 +82,69 @@ public Intake(PneumaticsControlModule pcm_0, PneumaticsControlModule pcm_1) {
rightClose = pcm_0.makeDoubleSolenoid(Constants.RIGHT_CLOSE_FORWARD_CHANNEL,
Constants.RIGHT_CLOSE_REVERSE_CHANNEL);
close = new DoubleSolenoidGroup(leftClose, rightClose);

// https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 20);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 20);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, 20);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 20);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 20);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, 20);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, 1000);

state = IntakeState.STOP;
speed = 0.0;
}

public Command up() {
return runOnce(() -> {
raise.set(DoubleSolenoid.Value.kForward);
});
return runOnce(() -> raise.set(DoubleSolenoid.Value.kForward));
}

public Command down() {
return runOnce(() -> {
raise.set(DoubleSolenoid.Value.kReverse);
});
return runOnce(() -> raise.set(DoubleSolenoid.Value.kReverse));
}

public Command open() {
return runOnce(() -> {
close.set(DoubleSolenoid.Value.kForward);
});
return runOnce(() -> close.set(DoubleSolenoid.Value.kForward));
}

public Command close() {
return runOnce(() -> {
close.set(DoubleSolenoid.Value.kReverse);
});
return runOnce(() -> close.set(DoubleSolenoid.Value.kReverse));
}

public Command intake() {
return runOnce(() -> {
state = IntakeState.INTAKE;
});
return runOnce(() -> state = IntakeState.INTAKE);
}

public Command hold() {
return runOnce(() -> {
state = IntakeState.HOLD;
});
return runOnce(() -> state = IntakeState.HOLD);
}

public Command eject() {
return runOnce(() -> {
state = IntakeState.EJECT;
});
return runOnce(() -> state = IntakeState.EJECT);
}

public Command stop() {
return runOnce(() -> {
state = IntakeState.STOP;
});
return runOnce(() -> state = IntakeState.STOP);
}

@Override
public void periodic() {
// Figure out what speed we should be running
double speed = switch (state) {
speed = switch (state) {
case INTAKE -> Constants.INTAKE_SPEED;
case EJECT -> -1.0 * Constants.INTAKE_SPEED;
case HOLD -> Constants.HOLD_SPEED;
Expand Down
51 changes: 39 additions & 12 deletions src/main/java/frc/robot/subsystems/Shooter.java
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,9 @@
package frc.robot.subsystems;

import org.littletonrobotics.junction.AutoLogOutput;

import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.DoubleSolenoid;
Expand Down Expand Up @@ -32,6 +35,9 @@ private static final class Constants {
private final DoubleSolenoid leftSolenoid, rightSolenoid;
private final DoubleSolenoidGroup solenoids;

@AutoLogOutput(key = "Shooter/Speed")
private double speed;

public Shooter(PneumaticsControlModule pcm) {
leftSolenoid = pcm.makeDoubleSolenoid(Constants.LEFT_FORWARD_CHANNEL, Constants.LEFT_REVERSE_CHANNEL);
rightSolenoid = pcm.makeDoubleSolenoid(Constants.RIGHT_FORWARD_CHANNEL, Constants.RIGHT_REVERSE_CHANNEL);
Expand All @@ -54,29 +60,50 @@ public Shooter(PneumaticsControlModule pcm) {
rightMotor.configContinuousCurrentLimit(Constants.CONTINUOUS_CURRENT_LIMIT);

motors = new MotorControllerGroup(leftMotor, rightMotor);

// https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 20);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 20);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, 20);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 1000);
leftMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 20);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 20);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, 20);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 1000);
rightMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, 1000);

speed = 0.0;
}

public Command punch() {
return runOnce(() -> {
solenoids.set(DoubleSolenoid.Value.kForward);
});
return runOnce(() -> solenoids.set(DoubleSolenoid.Value.kForward));
}

public Command retract() {
return runOnce(() -> {
solenoids.set(DoubleSolenoid.Value.kReverse);
});
return runOnce(() -> solenoids.set(DoubleSolenoid.Value.kReverse));
}

public Command spin() {
return runOnce(() -> {
motors.set(Constants.SHOOT_SPEED);
});
return runOnce(() -> speed = Constants.SHOOT_SPEED);
}

public Command stop() {
return runOnce(() -> {
motors.set(0.0);
});
return runOnce(() -> speed = 0.0);
}

@Override
public void periodic() {
motors.set(speed);
}
}

0 comments on commit a0c9ef1

Please sign in to comment.