Swerve and PDP Logging

4698RaiderRobotics · Nov 7, 2023 · ab743a9 · ab743a9
1 parent d47b14a
commit ab743a9
Show file tree

Hide file tree

Showing 10 changed files with 295 additions and 100 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -113,7 +113,7 @@
     "xlocnum": "cpp"
   },
   "editor.fontLigatures": true,
-  "editor.formatOnPaste": true,
+  "editor.formatOnPaste": false,
   "editor.formatOnSaveMode": "modifications",
   "editor.formatOnSave": false,
   "editor.formatOnType": false,

diff --git a/build.gradle b/build.gradle
@@ -36,7 +36,7 @@ deploy {
 def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
 
 // Set this to true to enable desktop support.
-def includeDesktopSupport = true
+def includeDesktopSupport = false
 
 // Set to true to run simulation in debug mode
 wpi.cpp.debugSimulation = false

diff --git a/src/main/cpp/DataLogger.cpp b/src/main/cpp/DataLogger.cpp
@@ -0,0 +1,57 @@
+// Log data to the Data Log file on the RoboRIO
+//
+
+#include <fstream>
+
+#include <wpi/DataLog.h>
+#include <frc/Filesystem.h>
+#include <frc/DataLogManager.h>
+
+#include "DataLogger.h"
+
+DataLogger* DataLogger::singleton = nullptr; 
+
+
+void DataLogger::Send( std::string_view s, double val ) { 
+    wpi::log::DoubleLogEntry le{ *(log), s };
+    le.Append( val );
+}
+
+void DataLogger::Send( std::string_view s, std::string_view val ) { 
+    wpi::log::StringLogEntry le{ *(log), s };
+    le.Append( val );
+}
+
+void DataLogger::Send( std::string_view s, bool val ) {
+    wpi::log::BooleanLogEntry le{ *(log), s };
+    le.Append( val );
+}
+
+void DataLogger::LogMetadata( void ) {
+        // Open the buildinfo.txt file and write the Metadata to the log file
+    std::ifstream binfo;
+    char line[256];
+
+    binfo.open( frc::filesystem::GetDeployDirectory() + "/buildinfo.txt", std::ios::in );
+    if( binfo.is_open() ) {
+        binfo.getline( line, 255 );
+        this->SendMetadata( "BUILD_DATE", line );
+        binfo.getline( line, 255 );
+        this->SendMetadata( "GIT_REPO", line );
+        binfo.getline( line, 255 );
+        this->SendMetadata( "GIT_BRANCH", line );
+        binfo.getline( line, 255 );
+        this->SendMetadata( "GIT_VERSION", line );
+        binfo.close();
+    }
+
+}
+
+void DataLogger::SendMetadata( std::string_view s, std::string_view val ) {
+        // AdvantageScope Chops off leading Character of the name so we add an underscore.
+        // Not sure why
+    std::string id = "RealMetadata/_";
+    id += s;
+    wpi::log::StringLogEntry le{ *(log), id };
+    le.Append( val );
+}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -11,14 +11,25 @@
 #include "frc/DataLogManager.h"
 #include "frc/DriverStation.h"
 
+#include "DataLogger.h"
+
 void Robot::RobotInit()
 {
   // Starts recording to data log
   frc::DataLogManager::Start();
+  wpi::log::DataLog& log = frc::DataLogManager::GetLog();
   // Record both DS control and joystick data
-  frc::DriverStation::StartDataLog(frc::DataLogManager::GetLog());
+  frc::DriverStation::StartDataLog(log);
   // Log in Telelop:
   //frc::CameraServer::StartAutomaticCapture();
+
+//  logger.StartDataLog( log );
+
+  // voltage = wpi::log::DoubleLogEntry( log, "PDP/Bus Voltage" );
+  // current = wpi::log::DoubleLogEntry( log, "PDP/Total Current" );
+  // power = wpi::log::DoubleLogEntry( log, "PDP/Total Power" );
+  // temp = wpi::log::DoubleLogEntry( log, "PDP/Temperature" );
+  // brown = wpi::log::BooleanLogEntry( log, "PDP/Brown Out" );
 }
 
 /**
@@ -31,6 +42,12 @@ ccc * <p> This runs after the mode specific periodic functions, but before
 void Robot::RobotPeriodic()
 {
   frc2::CommandScheduler::GetInstance().Run();
+
+  DataLogger::GetInstance().Send("PDP/Bus Voltage", m_pdp.GetVoltage());
+  DataLogger::GetInstance().Send("PDP/Total Current", m_pdp.GetTotalCurrent());
+  DataLogger::GetInstance().Send("PDP/Temperature", m_pdp.GetTemperature());
+  DataLogger::GetInstance().Send("PDP/Total Power", m_pdp.GetTotalPower());
+  DataLogger::GetInstance().Send("PDP/Brown Out", (bool) m_pdp.GetFaults().Brownout);
 }
 
 /**

diff --git a/src/main/cpp/SwerveModule.cpp b/src/main/cpp/SwerveModule.cpp
@@ -9,6 +9,8 @@
 #include <frc/geometry/Rotation2d.h>
 #include <frc/smartdashboard/SmartDashboard.h>
 
+#include "DataLogger.h"
+
 
 SwerveModule::SwerveModule( const int turnMotorChannel, 
                            const int driveMotorChannel, const int absoluteEncoderChannel, const double absoluteEncoderOffset ) 
@@ -24,10 +26,15 @@ SwerveModule::SwerveModule( const int turnMotorChannel,
     m_drivePIDController.SetFF( pidf::kDriveFF );
 
     m_turnMotor.RestoreFactoryDefaults();
+
+    m_turnMotor.SetSmartCurrentLimit( 30 );
+    m_driveMotor.SetSmartCurrentLimit( 30 );
 
     m_turnPIDController.EnableContinuousInput(-180, 180);
     m_turnPIDController.SetP( pidf::kTurnP );   
     m_turnPIDController.SetD( pidf::kTurnD );
+
+    m_name = fmt::format( "Swerve Module({}-{})", turnMotorChannel, driveMotorChannel );
 }
 
 // Sets each individual SwerveModule to an optimized SwerveModuleState
@@ -47,8 +54,24 @@ void SwerveModule::SetDesiredState( const frc::SwerveModuleState& referenceState
     // The onboard PID controller has a SetReference() function that automatically sets the motor to the correct speed.
     m_drivePIDController.SetReference( opRPM.value(), rev::CANSparkMaxLowLevel::ControlType::kVelocity );
 
+
     // The software PID controller outputs a value 0 to 1 which must be set using the Set() function of the motor.
-    m_turnMotor.Set(m_turnPIDController.Calculate(m_turnEncoder.GetRollOverPosition().value(), state.angle.Degrees().value()));
+    double pidOutput = m_turnPIDController.Calculate(m_turnEncoder.GetRollOverPosition().value(), state.angle.Degrees().value());
+    m_turnMotor.Set( pidOutput );
+
+    DataLogger::GetInstance().Send( m_name + "/Turn Setpoint", state.angle.Degrees().value() );
+    DataLogger::GetInstance().Send( m_name + "/Turn Position", m_turnEncoder.GetRollOverPosition().value() );
+    DataLogger::GetInstance().Send( m_name + "/Turn pidoutput", pidOutput );
+    // DataLogger::GetInstance().Send( m_name + "/Turn feedforward", feedforwardOutput / 12 );
+
+
+    DataLogger::GetInstance().Send( m_name + "/Delta Theta", dTheta.value() );
+    DataLogger::GetInstance().Send( m_name + "/Desired RPM", rpm.value() );
+    DataLogger::GetInstance().Send( m_name + "/Optimized RPM", opRPM.value() );
+    DataLogger::GetInstance().Send( m_name + "/Drive Current", m_driveMotor.GetOutputCurrent() );
+    DataLogger::GetInstance().Send( m_name + "/Turn Current", m_turnMotor.GetOutputCurrent() );
+    DataLogger::GetInstance().Send( m_name + "/Drive Motor Speed", m_driveEncoder.GetVelocity() );
+    DataLogger::GetInstance().Send( m_name + "/Turn Motor Speed", m_turnMotorEncoder.GetVelocity() );
 }
 
 void SwerveModule::StopMotors( ) {
@@ -59,7 +82,8 @@ void SwerveModule::StopMotors( ) {
 // Returns the current state of the SwerveModule
 frc::SwerveModuleState SwerveModule::GetState( void ) {
     //undo this for odometry
-    return { units::meters_per_second_t{ round(m_driveEncoder.GetVelocity() * physical::kDriveMetersPerRotation.value() / 60.0)}, units::radian_t{ m_turnEncoder.GetRollOverPosition() } };
+    return { units::meters_per_second_t{ round(m_driveEncoder.GetVelocity() * physical::kDriveMetersPerRotation.value() / 60.0)}, 
+            units::radian_t{ m_turnEncoder.GetRollOverPosition() } };
 }
 
 frc::SwerveModulePosition SwerveModule::GetPosition( void ) {
@@ -68,26 +92,26 @@ frc::SwerveModulePosition SwerveModule::GetPosition( void ) {
 
 void SwerveModule::ModuleSetup() {
 
-    frc::SmartDashboard::PutNumber( "kDriveP", kDriveP );
-    frc::SmartDashboard::PutNumber( "kDriveD", kDriveD );
-    frc::SmartDashboard::PutNumber( "kDriveFF", kDriveFF );
-    frc::SmartDashboard::PutNumber( "kTurnP", kTurnP );
-    frc::SmartDashboard::PutNumber( "kTurnD", kTurnD );
+    // frc::SmartDashboard::PutNumber( "kDriveP", kDriveP );
+    // frc::SmartDashboard::PutNumber( "kDriveD", kDriveD );
+    // frc::SmartDashboard::PutNumber( "kDriveFF", kDriveFF );
+    // frc::SmartDashboard::PutNumber( "kTurnP", kTurnP );
+    // frc::SmartDashboard::PutNumber( "kTurnD", kTurnD );
 
 }
 
 void SwerveModule::ModuleTest( std::string name) {
-    double tP = frc::SmartDashboard::GetNumber( "kTurnP", 0.0 );
-    double tD = frc::SmartDashboard::GetNumber( "kTurnD", 0.0 );
-    double dP = frc::SmartDashboard::GetNumber( "kDriveP", 0.0 );
-    double dD = frc::SmartDashboard::GetNumber( "kDriveD", 0.0 );
-    double dFF = frc::SmartDashboard::GetNumber( "kDriveFF", 0.0 );
-
-    if(tP != kTurnP) { kTurnP = tP; m_turnPIDController.SetP( kTurnP ); }
-    if(tD != kTurnD) { kTurnD = tD; m_turnPIDController.SetD( kTurnD ); }
-    if(dP != kDriveP) { kDriveP = dP; m_drivePIDController.SetP( kDriveP ); }
-    if(dD != kDriveD) { kDriveD = dD; m_drivePIDController.SetD( kDriveD ); }
-    if(dFF != kDriveFF) { kDriveFF = dFF; m_drivePIDController.SetFF( kDriveFF ); }
+    // double tP = frc::SmartDashboard::GetNumber( "kTurnP", 0.0 );
+    // double tD = frc::SmartDashboard::GetNumber( "kTurnD", 0.0 );
+    // double dP = frc::SmartDashboard::GetNumber( "kDriveP", 0.0 );
+    // double dD = frc::SmartDashboard::GetNumber( "kDriveD", 0.0 );
+    // double dFF = frc::SmartDashboard::GetNumber( "kDriveFF", 0.0 );
+
+    // if(tP != kTurnP) { kTurnP = tP; m_turnPIDController.SetP( kTurnP ); }
+    // if(tD != kTurnD) { kTurnD = tD; m_turnPIDController.SetD( kTurnD ); }
+    // if(dP != kDriveP) { kDriveP = dP; m_drivePIDController.SetP( kDriveP ); }
+    // if(dD != kDriveD) { kDriveD = dD; m_drivePIDController.SetD( kDriveD ); }
+    // if(dFF != kDriveFF) { kDriveFF = dFF; m_drivePIDController.SetFF( kDriveFF ); }
 
     frc::SmartDashboard::PutNumber(name, m_turnEncoder.GetRawPosition());
     frc::SmartDashboard::PutNumber( name + " Velocity", m_driveEncoder.GetVelocity() * physical::kDriveMetersPerRotation.value() / 60.0 );

diff --git a/src/main/cpp/subsystems/Drivetrain.cpp b/src/main/cpp/subsystems/Drivetrain.cpp
@@ -6,14 +6,24 @@
 #include "subsystems/Limelight.h"
 
 Drivetrain::Drivetrain( Limelight *ll ) : m_limelight{ll} {
-  ResetGyro( 180_deg );
+    ResetGyro( 180_deg );
 
-  for( int n=0; n<9; ++n ) {
-    redAllianceGridPoints[n] = { 14.3_m, 20_in + 22_in*n, 0_deg };
-    blueAllianceGridPoints[n] = { 2.25_m, 20_in + 22_in*n, 180_deg };
-  }
+    for( int n=0; n<9; ++n ) {
+        redAllianceGridPoints[n] = { 14.3_m, 20_in + 22_in*n, 0_deg };
+        blueAllianceGridPoints[n] = { 2.25_m, 20_in + 22_in*n, 180_deg };
+    }
+
+    frc::DataLogManager::Start();
+
+    frc::DriverStation::StartDataLog( frc::DataLogManager::GetLog() );
+
+    wpi::log::DataLog& log = frc::DataLogManager::GetLog();
 
-  frc::SmartDashboard::PutData("Field", &m_field);
+    m_actualLogEntry = wpi::log::DoubleArrayLogEntry( log, "/Swerve/Actual States" );
+    m_desiredLogEntry = wpi::log::DoubleArrayLogEntry( log, "/Swerve/Desired States" );
+    m_poseLogEntry = wpi::log::DoubleArrayLogEntry( log, "/Robot/Robot2D" );
+
+    frc::SmartDashboard::PutData("Field", &m_field);
 }
 
 // Drives with joystick inputs
@@ -42,19 +52,16 @@ void Drivetrain::ArcadeDrive( double xSpeed, double ySpeed, double omegaSpeed, b
 void Drivetrain::Drive( frc::ChassisSpeeds speeds, bool fieldRelative ) {
     //m_field.SetRobotPose(m_odometry.GetPose());
     // An array of SwerveModuleStates computed from the ChassisSpeeds object
-    auto states = m_kinematics.ToSwerveModuleStates( fieldRelative ? speeds.FromFieldRelativeSpeeds( 
+    m_desiredStates = m_kinematics.ToSwerveModuleStates( fieldRelative ? speeds.FromFieldRelativeSpeeds( 
                     speeds.vx, speeds.vy, speeds.omega, m_odometry.GetEstimatedPosition().Rotation() ) :
                     speeds );
-    m_kinematics.DesaturateWheelSpeeds( &states, physical::kMaxDriveSpeed );
+    m_kinematics.DesaturateWheelSpeeds( &m_desiredStates, physical::kMaxDriveSpeed );
 
-    auto [ fl, fr, bl, br ] = states;
+
     //fmt::print("Drivetrain::Drive Speed = {}\n", speeds.vx);
 
     // Sets each SwerveModule to the correct SwerveModuleState
-    m_frontLeft.SetDesiredState( fl );
-    m_frontRight.SetDesiredState( fr );
-    m_backLeft.SetDesiredState( bl );
-    m_backRight.SetDesiredState( br );
+
 
  //   wpi::array opStates = { m_frontLeft.GetState(), m_frontRight.GetState(), m_backLeft.GetState(), m_backRight.GetState() };
     // Displays the SwerveModules current position
@@ -98,6 +105,11 @@ void Drivetrain::StopDrive( ) {
 }
 
 void Drivetrain::Periodic( ) {
+    m_frontLeft.SetDesiredState( m_desiredStates[0] );
+    m_frontRight.SetDesiredState( m_desiredStates[1] );
+    m_backLeft.SetDesiredState( m_desiredStates[2] );
+    m_backRight.SetDesiredState( m_desiredStates[3] );
+
     // Updates the odometry of the robot given the SwerveModules' states
     m_odometry.Update( frc::Rotation2d{ units::degree_t{ m_gyro.GetYaw() } },
     {
@@ -151,6 +163,19 @@ void Drivetrain::Periodic( ) {
 
     m_field.SetRobotPose( estm_pose );
     m_field.GetObject( "Vision Pose" )->SetPose( visionPose );
+
+    m_actualStates[0] = m_frontLeft.GetState();
+    m_actualStates[1] = m_frontRight.GetState();
+    m_actualStates[2] = m_backLeft.GetState();
+    m_actualStates[3] = m_backRight.GetState();
+    LogSwerveStateArray(m_actualLogEntry, m_actualStates);
+    LogSwerveStateArray(m_desiredLogEntry, m_desiredStates);
+
+    frc::Pose2d currentPose = m_odometry.GetEstimatedPosition();
+    m_poseLogEntry.Append({ currentPose.X().value(),
+                            currentPose.Y().value(),
+                            currentPose.Rotation().Degrees().value() });
+
 }
 
 // Resets the gyro to an angle
@@ -261,4 +286,14 @@ void Drivetrain::PoseToNetworkTables() {
     frc::SmartDashboard::PutNumber( "Robot Pose X", m_odometry.GetEstimatedPosition().X().value() );
     frc::SmartDashboard::PutNumber( "Robot Pose Y", m_odometry.GetEstimatedPosition().Y().value() );
     frc::SmartDashboard::PutNumber( "Robot Pose Theta", m_odometry.GetEstimatedPosition().Rotation().Degrees().value() );
+}
+
+void Drivetrain::LogSwerveStateArray(wpi::log::DoubleArrayLogEntry& logEntry, wpi::array<frc::SwerveModuleState, 4U> states) {
+    static double state_array[8];
+
+    for (int i = 0; i < 4; ++i) {
+        state_array[2 * i] = states[i].angle.Degrees().value();
+        state_array[2 * i + 1] = states[i].speed.value();
+    }
+    logEntry.Append(state_array);
 }