Skip to content

Commit

Permalink
Update Telemetry class in swerve examples
Browse files Browse the repository at this point in the history
  • Loading branch information
@bhall-ctre
bhall-ctre committed Nov 20, 2024
1 parent 1602cd4 commit 33cb134
Show file tree
Hide file tree
Showing 7 changed files with 233 additions and 191 deletions.
54 changes: 29 additions & 25 deletions cpp/SwerveWithChoreo/src/main/cpp/Telemetry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,39 +5,43 @@ using namespace ctre::phoenix6;

void Telemetry::Telemeterize(subsystems::CommandSwerveDrivetrain::SwerveDriveState const &state)
{
/* Telemeterize the pose */
frc::Pose2d const pose = state.Pose;
/* Telemeterize the swerve drive state */
drivePose.Set(state.Pose);
driveSpeeds.Set(state.Speeds);
driveModuleStates.Set(state.ModuleStates);
driveModuleTargets.Set(state.ModuleTargets);
driveModulePositions.Set(state.ModulePositions);
driveTimestamp.Set(state.Timestamp.value());
driveOdometryFrequency.Set(1.0 / state.OdometryPeriod.value());

/* Also write to log file */
std::array<double, 8> moduleStatesArray{};
std::array<double, 8> moduleTargetsArray{};
for (int i = 0; i < 4; ++i) {
moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.Radians().value();
moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speed.value();
moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.Radians().value();
moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speed.value();
}
SignalLogger::WriteDoubleArray("DriveState/Pose", {state.Pose.X().value(), state.Pose.Y().value(), state.Pose.Rotation().Degrees().value()});
SignalLogger::WriteDoubleArray("DriveState/ModuleStates", moduleStatesArray);
SignalLogger::WriteDoubleArray("DriveState/ModuleTargets", moduleTargetsArray);
SignalLogger::WriteValue("DriveState/OdometryPeriod", state.OdometryPeriod);

/* Telemeterize the pose to a Field2d */
fieldTypePub.Set("Field2d");
fieldPub.Set(std::array{
pose.X().value(),
pose.Y().value(),
pose.Rotation().Degrees().value()
state.Pose.X().value(),
state.Pose.Y().value(),
state.Pose.Rotation().Degrees().value()
});

/* Telemeterize the robot's general speeds */
units::second_t const currentTime = utils::GetCurrentTime();
units::second_t const diffTime = currentTime - lastTime;
lastTime = currentTime;

frc::Translation2d const distanceDiff = (pose - m_lastPose).Translation();
m_lastPose = pose;

frc::Translation2d const velocities = distanceDiff / diffTime.value();

speed.Set(velocities.Norm().value());
velocityX.Set(velocities.X().value());
velocityY.Set(velocities.Y().value());
odomFreq.Set((1.0 / state.OdometryPeriod).value());

/* Telemeterize the module's states */
/* Telemeterize the module states to a Mechanism2d */
for (size_t i = 0; i < m_moduleSpeeds.size(); ++i) {
m_moduleSpeeds[i]->SetAngle(state.ModuleStates[i].angle.Degrees());
m_moduleDirections[i]->SetAngle(state.ModuleStates[i].angle.Degrees());
m_moduleSpeeds[i]->SetAngle(state.ModuleStates[i].angle.Degrees());
m_moduleSpeeds[i]->SetLength(state.ModuleStates[i].speed / (2 * MaxSpeed));

frc::SmartDashboard::PutData("Module " + std::to_string(i), &m_moduleMechanisms[i]);
}

SignalLogger::WriteDoubleArray("odometry", {pose.X().value(), pose.Y().value(), pose.Rotation().Degrees().value()});
SignalLogger::WriteDouble("odom period", state.OdometryPeriod.value(), "seconds");
}
23 changes: 12 additions & 11 deletions cpp/SwerveWithChoreo/src/main/include/Telemetry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@
#include <networktables/DoubleArrayTopic.h>
#include <networktables/DoubleTopic.h>
#include <networktables/StringTopic.h>
#include <networktables/StructArrayTopic.h>
#include <networktables/StructTopic.h>

#include "subsystems/CommandSwerveDrivetrain.h"

