From 41ecc8a25cf0db8cd9a609e8192998a4f1ba19db Mon Sep 17 00:00:00 2001 From: Benjamin Hall Date: Wed, 30 Oct 2024 20:38:55 -0400 Subject: [PATCH] Clean up all C++ and Python examples --- cpp/ArcadeDrive/src/main/cpp/Robot.cpp | 11 ++- cpp/ArcadeDrive/src/main/include/Robot.h | 10 +-- .../src/main/cpp/Robot.cpp | 2 +- cpp/CANcoder/src/main/cpp/Robot.cpp | 7 +- cpp/CommandBasedDrive/src/main/cpp/Robot.cpp | 6 +- .../src/main/cpp/RobotContainer.cpp | 27 ++++---- .../cpp/commands/DriveStraightCommand.cpp | 13 ++-- .../main/cpp/subsystems/DriveSubsystem.cpp | 14 ++-- .../src/main/include/Robot.h | 2 +- .../src/main/include/RobotContainer.h | 18 +++-- .../include/commands/DriveStraightCommand.h | 4 +- .../main/include/subsystems/DriveSubsystem.h | 2 +- .../src/main/cpp/Robot.cpp | 8 ++- cpp/CurrentLimits/src/main/cpp/Robot.cpp | 5 +- cpp/FusedCANcoder/src/main/cpp/Robot.cpp | 7 +- cpp/FusedCANcoder/src/main/include/Robot.h | 18 ++--- cpp/MotionMagic/src/main/cpp/Robot.cpp | 2 +- .../include/subsystems/FlywheelMechanism.h | 2 +- cpp/Pigeon2/src/main/cpp/Robot.cpp | 7 +- cpp/Simulation/src/main/cpp/Robot.cpp | 54 ++++++++------- .../src/main/include/Robot.h | 6 +- cpp/WaitForAll/src/main/cpp/Robot.cpp | 69 ++++++++++--------- cpp/WaitForAll/src/main/include/Robot.h | 18 ++--- python/ArcadeDrive/robot.py | 16 ++--- python/CANcoder/robot.py | 4 +- python/TalonFX/robot.py | 4 +- python/VelocityClosedLoop/robot.py | 8 +-- 27 files changed, 179 insertions(+), 165 deletions(-) diff --git a/cpp/ArcadeDrive/src/main/cpp/Robot.cpp b/cpp/ArcadeDrive/src/main/cpp/Robot.cpp index 03b6ab52..a968ab25 100644 --- a/cpp/ArcadeDrive/src/main/cpp/Robot.cpp +++ b/cpp/ArcadeDrive/src/main/cpp/Robot.cpp @@ -13,8 +13,8 @@ void Robot::RobotInit() { configs::TalonFXConfiguration rightConfiguration{}; /* User can optionally change the configs, or leave it alone to perform a factory default */ - leftConfiguration.MotorOutput.Inverted = false; - rightConfiguration.MotorOutput.Inverted = true; + leftConfiguration.MotorOutput.Inverted = signals::InvertedValue::CounterClockwise_Positive; + rightConfiguration.MotorOutput.Inverted = signals::InvertedValue::Clockwise_Positive; leftLeader.GetConfigurator().Apply(leftConfiguration); leftFollower.GetConfigurator().Apply(leftConfiguration); @@ -25,9 +25,9 @@ void Robot::RobotInit() { leftFollower.SetControl(controls::Follower{leftLeader.GetDeviceID(), false}); rightFollower.SetControl(controls::Follower{rightLeader.GetDeviceID(), false}); } + void Robot::RobotPeriodic() { - if (++printCount >= 10) - { + if (++printCount >= 10) { printCount = 0; std::cout << "Left out: " << leftLeader.Get() << std::endl; std::cout << "Right out: " << rightLeader.Get() << std::endl; @@ -48,8 +48,7 @@ void Robot::TeleopPeriodic() { /* Set output to control frames */ leftOut.Output = fwd + rot; rightOut.Output = fwd - rot; - if (!joystick.GetAButton()) - { + if (!joystick.GetAButton()) { /* And set them to the motors */ leftLeader.SetControl(leftOut); rightLeader.SetControl(rightOut); diff --git a/cpp/ArcadeDrive/src/main/include/Robot.h b/cpp/ArcadeDrive/src/main/include/Robot.h index 72b8639d..5ee1929d 100644 --- a/cpp/ArcadeDrive/src/main/include/Robot.h +++ b/cpp/ArcadeDrive/src/main/include/Robot.h @@ -10,12 +10,12 @@ class Robot : public frc::TimedRobot { private: - static constexpr char const *CANBUS_NAME{"rio"}; + ctre::phoenix6::CANBus kCANBus{"rio"}; - ctre::phoenix6::hardware::TalonFX leftLeader{1, CANBUS_NAME}; - ctre::phoenix6::hardware::TalonFX leftFollower{2, CANBUS_NAME}; - ctre::phoenix6::hardware::TalonFX rightLeader{3, CANBUS_NAME}; - ctre::phoenix6::hardware::TalonFX rightFollower{4, CANBUS_NAME}; + ctre::phoenix6::hardware::TalonFX leftLeader{1, kCANBus}; + ctre::phoenix6::hardware::TalonFX leftFollower{2, kCANBus}; + ctre::phoenix6::hardware::TalonFX rightLeader{3, kCANBus}; + ctre::phoenix6::hardware::TalonFX rightFollower{4, kCANBus}; ctre::phoenix6::controls::DutyCycleOut leftOut{0}; // Initialize output to 0% ctre::phoenix6::controls::DutyCycleOut rightOut{0}; // Initialize output to 0% diff --git a/cpp/BasicLatencyCompensation/src/main/cpp/Robot.cpp b/cpp/BasicLatencyCompensation/src/main/cpp/Robot.cpp index 6d8cb45f..d4f046e7 100644 --- a/cpp/BasicLatencyCompensation/src/main/cpp/Robot.cpp +++ b/cpp/BasicLatencyCompensation/src/main/cpp/Robot.cpp @@ -19,7 +19,7 @@ void Robot::RobotPeriodic() { auto p2CompensatedYaw = BaseStatusSignal::GetLatencyCompensatedValue(m_p2yaw, m_p2yawRate); /* Print out both values so it shows how they perform */ - if (m_printCount++ > 10 && m_joystick.GetAButton()) { + if (++m_printCount >= 10 && m_joystick.GetAButton()) { m_printCount = 0; printf("CANcoder: Pos: %10.3f - Latency-Compensated: %10.3f - Difference: %6.5f\n", m_ccpos.GetValue().value(), ccCompensatedPos.value(), (m_ccpos.GetValue() - ccCompensatedPos).value()); printf("Talon FX: Pos: %10.3f - Latency-Compensated: %10.3f - Difference: %6.5f\n", m_fxpos.GetValue().value(), fxCompensatedPos.value(), (m_fxpos.GetValue() - fxCompensatedPos).value()); diff --git a/cpp/CANcoder/src/main/cpp/Robot.cpp b/cpp/CANcoder/src/main/cpp/Robot.cpp index 6b858fed..b47a1a01 100644 --- a/cpp/CANcoder/src/main/cpp/Robot.cpp +++ b/cpp/CANcoder/src/main/cpp/Robot.cpp @@ -22,8 +22,9 @@ void Robot::RobotInit() { } void Robot::RobotPeriodic() { /* Every print_period get the CANcoder position/velocity and report it */ - if (frc::Timer::GetFPGATimestamp() - currentTime > print_period) { + if (frc::Timer::GetFPGATimestamp() - currentTime >= print_period) { currentTime += print_period; + /** * GetPosition automatically calls Refresh(), no need to manually refresh. * @@ -34,9 +35,9 @@ void Robot::RobotPeriodic() { std::cout << "Position is " << pos << " with " << pos.GetTimestamp().GetLatency().value() << " seconds of latency" << std::endl; /** - * Get the velocity StatusSignalValue + * Get the velocity StatusSignalValue without refreshing */ - auto &vel = cancoder.GetVelocity(); + auto &vel = cancoder.GetVelocity(false); /* This time wait for the signal to reduce latency */ vel.WaitForUpdate(print_period); // Wait up to our period /** diff --git a/cpp/CommandBasedDrive/src/main/cpp/Robot.cpp b/cpp/CommandBasedDrive/src/main/cpp/Robot.cpp index 426b989a..d486663d 100644 --- a/cpp/CommandBasedDrive/src/main/cpp/Robot.cpp +++ b/cpp/CommandBasedDrive/src/main/cpp/Robot.cpp @@ -37,7 +37,7 @@ void Robot::DisabledPeriodic() {} void Robot::AutonomousInit() { m_autonomousCommand = m_container.GetAutonomousCommand(); - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Schedule(); } } @@ -49,9 +49,9 @@ void Robot::TeleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Cancel(); - m_autonomousCommand = nullptr; + m_autonomousCommand = std::nullopt; } } diff --git a/cpp/CommandBasedDrive/src/main/cpp/RobotContainer.cpp b/cpp/CommandBasedDrive/src/main/cpp/RobotContainer.cpp index f454dd1a..dadfaf0e 100644 --- a/cpp/CommandBasedDrive/src/main/cpp/RobotContainer.cpp +++ b/cpp/CommandBasedDrive/src/main/cpp/RobotContainer.cpp @@ -4,17 +4,17 @@ #include "RobotContainer.h" #include +#include RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here - frc2::RunCommand teleopDrive {[this]() - { - /* invert the joystick Y because forward Y is negative */ - m_driveSubsystem.ArcadeDrive(-m_joystick.GetLeftY(), m_joystick.GetRightX()); - }, - {&m_driveSubsystem}}; - m_driveSubsystem.SetDefaultCommand(std::move(teleopDrive)); + m_driveSubsystem.SetDefaultCommand( + m_driveSubsystem.Run([this] { + /* invert the joystick Y because forward Y is negative */ + m_driveSubsystem.ArcadeDrive(-m_joystick.GetLeftY(), m_joystick.GetRightX()); + }) + ); // Configure the button bindings ConfigureButtonBindings(); @@ -23,13 +23,16 @@ RobotContainer::RobotContainer() void RobotContainer::ConfigureButtonBindings() { // Configure your button bindings here - frc2::Trigger{[this]() - { return m_joystick.GetLeftBumperButton(); }} - .WhileTrue(&m_driveStraightCommand); + m_joystick.LeftBumper().WhileTrue( + DriveStraightCommand{ + m_driveSubsystem, + [this] { return -m_joystick.GetLeftY(); } + }.ToPtr() + ); } -frc2::Command *RobotContainer::GetAutonomousCommand() +frc2::CommandPtr RobotContainer::GetAutonomousCommand() { // An example command will be run in autonomous - return nullptr; + return frc2::cmd::Print("No autonomous command configured"); } diff --git a/cpp/CommandBasedDrive/src/main/cpp/commands/DriveStraightCommand.cpp b/cpp/CommandBasedDrive/src/main/cpp/commands/DriveStraightCommand.cpp index a5fc8f23..9a43ed94 100644 --- a/cpp/CommandBasedDrive/src/main/cpp/commands/DriveStraightCommand.cpp +++ b/cpp/CommandBasedDrive/src/main/cpp/commands/DriveStraightCommand.cpp @@ -4,13 +4,12 @@ #include "commands/DriveStraightCommand.h" -DriveStraightCommand::DriveStraightCommand(DriveSubsystem &subsystem, - std::function throttle) : m_driveSubsystem{subsystem}, - m_throttle{std::move(throttle)}, - m_yawGetter{subsystem.GetYaw()}, - m_holdYaw{0}, - m_driveStraightThread{[this]() - { DriveStraightExecution(); }} +DriveStraightCommand::DriveStraightCommand(DriveSubsystem &subsystem, std::function throttle) : + m_driveSubsystem{subsystem}, + m_throttle{std::move(throttle)}, + m_yawGetter{subsystem.GetYaw()}, + m_holdYaw{0_deg}, + m_driveStraightThread{[this] { DriveStraightExecution(); }} { AddRequirements(&subsystem); } diff --git a/cpp/CommandBasedDrive/src/main/cpp/subsystems/DriveSubsystem.cpp b/cpp/CommandBasedDrive/src/main/cpp/subsystems/DriveSubsystem.cpp index 303a7c12..2d66c746 100644 --- a/cpp/CommandBasedDrive/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/cpp/CommandBasedDrive/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -99,9 +99,11 @@ void DriveSubsystem::Periodic() * in the previous article while in simulation, but will use * real values on the robot itself. */ - m_odometry.Update(m_pigeon2.GetRotation2d(), - rotationsToMeters(m_leftLeader.GetPosition().GetValue()), - rotationsToMeters(m_rightLeader.GetPosition().GetValue())); + m_odometry.Update( + m_pigeon2.GetRotation2d(), + rotationsToMeters(m_leftLeader.GetPosition().GetValue()), + rotationsToMeters(m_rightLeader.GetPosition().GetValue()) + ); m_field.SetRobotPose(m_odometry.GetPose()); } @@ -124,8 +126,10 @@ void DriveSubsystem::SimulationPeriodic() * WPILib expects +V to be forward. We have already configured * our orientations to match this behavior. */ - m_driveSim.SetInputs(m_leftSimState.GetMotorVoltage(), - m_rightSimState.GetMotorVoltage()); + m_driveSim.SetInputs( + m_leftSimState.GetMotorVoltage(), + m_rightSimState.GetMotorVoltage() + ); /* * Advance the model by 20 ms. Note that if you are running this diff --git a/cpp/CommandBasedDrive/src/main/include/Robot.h b/cpp/CommandBasedDrive/src/main/include/Robot.h index 25e3229f..1fb70dd2 100644 --- a/cpp/CommandBasedDrive/src/main/include/Robot.h +++ b/cpp/CommandBasedDrive/src/main/include/Robot.h @@ -26,7 +26,7 @@ class Robot : public frc::TimedRobot { private: // Have it null by default so that if testing teleop it // doesn't have undefined behavior and potentially crash. - frc2::Command* m_autonomousCommand = nullptr; + std::optional m_autonomousCommand; RobotContainer m_container; }; diff --git a/cpp/CommandBasedDrive/src/main/include/RobotContainer.h b/cpp/CommandBasedDrive/src/main/include/RobotContainer.h index fb2efa6c..196235d5 100644 --- a/cpp/CommandBasedDrive/src/main/include/RobotContainer.h +++ b/cpp/CommandBasedDrive/src/main/include/RobotContainer.h @@ -6,7 +6,7 @@ #include #include -#include +#include #include "subsystems/DriveSubsystem.h" #include "commands/DriveStraightCommand.h" @@ -19,19 +19,17 @@ */ class RobotContainer { +private: + // The robot's subsystems and joysticks are defined here... + frc2::CommandXboxController m_joystick{0}; + public: + DriveSubsystem m_driveSubsystem{}; + RobotContainer(); - frc2::Command *GetAutonomousCommand(); + frc2::CommandPtr GetAutonomousCommand(); private: - // The robot's subsystems and commands are defined here... - frc::XboxController m_joystick{0}; - - DriveSubsystem m_driveSubsystem{}; - DriveStraightCommand m_driveStraightCommand{m_driveSubsystem, - [this]() - { return -m_joystick.GetLeftY(); }}; - void ConfigureButtonBindings(); }; diff --git a/cpp/CommandBasedDrive/src/main/include/commands/DriveStraightCommand.h b/cpp/CommandBasedDrive/src/main/include/commands/DriveStraightCommand.h index 531fe383..0aa3003a 100644 --- a/cpp/CommandBasedDrive/src/main/include/commands/DriveStraightCommand.h +++ b/cpp/CommandBasedDrive/src/main/include/commands/DriveStraightCommand.h @@ -10,8 +10,8 @@ #include #include -class DriveStraightCommand - : public frc2::CommandHelper { +class DriveStraightCommand : public frc2::CommandHelper +{ private: static constexpr units::time::second_t MAX_UPDATE_PERIOD{0.050_s}; diff --git a/cpp/CommandBasedDrive/src/main/include/subsystems/DriveSubsystem.h b/cpp/CommandBasedDrive/src/main/include/subsystems/DriveSubsystem.h index 9ac4f963..5758f267 100644 --- a/cpp/CommandBasedDrive/src/main/include/subsystems/DriveSubsystem.h +++ b/cpp/CommandBasedDrive/src/main/include/subsystems/DriveSubsystem.h @@ -43,7 +43,7 @@ class DriveSubsystem : public frc2::SubsystemBase static constexpr units::inch_t kWheelRadiusInches = 3_in; frc::sim::DifferentialDrivetrainSim m_driveSim{ - frc::DCMotor::Falcon500FOC(2), + frc::DCMotor::KrakenX60FOC(2), kGearRatio, 2.1_kg_sq_m, // MOI of 2.1 kg m^2 (from CAD model) 26.5_kg, // Mass of robot is 26.5 kg diff --git a/cpp/ControlRequestLimits/src/main/cpp/Robot.cpp b/cpp/ControlRequestLimits/src/main/cpp/Robot.cpp index 68b44a42..619aee30 100644 --- a/cpp/ControlRequestLimits/src/main/cpp/Robot.cpp +++ b/cpp/ControlRequestLimits/src/main/cpp/Robot.cpp @@ -35,9 +35,11 @@ void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() {} void Robot::TeleopPeriodic() { - m_fx.SetControl(m_out.WithOutput(-m_joystick.GetLeftY()) - .WithLimitForwardMotion(m_joystick.GetLeftBumper()) - .WithLimitReverseMotion(m_joystick.GetRightBumper())); + m_fx.SetControl( + m_out.WithOutput(-m_joystick.GetLeftY()) + .WithLimitForwardMotion(m_joystick.GetLeftBumper()) + .WithLimitReverseMotion(m_joystick.GetRightBumper()) + ); } void Robot::DisabledInit() {} diff --git a/cpp/CurrentLimits/src/main/cpp/Robot.cpp b/cpp/CurrentLimits/src/main/cpp/Robot.cpp index 6a9c106d..171bf5ff 100644 --- a/cpp/CurrentLimits/src/main/cpp/Robot.cpp +++ b/cpp/CurrentLimits/src/main/cpp/Robot.cpp @@ -39,8 +39,9 @@ void Robot::RobotPeriodic() { std::cout << "Setting stator limit to " << m_currentLimits.StatorCurrentLimitEnable << std::endl; m_fx.GetConfigurator().Apply(m_currentLimits); } - if (printCount++ > 20) { - printCount= 0; + + if (++printCount >= 20) { + printCount = 0; std::cout << "Supply current: " << m_fx.GetSupplyCurrent() << std::endl; std::cout << "Stator current: " << m_fx.GetStatorCurrent() << std::endl; } diff --git a/cpp/FusedCANcoder/src/main/cpp/Robot.cpp b/cpp/FusedCANcoder/src/main/cpp/Robot.cpp index f96e527d..3c28690e 100644 --- a/cpp/FusedCANcoder/src/main/cpp/Robot.cpp +++ b/cpp/FusedCANcoder/src/main/cpp/Robot.cpp @@ -24,7 +24,7 @@ void Robot::RobotInit() { m_fx.GetConfigurator().Apply(fx_cfg); } void Robot::RobotPeriodic() { - if (printCount++ > 10) { + if (++printCount >= 10) { printCount = 0; BaseStatusSignal::RefreshAll( @@ -33,11 +33,12 @@ void Robot::RobotPeriodic() { f_remoteSensorInvalid, sf_remoteSensorInvalid, fx_pos, fx_vel, - cc_pos, cc_vel); + cc_pos, cc_vel + ); // If any faults happen, print them out. Sticky faults will always be present if live-fault occurs bool anyFault = sf_fusedSensorOutOfSync.GetValue() || sf_remoteSensorInvalid.GetValue(); - if(anyFault) { + if (anyFault) { std::cout << "A fault has occurred:" << std::endl;; /* If we're live, indicate live, otherwise if we're sticky indicate sticky, otherwise do nothing */ if (f_fusedSensorOutOfSync.GetValue()) { diff --git a/cpp/FusedCANcoder/src/main/include/Robot.h b/cpp/FusedCANcoder/src/main/include/Robot.h index 788464f5..2d580c1e 100644 --- a/cpp/FusedCANcoder/src/main/include/Robot.h +++ b/cpp/FusedCANcoder/src/main/include/Robot.h @@ -12,15 +12,15 @@ class Robot : public frc::TimedRobot { ctre::phoenix6::hardware::TalonFX m_fx{1, "canivore"}; ctre::phoenix6::hardware::CANcoder m_cc{1, "canivore"}; - ctre::phoenix6::StatusSignal &f_fusedSensorOutOfSync = m_fx.GetFault_FusedSensorOutOfSync(); - ctre::phoenix6::StatusSignal &sf_fusedSensorOutOfSync = m_fx.GetStickyFault_FusedSensorOutOfSync(); - ctre::phoenix6::StatusSignal &f_remoteSensorInvalid = m_fx.GetFault_RemoteSensorDataInvalid(); - ctre::phoenix6::StatusSignal &sf_remoteSensorInvalid = m_fx.GetStickyFault_RemoteSensorDataInvalid(); - - ctre::phoenix6::StatusSignal &fx_pos = m_fx.GetPosition(); - ctre::phoenix6::StatusSignal &fx_vel = m_fx.GetVelocity(); - ctre::phoenix6::StatusSignal &cc_pos = m_cc.GetPosition(); - ctre::phoenix6::StatusSignal &cc_vel = m_cc.GetVelocity(); + ctre::phoenix6::StatusSignal &f_fusedSensorOutOfSync = m_fx.GetFault_FusedSensorOutOfSync(false); + ctre::phoenix6::StatusSignal &sf_fusedSensorOutOfSync = m_fx.GetStickyFault_FusedSensorOutOfSync(false); + ctre::phoenix6::StatusSignal &f_remoteSensorInvalid = m_fx.GetFault_RemoteSensorDataInvalid(false); + ctre::phoenix6::StatusSignal &sf_remoteSensorInvalid = m_fx.GetStickyFault_RemoteSensorDataInvalid(false); + + ctre::phoenix6::StatusSignal &fx_pos = m_fx.GetPosition(false); + ctre::phoenix6::StatusSignal &fx_vel = m_fx.GetVelocity(false); + ctre::phoenix6::StatusSignal &cc_pos = m_cc.GetPosition(false); + ctre::phoenix6::StatusSignal &cc_vel = m_cc.GetVelocity(false); ctre::phoenix6::controls::DutyCycleOut m_dutyCycleControl{0}; diff --git a/cpp/MotionMagic/src/main/cpp/Robot.cpp b/cpp/MotionMagic/src/main/cpp/Robot.cpp index 18d0e49b..db9fb157 100644 --- a/cpp/MotionMagic/src/main/cpp/Robot.cpp +++ b/cpp/MotionMagic/src/main/cpp/Robot.cpp @@ -48,7 +48,7 @@ void Robot::RobotInit() { } } void Robot::RobotPeriodic() { - if (m_printCount++ > 10) { + if (++m_printCount >= 10) { m_printCount = 0; std::cout << "Pos: " << m_motor.GetPosition() << std::endl; std::cout << "Vel: " << m_motor.GetVelocity() << std::endl; diff --git a/cpp/PhoenixSysId/src/main/include/subsystems/FlywheelMechanism.h b/cpp/PhoenixSysId/src/main/include/subsystems/FlywheelMechanism.h index c2775861..82ba0560 100644 --- a/cpp/PhoenixSysId/src/main/include/subsystems/FlywheelMechanism.h +++ b/cpp/PhoenixSysId/src/main/include/subsystems/FlywheelMechanism.h @@ -26,7 +26,7 @@ class FlywheelMechanism : public frc2::SubsystemBase }, frc2::sysid::Mechanism{ [this](units::volt_t volts) { m_motorToTest.SetControl(m_sysIdControl.WithOutput(volts)); }, - [](auto) {}, + {}, this } }; diff --git a/cpp/Pigeon2/src/main/cpp/Robot.cpp b/cpp/Pigeon2/src/main/cpp/Robot.cpp index 1ce8d284..9066ba78 100644 --- a/cpp/Pigeon2/src/main/cpp/Robot.cpp +++ b/cpp/Pigeon2/src/main/cpp/Robot.cpp @@ -22,8 +22,9 @@ void Robot::RobotInit() { } void Robot::RobotPeriodic() { /* Every print_period get the CANcoder position/velocity and report it */ - if (frc::Timer::GetFPGATimestamp() - currentTime > print_period) { + if (frc::Timer::GetFPGATimestamp() - currentTime >= print_period) { currentTime += print_period; + /** * GetYaw automatically calls Refresh(), no need to manually refresh. * @@ -34,9 +35,9 @@ void Robot::RobotPeriodic() { std::cout << "Yaw is " << yaw << " with " << yaw.GetTimestamp().GetLatency().value() << " seconds of latency" << std::endl; /** - * Get the Gravity Vector Z component StatusSignalValue + * Get the Gravity Vector Z component StatusSignalValue without refreshing */ - auto &gravityZ = pidgey.GetGravityVectorZ(); + auto &gravityZ = pidgey.GetGravityVectorZ(false); /* This time wait for the signal to reduce latency */ gravityZ.WaitForUpdate(print_period); // Wait up to our period /** diff --git a/cpp/Simulation/src/main/cpp/Robot.cpp b/cpp/Simulation/src/main/cpp/Robot.cpp index 64b813cf..3211304f 100644 --- a/cpp/Simulation/src/main/cpp/Robot.cpp +++ b/cpp/Simulation/src/main/cpp/Robot.cpp @@ -160,42 +160,44 @@ void Robot::SimulationPeriodic() rightSim.SetReverseLimit(joystick.GetRightTriggerAxis() > 0.5); } -units::meter_t Robot::rotationsToMeters(units::turn_t rotations) +units::meter_t DriveSubsystem::rotationsToMeters(units::turn_t rotations) { - /* Get circumference of wheel */ - constexpr auto circumference = kWheelRadiusInches * 2 * 3.14159 / 1_tr; - /* Every rotation of the wheel travels this many inches */ - /* Now multiply rotations by meters per rotation */ - return rotations * circumference; + /* Every radian of rotation, the wheel travels this many inches */ + constexpr auto wheelDistancePerRad = kWheelRadiusInches / 1_rad; + /* Now apply gear ratio to input rotations */ + auto gearedRotations = rotations / kGearRatio; + /* And multiply geared rotations by meters per rotation */ + return gearedRotations * wheelDistancePerRad; } -units::turn_t Robot::metersToRotations(units::meter_t meters) +units::turn_t DriveSubsystem::metersToRotations(units::meter_t meters) { - /* Get circumference of wheel */ - constexpr auto circumference = kWheelRadiusInches * 2 * 3.14159 / 1_tr; - /* Every rotation of the wheel travels this many inches */ - /* Now apply wheel rotations to input meters */ - auto wheelRotations = meters / circumference; - return wheelRotations; + /* Every radian of rotation, the wheel travels this many inches */ + constexpr auto wheelDistancePerRad = kWheelRadiusInches / 1_rad; + /* Now get wheel rotations from input meters */ + auto wheelRadians = meters / wheelDistancePerRad; + /* And multiply by gear ratio to get rotor rotations */ + return wheelRadians * kGearRatio; } -units::meters_per_second_t Robot::rotationsToMetersVel(units::turns_per_second_t rotations) +units::meters_per_second_t DriveSubsystem::rotationsToMetersVel(units::turns_per_second_t rotations) { - /* Get circumference of wheel */ - constexpr auto circumference = kWheelRadiusInches * 2 * 3.14159 / 1_tr; - /* Every rotation of the wheel travels this many inches */ - /* Now multiply rotations by meters per rotation */ - return rotations * circumference; + /* Every radian of rotation, the wheel travels this many inches */ + constexpr auto wheelDistancePerRad = kWheelRadiusInches / 1_rad; + /* Now apply gear ratio to input rotations */ + auto gearedRotations = rotations / kGearRatio; + /* And multiply geared rotations by meters per rotation */ + return gearedRotations * wheelDistancePerRad; } -units::turns_per_second_t Robot::metersToRotationsVel(units::meters_per_second_t meters) +units::turns_per_second_t DriveSubsystem::metersToRotationsVel(units::meters_per_second_t meters) { - /* Get circumference of wheel */ - constexpr auto circumference = kWheelRadiusInches * 2 * 3.14159 / 1_tr; - /* Every rotation of the wheel travels this many inches */ - /* Now apply wheel rotations to input meters */ - auto wheelRotations = meters / circumference; - return wheelRotations; + /* Every radian of rotation, the wheel travels this many inches */ + constexpr auto wheelDistancePerRad = kWheelRadiusInches / 1_rad; + /* Now get wheel rotations from input meters */ + auto wheelRadians = meters / wheelDistancePerRad; + /* And multiply by gear ratio to get rotor rotations */ + return wheelRadians * kGearRatio; } #ifndef RUNNING_FRC_TESTS diff --git a/cpp/VelocityClosedLoop/src/main/include/Robot.h b/cpp/VelocityClosedLoop/src/main/include/Robot.h index 29f0603a..ba60b6f8 100644 --- a/cpp/VelocityClosedLoop/src/main/include/Robot.h +++ b/cpp/VelocityClosedLoop/src/main/include/Robot.h @@ -9,10 +9,10 @@ #include class Robot : public frc::TimedRobot { - static constexpr char const *CANBUS_NAME = "canivore"; + ctre::phoenix6::CANBus kCANBus{"canivore"}; - ctre::phoenix6::hardware::TalonFX m_fx{0, CANBUS_NAME}; - ctre::phoenix6::hardware::TalonFX m_fllr{1, CANBUS_NAME}; + ctre::phoenix6::hardware::TalonFX m_fx{0, kCANBus}; + ctre::phoenix6::hardware::TalonFX m_fllr{1, kCANBus}; /* Be able to switch which control request to use based on a button press */ /* Start at velocity 0, use slot 0 */ diff --git a/cpp/WaitForAll/src/main/cpp/Robot.cpp b/cpp/WaitForAll/src/main/cpp/Robot.cpp index 30bb9800..70611aa1 100644 --- a/cpp/WaitForAll/src/main/cpp/Robot.cpp +++ b/cpp/WaitForAll/src/main/cpp/Robot.cpp @@ -7,51 +7,54 @@ void Robot::RobotInit() {} void Robot::RobotPeriodic() { - - if(m_joystick.GetLeftBumperButton()) { + if (m_joystick.GetLeftBumperButton()) { m_waitForAllTimeout = 0.1_s; std::cout << "Timeout is now at " << m_waitForAllTimeout.value() << std::endl; } - if(m_joystick.GetRightBumperButton()) { + if (m_joystick.GetRightBumperButton()) { m_waitForAllTimeout = 0_s; std::cout << "Timeout is now at " << m_waitForAllTimeout.value() << std::endl; } /* If we press the A button, test what happens when we wait on lots of signals (normal use case) */ - if(m_joystick.GetAButtonPressed()) { - ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, m_lotsOfSignals); - std::cout << "Status of waiting on signals (normal use case): " << status.GetName() << std::endl; - for(auto const &sig : m_lotsOfSignals) { - std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; - } + if (m_joystick.GetAButtonPressed()) { + ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, m_lotsOfSignals); + std::cout << "Status of waiting on signals (normal use case): " << status.GetName() << std::endl; + for(auto const &sig : m_lotsOfSignals) { + std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; } - /* If we press the B button, test what happens when we wait on signals from different busses */ - if(m_joystick.GetBButtonPressed()) { - ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, m_signalsAcrossCANbuses); - std::cout << "Status of waiting on signals across different CAN busses: " << status.GetName() << std::endl; - for(auto const& sig : m_signalsAcrossCANbuses) { - std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; - } + } + + /* If we press the B button, test what happens when we wait on signals from different busses */ + if (m_joystick.GetBButtonPressed()) { + ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, m_signalsAcrossCANbuses); + std::cout << "Status of waiting on signals across different CAN busses: " << status.GetName() << std::endl; + for(auto const& sig : m_signalsAcrossCANbuses) { + std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; } - /* If we press the Y button, test what happens when we wait on no signals */ - if(m_joystick.GetYButtonPressed()) { - ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, m_noSignals); - std::cout << "Status of waiting on no signals: " << status.GetName() << std::endl; - for(auto const& sig : m_noSignals) { - std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; - } + } + + /* If we press the Y button, test what happens when we wait on no signals */ + if (m_joystick.GetYButtonPressed()) { + ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, m_noSignals); + std::cout << "Status of waiting on no signals: " << status.GetName() << std::endl; + for(auto const& sig : m_noSignals) { + std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; } - /* If we press the X button, test what happens when we wait on signals with the transcient motor controller */ - if(m_joystick.GetXButtonPressed()) { - ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, {&m_canbus1signal1, - &m_canbus1signal2, - &m_canbus1transcient1, - &m_canbus1transcient2}); - std::cout << "Status of waiting on transcient signals: " << status.GetName() << std::endl; - for(auto const& sig : m_tanscientSignals) { - std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; - } + } + + /* If we press the X button, test what happens when we wait on signals with the transcient motor controller */ + if (m_joystick.GetXButtonPressed()) { + ctre::phoenix::StatusCode status = ctre::phoenix6::BaseStatusSignal::WaitForAll(m_waitForAllTimeout, + m_canbus1signal1, + m_canbus1signal2, + m_canbus1transcient1, + m_canbus1transcient2); + std::cout << "Status of waiting on transcient signals: " << status.GetName() << std::endl; + for (auto const &sig : m_tanscientSignals) { + std::cout << "Signal status: " << sig->GetStatus().GetName() << std::endl; } + } } void Robot::AutonomousInit() {} diff --git a/cpp/WaitForAll/src/main/include/Robot.h b/cpp/WaitForAll/src/main/include/Robot.h index 266d93da..aa6a0303 100644 --- a/cpp/WaitForAll/src/main/include/Robot.h +++ b/cpp/WaitForAll/src/main/include/Robot.h @@ -33,19 +33,19 @@ class Robot : public frc::TimedRobot { ctre::phoenix6::hardware::TalonFX m_motor1{0, "*"}; // Pick the first CANivore bus ctre::phoenix6::hardware::Pigeon2 m_pigdey{1, "*"}; // Pick the first CANivore bus also ctre::phoenix6::hardware::TalonFX m_transcientMotor{20, "*"}; // This motor may or may not be on the bus, - // selectively power it to completely test this example + // selectively power it to completely test this example ctre::phoenix6::hardware::TalonFX m_motor2{0, "rio"}; // Pick the RIO bus to force a failure we can detect - ctre::phoenix6::StatusSignal m_canbus1signal1 = m_motor1.GetPosition(); - ctre::phoenix6::StatusSignal m_canbus1signal2 = m_motor1.GetVelocity(); - ctre::phoenix6::StatusSignal m_canbus1signal3 = m_motor1.GetControlMode(); - ctre::phoenix6::StatusSignal m_canbus1signal4 = m_pigdey.GetYaw(); - ctre::phoenix6::StatusSignal m_canbus1signal5 = m_pigdey.GetRoll(); + ctre::phoenix6::StatusSignal m_canbus1signal1 = m_motor1.GetPosition(false); + ctre::phoenix6::StatusSignal m_canbus1signal2 = m_motor1.GetVelocity(false); + ctre::phoenix6::StatusSignal m_canbus1signal3 = m_motor1.GetControlMode(false); + ctre::phoenix6::StatusSignal m_canbus1signal4 = m_pigdey.GetYaw(false); + ctre::phoenix6::StatusSignal m_canbus1signal5 = m_pigdey.GetRoll(false); - ctre::phoenix6::StatusSignal m_canbus2signal1 = m_motor2.GetPosition(); + ctre::phoenix6::StatusSignal m_canbus2signal1 = m_motor2.GetPosition(false); - ctre::phoenix6::StatusSignal m_canbus1transcient1 = m_transcientMotor.GetPosition(); - ctre::phoenix6::StatusSignal m_canbus1transcient2 = m_transcientMotor.GetVelocity(); + ctre::phoenix6::StatusSignal m_canbus1transcient1 = m_transcientMotor.GetPosition(false); + ctre::phoenix6::StatusSignal m_canbus1transcient2 = m_transcientMotor.GetVelocity(false); std::vector m_signalsAcrossCANbuses = { &m_canbus1signal1, diff --git a/python/ArcadeDrive/robot.py b/python/ArcadeDrive/robot.py index f3cef4cd..cd85a128 100644 --- a/python/ArcadeDrive/robot.py +++ b/python/ArcadeDrive/robot.py @@ -3,7 +3,7 @@ This is a demo program for arcade drive in Python with Phoenix 6 """ import wpilib -from phoenix6 import hardware, controls, configs +from phoenix6 import CANBus, configs, controls, hardware class MyRobot(wpilib.TimedRobot): @@ -16,11 +16,11 @@ def robotInit(self): """Robot initialization function""" # Keep a reference to all the motor controllers used - canivore_name = "canivore" - self.front_left_motor = hardware.TalonFX(0, canivore_name) - self.rear_left_motor = hardware.TalonFX(1, canivore_name) - self.front_right_motor = hardware.TalonFX(2, canivore_name) - self.rear_right_motor = hardware.TalonFX(3, canivore_name) + self.canivore = CANBus("canivore") + self.front_left_motor = hardware.TalonFX(0, self.canivore) + self.rear_left_motor = hardware.TalonFX(1, self.canivore) + self.front_right_motor = hardware.TalonFX(2, self.canivore) + self.rear_right_motor = hardware.TalonFX(3, self.canivore) cfg = configs.TalonFXConfiguration() cfg.motor_output.inverted = configs.config_groups.InvertedValue.COUNTER_CLOCKWISE_POSITIVE @@ -37,8 +37,8 @@ def robotInit(self): self.rear_right_motor.set_control(follow_right_request) # Keep a reference to the DutyCycleOut control request to update periodically - self.left_out = controls.DutyCycleOut(output=0) - self.right_out = controls.DutyCycleOut(output=0) + self.left_out = controls.DutyCycleOut(0) + self.right_out = controls.DutyCycleOut(0) # Keep a reference to an Xbox Controller for teleop control self.joy = wpilib.XboxController(0) diff --git a/python/CANcoder/robot.py b/python/CANcoder/robot.py index d0b69f7d..312c8676 100644 --- a/python/CANcoder/robot.py +++ b/python/CANcoder/robot.py @@ -33,8 +33,8 @@ def teleopPeriodic(self): pos = self.cancoder.get_position() print(f"Positions is {str(pos)} with {pos.timestamp.get_latency()} seconds of latency") - # Get the velocity StatusSignal - vel = self.cancoder.get_velocity() + # Get the velocity StatusSignal without refreshing + vel = self.cancoder.get_velocity(False) # This time wait for the signal to reduce latency vel.wait_for_update(0.1) print(f"Velocity is {vel} with {vel.timestamp.get_latency()} seconds of latency") diff --git a/python/TalonFX/robot.py b/python/TalonFX/robot.py index 701bf7c3..35ab5a6b 100644 --- a/python/TalonFX/robot.py +++ b/python/TalonFX/robot.py @@ -37,8 +37,8 @@ def teleopPeriodic(self): pos = self.talonfx.get_position() print(f"Positions is {str(pos)} with {pos.timestamp.get_latency()} seconds of latency") - # Get the velocity StatusSignal - vel = self.talonfx.get_velocity() + # Get the velocity StatusSignal without refreshing + vel = self.talonfx.get_velocity(False) # This time wait for the signal to reduce latency vel.wait_for_update(0.1) print(f"Velocity is {vel} with {vel.timestamp.get_latency()} seconds of latency") diff --git a/python/VelocityClosedLoop/robot.py b/python/VelocityClosedLoop/robot.py index 19cccb80..a75db213 100644 --- a/python/VelocityClosedLoop/robot.py +++ b/python/VelocityClosedLoop/robot.py @@ -4,7 +4,7 @@ """ import wpilib from wpilib import Timer, XboxController -from phoenix6 import hardware, controls, configs, StatusCode +from phoenix6 import CANBus, configs, controls, hardware, StatusCode class MyRobot(wpilib.TimedRobot): """ @@ -15,11 +15,11 @@ class MyRobot(wpilib.TimedRobot): def robotInit(self): """Robot initialization function""" - canbusName = "canivore" + self.canbus = CANBus("canivore") # Keep a reference to all the motor controllers used - self.talonfx = hardware.TalonFX(0, canbusName) - self.talonfx_follower = hardware.TalonFX(1, canbusName) + self.talonfx = hardware.TalonFX(0, self.canbus) + self.talonfx_follower = hardware.TalonFX(1, self.canbus) # Be able to switch which control request to use based on a button press # Start at velocity 0, use slot 0