Expand All @@ -17,22 +19,21 @@ class Telemetry {
/* What to publish over networktables for telemetry */
nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();

/* Robot swerve drive state */
std::shared_ptr<nt::NetworkTable> driveStateTable = inst.GetTable("DriveState");
nt::StructPublisher<frc::Pose2d> drivePose = driveStateTable->GetStructTopic<frc::Pose2d>("Pose").Publish();
nt::StructPublisher<frc::ChassisSpeeds> driveSpeeds = driveStateTable->GetStructTopic<frc::ChassisSpeeds>("Speeds").Publish();
nt::StructArrayPublisher<frc::SwerveModuleState> driveModuleStates = driveStateTable->GetStructArrayTopic<frc::SwerveModuleState>("ModuleStates").Publish();
nt::StructArrayPublisher<frc::SwerveModuleState> driveModuleTargets = driveStateTable->GetStructArrayTopic<frc::SwerveModuleState>("ModuleTargets").Publish();
nt::StructArrayPublisher<frc::SwerveModulePosition> driveModulePositions = driveStateTable->GetStructArrayTopic<frc::SwerveModulePosition>("ModulePositions").Publish();
nt::DoublePublisher driveTimestamp = driveStateTable->GetDoubleTopic("Timestamp").Publish();
nt::DoublePublisher driveOdometryFrequency = driveStateTable->GetDoubleTopic("OdometryFrequency").Publish();

/* Robot pose for field positioning */
std::shared_ptr<nt::NetworkTable> table = inst.GetTable("Pose");
nt::DoubleArrayPublisher fieldPub = table->GetDoubleArrayTopic("robotPose").Publish();
nt::StringPublisher fieldTypePub = table->GetStringTopic(".type").Publish();

/* Robot speeds for general checking */
std::shared_ptr<nt::NetworkTable> driveStats = inst.GetTable("Drive");
nt::DoublePublisher velocityX = driveStats->GetDoubleTopic("Velocity X").Publish();
nt::DoublePublisher velocityY = driveStats->GetDoubleTopic("Velocity Y").Publish();
nt::DoublePublisher speed = driveStats->GetDoubleTopic("Speed").Publish();
nt::DoublePublisher odomFreq = driveStats->GetDoubleTopic("Odometry Frequency").Publish();

/* Keep a reference of the last pose to calculate the speeds */
frc::Pose2d m_lastPose{};
units::second_t lastTime = ctre::phoenix6::utils::GetCurrentTime();

/* Mechanisms to represent the swerve module states */
std::array<frc::Mechanism2d, 4> m_moduleMechanisms{
frc::Mechanism2d{1, 1},
Expand Down
54 changes: 29 additions & 25 deletions cpp/SwerveWithPathPlanner/src/main/cpp/Telemetry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,39 +5,43 @@ using namespace ctre::phoenix6;

void Telemetry::Telemeterize(subsystems::CommandSwerveDrivetrain::SwerveDriveState const &state)
{
/* Telemeterize the pose */
frc::Pose2d const pose = state.Pose;
/* Telemeterize the swerve drive state */
drivePose.Set(state.Pose);
driveSpeeds.Set(state.Speeds);
driveModuleStates.Set(state.ModuleStates);
driveModuleTargets.Set(state.ModuleTargets);
driveModulePositions.Set(state.ModulePositions);
driveTimestamp.Set(state.Timestamp.value());
driveOdometryFrequency.Set(1.0 / state.OdometryPeriod.value());

/* Also write to log file */
std::array<double, 8> moduleStatesArray{};
std::array<double, 8> moduleTargetsArray{};
for (int i = 0; i < 4; ++i) {
moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.Radians().value();
moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speed.value();
moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.Radians().value();
moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speed.value();
}
SignalLogger::WriteDoubleArray("DriveState/Pose", {state.Pose.X().value(), state.Pose.Y().value(), state.Pose.Rotation().Degrees().value()});
SignalLogger::WriteDoubleArray("DriveState/ModuleStates", moduleStatesArray);
SignalLogger::WriteDoubleArray("DriveState/ModuleTargets", moduleTargetsArray);
SignalLogger::WriteValue("DriveState/OdometryPeriod", state.OdometryPeriod);

/* Telemeterize the pose to a Field2d */
fieldTypePub.Set("Field2d");
fieldPub.Set(std::array{
pose.X().value(),
pose.Y().value(),
pose.Rotation().Degrees().value()
state.Pose.X().value(),
state.Pose.Y().value(),
state.Pose.Rotation().Degrees().value()
});

/* Telemeterize the robot's general speeds */
units::second_t const currentTime = utils::GetCurrentTime();
units::second_t const diffTime = currentTime - lastTime;
lastTime = currentTime;

frc::Translation2d const distanceDiff = (pose - m_lastPose).Translation();
m_lastPose = pose;

frc::Translation2d const velocities = distanceDiff / diffTime.value();

speed.Set(velocities.Norm().value());
velocityX.Set(velocities.X().value());
velocityY.Set(velocities.Y().value());
odomFreq.Set((1.0 / state.OdometryPeriod).value());

/* Telemeterize the module's states */
/* Telemeterize the module states to a Mechanism2d */
for (size_t i = 0; i < m_moduleSpeeds.size(); ++i) {
m_moduleSpeeds[i]->SetAngle(state.ModuleStates[i].angle.Degrees());
m_moduleDirections[i]->SetAngle(state.ModuleStates[i].angle.Degrees());
m_moduleSpeeds[i]->SetAngle(state.ModuleStates[i].angle.Degrees());
m_moduleSpeeds[i]->SetLength(state.ModuleStates[i].speed / (2 * MaxSpeed));

frc::SmartDashboard::PutData("Module " + std::to_string(i), &m_moduleMechanisms[i]);
}

SignalLogger::WriteDoubleArray("odometry", {pose.X().value(), pose.Y().value(), pose.Rotation().Degrees().value()});
SignalLogger::WriteDouble("odom period", state.OdometryPeriod.value(), "seconds");
}
23 changes: 12 additions & 11 deletions cpp/SwerveWithPathPlanner/src/main/include/Telemetry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@
#include <networktables/DoubleArrayTopic.h>
#include <networktables/DoubleTopic.h>
#include <networktables/StringTopic.h>
#include <networktables/StructArrayTopic.h>
#include <networktables/StructTopic.h>

#include "subsystems/CommandSwerveDrivetrain.h"

Expand All @@ -17,22 +19,21 @@ class Telemetry {
/* What to publish over networktables for telemetry */
nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();

/* Robot swerve drive state */
std::shared_ptr<nt::NetworkTable> driveStateTable = inst.GetTable("DriveState");
nt::StructPublisher<frc::Pose2d> drivePose = driveStateTable->GetStructTopic<frc::Pose2d>("Pose").Publish();
nt::StructPublisher<frc::ChassisSpeeds> driveSpeeds = driveStateTable->GetStructTopic<frc::ChassisSpeeds>("Speeds").Publish();
nt::StructArrayPublisher<frc::SwerveModuleState> driveModuleStates = driveStateTable->GetStructArrayTopic<frc::SwerveModuleState>("ModuleStates").Publish();
nt::StructArrayPublisher<frc::SwerveModuleState> driveModuleTargets = driveStateTable->GetStructArrayTopic<frc::SwerveModuleState>("ModuleTargets").Publish();
nt::StructArrayPublisher<frc::SwerveModulePosition> driveModulePositions = driveStateTable->GetStructArrayTopic<frc::SwerveModulePosition>("ModulePositions").Publish();
nt::DoublePublisher driveTimestamp = driveStateTable->GetDoubleTopic("Timestamp").Publish();
nt::DoublePublisher driveOdometryFrequency = driveStateTable->GetDoubleTopic("OdometryFrequency").Publish();

/* Robot pose for field positioning */
std::shared_ptr<nt::NetworkTable> table = inst.GetTable("Pose");
nt::DoubleArrayPublisher fieldPub = table->GetDoubleArrayTopic("robotPose").Publish();
nt::StringPublisher fieldTypePub = table->GetStringTopic(".type").Publish();

/* Robot speeds for general checking */
std::shared_ptr<nt::NetworkTable> driveStats = inst.GetTable("Drive");
nt::DoublePublisher velocityX = driveStats->GetDoubleTopic("Velocity X").Publish();
nt::DoublePublisher velocityY = driveStats->GetDoubleTopic("Velocity Y").Publish();
nt::DoublePublisher speed = driveStats->GetDoubleTopic("Speed").Publish();
nt::DoublePublisher odomFreq = driveStats->GetDoubleTopic("Odometry Frequency").Publish();

/* Keep a reference of the last pose to calculate the speeds */
frc::Pose2d m_lastPose{};
units::second_t lastTime = ctre::phoenix6::utils::GetCurrentTime();

/* Mechanisms to represent the swerve module states */
std::array<frc::Mechanism2d, 4> m_moduleMechanisms{
frc::Mechanism2d{1, 1},
Expand Down
80 changes: 43 additions & 37 deletions java/SwerveWithChoreo/src/main/java/frc/robot/Telemetry.java
View file
Original file line number Diff line number Diff line change
@@ -1,16 +1,19 @@
package frc.robot;

import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.networktables.DoubleArrayPublisher;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.StructArrayPublisher;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
Expand All @@ -33,22 +36,21 @@ public Telemetry(double maxSpeed) {
/* What to publish over networktables for telemetry */
private final NetworkTableInstance inst = NetworkTableInstance.getDefault();

/* Robot swerve drive state */
private final NetworkTable driveStateTable = inst.getTable("DriveState");
private final StructPublisher<Pose2d> drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish();
private final StructPublisher<ChassisSpeeds> driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish();
private final StructArrayPublisher<SwerveModuleState> driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish();
private final StructArrayPublisher<SwerveModuleState> driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish();
private final StructArrayPublisher<SwerveModulePosition> driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish();
private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish();
private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish();

/* Robot pose for field positioning */
private final NetworkTable table = inst.getTable("Pose");
private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();

/* Robot speeds for general checking */
private final NetworkTable driveStats = inst.getTable("Drive");
private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
private final DoublePublisher odomFreq = driveStats.getDoubleTopic("Odometry Frequency").publish();

/* Keep a reference of the last pose to calculate the speeds */
private Pose2d m_lastPose = new Pose2d();
private double lastTime = Utils.getCurrentTimeSeconds();

/* Mechanisms to represent the swerve module states */
private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] {
new Mechanism2d(1, 1),
Expand Down Expand Up @@ -76,43 +78,47 @@ public Telemetry(double maxSpeed) {
};

private final double[] m_poseArray = new double[3];
private final double[] m_moduleStatesArray = new double[8];
private final double[] m_moduleTargetsArray = new double[8];

/** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
public void telemeterize(SwerveDriveState state) {
/* Telemeterize the pose */
Pose2d pose = state.Pose;
m_poseArray[0] = pose.getX();
m_poseArray[1] = pose.getY();
m_poseArray[2] = pose.getRotation().getDegrees();
/* Telemeterize the swerve drive state */
drivePose.set(state.Pose);
driveSpeeds.set(state.Speeds);
driveModuleStates.set(state.ModuleStates);
driveModuleTargets.set(state.ModuleTargets);
driveModulePositions.set(state.ModulePositions);
driveTimestamp.set(state.Timestamp);
driveOdometryFrequency.set(1.0 / state.OdometryPeriod);

/* Also write to log file */
m_poseArray[0] = state.Pose.getX();
m_poseArray[1] = state.Pose.getY();
m_poseArray[2] = state.Pose.getRotation().getDegrees();
for (int i = 0; i < 4; ++i) {
m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians();
m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond;
m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians();
m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond;
}

SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray);
SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray);
SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray);
SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds");

/* Telemeterize the pose to a Field2d */
fieldTypePub.set("Field2d");
fieldPub.set(m_poseArray);

/* Telemeterize the robot's general speeds */
double currentTime = Utils.getCurrentTimeSeconds();
double diffTime = currentTime - lastTime;
lastTime = currentTime;

Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation();
m_lastPose = pose;

Translation2d velocities = distanceDiff.div(diffTime);

speed.set(velocities.getNorm());
velocityX.set(velocities.getX());
velocityY.set(velocities.getY());
odomFreq.set(1.0 / state.OdometryPeriod);

/* Telemeterize the module's states */
/* Telemeterize the module states to a Mechanism2d */
for (int i = 0; i < 4; ++i) {
m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed));

SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
}

SignalLogger.writeDoubleArray("odometry", m_poseArray);
SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds");
}
}
Loading

0 comments on commit 33cb134

Please sign in to comment.