From e14e45da76074dc472ab0046ffa2951a1fe9f6e9 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 20 May 2016 17:30:37 -0700 Subject: [PATCH] Add format script which invokes clang-format on the C++ source code (#41) On Windows machines, clang-format.exe must be in the PATH environment variable. --- .clang-format | 90 + hal/include/HAL/Accelerometer.hpp | 16 +- hal/include/HAL/Analog.hpp | 138 +- hal/include/HAL/CAN.hpp | 21 +- hal/include/HAL/Compressor.hpp | 35 +- hal/include/HAL/Digital.hpp | 256 +-- hal/include/HAL/Errors.hpp | 43 +- hal/include/HAL/HAL.hpp | 368 ++-- hal/include/HAL/Interrupts.hpp | 36 +- hal/include/HAL/Notifier.hpp | 15 +- hal/include/HAL/PDP.hpp | 21 +- hal/include/HAL/Port.h | 7 +- hal/include/HAL/Power.hpp | 31 +- hal/include/HAL/Semaphore.hpp | 18 +- hal/include/HAL/SerialPort.hpp | 40 +- hal/include/HAL/Solenoid.hpp | 24 +- hal/include/HAL/Task.hpp | 18 +- hal/include/HAL/Utilities.hpp | 13 +- hal/include/HAL/cpp/Resource.hpp | 34 +- hal/include/HAL/cpp/Semaphore.hpp | 2 +- .../HAL/cpp/priority_condition_variable.h | 46 +- hal/include/HAL/cpp/priority_mutex.h | 30 +- hal/include/Log.hpp | 179 +- hal/lib/Athena/Accelerometer.cpp | 267 +-- hal/lib/Athena/Analog.cpp | 400 +++-- hal/lib/Athena/ChipObject.h | 6 +- hal/lib/Athena/Compressor.cpp | 167 +- hal/lib/Athena/Digital.cpp | 1598 +++++++++-------- hal/lib/Athena/HALAthena.cpp | 610 +++---- hal/lib/Athena/Interrupts.cpp | 140 +- hal/lib/Athena/Notifier.cpp | 232 ++- hal/lib/Athena/PDP.cpp | 66 +- hal/lib/Athena/Power.cpp | 100 +- hal/lib/Athena/Semaphore.cpp | 8 +- hal/lib/Athena/SerialPort.cpp | 224 ++- hal/lib/Athena/Solenoid.cpp | 111 +- hal/lib/Athena/Task.cpp | 19 +- hal/lib/Athena/Utilities.cpp | 57 +- hal/lib/Athena/cpp/Resource.cpp | 110 +- hal/lib/Athena/cpp/Semaphore.cpp | 9 +- hal/lib/Athena/cpp/priority_mutex.cpp | 24 +- hal/lib/Desktop/HALDesktop.cpp | 2 +- hal/lib/Shared/HAL.cpp | 204 +-- .../frc_gazebo_plugins/clock/src/clock.cpp | 10 +- .../frc_gazebo_plugins/clock/src/clock.h | 24 +- .../dc_motor/src/dc_motor.cpp | 21 +- .../dc_motor/src/dc_motor.h | 25 +- .../encoder/src/encoder.cpp | 24 +- .../frc_gazebo_plugins/encoder/src/encoder.h | 30 +- .../frc_gazebo_plugins/gyro/src/gyro.cpp | 40 +- simulation/frc_gazebo_plugins/gyro/src/gyro.h | 33 +- .../src/external_limit_switch.cpp | 6 +- .../limit_switch/src/external_limit_switch.h | 8 +- .../src/internal_limit_switch.cpp | 8 +- .../limit_switch/src/internal_limit_switch.h | 8 +- .../limit_switch/src/limit_switch.cpp | 13 +- .../limit_switch/src/limit_switch.h | 22 +- .../limit_switch/src/switch.h | 2 +- .../pneumatic_piston/src/pneumatic_piston.cpp | 30 +- .../pneumatic_piston/src/pneumatic_piston.h | 20 +- .../potentiometer/src/potentiometer.cpp | 23 +- .../potentiometer/src/potentiometer.h | 18 +- .../rangefinder/src/rangefinder.cpp | 23 +- .../rangefinder/src/rangefinder.h | 17 +- .../frc_gazebo_plugins/servo/src/servo.cpp | 101 +- .../frc_gazebo_plugins/servo/src/servo.h | 48 +- styleguide/format.py | 47 + wpilibc/Athena/include/ADXL345_I2C.h | 7 +- wpilibc/Athena/include/ADXL345_SPI.h | 6 +- wpilibc/Athena/include/ADXL362.h | 6 +- wpilibc/Athena/include/ADXRS450_Gyro.h | 2 +- wpilibc/Athena/include/AnalogAccelerometer.h | 12 +- wpilibc/Athena/include/AnalogGyro.h | 24 +- wpilibc/Athena/include/AnalogInput.h | 27 +- wpilibc/Athena/include/AnalogOutput.h | 10 +- wpilibc/Athena/include/AnalogPotentiometer.h | 4 +- wpilibc/Athena/include/AnalogTrigger.h | 9 +- wpilibc/Athena/include/AnalogTriggerOutput.h | 59 +- wpilibc/Athena/include/BuiltInAccelerometer.h | 4 +- wpilibc/Athena/include/CANJaguar.h | 55 +- wpilibc/Athena/include/CANSpeedController.h | 8 +- wpilibc/Athena/include/CANTalon.h | 309 ++-- wpilibc/Athena/include/CameraServer.h | 10 +- wpilibc/Athena/include/Compressor.h | 4 +- wpilibc/Athena/include/Counter.h | 36 +- wpilibc/Athena/include/DigitalGlitchFilter.h | 16 +- wpilibc/Athena/include/DigitalInput.h | 12 +- wpilibc/Athena/include/DigitalOutput.h | 6 +- wpilibc/Athena/include/DoubleSolenoid.h | 2 +- wpilibc/Athena/include/DriverStation.h | 28 +- wpilibc/Athena/include/Encoder.h | 54 +- wpilibc/Athena/include/GearTooth.h | 10 +- wpilibc/Athena/include/I2C.h | 10 +- .../Athena/include/InterruptableSensorBase.h | 13 +- wpilibc/Athena/include/IterativeRobot.h | 19 +- wpilibc/Athena/include/Joystick.h | 13 +- wpilibc/Athena/include/MotorSafetyHelper.h | 22 +- wpilibc/Athena/include/Notifier.h | 7 +- wpilibc/Athena/include/PWM.h | 27 +- wpilibc/Athena/include/PWMSpeedController.h | 2 + .../Athena/include/PowerDistributionPanel.h | 2 +- wpilibc/Athena/include/Preferences.h | 20 +- wpilibc/Athena/include/Relay.h | 21 +- wpilibc/Athena/include/RobotBase.h | 29 +- wpilibc/Athena/include/RobotDrive.h | 57 +- wpilibc/Athena/include/SPI.h | 8 +- wpilibc/Athena/include/SafePWM.h | 14 +- wpilibc/Athena/include/SerialPort.h | 9 +- wpilibc/Athena/include/Solenoid.h | 5 +- wpilibc/Athena/include/SolenoidBase.h | 6 +- wpilibc/Athena/include/USBCamera.h | 24 +- wpilibc/Athena/include/Ultrasonic.h | 35 +- wpilibc/Athena/include/Vision/AxisCamera.h | 16 +- wpilibc/Athena/include/Vision/BaeUtilities.h | 16 +- wpilibc/Athena/include/Vision/BinaryImage.h | 20 +- wpilibc/Athena/include/Vision/ColorImage.h | 84 +- wpilibc/Athena/include/Vision/HSLImage.h | 2 +- wpilibc/Athena/include/Vision/ImageBase.h | 8 +- wpilibc/Athena/include/Vision/MonoImage.h | 10 +- wpilibc/Athena/include/Vision/RGBImage.h | 2 +- wpilibc/Athena/include/Vision/VisionAPI.h | 28 +- wpilibc/Athena/include/WPILib.h | 20 +- wpilibc/Athena/src/ADXL345_I2C.cpp | 17 +- wpilibc/Athena/src/ADXL345_SPI.cpp | 6 +- wpilibc/Athena/src/ADXL362.cpp | 12 +- wpilibc/Athena/src/ADXRS450_Gyro.cpp | 30 +- wpilibc/Athena/src/AnalogAccelerometer.cpp | 25 +- wpilibc/Athena/src/AnalogGyro.cpp | 92 +- wpilibc/Athena/src/AnalogInput.cpp | 56 +- wpilibc/Athena/src/AnalogOutput.cpp | 14 +- wpilibc/Athena/src/AnalogPotentiometer.cpp | 37 +- wpilibc/Athena/src/AnalogTrigger.cpp | 44 +- wpilibc/Athena/src/AnalogTriggerOutput.cpp | 10 +- wpilibc/Athena/src/BuiltInAccelerometer.cpp | 7 +- wpilibc/Athena/src/CANJaguar.cpp | 447 ++--- wpilibc/Athena/src/CANTalon.cpp | 583 +++--- wpilibc/Athena/src/CameraServer.cpp | 13 +- wpilibc/Athena/src/Compressor.cpp | 60 +- wpilibc/Athena/src/ControllerPower.cpp | 49 +- wpilibc/Athena/src/Counter.cpp | 216 ++- wpilibc/Athena/src/DigitalGlitchFilter.cpp | 28 +- wpilibc/Athena/src/DigitalInput.cpp | 4 +- wpilibc/Athena/src/DigitalOutput.cpp | 19 +- wpilibc/Athena/src/DoubleSolenoid.cpp | 28 +- wpilibc/Athena/src/DriverStation.cpp | 107 +- wpilibc/Athena/src/Encoder.cpp | 233 +-- wpilibc/Athena/src/GearTooth.cpp | 25 +- wpilibc/Athena/src/I2C.cpp | 63 +- .../Athena/src/InterruptableSensorBase.cpp | 63 +- wpilibc/Athena/src/IterativeRobot.cpp | 25 +- wpilibc/Athena/src/Jaguar.cpp | 6 +- wpilibc/Athena/src/Joystick.cpp | 87 +- wpilibc/Athena/src/MotorSafetyHelper.cpp | 30 +- wpilibc/Athena/src/Notifier.cpp | 17 +- wpilibc/Athena/src/PIDController.cpp | 133 +- wpilibc/Athena/src/PWM.cpp | 49 +- wpilibc/Athena/src/PWMSpeedController.cpp | 23 +- wpilibc/Athena/src/PowerDistributionPanel.cpp | 29 +- wpilibc/Athena/src/Preferences.cpp | 53 +- wpilibc/Athena/src/Relay.cpp | 45 +- wpilibc/Athena/src/RobotBase.cpp | 38 +- wpilibc/Athena/src/RobotDrive.cpp | 289 +-- wpilibc/Athena/src/SD540.cpp | 14 +- wpilibc/Athena/src/SPI.cpp | 39 +- wpilibc/Athena/src/SafePWM.cpp | 29 +- wpilibc/Athena/src/SampleRobot.cpp | 54 +- wpilibc/Athena/src/SensorBase.cpp | 26 +- wpilibc/Athena/src/SerialPort.cpp | 28 +- wpilibc/Athena/src/Servo.cpp | 14 +- wpilibc/Athena/src/Solenoid.cpp | 13 +- wpilibc/Athena/src/SolenoidBase.cpp | 31 +- wpilibc/Athena/src/Spark.cpp | 13 +- wpilibc/Athena/src/Talon.cpp | 5 +- wpilibc/Athena/src/TalonSRX.cpp | 5 +- wpilibc/Athena/src/Task.cpp | 20 +- wpilibc/Athena/src/Timer.cpp | 30 +- wpilibc/Athena/src/USBCamera.cpp | 17 +- wpilibc/Athena/src/Ultrasonic.cpp | 112 +- wpilibc/Athena/src/Utility.cpp | 61 +- wpilibc/Athena/src/Victor.cpp | 7 +- wpilibc/Athena/src/VictorSP.cpp | 34 +- wpilibc/Athena/src/Vision/AxisCamera.cpp | 95 +- wpilibc/Athena/src/Vision/BaeUtilities.cpp | 89 +- wpilibc/Athena/src/Vision/BinaryImage.cpp | 78 +- wpilibc/Athena/src/Vision/ColorImage.cpp | 324 ++-- wpilibc/Athena/src/Vision/FrcError.cpp | 18 +- wpilibc/Athena/src/Vision/HSLImage.cpp | 3 +- wpilibc/Athena/src/Vision/ImageBase.cpp | 13 +- wpilibc/Athena/src/Vision/MonoImage.cpp | 22 +- wpilibc/Athena/src/Vision/RGBImage.cpp | 3 +- wpilibc/Athena/src/Vision/VisionAPI.cpp | 892 ++++----- wpilibc/shared/include/Base.h | 55 +- wpilibc/shared/include/Buttons/Button.h | 10 +- .../shared/include/Buttons/ButtonScheduler.h | 6 +- .../include/Buttons/CancelButtonScheduler.h | 2 +- .../include/Buttons/HeldButtonScheduler.h | 2 +- .../shared/include/Buttons/JoystickButton.h | 6 +- .../shared/include/Buttons/NetworkButton.h | 8 +- .../include/Buttons/PressedButtonScheduler.h | 2 +- .../include/Buttons/ReleasedButtonScheduler.h | 2 +- .../include/Buttons/ToggleButtonScheduler.h | 2 +- wpilibc/shared/include/Buttons/Trigger.h | 12 +- wpilibc/shared/include/CircularBuffer.h | 2 +- wpilibc/shared/include/Commands/Command.h | 78 +- .../shared/include/Commands/CommandGroup.h | 31 +- .../include/Commands/CommandGroupEntry.h | 4 +- wpilibc/shared/include/Commands/PIDCommand.h | 9 +- .../shared/include/Commands/PIDSubsystem.h | 14 +- .../shared/include/Commands/PrintCommand.h | 4 +- wpilibc/shared/include/Commands/Scheduler.h | 28 +- .../shared/include/Commands/StartCommand.h | 4 +- wpilibc/shared/include/Commands/Subsystem.h | 18 +- wpilibc/shared/include/Commands/WaitCommand.h | 2 +- .../shared/include/Commands/WaitForChildren.h | 2 +- .../include/Commands/WaitUntilCommand.h | 2 +- wpilibc/shared/include/Controller.h | 8 +- wpilibc/shared/include/Error.h | 15 +- wpilibc/shared/include/ErrorBase.h | 3 +- .../include/Filters/LinearDigitalFilter.h | 5 +- wpilibc/shared/include/GenericHID.h | 3 +- wpilibc/shared/include/GyroBase.h | 11 +- .../shared/include/LiveWindow/LiveWindow.h | 41 +- wpilibc/shared/include/PIDController.h | 59 +- wpilibc/shared/include/PIDOutput.h | 2 +- wpilibc/shared/include/PIDSource.h | 3 +- wpilibc/shared/include/Resource.h | 9 +- wpilibc/shared/include/SensorBase.h | 5 +- .../include/SmartDashboard/NamedSendable.h | 2 +- .../shared/include/SmartDashboard/Sendable.h | 4 +- .../include/SmartDashboard/SendableChooser.h | 28 +- .../include/SmartDashboard/SmartDashboard.h | 17 +- wpilibc/shared/include/Task.h | 4 +- wpilibc/shared/include/Task.inc | 9 +- wpilibc/shared/include/Timer.h | 12 +- wpilibc/shared/include/Utility.h | 22 +- wpilibc/shared/include/WPIErrors.h | 4 +- wpilibc/shared/src/Buttons/Button.cpp | 33 +- .../shared/src/Buttons/ButtonScheduler.cpp | 2 +- .../src/Buttons/CancelButtonScheduler.cpp | 4 +- .../src/Buttons/HeldButtonScheduler.cpp | 4 +- wpilibc/shared/src/Buttons/JoystickButton.cpp | 2 +- wpilibc/shared/src/Buttons/NetworkButton.cpp | 6 +- .../src/Buttons/PressedButtonScheduler.cpp | 4 +- .../src/Buttons/ReleasedButtonScheduler.cpp | 4 +- .../src/Buttons/ToggleButtonScheduler.cpp | 4 +- wpilibc/shared/src/Buttons/Trigger.cpp | 12 +- wpilibc/shared/src/Commands/Command.cpp | 112 +- wpilibc/shared/src/Commands/CommandGroup.cpp | 98 +- .../shared/src/Commands/CommandGroupEntry.cpp | 2 +- wpilibc/shared/src/Commands/PIDCommand.cpp | 11 +- wpilibc/shared/src/Commands/PIDSubsystem.cpp | 124 +- wpilibc/shared/src/Commands/PrintCommand.cpp | 8 +- wpilibc/shared/src/Commands/Scheduler.cpp | 57 +- wpilibc/shared/src/Commands/StartCommand.cpp | 2 +- wpilibc/shared/src/Commands/Subsystem.cpp | 37 +- wpilibc/shared/src/Commands/WaitCommand.cpp | 4 +- .../shared/src/Commands/WaitForChildren.cpp | 2 +- .../shared/src/Commands/WaitUntilCommand.cpp | 3 +- wpilibc/shared/src/Error.cpp | 17 +- wpilibc/shared/src/ErrorBase.cpp | 54 +- wpilibc/shared/src/Filters/Filter.cpp | 8 +- .../src/Filters/LinearDigitalFilter.cpp | 78 +- wpilibc/shared/src/GyroBase.cpp | 2 +- wpilibc/shared/src/LiveWindow/LiveWindow.cpp | 78 +- .../LiveWindow/LiveWindowStatusListener.cpp | 2 +- wpilibc/shared/src/PIDSource.cpp | 4 +- wpilibc/shared/src/Resource.cpp | 40 +- wpilibc/shared/src/RobotState.cpp | 3 +- .../src/SmartDashboard/SendableChooser.cpp | 35 +- .../src/SmartDashboard/SmartDashboard.cpp | 69 +- .../shared/src/interfaces/Potentiometer.cpp | 2 +- wpilibc/simulation/include/AnalogGyro.h | 51 +- wpilibc/simulation/include/AnalogInput.h | 67 +- .../simulation/include/AnalogPotentiometer.h | 119 +- wpilibc/simulation/include/Counter.h | 135 +- wpilibc/simulation/include/CounterBase.h | 31 +- wpilibc/simulation/include/DigitalInput.h | 44 +- wpilibc/simulation/include/DoubleSolenoid.h | 49 +- wpilibc/simulation/include/DriverStation.h | 211 ++- wpilibc/simulation/include/Encoder.h | 136 +- wpilibc/simulation/include/IterativeRobot.h | 94 +- wpilibc/simulation/include/Jaguar.h | 21 +- wpilibc/simulation/include/Joystick.h | 104 +- wpilibc/simulation/include/MotorSafety.h | 19 +- .../simulation/include/MotorSafetyHelper.h | 21 +- wpilibc/simulation/include/Notifier.h | 7 +- wpilibc/simulation/include/PWM.h | 139 +- wpilibc/simulation/include/Relay.h | 74 +- wpilibc/simulation/include/RobotBase.h | 65 +- wpilibc/simulation/include/RobotDrive.h | 191 +- wpilibc/simulation/include/SafePWM.h | 45 +- wpilibc/simulation/include/SampleRobot.h | 27 +- wpilibc/simulation/include/Solenoid.h | 43 +- wpilibc/simulation/include/SpeedController.h | 42 +- wpilibc/simulation/include/Talon.h | 21 +- wpilibc/simulation/include/Victor.h | 21 +- wpilibc/simulation/include/WPILib.h | 31 +- .../simulation/include/simulation/MainNode.h | 76 +- .../include/simulation/SimContinuousOutput.h | 43 +- .../include/simulation/SimDigitalInput.h | 23 +- .../include/simulation/SimEncoder.h | 33 +- .../include/simulation/SimFloatInput.h | 23 +- .../simulation/include/simulation/SimGyro.h | 27 +- .../simulation/include/simulation/simTime.h | 17 +- wpilibc/simulation/src/AnalogGyro.cpp | 73 +- wpilibc/simulation/src/AnalogInput.cpp | 84 +- .../simulation/src/AnalogPotentiometer.cpp | 53 +- wpilibc/simulation/src/DigitalInput.cpp | 47 +- wpilibc/simulation/src/DoubleSolenoid.cpp | 119 +- wpilibc/simulation/src/DriverStation.cpp | 394 ++-- wpilibc/simulation/src/Encoder.cpp | 417 +++-- wpilibc/simulation/src/IterativeRobot.cpp | 375 ++-- wpilibc/simulation/src/Jaguar.cpp | 52 +- wpilibc/simulation/src/Joystick.cpp | 228 ++- wpilibc/simulation/src/MotorSafetyHelper.cpp | 53 +- wpilibc/simulation/src/Notifier.cpp | 375 ++-- wpilibc/simulation/src/PIDController.cpp | 687 ++++--- wpilibc/simulation/src/PWM.cpp | 216 +-- wpilibc/simulation/src/Relay.cpp | 256 ++- wpilibc/simulation/src/RobotBase.cpp | 89 +- wpilibc/simulation/src/RobotDrive.cpp | 980 +++++----- wpilibc/simulation/src/SafePWM.cpp | 81 +- wpilibc/simulation/src/SampleRobot.cpp | 184 +- wpilibc/simulation/src/SensorBase.cpp | 122 +- wpilibc/simulation/src/Solenoid.cpp | 70 +- wpilibc/simulation/src/Talon.cpp | 25 +- wpilibc/simulation/src/Timer.cpp | 209 +-- wpilibc/simulation/src/Utility.cpp | 201 +-- wpilibc/simulation/src/Victor.cpp | 25 +- .../src/simulation/SimContinuousOutput.cpp | 14 +- .../src/simulation/SimDigitalInput.cpp | 11 +- .../simulation/src/simulation/SimEncoder.cpp | 60 +- .../src/simulation/SimFloatInput.cpp | 11 +- wpilibc/simulation/src/simulation/SimGyro.cpp | 48 +- .../src/AnalogLoopTest.cpp | 13 +- .../src/AnalogPotentiometerTest.cpp | 6 +- wpilibcIntegrationTests/src/CANJaguarTest.cpp | 22 +- wpilibcIntegrationTests/src/CANTalonTest.cpp | 6 +- .../src/CircularBufferTest.cpp | 27 +- .../src/ConditionVariableTest.cpp | 44 +- wpilibcIntegrationTests/src/CounterTest.cpp | 14 +- wpilibcIntegrationTests/src/DIOLoopTest.cpp | 26 +- .../src/DigitalGlitchFilterTest.cpp | 2 +- .../src/FakeEncoderTest.cpp | 12 +- .../src/FilterNoiseTest.cpp | 34 +- .../src/FilterOutputTest.cpp | 35 +- .../src/MotorEncoderTest.cpp | 16 +- .../src/MotorInvertingTest.cpp | 8 +- wpilibcIntegrationTests/src/MutexTest.cpp | 15 +- wpilibcIntegrationTests/src/NotifierTest.cpp | 4 +- wpilibcIntegrationTests/src/PCMTest.cpp | 19 +- .../src/PIDToleranceTest.cpp | 133 +- .../src/PowerDistributionPanelTest.cpp | 11 +- .../src/PreferencesTest.cpp | 28 +- wpilibcIntegrationTests/src/RelayTest.cpp | 10 +- .../src/TestEnvironment.cpp | 2 +- .../src/TiltPanCameraTest.cpp | 8 +- wpilibcIntegrationTests/src/TimerTest.cpp | 4 +- .../src/command/CommandTest.cpp | 8 +- .../src/athena/cpp/lib/AccelerometerJNI.cpp | 42 +- wpilibj/src/athena/cpp/lib/AnalogJNI.cpp | 690 +++---- wpilibj/src/athena/cpp/lib/CANJNI.cpp | 117 +- wpilibj/src/athena/cpp/lib/CompressorJNI.cpp | 202 +-- wpilibj/src/athena/cpp/lib/CounterJNI.cpp | 502 +++--- wpilibj/src/athena/cpp/lib/DIOJNI.cpp | 225 ++- .../athena/cpp/lib/DigitalGlitchFilterJNI.cpp | 24 +- wpilibj/src/athena/cpp/lib/EncoderJNI.cpp | 295 +-- .../lib/FRCNetworkCommunicationsLibrary.cpp | 370 ++-- wpilibj/src/athena/cpp/lib/HALUtil.cpp | 127 +- wpilibj/src/athena/cpp/lib/I2CJNI.cpp | 132 +- wpilibj/src/athena/cpp/lib/InterruptJNI.cpp | 85 +- wpilibj/src/athena/cpp/lib/JNIWrapper.cpp | 52 +- wpilibj/src/athena/cpp/lib/NotifierJNI.cpp | 65 +- wpilibj/src/athena/cpp/lib/PDPJNI.cpp | 91 +- wpilibj/src/athena/cpp/lib/PWMJNI.cpp | 209 ++- wpilibj/src/athena/cpp/lib/PowerJNI.cpp | 133 +- wpilibj/src/athena/cpp/lib/RelayJNI.cpp | 92 +- wpilibj/src/athena/cpp/lib/SPIJNI.cpp | 417 +++-- wpilibj/src/athena/cpp/lib/SafeThread.cpp | 2 +- wpilibj/src/athena/cpp/lib/SafeThread.h | 2 +- wpilibj/src/athena/cpp/lib/SerialPortJNI.cpp | 339 ++-- wpilibj/src/athena/cpp/lib/SolenoidJNI.cpp | 178 +- wpilibj/src/athena/cpp/nivision/dxattr.h | 74 +- 383 files changed, 13787 insertions(+), 13198 deletions(-) create mode 100644 .clang-format create mode 100755 styleguide/format.py diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000000..d332ee08ca --- /dev/null +++ b/.clang-format @@ -0,0 +1,90 @@ +--- +Language: Cpp +BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlinesLeft: true +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeCategories: + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IndentCaseLabels: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 8 +UseTab: Never +... + diff --git a/hal/include/HAL/Accelerometer.hpp b/hal/include/HAL/Accelerometer.hpp index 103fb2a318..ac17a9fb76 100644 --- a/hal/include/HAL/Accelerometer.hpp +++ b/hal/include/HAL/Accelerometer.hpp @@ -1,15 +1,15 @@ #pragma once enum AccelerometerRange { - kRange_2G = 0, - kRange_4G = 1, - kRange_8G = 2, + kRange_2G = 0, + kRange_4G = 1, + kRange_8G = 2, }; extern "C" { - void setAccelerometerActive(bool); - void setAccelerometerRange(AccelerometerRange); - double getAccelerometerX(); - double getAccelerometerY(); - double getAccelerometerZ(); +void setAccelerometerActive(bool); +void setAccelerometerRange(AccelerometerRange); +double getAccelerometerX(); +double getAccelerometerY(); +double getAccelerometerZ(); } diff --git a/hal/include/HAL/Analog.hpp b/hal/include/HAL/Analog.hpp index 2aa5e43ede..a4decebbe6 100644 --- a/hal/include/HAL/Analog.hpp +++ b/hal/include/HAL/Analog.hpp @@ -2,77 +2,85 @@ #include -enum AnalogTriggerType -{ - kInWindow = 0, - kState = 1, - kRisingPulse = 2, - kFallingPulse = 3 +enum AnalogTriggerType { + kInWindow = 0, + kState = 1, + kRisingPulse = 2, + kFallingPulse = 3 }; -extern "C" -{ - // Analog output functions - void* initializeAnalogOutputPort(void* port_pointer, int32_t *status); - void freeAnalogOutputPort(void* analog_port_pointer); - void setAnalogOutput(void* analog_port_pointer, double voltage, int32_t *status); - double getAnalogOutput(void* analog_port_pointer, int32_t *status); - bool checkAnalogOutputChannel(uint32_t pin); +extern "C" { +// Analog output functions +void* initializeAnalogOutputPort(void* port_pointer, int32_t* status); +void freeAnalogOutputPort(void* analog_port_pointer); +void setAnalogOutput(void* analog_port_pointer, double voltage, + int32_t* status); +double getAnalogOutput(void* analog_port_pointer, int32_t* status); +bool checkAnalogOutputChannel(uint32_t pin); - // Analog input functions - void* initializeAnalogInputPort(void* port_pointer, int32_t *status); - void freeAnalogInputPort(void* analog_port_pointer); - bool checkAnalogModule(uint8_t module); - bool checkAnalogInputChannel(uint32_t pin); +// Analog input functions +void* initializeAnalogInputPort(void* port_pointer, int32_t* status); +void freeAnalogInputPort(void* analog_port_pointer); +bool checkAnalogModule(uint8_t module); +bool checkAnalogInputChannel(uint32_t pin); - void setAnalogSampleRate(double samplesPerSecond, int32_t *status); - float getAnalogSampleRate(int32_t *status); - void setAnalogAverageBits(void* analog_port_pointer, uint32_t bits, int32_t *status); - uint32_t getAnalogAverageBits(void* analog_port_pointer, int32_t *status); - void setAnalogOversampleBits(void* analog_port_pointer, uint32_t bits, int32_t *status); - uint32_t getAnalogOversampleBits(void* analog_port_pointer, int32_t *status); - int16_t getAnalogValue(void* analog_port_pointer, int32_t *status); - int32_t getAnalogAverageValue(void* analog_port_pointer, int32_t *status); - int32_t getAnalogVoltsToValue(void* analog_port_pointer, double voltage, int32_t *status); - float getAnalogVoltage(void* analog_port_pointer, int32_t *status); - float getAnalogAverageVoltage(void* analog_port_pointer, int32_t *status); - uint32_t getAnalogLSBWeight(void* analog_port_pointer, int32_t *status); - int32_t getAnalogOffset(void* analog_port_pointer, int32_t *status); +void setAnalogSampleRate(double samplesPerSecond, int32_t* status); +float getAnalogSampleRate(int32_t* status); +void setAnalogAverageBits(void* analog_port_pointer, uint32_t bits, + int32_t* status); +uint32_t getAnalogAverageBits(void* analog_port_pointer, int32_t* status); +void setAnalogOversampleBits(void* analog_port_pointer, uint32_t bits, + int32_t* status); +uint32_t getAnalogOversampleBits(void* analog_port_pointer, int32_t* status); +int16_t getAnalogValue(void* analog_port_pointer, int32_t* status); +int32_t getAnalogAverageValue(void* analog_port_pointer, int32_t* status); +int32_t getAnalogVoltsToValue(void* analog_port_pointer, double voltage, + int32_t* status); +float getAnalogVoltage(void* analog_port_pointer, int32_t* status); +float getAnalogAverageVoltage(void* analog_port_pointer, int32_t* status); +uint32_t getAnalogLSBWeight(void* analog_port_pointer, int32_t* status); +int32_t getAnalogOffset(void* analog_port_pointer, int32_t* status); - bool isAccumulatorChannel(void* analog_port_pointer, int32_t *status); - void initAccumulator(void* analog_port_pointer, int32_t *status); - void resetAccumulator(void* analog_port_pointer, int32_t *status); - void setAccumulatorCenter(void* analog_port_pointer, int32_t center, int32_t *status); - void setAccumulatorDeadband(void* analog_port_pointer, int32_t deadband, int32_t *status); - int64_t getAccumulatorValue(void* analog_port_pointer, int32_t *status); - uint32_t getAccumulatorCount(void* analog_port_pointer, int32_t *status); - void getAccumulatorOutput(void* analog_port_pointer, int64_t *value, uint32_t *count, - int32_t *status); +bool isAccumulatorChannel(void* analog_port_pointer, int32_t* status); +void initAccumulator(void* analog_port_pointer, int32_t* status); +void resetAccumulator(void* analog_port_pointer, int32_t* status); +void setAccumulatorCenter(void* analog_port_pointer, int32_t center, + int32_t* status); +void setAccumulatorDeadband(void* analog_port_pointer, int32_t deadband, + int32_t* status); +int64_t getAccumulatorValue(void* analog_port_pointer, int32_t* status); +uint32_t getAccumulatorCount(void* analog_port_pointer, int32_t* status); +void getAccumulatorOutput(void* analog_port_pointer, int64_t* value, + uint32_t* count, int32_t* status); - void* initializeAnalogTrigger(void* port_pointer, uint32_t *index, int32_t *status); - void cleanAnalogTrigger(void* analog_trigger_pointer, int32_t *status); - void setAnalogTriggerLimitsRaw(void* analog_trigger_pointer, int32_t lower, int32_t upper, - int32_t *status); - void setAnalogTriggerLimitsVoltage(void* analog_trigger_pointer, double lower, double upper, - int32_t *status); - void setAnalogTriggerAveraged(void* analog_trigger_pointer, bool useAveragedValue, - int32_t *status); - void setAnalogTriggerFiltered(void* analog_trigger_pointer, bool useFilteredValue, - int32_t *status); - bool getAnalogTriggerInWindow(void* analog_trigger_pointer, int32_t *status); - bool getAnalogTriggerTriggerState(void* analog_trigger_pointer, int32_t *status); - bool getAnalogTriggerOutput(void* analog_trigger_pointer, AnalogTriggerType type, - int32_t *status); +void* initializeAnalogTrigger(void* port_pointer, uint32_t* index, + int32_t* status); +void cleanAnalogTrigger(void* analog_trigger_pointer, int32_t* status); +void setAnalogTriggerLimitsRaw(void* analog_trigger_pointer, int32_t lower, + int32_t upper, int32_t* status); +void setAnalogTriggerLimitsVoltage(void* analog_trigger_pointer, double lower, + double upper, int32_t* status); +void setAnalogTriggerAveraged(void* analog_trigger_pointer, + bool useAveragedValue, int32_t* status); +void setAnalogTriggerFiltered(void* analog_trigger_pointer, + bool useFilteredValue, int32_t* status); +bool getAnalogTriggerInWindow(void* analog_trigger_pointer, int32_t* status); +bool getAnalogTriggerTriggerState(void* analog_trigger_pointer, + int32_t* status); +bool getAnalogTriggerOutput(void* analog_trigger_pointer, + AnalogTriggerType type, int32_t* status); - //// Float JNA Hack - // Float - int getAnalogSampleRateIntHack(int32_t *status); - int getAnalogVoltageIntHack(void* analog_port_pointer, int32_t *status); - int getAnalogAverageVoltageIntHack(void* analog_port_pointer, int32_t *status); +//// Float JNA Hack +// Float +int getAnalogSampleRateIntHack(int32_t* status); +int getAnalogVoltageIntHack(void* analog_port_pointer, int32_t* status); +int getAnalogAverageVoltageIntHack(void* analog_port_pointer, int32_t* status); - // Doubles - void setAnalogSampleRateIntHack(int samplesPerSecond, int32_t *status); - int32_t getAnalogVoltsToValueIntHack(void* analog_port_pointer, int voltage, int32_t *status); - void setAnalogTriggerLimitsVoltageIntHack(void* analog_trigger_pointer, int lower, int upper, - int32_t *status); +// Doubles +void setAnalogSampleRateIntHack(int samplesPerSecond, int32_t* status); +int32_t getAnalogVoltsToValueIntHack(void* analog_port_pointer, int voltage, + int32_t* status); +void setAnalogTriggerLimitsVoltageIntHack(void* analog_trigger_pointer, + int lower, int upper, + int32_t* status); } diff --git a/hal/include/HAL/CAN.hpp b/hal/include/HAL/CAN.hpp index c10450b58b..ba535ec23c 100644 --- a/hal/include/HAL/CAN.hpp +++ b/hal/include/HAL/CAN.hpp @@ -3,24 +3,25 @@ #include #include "FRC_NetworkCommunication/CANSessionMux.h" -void canTxSend(uint32_t arbID, uint8_t length, int32_t period = CAN_SEND_PERIOD_NO_REPEAT); +void canTxSend(uint32_t arbID, uint8_t length, + int32_t period = CAN_SEND_PERIOD_NO_REPEAT); -void canTxPackInt8 (uint32_t arbID, uint8_t offset, uint8_t value); +void canTxPackInt8(uint32_t arbID, uint8_t offset, uint8_t value); void canTxPackInt16(uint32_t arbID, uint8_t offset, uint16_t value); void canTxPackInt32(uint32_t arbID, uint8_t offset, uint32_t value); -void canTxPackFXP16(uint32_t arbID, uint8_t offset, double value); -void canTxPackFXP32(uint32_t arbID, uint8_t offset, double value); +void canTxPackFXP16(uint32_t arbID, uint8_t offset, double value); +void canTxPackFXP32(uint32_t arbID, uint8_t offset, double value); -uint8_t canTxUnpackInt8 (uint32_t arbID, uint8_t offset); +uint8_t canTxUnpackInt8(uint32_t arbID, uint8_t offset); uint32_t canTxUnpackInt32(uint32_t arbID, uint8_t offset); uint16_t canTxUnpackInt16(uint32_t arbID, uint8_t offset); -double canTxUnpackFXP16(uint32_t arbID, uint8_t offset); -double canTxUnpackFXP32(uint32_t arbID, uint8_t offset); +double canTxUnpackFXP16(uint32_t arbID, uint8_t offset); +double canTxUnpackFXP32(uint32_t arbID, uint8_t offset); bool canRxReceive(uint32_t arbID); -uint8_t canRxUnpackInt8 (uint32_t arbID, uint8_t offset); +uint8_t canRxUnpackInt8(uint32_t arbID, uint8_t offset); uint16_t canRxUnpackInt16(uint32_t arbID, uint8_t offset); uint32_t canRxUnpackInt32(uint32_t arbID, uint8_t offset); -double canRxUnpackFXP16(uint32_t arbID, uint8_t offset); -double canRxUnpackFXP32(uint32_t arbID, uint8_t offset); +double canRxUnpackFXP16(uint32_t arbID, uint8_t offset); +double canRxUnpackFXP32(uint32_t arbID, uint8_t offset); diff --git a/hal/include/HAL/Compressor.hpp b/hal/include/HAL/Compressor.hpp index 465a63ba64..1062c0e4c8 100644 --- a/hal/include/HAL/Compressor.hpp +++ b/hal/include/HAL/Compressor.hpp @@ -9,25 +9,24 @@ #define __HAL_COMPRESSOR_H__ extern "C" { - void *initializeCompressor(uint8_t module); - bool checkCompressorModule(uint8_t module); - - bool getCompressor(void *pcm_pointer, int32_t *status); - - void setClosedLoopControl(void *pcm_pointer, bool value, int32_t *status); - bool getClosedLoopControl(void *pcm_pointer, int32_t *status); - - bool getPressureSwitch(void *pcm_pointer, int32_t *status); - float getCompressorCurrent(void *pcm_pointer, int32_t *status); +void* initializeCompressor(uint8_t module); +bool checkCompressorModule(uint8_t module); - bool getCompressorCurrentTooHighFault(void *pcm_pointer, int32_t *status); - bool getCompressorCurrentTooHighStickyFault(void *pcm_pointer, int32_t *status); - bool getCompressorShortedStickyFault(void *pcm_pointer, int32_t *status); - bool getCompressorShortedFault(void *pcm_pointer, int32_t *status); - bool getCompressorNotConnectedStickyFault(void *pcm_pointer, int32_t *status); - bool getCompressorNotConnectedFault(void *pcm_pointer, int32_t *status); - void clearAllPCMStickyFaults(void *pcm_pointer, int32_t *status); +bool getCompressor(void* pcm_pointer, int32_t* status); + +void setClosedLoopControl(void* pcm_pointer, bool value, int32_t* status); +bool getClosedLoopControl(void* pcm_pointer, int32_t* status); + +bool getPressureSwitch(void* pcm_pointer, int32_t* status); +float getCompressorCurrent(void* pcm_pointer, int32_t* status); + +bool getCompressorCurrentTooHighFault(void* pcm_pointer, int32_t* status); +bool getCompressorCurrentTooHighStickyFault(void* pcm_pointer, int32_t* status); +bool getCompressorShortedStickyFault(void* pcm_pointer, int32_t* status); +bool getCompressorShortedFault(void* pcm_pointer, int32_t* status); +bool getCompressorNotConnectedStickyFault(void* pcm_pointer, int32_t* status); +bool getCompressorNotConnectedFault(void* pcm_pointer, int32_t* status); +void clearAllPCMStickyFaults(void* pcm_pointer, int32_t* status); } #endif - diff --git a/hal/include/HAL/Digital.hpp b/hal/include/HAL/Digital.hpp index 64851f18f8..7f580209da 100644 --- a/hal/include/HAL/Digital.hpp +++ b/hal/include/HAL/Digital.hpp @@ -3,137 +3,157 @@ #include "HAL/cpp/priority_mutex.h" -enum Mode -{ - kTwoPulse = 0, - kSemiperiod = 1, - kPulseLength = 2, - kExternalDirection = 3 +enum Mode { + kTwoPulse = 0, + kSemiperiod = 1, + kPulseLength = 2, + kExternalDirection = 3 }; priority_recursive_mutex& spiGetSemaphore(uint8_t port); -extern "C" -{ - void* initializeDigitalPort(void* port_pointer, int32_t *status); - void freeDigitalPort(void* digital_port_pointer); - bool checkPWMChannel(void* digital_port_pointer); - bool checkRelayChannel(void* digital_port_pointer); +extern "C" { +void* initializeDigitalPort(void* port_pointer, int32_t* status); +void freeDigitalPort(void* digital_port_pointer); +bool checkPWMChannel(void* digital_port_pointer); +bool checkRelayChannel(void* digital_port_pointer); - void setPWM(void* digital_port_pointer, unsigned short value, int32_t *status); - bool allocatePWMChannel(void* digital_port_pointer, int32_t *status); - void freePWMChannel(void* digital_port_pointer, int32_t *status); - unsigned short getPWM(void* digital_port_pointer, int32_t *status); - void latchPWMZero(void* digital_port_pointer, int32_t *status); - void setPWMPeriodScale(void* digital_port_pointer, uint32_t squelchMask, int32_t *status); - void* allocatePWM(int32_t *status); - void freePWM(void* pwmGenerator, int32_t *status); - void setPWMRate(double rate, int32_t *status); - void setPWMDutyCycle(void* pwmGenerator, double dutyCycle, int32_t *status); - void setPWMOutputChannel(void* pwmGenerator, uint32_t pin, int32_t *status); +void setPWM(void* digital_port_pointer, unsigned short value, int32_t* status); +bool allocatePWMChannel(void* digital_port_pointer, int32_t* status); +void freePWMChannel(void* digital_port_pointer, int32_t* status); +unsigned short getPWM(void* digital_port_pointer, int32_t* status); +void latchPWMZero(void* digital_port_pointer, int32_t* status); +void setPWMPeriodScale(void* digital_port_pointer, uint32_t squelchMask, + int32_t* status); +void* allocatePWM(int32_t* status); +void freePWM(void* pwmGenerator, int32_t* status); +void setPWMRate(double rate, int32_t* status); +void setPWMDutyCycle(void* pwmGenerator, double dutyCycle, int32_t* status); +void setPWMOutputChannel(void* pwmGenerator, uint32_t pin, int32_t* status); - void setRelayForward(void* digital_port_pointer, bool on, int32_t *status); - void setRelayReverse(void* digital_port_pointer, bool on, int32_t *status); - bool getRelayForward(void* digital_port_pointer, int32_t *status); - bool getRelayReverse(void* digital_port_pointer, int32_t *status); +void setRelayForward(void* digital_port_pointer, bool on, int32_t* status); +void setRelayReverse(void* digital_port_pointer, bool on, int32_t* status); +bool getRelayForward(void* digital_port_pointer, int32_t* status); +bool getRelayReverse(void* digital_port_pointer, int32_t* status); - bool allocateDIO(void* digital_port_pointer, bool input, int32_t *status); - void freeDIO(void* digital_port_pointer, int32_t *status); - void setDIO(void* digital_port_pointer, short value, int32_t *status); - bool getDIO(void* digital_port_pointer, int32_t *status); - bool getDIODirection(void* digital_port_pointer, int32_t *status); - void pulse(void* digital_port_pointer, double pulseLength, int32_t *status); - bool isPulsing(void* digital_port_pointer, int32_t *status); - bool isAnyPulsing(int32_t *status); +bool allocateDIO(void* digital_port_pointer, bool input, int32_t* status); +void freeDIO(void* digital_port_pointer, int32_t* status); +void setDIO(void* digital_port_pointer, short value, int32_t* status); +bool getDIO(void* digital_port_pointer, int32_t* status); +bool getDIODirection(void* digital_port_pointer, int32_t* status); +void pulse(void* digital_port_pointer, double pulseLength, int32_t* status); +bool isPulsing(void* digital_port_pointer, int32_t* status); +bool isAnyPulsing(int32_t* status); - void setFilterSelect(void* digital_port_pointer, int filter_index, +void setFilterSelect(void* digital_port_pointer, int filter_index, + int32_t* status); +int getFilterSelect(void* digital_port_pointer, int32_t* status); + +void setFilterPeriod(int filter_index, uint32_t value, int32_t* status); +uint32_t getFilterPeriod(int filter_index, int32_t* status); + +void* initializeCounter(Mode mode, uint32_t* index, int32_t* status); +void freeCounter(void* counter_pointer, int32_t* status); +void setCounterAverageSize(void* counter_pointer, int32_t size, + int32_t* status); +void setCounterUpSource(void* counter_pointer, uint32_t pin, bool analogTrigger, + int32_t* status); +void setCounterUpSourceEdge(void* counter_pointer, bool risingEdge, + bool fallingEdge, int32_t* status); +void clearCounterUpSource(void* counter_pointer, int32_t* status); +void setCounterDownSource(void* counter_pointer, uint32_t pin, + bool analogTrigger, int32_t* status); +void setCounterDownSourceEdge(void* counter_pointer, bool risingEdge, + bool fallingEdge, int32_t* status); +void clearCounterDownSource(void* counter_pointer, int32_t* status); +void setCounterUpDownMode(void* counter_pointer, int32_t* status); +void setCounterExternalDirectionMode(void* counter_pointer, int32_t* status); +void setCounterSemiPeriodMode(void* counter_pointer, bool highSemiPeriod, + int32_t* status); +void setCounterPulseLengthMode(void* counter_pointer, double threshold, + int32_t* status); +int32_t getCounterSamplesToAverage(void* counter_pointer, int32_t* status); +void setCounterSamplesToAverage(void* counter_pointer, int samplesToAverage, + int32_t* status); +void resetCounter(void* counter_pointer, int32_t* status); +int32_t getCounter(void* counter_pointer, int32_t* status); +double getCounterPeriod(void* counter_pointer, int32_t* status); +void setCounterMaxPeriod(void* counter_pointer, double maxPeriod, int32_t* status); - int getFilterSelect(void* digital_port_pointer, int32_t* status); +void setCounterUpdateWhenEmpty(void* counter_pointer, bool enabled, + int32_t* status); +bool getCounterStopped(void* counter_pointer, int32_t* status); +bool getCounterDirection(void* counter_pointer, int32_t* status); +void setCounterReverseDirection(void* counter_pointer, bool reverseDirection, + int32_t* status); - void setFilterPeriod(int filter_index, uint32_t value, int32_t* status); - uint32_t getFilterPeriod(int filter_index, int32_t* status); +void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, + bool port_a_analog_trigger, uint8_t port_b_module, + uint32_t port_b_pin, bool port_b_analog_trigger, + bool reverseDirection, int32_t* index, + int32_t* status); // TODO: fix routing +void freeEncoder(void* encoder_pointer, int32_t* status); +void resetEncoder(void* encoder_pointer, int32_t* status); +int32_t getEncoder(void* encoder_pointer, int32_t* status); // Raw value +double getEncoderPeriod(void* encoder_pointer, int32_t* status); +void setEncoderMaxPeriod(void* encoder_pointer, double maxPeriod, + int32_t* status); +bool getEncoderStopped(void* encoder_pointer, int32_t* status); +bool getEncoderDirection(void* encoder_pointer, int32_t* status); +void setEncoderReverseDirection(void* encoder_pointer, bool reverseDirection, + int32_t* status); +void setEncoderSamplesToAverage(void* encoder_pointer, + uint32_t samplesToAverage, int32_t* status); +uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t* status); +void setEncoderIndexSource(void* encoder_pointer, uint32_t pin, + bool analogTrigger, bool activeHigh, + bool edgeSensitive, int32_t* status); - void* initializeCounter(Mode mode, uint32_t *index, int32_t *status); - void freeCounter(void* counter_pointer, int32_t *status); - void setCounterAverageSize(void* counter_pointer, int32_t size, int32_t *status); - void setCounterUpSource(void* counter_pointer, uint32_t pin, bool analogTrigger, int32_t *status); - void setCounterUpSourceEdge(void* counter_pointer, bool risingEdge, bool fallingEdge, - int32_t *status); - void clearCounterUpSource(void* counter_pointer, int32_t *status); - void setCounterDownSource(void* counter_pointer, uint32_t pin, bool analogTrigger, int32_t *status); - void setCounterDownSourceEdge(void* counter_pointer, bool risingEdge, bool fallingEdge, - int32_t *status); - void clearCounterDownSource(void* counter_pointer, int32_t *status); - void setCounterUpDownMode(void* counter_pointer, int32_t *status); - void setCounterExternalDirectionMode(void* counter_pointer, int32_t *status); - void setCounterSemiPeriodMode(void* counter_pointer, bool highSemiPeriod, int32_t *status); - void setCounterPulseLengthMode(void* counter_pointer, double threshold, int32_t *status); - int32_t getCounterSamplesToAverage(void* counter_pointer, int32_t *status); - void setCounterSamplesToAverage(void* counter_pointer, int samplesToAverage, int32_t *status); - void resetCounter(void* counter_pointer, int32_t *status); - int32_t getCounter(void* counter_pointer, int32_t *status); - double getCounterPeriod(void* counter_pointer, int32_t *status); - void setCounterMaxPeriod(void* counter_pointer, double maxPeriod, int32_t *status); - void setCounterUpdateWhenEmpty(void* counter_pointer, bool enabled, int32_t *status); - bool getCounterStopped(void* counter_pointer, int32_t *status); - bool getCounterDirection(void* counter_pointer, int32_t *status); - void setCounterReverseDirection(void* counter_pointer, bool reverseDirection, int32_t *status); +uint16_t getLoopTiming(int32_t* status); - void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, bool port_a_analog_trigger, - uint8_t port_b_module, uint32_t port_b_pin, bool port_b_analog_trigger, - bool reverseDirection, int32_t *index, int32_t *status); // TODO: fix routing - void freeEncoder(void* encoder_pointer, int32_t *status); - void resetEncoder(void* encoder_pointer, int32_t *status); - int32_t getEncoder(void* encoder_pointer, int32_t *status); // Raw value - double getEncoderPeriod(void* encoder_pointer, int32_t *status); - void setEncoderMaxPeriod(void* encoder_pointer, double maxPeriod, int32_t *status); - bool getEncoderStopped(void* encoder_pointer, int32_t *status); - bool getEncoderDirection(void* encoder_pointer, int32_t *status); - void setEncoderReverseDirection(void* encoder_pointer, bool reverseDirection, int32_t *status); - void setEncoderSamplesToAverage(void* encoder_pointer, uint32_t samplesToAverage, - int32_t *status); - uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t *status); - void setEncoderIndexSource(void *encoder_pointer, uint32_t pin, bool analogTrigger, bool activeHigh, - bool edgeSensitive, int32_t *status); +void spiInitialize(uint8_t port, int32_t* status); +int32_t spiTransaction(uint8_t port, uint8_t* dataToSend, uint8_t* dataReceived, + uint8_t size); +int32_t spiWrite(uint8_t port, uint8_t* dataToSend, uint8_t sendSize); +int32_t spiRead(uint8_t port, uint8_t* buffer, uint8_t count); +void spiClose(uint8_t port); +void spiSetSpeed(uint8_t port, uint32_t speed); +void spiSetOpts(uint8_t port, int msb_first, int sample_on_trailing, + int clk_idle_high); +void spiSetChipSelectActiveHigh(uint8_t port, int32_t* status); +void spiSetChipSelectActiveLow(uint8_t port, int32_t* status); +int32_t spiGetHandle(uint8_t port); +void spiSetHandle(uint8_t port, int32_t handle); - uint16_t getLoopTiming(int32_t *status); +void spiInitAccumulator(uint8_t port, uint32_t period, uint32_t cmd, + uint8_t xfer_size, uint32_t valid_mask, + uint32_t valid_value, uint8_t data_shift, + uint8_t data_size, bool is_signed, bool big_endian, + int32_t* status); +void spiFreeAccumulator(uint8_t port, int32_t* status); +void spiResetAccumulator(uint8_t port, int32_t* status); +void spiSetAccumulatorCenter(uint8_t port, int32_t center, int32_t* status); +void spiSetAccumulatorDeadband(uint8_t port, int32_t deadband, int32_t* status); +int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t* status); +int64_t spiGetAccumulatorValue(uint8_t port, int32_t* status); +uint32_t spiGetAccumulatorCount(uint8_t port, int32_t* status); +double spiGetAccumulatorAverage(uint8_t port, int32_t* status); +void spiGetAccumulatorOutput(uint8_t port, int64_t* value, uint32_t* count, + int32_t* status); - void spiInitialize(uint8_t port, int32_t *status); - int32_t spiTransaction(uint8_t port, uint8_t *dataToSend, uint8_t *dataReceived, uint8_t size); - int32_t spiWrite(uint8_t port, uint8_t* dataToSend, uint8_t sendSize); - int32_t spiRead(uint8_t port, uint8_t *buffer, uint8_t count); - void spiClose(uint8_t port); - void spiSetSpeed(uint8_t port, uint32_t speed); - void spiSetOpts(uint8_t port, int msb_first, int sample_on_trailing, int clk_idle_high); - void spiSetChipSelectActiveHigh(uint8_t port, int32_t *status); - void spiSetChipSelectActiveLow(uint8_t port, int32_t *status); - int32_t spiGetHandle(uint8_t port); - void spiSetHandle(uint8_t port, int32_t handle); +void i2CInitialize(uint8_t port, int32_t* status); +int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, + uint8_t sendSize, uint8_t* dataReceived, + uint8_t receiveSize); +int32_t i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, + uint8_t sendSize); +int32_t i2CRead(uint8_t port, uint8_t deviceAddress, uint8_t* buffer, + uint8_t count); +void i2CClose(uint8_t port); - void spiInitAccumulator(uint8_t port, uint32_t period, uint32_t cmd, - uint8_t xfer_size, uint32_t valid_mask, - uint32_t valid_value, uint8_t data_shift, - uint8_t data_size, bool is_signed, bool big_endian, - int32_t *status); - void spiFreeAccumulator(uint8_t port, int32_t *status); - void spiResetAccumulator(uint8_t port, int32_t *status); - void spiSetAccumulatorCenter(uint8_t port, int32_t center, int32_t *status); - void spiSetAccumulatorDeadband(uint8_t port, int32_t deadband, int32_t *status); - int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t *status); - int64_t spiGetAccumulatorValue(uint8_t port, int32_t *status); - uint32_t spiGetAccumulatorCount(uint8_t port, int32_t *status); - double spiGetAccumulatorAverage(uint8_t port, int32_t *status); - void spiGetAccumulatorOutput(uint8_t port, int64_t *value, uint32_t *count, - int32_t *status); - - void i2CInitialize(uint8_t port, int32_t *status); - int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend, uint8_t sendSize, uint8_t *dataReceived, uint8_t receiveSize); - int32_t i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend, uint8_t sendSize); - int32_t i2CRead(uint8_t port, uint8_t deviceAddress, uint8_t *buffer, uint8_t count); - void i2CClose(uint8_t port); - - //// Float JNA Hack - // double - void setPWMRateIntHack(int rate, int32_t *status); - void setPWMDutyCycleIntHack(void* pwmGenerator, int32_t dutyCycle, int32_t *status); +//// Float JNA Hack +// double +void setPWMRateIntHack(int rate, int32_t* status); +void setPWMDutyCycleIntHack(void* pwmGenerator, int32_t dutyCycle, + int32_t* status); } diff --git a/hal/include/HAL/Errors.hpp b/hal/include/HAL/Errors.hpp index 35ed9619af..c29ced6254 100644 --- a/hal/include/HAL/Errors.hpp +++ b/hal/include/HAL/Errors.hpp @@ -3,17 +3,20 @@ #define CTR_RxTimeout_MESSAGE "CTRE CAN Recieve Timeout" #define CTR_TxTimeout_MESSAGE "CTRE CAN Transmit Timeout" #define CTR_InvalidParamValue_MESSAGE "CTRE CAN Invalid Parameter" -#define CTR_UnexpectedArbId_MESSAGE "CTRE Unexpected Arbitration ID (CAN Node ID)" +#define CTR_UnexpectedArbId_MESSAGE \ + "CTRE Unexpected Arbitration ID (CAN Node ID)" #define CTR_TxFailed_MESSAGE "CTRE CAN Transmit Error" #define CTR_SigNotUpdated_MESSAGE "CTRE CAN Signal Not Updated" #define NiFpga_Status_FifoTimeout_MESSAGE "NIFPGA: FIFO timeout error" #define NiFpga_Status_TransferAborted_MESSAGE "NIFPGA: Transfer aborted error" -#define NiFpga_Status_MemoryFull_MESSAGE "NIFPGA: Memory Allocation failed, memory full" +#define NiFpga_Status_MemoryFull_MESSAGE \ + "NIFPGA: Memory Allocation failed, memory full" #define NiFpga_Status_SoftwareFault_MESSAGE "NIFPGA: Unexepected software error" #define NiFpga_Status_InvalidParameter_MESSAGE "NIFPGA: Invalid Parameter" #define NiFpga_Status_ResourceNotFound_MESSAGE "NIFPGA: Resource not found" -#define NiFpga_Status_ResourceNotInitialized_MESSAGE "NIFPGA: Resource not initialized" +#define NiFpga_Status_ResourceNotInitialized_MESSAGE \ + "NIFPGA: Resource not initialized" #define NiFpga_Status_HardwareFault_MESSAGE "NIFPGA: Hardware Fault" #define NiFpga_Status_IrqTimeout_MESSAGE "NIFPGA: Interrupt timeout" @@ -21,30 +24,38 @@ #define ERR_CANSessionMux_MessageNotFound_MESSAGE "CAN: Message not found" #define WARN_CANSessionMux_NoToken_MESSAGE "CAN: No token" #define ERR_CANSessionMux_NotAllowed_MESSAGE "CAN: Not allowed" -#define ERR_CANSessionMux_NotInitialized_MESSAGE "CAN: Not initialized" +#define ERR_CANSessionMux_NotInitialized_MESSAGE "CAN: Not initialized" #define SAMPLE_RATE_TOO_HIGH 1001 -#define SAMPLE_RATE_TOO_HIGH_MESSAGE "HAL: Analog module sample rate is too high" +#define SAMPLE_RATE_TOO_HIGH_MESSAGE \ + "HAL: Analog module sample rate is too high" #define VOLTAGE_OUT_OF_RANGE 1002 -#define VOLTAGE_OUT_OF_RANGE_MESSAGE "HAL: Voltage to convert to raw value is out of range [0; 5]" +#define VOLTAGE_OUT_OF_RANGE_MESSAGE \ + "HAL: Voltage to convert to raw value is out of range [0; 5]" #define LOOP_TIMING_ERROR 1004 -#define LOOP_TIMING_ERROR_MESSAGE "HAL: Digital module loop timing is not the expected value" +#define LOOP_TIMING_ERROR_MESSAGE \ + "HAL: Digital module loop timing is not the expected value" #define SPI_WRITE_NO_MOSI 1012 -#define SPI_WRITE_NO_MOSI_MESSAGE "HAL: Cannot write to SPI port with no MOSI output" +#define SPI_WRITE_NO_MOSI_MESSAGE \ + "HAL: Cannot write to SPI port with no MOSI output" #define SPI_READ_NO_MISO 1013 -#define SPI_READ_NO_MISO_MESSAGE "HAL: Cannot read from SPI port with no MISO input" +#define SPI_READ_NO_MISO_MESSAGE \ + "HAL: Cannot read from SPI port with no MISO input" #define SPI_READ_NO_DATA 1014 #define SPI_READ_NO_DATA_MESSAGE "HAL: No data available to read from SPI" #define INCOMPATIBLE_STATE 1015 -#define INCOMPATIBLE_STATE_MESSAGE "HAL: Incompatible State: The operation cannot be completed" +#define INCOMPATIBLE_STATE_MESSAGE \ + "HAL: Incompatible State: The operation cannot be completed" #define NO_AVAILABLE_RESOURCES -1004 #define NO_AVAILABLE_RESOURCES_MESSAGE "HAL: No available resources to allocate" #define NULL_PARAMETER -1005 #define NULL_PARAMETER_MESSAGE "HAL: A pointer parameter to a method is NULL" #define ANALOG_TRIGGER_LIMIT_ORDER_ERROR -1010 -#define ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE "HAL: AnalogTrigger limits error. Lower limit > Upper Limit" +#define ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE \ + "HAL: AnalogTrigger limits error. Lower limit > Upper Limit" #define ANALOG_TRIGGER_PULSE_OUTPUT_ERROR -1011 -#define ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE "HAL: Attempted to read AnalogTrigger pulse output." +#define ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE \ + "HAL: Attempted to read AnalogTrigger pulse output." #define PARAMETER_OUT_OF_RANGE -1028 #define PARAMETER_OUT_OF_RANGE_MESSAGE "HAL: A parameter is out of range." #define RESOURCE_IS_ALLOCATED -1029 @@ -54,11 +65,11 @@ #define VI_ERROR_INV_OBJECT_MESSAGE "HAL - VISA: Invalid Object" #define VI_ERROR_RSRC_LOCKED_MESSAGE "HAL - VISA: Resource Locked" #define VI_ERROR_RSRC_NFOUND_MESSAGE "HAL - VISA: Resource Not Found" -#define VI_ERROR_INV_RSRC_NAME_MESSAGE "HAL - VISA: Invalid Resource Name" -#define VI_ERROR_QUEUE_OVERFLOW_MESSAGE "HAL - VISA: Queue Overflow" +#define VI_ERROR_INV_RSRC_NAME_MESSAGE "HAL - VISA: Invalid Resource Name" +#define VI_ERROR_QUEUE_OVERFLOW_MESSAGE "HAL - VISA: Queue Overflow" #define VI_ERROR_IO_MESSAGE "HAL - VISA: General IO Error" #define VI_ERROR_ASRL_PARITY_MESSAGE "HAL - VISA: Parity Error" #define VI_ERROR_ASRL_FRAMING_MESSAGE "HAL - VISA: Framing Error" -#define VI_ERROR_ASRL_OVERRUN_MESSAGE "HAL - VISA: Buffer Overrun Error" +#define VI_ERROR_ASRL_OVERRUN_MESSAGE "HAL - VISA: Buffer Overrun Error" #define VI_ERROR_RSRC_BUSY_MESSAGE "HAL - VISA: Resource Busy" -#define VI_ERROR_INV_PARAMETER_MESSAGE "HAL - VISA: Invalid Parameter" +#define VI_ERROR_INV_PARAMETER_MESSAGE "HAL - VISA: Invalid Parameter" diff --git a/hal/include/HAL/HAL.hpp b/hal/include/HAL/HAL.hpp index fb3c6aa325..8d5e537ae0 100644 --- a/hal/include/HAL/HAL.hpp +++ b/hal/include/HAL/HAL.hpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2013. All Rights Reserved. */ +/* Copyright (c) FIRST 2013. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */ /*----------------------------------------------------------------------------*/ @@ -12,21 +12,22 @@ #include "Analog.hpp" #include "Compressor.hpp" #include "Digital.hpp" -#include "Solenoid.hpp" -#include "Notifier.hpp" -#include "Interrupts.hpp" #include "Errors.hpp" +#include "Interrupts.hpp" +#include "Notifier.hpp" #include "PDP.hpp" #include "Power.hpp" #include "SerialPort.hpp" +#include "Solenoid.hpp" -#include "Utilities.hpp" #include "Semaphore.hpp" #include "Task.hpp" +#include "Utilities.hpp" #define HAL_IO_CONFIG_DATA_SIZE 32 #define HAL_SYS_STATUS_DATA_SIZE 44 -#define HAL_USER_STATUS_DATA_SIZE (984 - HAL_IO_CONFIG_DATA_SIZE - HAL_SYS_STATUS_DATA_SIZE) +#define HAL_USER_STATUS_DATA_SIZE \ + (984 - HAL_IO_CONFIG_DATA_SIZE - HAL_SYS_STATUS_DATA_SIZE) #define HALFRC_NetworkCommunication_DynamicType_DSEnhancedIO_Input 17 #define HALFRC_NetworkCommunication_DynamicType_DSEnhancedIO_Output 18 @@ -38,234 +39,225 @@ #define HALFRC_NetworkCommunication_DynamicType_Kinect_Joystick 24 #define HALFRC_NetworkCommunication_DynamicType_Kinect_Custom 25 -namespace HALUsageReporting -{ - enum tResourceType - { - kResourceType_Controller, - kResourceType_Module, - kResourceType_Language, - kResourceType_CANPlugin, - kResourceType_Accelerometer, - kResourceType_ADXL345, - kResourceType_AnalogChannel, - kResourceType_AnalogTrigger, - kResourceType_AnalogTriggerOutput, - kResourceType_CANJaguar, - kResourceType_Compressor, - kResourceType_Counter, - kResourceType_Dashboard, - kResourceType_DigitalInput, - kResourceType_DigitalOutput, - kResourceType_DriverStationCIO, - kResourceType_DriverStationEIO, - kResourceType_DriverStationLCD, - kResourceType_Encoder, - kResourceType_GearTooth, - kResourceType_Gyro, - kResourceType_I2C, - kResourceType_Framework, - kResourceType_Jaguar, - kResourceType_Joystick, - kResourceType_Kinect, - kResourceType_KinectStick, - kResourceType_PIDController, - kResourceType_Preferences, - kResourceType_PWM, - kResourceType_Relay, - kResourceType_RobotDrive, - kResourceType_SerialPort, - kResourceType_Servo, - kResourceType_Solenoid, - kResourceType_SPI, - kResourceType_Task, - kResourceType_Ultrasonic, - kResourceType_Victor, - kResourceType_Button, - kResourceType_Command, - kResourceType_AxisCamera, - kResourceType_PCVideoServer, - kResourceType_SmartDashboard, - kResourceType_Talon, - kResourceType_HiTechnicColorSensor, - kResourceType_HiTechnicAccel, - kResourceType_HiTechnicCompass, - kResourceType_SRF08, - kResourceType_AnalogOutput, - kResourceType_VictorSP, - kResourceType_TalonSRX, - kResourceType_CANTalonSRX, - kResourceType_ADXL362, - kResourceType_ADXRS450, - kResourceType_RevSPARK, - kResourceType_MindsensorsSD540, - kResourceType_DigitalFilter, - }; +namespace HALUsageReporting { +enum tResourceType { + kResourceType_Controller, + kResourceType_Module, + kResourceType_Language, + kResourceType_CANPlugin, + kResourceType_Accelerometer, + kResourceType_ADXL345, + kResourceType_AnalogChannel, + kResourceType_AnalogTrigger, + kResourceType_AnalogTriggerOutput, + kResourceType_CANJaguar, + kResourceType_Compressor, + kResourceType_Counter, + kResourceType_Dashboard, + kResourceType_DigitalInput, + kResourceType_DigitalOutput, + kResourceType_DriverStationCIO, + kResourceType_DriverStationEIO, + kResourceType_DriverStationLCD, + kResourceType_Encoder, + kResourceType_GearTooth, + kResourceType_Gyro, + kResourceType_I2C, + kResourceType_Framework, + kResourceType_Jaguar, + kResourceType_Joystick, + kResourceType_Kinect, + kResourceType_KinectStick, + kResourceType_PIDController, + kResourceType_Preferences, + kResourceType_PWM, + kResourceType_Relay, + kResourceType_RobotDrive, + kResourceType_SerialPort, + kResourceType_Servo, + kResourceType_Solenoid, + kResourceType_SPI, + kResourceType_Task, + kResourceType_Ultrasonic, + kResourceType_Victor, + kResourceType_Button, + kResourceType_Command, + kResourceType_AxisCamera, + kResourceType_PCVideoServer, + kResourceType_SmartDashboard, + kResourceType_Talon, + kResourceType_HiTechnicColorSensor, + kResourceType_HiTechnicAccel, + kResourceType_HiTechnicCompass, + kResourceType_SRF08, + kResourceType_AnalogOutput, + kResourceType_VictorSP, + kResourceType_TalonSRX, + kResourceType_CANTalonSRX, + kResourceType_ADXL362, + kResourceType_ADXRS450, + kResourceType_RevSPARK, + kResourceType_MindsensorsSD540, + kResourceType_DigitalFilter, +}; - enum tInstances - { - kLanguage_LabVIEW = 1, - kLanguage_CPlusPlus = 2, - kLanguage_Java = 3, - kLanguage_Python = 4, +enum tInstances { + kLanguage_LabVIEW = 1, + kLanguage_CPlusPlus = 2, + kLanguage_Java = 3, + kLanguage_Python = 4, - kCANPlugin_BlackJagBridge = 1, - kCANPlugin_2CAN = 2, + kCANPlugin_BlackJagBridge = 1, + kCANPlugin_2CAN = 2, - kFramework_Iterative = 1, - kFramework_Sample = 2, - kFramework_CommandControl = 3, + kFramework_Iterative = 1, + kFramework_Sample = 2, + kFramework_CommandControl = 3, - kRobotDrive_ArcadeStandard = 1, - kRobotDrive_ArcadeButtonSpin = 2, - kRobotDrive_ArcadeRatioCurve = 3, - kRobotDrive_Tank = 4, - kRobotDrive_MecanumPolar = 5, - kRobotDrive_MecanumCartesian = 6, + kRobotDrive_ArcadeStandard = 1, + kRobotDrive_ArcadeButtonSpin = 2, + kRobotDrive_ArcadeRatioCurve = 3, + kRobotDrive_Tank = 4, + kRobotDrive_MecanumPolar = 5, + kRobotDrive_MecanumCartesian = 6, - kDriverStationCIO_Analog = 1, - kDriverStationCIO_DigitalIn = 2, - kDriverStationCIO_DigitalOut = 3, + kDriverStationCIO_Analog = 1, + kDriverStationCIO_DigitalIn = 2, + kDriverStationCIO_DigitalOut = 3, - kDriverStationEIO_Acceleration = 1, - kDriverStationEIO_AnalogIn = 2, - kDriverStationEIO_AnalogOut = 3, - kDriverStationEIO_Button = 4, - kDriverStationEIO_LED = 5, - kDriverStationEIO_DigitalIn = 6, - kDriverStationEIO_DigitalOut = 7, - kDriverStationEIO_FixedDigitalOut = 8, - kDriverStationEIO_PWM = 9, - kDriverStationEIO_Encoder = 10, - kDriverStationEIO_TouchSlider = 11, + kDriverStationEIO_Acceleration = 1, + kDriverStationEIO_AnalogIn = 2, + kDriverStationEIO_AnalogOut = 3, + kDriverStationEIO_Button = 4, + kDriverStationEIO_LED = 5, + kDriverStationEIO_DigitalIn = 6, + kDriverStationEIO_DigitalOut = 7, + kDriverStationEIO_FixedDigitalOut = 8, + kDriverStationEIO_PWM = 9, + kDriverStationEIO_Encoder = 10, + kDriverStationEIO_TouchSlider = 11, - kADXL345_SPI = 1, - kADXL345_I2C = 2, + kADXL345_SPI = 1, + kADXL345_I2C = 2, - kCommand_Scheduler = 1, + kCommand_Scheduler = 1, - kSmartDashboard_Instance = 1, - }; + kSmartDashboard_Instance = 1, +}; } struct HALControlWord { - uint32_t enabled : 1; - uint32_t autonomous : 1; - uint32_t test :1; - uint32_t eStop : 1; - uint32_t fmsAttached:1; - uint32_t dsAttached:1; - uint32_t control_reserved : 26; + uint32_t enabled : 1; + uint32_t autonomous : 1; + uint32_t test : 1; + uint32_t eStop : 1; + uint32_t fmsAttached : 1; + uint32_t dsAttached : 1; + uint32_t control_reserved : 26; }; enum HALAllianceStationID { - kHALAllianceStationID_red1, - kHALAllianceStationID_red2, - kHALAllianceStationID_red3, - kHALAllianceStationID_blue1, - kHALAllianceStationID_blue2, - kHALAllianceStationID_blue3, + kHALAllianceStationID_red1, + kHALAllianceStationID_red2, + kHALAllianceStationID_red3, + kHALAllianceStationID_blue1, + kHALAllianceStationID_blue2, + kHALAllianceStationID_blue3, }; /* The maximum number of axes that will be stored in a single HALJoystickAxes - struct. This is used for allocating buffers, not bounds checking, since - there are usually less axes in practice. */ + * struct. This is used for allocating buffers, not bounds checking, since + * there are usually less axes in practice. + */ static const size_t kMaxJoystickAxes = 12; static const size_t kMaxJoystickPOVs = 12; struct HALJoystickAxes { - uint16_t count; - int16_t axes[kMaxJoystickAxes]; + uint16_t count; + int16_t axes[kMaxJoystickAxes]; }; struct HALJoystickPOVs { - uint16_t count; - int16_t povs[kMaxJoystickPOVs]; + uint16_t count; + int16_t povs[kMaxJoystickPOVs]; }; struct HALJoystickButtons { - uint32_t buttons; - uint8_t count; + uint32_t buttons; + uint8_t count; }; struct HALJoystickDescriptor { - uint8_t isXbox; - uint8_t type; - char name[256]; - uint8_t axisCount; - uint8_t axisTypes[kMaxJoystickAxes]; - uint8_t buttonCount; - uint8_t povCount; + uint8_t isXbox; + uint8_t type; + char name[256]; + uint8_t axisCount; + uint8_t axisTypes[kMaxJoystickAxes]; + uint8_t buttonCount; + uint8_t povCount; }; -inline float intToFloat(int value) -{ - return (float)value; -} +inline float intToFloat(int value) { return (float)value; } -inline int floatToInt(float value) -{ - return round(value); -} +inline int floatToInt(float value) { return round(value); } -extern "C" -{ - extern const uint32_t dio_kNumSystems; - extern const uint32_t solenoid_kNumDO7_0Elements; - extern const uint32_t interrupt_kNumSystems; - extern const uint32_t kSystemClockTicksPerMicrosecond; +extern "C" { +extern const uint32_t dio_kNumSystems; +extern const uint32_t solenoid_kNumDO7_0Elements; +extern const uint32_t interrupt_kNumSystems; +extern const uint32_t kSystemClockTicksPerMicrosecond; - void* getPort(uint8_t pin); - void* getPortWithModule(uint8_t module, uint8_t pin); - void freePort(void* port); - const char* getHALErrorMessage(int32_t code); +void* getPort(uint8_t pin); +void* getPortWithModule(uint8_t module, uint8_t pin); +void freePort(void* port); +const char* getHALErrorMessage(int32_t code); - uint16_t getFPGAVersion(int32_t *status); - uint32_t getFPGARevision(int32_t *status); - uint64_t getFPGATime(int32_t *status); +uint16_t getFPGAVersion(int32_t* status); +uint32_t getFPGARevision(int32_t* status); +uint64_t getFPGATime(int32_t* status); - bool getFPGAButton(int32_t *status); +bool getFPGAButton(int32_t* status); - int HALSetErrorData(const char *errors, int errorsLength, int wait_ms); - int HALSendError(int isError, int32_t errorCode, int isLVCode, - const char *details, const char *location, const char *callStack, - int printMsg); +int HALSetErrorData(const char* errors, int errorsLength, int wait_ms); +int HALSendError(int isError, int32_t errorCode, int isLVCode, + const char* details, const char* location, + const char* callStack, int printMsg); - int HALGetControlWord(HALControlWord *data); - int HALGetAllianceStation(enum HALAllianceStationID *allianceStation); - int HALGetJoystickAxes(uint8_t joystickNum, HALJoystickAxes *axes); - int HALGetJoystickPOVs(uint8_t joystickNum, HALJoystickPOVs *povs); - int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons *buttons); - int HALGetJoystickDescriptor(uint8_t joystickNum, HALJoystickDescriptor *desc); - int HALGetJoystickIsXbox(uint8_t joystickNum); - int HALGetJoystickType(uint8_t joystickNum); - char* HALGetJoystickName(uint8_t joystickNum); - int HALGetJoystickAxisType(uint8_t joystickNum, uint8_t axis); - int HALSetJoystickOutputs(uint8_t joystickNum, uint32_t outputs, uint16_t leftRumble, uint16_t rightRumble); - int HALGetMatchTime(float *matchTime); +int HALGetControlWord(HALControlWord* data); +int HALGetAllianceStation(enum HALAllianceStationID* allianceStation); +int HALGetJoystickAxes(uint8_t joystickNum, HALJoystickAxes* axes); +int HALGetJoystickPOVs(uint8_t joystickNum, HALJoystickPOVs* povs); +int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons* buttons); +int HALGetJoystickDescriptor(uint8_t joystickNum, HALJoystickDescriptor* desc); +int HALGetJoystickIsXbox(uint8_t joystickNum); +int HALGetJoystickType(uint8_t joystickNum); +char* HALGetJoystickName(uint8_t joystickNum); +int HALGetJoystickAxisType(uint8_t joystickNum, uint8_t axis); +int HALSetJoystickOutputs(uint8_t joystickNum, uint32_t outputs, + uint16_t leftRumble, uint16_t rightRumble); +int HALGetMatchTime(float* matchTime); - void HALSetNewDataSem(MULTIWAIT_ID sem); +void HALSetNewDataSem(MULTIWAIT_ID sem); - bool HALGetSystemActive(int32_t *status); - bool HALGetBrownedOut(int32_t *status); +bool HALGetSystemActive(int32_t* status); +bool HALGetBrownedOut(int32_t* status); - int HALInitialize(int mode = 0); - void HALNetworkCommunicationObserveUserProgramStarting(); - void HALNetworkCommunicationObserveUserProgramDisabled(); - void HALNetworkCommunicationObserveUserProgramAutonomous(); - void HALNetworkCommunicationObserveUserProgramTeleop(); - void HALNetworkCommunicationObserveUserProgramTest(); +int HALInitialize(int mode = 0); +void HALNetworkCommunicationObserveUserProgramStarting(); +void HALNetworkCommunicationObserveUserProgramDisabled(); +void HALNetworkCommunicationObserveUserProgramAutonomous(); +void HALNetworkCommunicationObserveUserProgramTeleop(); +void HALNetworkCommunicationObserveUserProgramTest(); - uint32_t HALReport(uint8_t resource, uint8_t instanceNumber, uint8_t context = 0, - const char *feature = NULL); +uint32_t HALReport(uint8_t resource, uint8_t instanceNumber, + uint8_t context = 0, const char* feature = NULL); } // TODO: HACKS for now... -extern "C" -{ +extern "C" { - void NumericArrayResize(); - void RTSetCleanupProc(); - void EDVR_CreateReference(); - void Occur(); +void NumericArrayResize(); +void RTSetCleanupProc(); +void EDVR_CreateReference(); +void Occur(); } diff --git a/hal/include/HAL/Interrupts.hpp b/hal/include/HAL/Interrupts.hpp index a41ca6be0e..f00415f0b3 100644 --- a/hal/include/HAL/Interrupts.hpp +++ b/hal/include/HAL/Interrupts.hpp @@ -5,22 +5,26 @@ #include #include "errno.h" -extern "C" -{ - typedef void (*InterruptHandlerFunction)(uint32_t interruptAssertedMask, void *param); +extern "C" { +typedef void (*InterruptHandlerFunction)(uint32_t interruptAssertedMask, + void* param); - void* initializeInterrupts(uint32_t interruptIndex, bool watcher, int32_t *status); - void cleanInterrupts(void* interrupt_pointer, int32_t *status); +void* initializeInterrupts(uint32_t interruptIndex, bool watcher, + int32_t* status); +void cleanInterrupts(void* interrupt_pointer, int32_t* status); - uint32_t waitForInterrupt(void* interrupt_pointer, double timeout, bool ignorePrevious, int32_t *status); - void enableInterrupts(void* interrupt_pointer, int32_t *status); - void disableInterrupts(void* interrupt_pointer, int32_t *status); - double readRisingTimestamp(void* interrupt_pointer, int32_t *status); - double readFallingTimestamp(void* interrupt_pointer, int32_t *status); - void requestInterrupts(void* interrupt_pointer, uint8_t routing_module, uint32_t routing_pin, - bool routing_analog_trigger, int32_t *status); - void attachInterruptHandler(void* interrupt_pointer, InterruptHandlerFunction handler, - void* param, int32_t *status); - void setInterruptUpSourceEdge(void* interrupt_pointer, bool risingEdge, bool fallingEdge, - int32_t *status); +uint32_t waitForInterrupt(void* interrupt_pointer, double timeout, + bool ignorePrevious, int32_t* status); +void enableInterrupts(void* interrupt_pointer, int32_t* status); +void disableInterrupts(void* interrupt_pointer, int32_t* status); +double readRisingTimestamp(void* interrupt_pointer, int32_t* status); +double readFallingTimestamp(void* interrupt_pointer, int32_t* status); +void requestInterrupts(void* interrupt_pointer, uint8_t routing_module, + uint32_t routing_pin, bool routing_analog_trigger, + int32_t* status); +void attachInterruptHandler(void* interrupt_pointer, + InterruptHandlerFunction handler, void* param, + int32_t* status); +void setInterruptUpSourceEdge(void* interrupt_pointer, bool risingEdge, + bool fallingEdge, int32_t* status); } diff --git a/hal/include/HAL/Notifier.hpp b/hal/include/HAL/Notifier.hpp index e8cb3812cc..b56545e6c8 100644 --- a/hal/include/HAL/Notifier.hpp +++ b/hal/include/HAL/Notifier.hpp @@ -2,11 +2,12 @@ #include -extern "C" -{ - void* initializeNotifier(void (*process)(uint64_t, void*), void* param, int32_t *status); - void cleanNotifier(void* notifier_pointer, int32_t *status); - void* getNotifierParam(void* notifier_pointer, int32_t *status); - void updateNotifierAlarm(void* notifier_pointer, uint64_t triggerTime, int32_t *status); - void stopNotifierAlarm(void* notifier_pointer, int32_t *status); +extern "C" { +void* initializeNotifier(void (*process)(uint64_t, void*), void* param, + int32_t* status); +void cleanNotifier(void* notifier_pointer, int32_t* status); +void* getNotifierParam(void* notifier_pointer, int32_t* status); +void updateNotifierAlarm(void* notifier_pointer, uint64_t triggerTime, + int32_t* status); +void stopNotifierAlarm(void* notifier_pointer, int32_t* status); } diff --git a/hal/include/HAL/PDP.hpp b/hal/include/HAL/PDP.hpp index 54ab6fa396..1dd78edcb8 100644 --- a/hal/include/HAL/PDP.hpp +++ b/hal/include/HAL/PDP.hpp @@ -2,15 +2,14 @@ #include -extern "C" -{ - void initializePDP(uint8_t module); - double getPDPTemperature(uint8_t module, int32_t *status); - double getPDPVoltage(uint8_t module, int32_t *status); - double getPDPChannelCurrent(uint8_t module, uint8_t channel, int32_t *status); - double getPDPTotalCurrent(uint8_t module, int32_t *status); - double getPDPTotalPower(uint8_t module, int32_t *status); - double getPDPTotalEnergy(uint8_t module, int32_t *status); - void resetPDPTotalEnergy(uint8_t module, int32_t *status); - void clearPDPStickyFaults(uint8_t module, int32_t *status); +extern "C" { +void initializePDP(uint8_t module); +double getPDPTemperature(uint8_t module, int32_t* status); +double getPDPVoltage(uint8_t module, int32_t* status); +double getPDPChannelCurrent(uint8_t module, uint8_t channel, int32_t* status); +double getPDPTotalCurrent(uint8_t module, int32_t* status); +double getPDPTotalPower(uint8_t module, int32_t* status); +double getPDPTotalEnergy(uint8_t module, int32_t* status); +void resetPDPTotalEnergy(uint8_t module, int32_t* status); +void clearPDPStickyFaults(uint8_t module, int32_t* status); } diff --git a/hal/include/HAL/Port.h b/hal/include/HAL/Port.h index b2e97c2b20..9e5aa1720b 100644 --- a/hal/include/HAL/Port.h +++ b/hal/include/HAL/Port.h @@ -7,8 +7,7 @@ #pragma once -typedef struct port_t -{ - uint8_t pin; - uint8_t module; +typedef struct port_t { + uint8_t pin; + uint8_t module; } Port; diff --git a/hal/include/HAL/Power.hpp b/hal/include/HAL/Power.hpp index b430c1eb84..00bff60538 100644 --- a/hal/include/HAL/Power.hpp +++ b/hal/include/HAL/Power.hpp @@ -2,20 +2,19 @@ #include -extern "C" -{ - float getVinVoltage(int32_t *status); - float getVinCurrent(int32_t *status); - float getUserVoltage6V(int32_t *status); - float getUserCurrent6V(int32_t *status); - bool getUserActive6V(int32_t *status); - int getUserCurrentFaults6V(int32_t *status); - float getUserVoltage5V(int32_t *status); - float getUserCurrent5V(int32_t *status); - bool getUserActive5V(int32_t *status); - int getUserCurrentFaults5V(int32_t *status); - float getUserVoltage3V3(int32_t *status); - float getUserCurrent3V3(int32_t *status); - bool getUserActive3V3(int32_t *status); - int getUserCurrentFaults3V3(int32_t *status); +extern "C" { +float getVinVoltage(int32_t* status); +float getVinCurrent(int32_t* status); +float getUserVoltage6V(int32_t* status); +float getUserCurrent6V(int32_t* status); +bool getUserActive6V(int32_t* status); +int getUserCurrentFaults6V(int32_t* status); +float getUserVoltage5V(int32_t* status); +float getUserCurrent5V(int32_t* status); +bool getUserActive5V(int32_t* status); +int getUserCurrentFaults5V(int32_t* status); +float getUserVoltage3V3(int32_t* status); +float getUserCurrent3V3(int32_t* status); +bool getUserActive3V3(int32_t* status); +int getUserCurrentFaults3V3(int32_t* status); } diff --git a/hal/include/HAL/Semaphore.hpp b/hal/include/HAL/Semaphore.hpp index 6b9c9e3b2d..a6c5a3a3de 100644 --- a/hal/include/HAL/Semaphore.hpp +++ b/hal/include/HAL/Semaphore.hpp @@ -8,14 +8,14 @@ typedef priority_condition_variable* MULTIWAIT_ID; typedef priority_condition_variable::native_handle_type NATIVE_MULTIWAIT_ID; extern "C" { - MUTEX_ID initializeMutexNormal(); - void deleteMutex(MUTEX_ID sem); - void takeMutex(MUTEX_ID sem); - bool tryTakeMutex(MUTEX_ID sem); - void giveMutex(MUTEX_ID sem); +MUTEX_ID initializeMutexNormal(); +void deleteMutex(MUTEX_ID sem); +void takeMutex(MUTEX_ID sem); +bool tryTakeMutex(MUTEX_ID sem); +void giveMutex(MUTEX_ID sem); - MULTIWAIT_ID initializeMultiWait(); - void deleteMultiWait(MULTIWAIT_ID sem); - void takeMultiWait(MULTIWAIT_ID sem, MUTEX_ID m); - void giveMultiWait(MULTIWAIT_ID sem); +MULTIWAIT_ID initializeMultiWait(); +void deleteMultiWait(MULTIWAIT_ID sem); +void takeMultiWait(MULTIWAIT_ID sem, MUTEX_ID m); +void giveMultiWait(MULTIWAIT_ID sem); } diff --git a/hal/include/HAL/SerialPort.hpp b/hal/include/HAL/SerialPort.hpp index 89f1817660..88adfb94c7 100644 --- a/hal/include/HAL/SerialPort.hpp +++ b/hal/include/HAL/SerialPort.hpp @@ -2,24 +2,24 @@ #include -extern "C" -{ - void serialInitializePort(uint8_t port, int32_t *status); - void serialSetBaudRate(uint8_t port, uint32_t baud, int32_t *status); - void serialSetDataBits(uint8_t port, uint8_t bits, int32_t *status); - void serialSetParity(uint8_t port, uint8_t parity, int32_t *status); - void serialSetStopBits(uint8_t port, uint8_t stopBits, int32_t *status); - void serialSetWriteMode(uint8_t port, uint8_t mode, int32_t *status); - void serialSetFlowControl(uint8_t port, uint8_t flow, int32_t *status); - void serialSetTimeout(uint8_t port, float timeout, int32_t *status); - void serialEnableTermination(uint8_t port, char terminator, int32_t *status); - void serialDisableTermination(uint8_t port, int32_t *status); - void serialSetReadBufferSize(uint8_t port, uint32_t size, int32_t *status); - void serialSetWriteBufferSize(uint8_t port, uint32_t size, int32_t *status); - int32_t serialGetBytesReceived(uint8_t port, int32_t *status); - uint32_t serialRead(uint8_t port, char* buffer, int32_t count, int32_t *status); - uint32_t serialWrite(uint8_t port, const char *buffer, int32_t count, int32_t *status); - void serialFlush(uint8_t port, int32_t *status); - void serialClear(uint8_t port, int32_t *status); - void serialClose(uint8_t port, int32_t *status); +extern "C" { +void serialInitializePort(uint8_t port, int32_t* status); +void serialSetBaudRate(uint8_t port, uint32_t baud, int32_t* status); +void serialSetDataBits(uint8_t port, uint8_t bits, int32_t* status); +void serialSetParity(uint8_t port, uint8_t parity, int32_t* status); +void serialSetStopBits(uint8_t port, uint8_t stopBits, int32_t* status); +void serialSetWriteMode(uint8_t port, uint8_t mode, int32_t* status); +void serialSetFlowControl(uint8_t port, uint8_t flow, int32_t* status); +void serialSetTimeout(uint8_t port, float timeout, int32_t* status); +void serialEnableTermination(uint8_t port, char terminator, int32_t* status); +void serialDisableTermination(uint8_t port, int32_t* status); +void serialSetReadBufferSize(uint8_t port, uint32_t size, int32_t* status); +void serialSetWriteBufferSize(uint8_t port, uint32_t size, int32_t* status); +int32_t serialGetBytesReceived(uint8_t port, int32_t* status); +uint32_t serialRead(uint8_t port, char* buffer, int32_t count, int32_t* status); +uint32_t serialWrite(uint8_t port, const char* buffer, int32_t count, + int32_t* status); +void serialFlush(uint8_t port, int32_t* status); +void serialClear(uint8_t port, int32_t* status); +void serialClose(uint8_t port, int32_t* status); } diff --git a/hal/include/HAL/Solenoid.hpp b/hal/include/HAL/Solenoid.hpp index 3b51f21fc4..1f9fe1af51 100644 --- a/hal/include/HAL/Solenoid.hpp +++ b/hal/include/HAL/Solenoid.hpp @@ -2,18 +2,18 @@ #include -extern "C" -{ - void* initializeSolenoidPort(void* port_pointer, int32_t *status); - void freeSolenoidPort(void* solenoid_port_pointer); - bool checkSolenoidModule(uint8_t module); +extern "C" { +void* initializeSolenoidPort(void* port_pointer, int32_t* status); +void freeSolenoidPort(void* solenoid_port_pointer); +bool checkSolenoidModule(uint8_t module); - bool getSolenoid(void* solenoid_port_pointer, int32_t *status); - uint8_t getAllSolenoids(void* solenoid_port_pointer, int32_t *status); - void setSolenoid(void* solenoid_port_pointer, bool value, int32_t *status); +bool getSolenoid(void* solenoid_port_pointer, int32_t* status); +uint8_t getAllSolenoids(void* solenoid_port_pointer, int32_t* status); +void setSolenoid(void* solenoid_port_pointer, bool value, int32_t* status); - int getPCMSolenoidBlackList(void* solenoid_port_pointer, int32_t *status); - bool getPCMSolenoidVoltageStickyFault(void* solenoid_port_pointer, int32_t *status); - bool getPCMSolenoidVoltageFault(void* solenoid_port_pointer, int32_t *status); - void clearAllPCMStickyFaults_sol(void *solenoid_port_pointer, int32_t *status); +int getPCMSolenoidBlackList(void* solenoid_port_pointer, int32_t* status); +bool getPCMSolenoidVoltageStickyFault(void* solenoid_port_pointer, + int32_t* status); +bool getPCMSolenoidVoltageFault(void* solenoid_port_pointer, int32_t* status); +void clearAllPCMStickyFaults_sol(void* solenoid_port_pointer, int32_t* status); } diff --git a/hal/include/HAL/Task.hpp b/hal/include/HAL/Task.hpp index 3feeec8fc2..223bcfa6d1 100644 --- a/hal/include/HAL/Task.hpp +++ b/hal/include/HAL/Task.hpp @@ -1,7 +1,7 @@ #pragma once -#include #include +#include #ifndef _FUNCPTR_DEFINED #define _FUNCPTR_DEFINED @@ -9,7 +9,7 @@ typedef int (*FUNCPTR)(...); /* ptr to function returning int */ #else -typedef int (*FUNCPTR) (); /* ptr to function returning int */ +typedef int (*FUNCPTR)(); /* ptr to function returning int */ #endif /* __cplusplus */ #endif /* _FUNCPTR_DEFINED */ @@ -29,12 +29,12 @@ typedef int STATUS; typedef pthread_t* TASK; extern "C" { - // Note: These constants used to be declared extern and were defined in - // Task.cpp. This caused issues with the JNI bindings for java, and so the - // instantiations were moved here. - const int32_t HAL_taskLib_ILLEGAL_PRIORITY = 22; // 22 is EINVAL +// Note: These constants used to be declared extern and were defined in +// Task.cpp. This caused issues with the JNI bindings for java, and so the +// instantiations were moved here. +const int32_t HAL_taskLib_ILLEGAL_PRIORITY = 22; // 22 is EINVAL - STATUS verifyTaskID(TASK task); - STATUS setTaskPriority(TASK task, int priority); // valid priority [1..99] - STATUS getTaskPriority(TASK task, int* priority); +STATUS verifyTaskID(TASK task); +STATUS setTaskPriority(TASK task, int priority); // valid priority [1..99] +STATUS getTaskPriority(TASK task, int* priority); } diff --git a/hal/include/HAL/Utilities.hpp b/hal/include/HAL/Utilities.hpp index df8ff5e83e..c16c31161b 100644 --- a/hal/include/HAL/Utilities.hpp +++ b/hal/include/HAL/Utilities.hpp @@ -2,12 +2,11 @@ #include -extern "C" -{ - extern const int32_t HAL_NO_WAIT; - extern const int32_t HAL_WAIT_FOREVER; +extern "C" { +extern const int32_t HAL_NO_WAIT; +extern const int32_t HAL_WAIT_FOREVER; - void delayTicks(int32_t ticks); - void delayMillis(double ms); - void delaySeconds(double s); +void delayTicks(int32_t ticks); +void delayMillis(double ms); +void delaySeconds(double s); } diff --git a/hal/include/HAL/cpp/Resource.hpp b/hal/include/HAL/cpp/Resource.hpp index ae31996692..502cb13814 100644 --- a/hal/include/HAL/cpp/Resource.hpp +++ b/hal/include/HAL/cpp/Resource.hpp @@ -1,13 +1,14 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008. All Rights Reserved. */ +/* Copyright (c) FIRST 2008. All Rights Reserved. + */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */ /*----------------------------------------------------------------------------*/ #pragma once +#include #include "../Errors.hpp" #include "HAL/cpp/priority_mutex.h" -#include #include @@ -24,23 +25,22 @@ namespace hal { * resources; it just tracks which indices were marked in use by * Allocate and not yet freed by Free. */ -class Resource -{ -public: - Resource(const Resource&) = delete; - Resource& operator=(const Resource&) = delete; - explicit Resource(uint32_t size); - virtual ~Resource() = default; - static void CreateResourceObject(Resource **r, uint32_t elements); - uint32_t Allocate(const char *resourceDesc); - uint32_t Allocate(uint32_t index, const char *resourceDesc); - void Free(uint32_t index); +class Resource { + public: + Resource(const Resource&) = delete; + Resource& operator=(const Resource&) = delete; + explicit Resource(uint32_t size); + virtual ~Resource() = default; + static void CreateResourceObject(Resource** r, uint32_t elements); + uint32_t Allocate(const char* resourceDesc); + uint32_t Allocate(uint32_t index, const char* resourceDesc); + void Free(uint32_t index); -private: - std::vector m_isAllocated; - priority_recursive_mutex m_allocateLock; + private: + std::vector m_isAllocated; + priority_recursive_mutex m_allocateLock; - static priority_recursive_mutex m_createLock; + static priority_recursive_mutex m_createLock; }; } // namespace hal diff --git a/hal/include/HAL/cpp/Semaphore.hpp b/hal/include/HAL/cpp/Semaphore.hpp index 6a6374b8dd..3142fc5de3 100644 --- a/hal/include/HAL/cpp/Semaphore.hpp +++ b/hal/include/HAL/cpp/Semaphore.hpp @@ -1,7 +1,7 @@ #pragma once -#include #include +#include #include "HAL/cpp/priority_mutex.h" diff --git a/hal/include/HAL/cpp/priority_condition_variable.h b/hal/include/HAL/cpp/priority_condition_variable.h index cd02ce4a90..cfcd023b4f 100644 --- a/hal/include/HAL/cpp/priority_condition_variable.h +++ b/hal/include/HAL/cpp/priority_condition_variable.h @@ -30,7 +30,8 @@ class priority_condition_variable { ~priority_condition_variable() = default; priority_condition_variable(const priority_condition_variable&) = delete; - priority_condition_variable& operator=(const priority_condition_variable&) = delete; + priority_condition_variable& operator=(const priority_condition_variable&) = + delete; void notify_one() noexcept { std::lock_guard lock(*m_mutex); @@ -42,7 +43,7 @@ class priority_condition_variable { m_cond.notify_all(); } - template + template void wait(Lock& _lock) { std::shared_ptr _mutex = m_mutex; std::unique_lock my_lock(*_mutex); @@ -54,14 +55,16 @@ class priority_condition_variable { m_cond.wait(my_lock2); } - template + template void wait(Lock& lock, Predicate p) { - while (!p()) { wait(lock); } + while (!p()) { + wait(lock); + } } - template - std::cv_status wait_until(Lock& _lock, - const std::chrono::time_point& atime) { + template + std::cv_status wait_until( + Lock& _lock, const std::chrono::time_point& atime) { std::shared_ptr _mutex = m_mutex; std::unique_lock my_lock(*_mutex); Unlock unlock(_lock); @@ -72,9 +75,11 @@ class priority_condition_variable { return m_cond.wait_until(my_lock2, atime); } - template + template bool wait_until(Lock& lock, - const std::chrono::time_point& atime, Predicate p) { + const std::chrono::time_point& atime, + Predicate p) { while (!p()) { if (wait_until(lock, atime) == std::cv_status::timeout) { return p(); @@ -83,35 +88,36 @@ class priority_condition_variable { return true; } - template - std::cv_status wait_for(Lock& lock, const std::chrono::duration& rtime) { + template + std::cv_status wait_for(Lock& lock, + const std::chrono::duration& rtime) { return wait_until(lock, clock_t::now() + rtime); } - template + template bool wait_for(Lock& lock, const std::chrono::duration& rtime, - Predicate p) { + Predicate p) { return wait_until(lock, clock_t::now() + rtime, std::move(p)); } - native_handle_type native_handle() { - return m_cond.native_handle(); - } + native_handle_type native_handle() { return m_cond.native_handle(); } private: std::condition_variable m_cond; std::shared_ptr m_mutex; // scoped unlock - unlocks in ctor, re-locks in dtor - template + template struct Unlock { explicit Unlock(Lock& lk) : m_lock(lk) { lk.unlock(); } ~Unlock() /*noexcept(false)*/ { if (std::uncaught_exception()) { - try { m_lock.lock(); } catch(...) {} - } - else { + try { + m_lock.lock(); + } catch (...) { + } + } else { m_lock.lock(); } } diff --git a/hal/include/HAL/cpp/priority_mutex.h b/hal/include/HAL/cpp/priority_mutex.h index b03f012f1d..1a5d070e93 100644 --- a/hal/include/HAL/cpp/priority_mutex.h +++ b/hal/include/HAL/cpp/priority_mutex.h @@ -15,17 +15,17 @@ // simulator, we do not care about priority inversion. typedef std::mutex priority_mutex; typedef std::recursive_mutex priority_recursive_mutex; -#else // Covers rest of file. +#else // Covers rest of file. #include class priority_recursive_mutex { public: - typedef pthread_mutex_t *native_handle_type; + typedef pthread_mutex_t* native_handle_type; constexpr priority_recursive_mutex() noexcept = default; - priority_recursive_mutex(const priority_recursive_mutex &) = delete; - priority_recursive_mutex &operator=(const priority_recursive_mutex &) = delete; + priority_recursive_mutex(const priority_recursive_mutex&) = delete; + priority_recursive_mutex& operator=(const priority_recursive_mutex&) = delete; // Lock the mutex, blocking until it's available. void lock(); @@ -36,11 +36,11 @@ class priority_recursive_mutex { // Tries to lock the mutex. bool try_lock() noexcept; - pthread_mutex_t *native_handle(); + pthread_mutex_t* native_handle(); private: - // Do the equivalent of setting PTHREAD_PRIO_INHERIT and - // PTHREAD_MUTEX_RECURSIVE_NP. +// Do the equivalent of setting PTHREAD_PRIO_INHERIT and +// PTHREAD_MUTEX_RECURSIVE_NP. #if __WORDSIZE == 64 pthread_mutex_t m_mutex = { {0, 0, 0, 0, 0x20 | PTHREAD_MUTEX_RECURSIVE_NP, 0, 0, {0, 0}}}; @@ -52,11 +52,11 @@ class priority_recursive_mutex { class priority_mutex { public: - typedef pthread_mutex_t *native_handle_type; + typedef pthread_mutex_t* native_handle_type; constexpr priority_mutex() noexcept = default; - priority_mutex(const priority_mutex &) = delete; - priority_mutex &operator=(const priority_mutex &) = delete; + priority_mutex(const priority_mutex&) = delete; + priority_mutex& operator=(const priority_mutex&) = delete; // Lock the mutex, blocking until it's available. void lock(); @@ -67,16 +67,14 @@ class priority_mutex { // Tries to lock the mutex. bool try_lock() noexcept; - pthread_mutex_t *native_handle(); + pthread_mutex_t* native_handle(); private: - // Do the equivalent of setting PTHREAD_PRIO_INHERIT. +// Do the equivalent of setting PTHREAD_PRIO_INHERIT. #if __WORDSIZE == 64 - pthread_mutex_t m_mutex = { - {0, 0, 0, 0, 0x20, 0, 0, {0, 0}}}; + pthread_mutex_t m_mutex = {{0, 0, 0, 0, 0x20, 0, 0, {0, 0}}}; #else - pthread_mutex_t m_mutex = { - {0, 0, 0, 0x20, 0, {0}}}; + pthread_mutex_t m_mutex = {{0, 0, 0, 0x20, 0, {0}}}; #endif }; diff --git a/hal/include/Log.hpp b/hal/include/Log.hpp index 81b7b8427e..1ec12410ae 100644 --- a/hal/include/Log.hpp +++ b/hal/include/Log.hpp @@ -1,119 +1,118 @@ #pragma once -#include -#include -#include #include +#include +#include +#include #ifdef _WIN32 - #include +#include #else - #include +#include #endif inline std::string NowTime(); -enum TLogLevel {logNONE, logERROR, logWARNING, logINFO, logDEBUG, logDEBUG1, logDEBUG2, logDEBUG3, logDEBUG4}; - -class Log -{ -public: - Log(); - virtual ~Log(); - std::ostringstream& Get(TLogLevel level = logINFO); -public: - static TLogLevel& ReportingLevel(); - static std::string ToString(TLogLevel level); - static TLogLevel FromString(const std::string& level); -protected: - std::ostringstream os; -private: - Log(const Log&); - Log& operator =(const Log&); +enum TLogLevel { + logNONE, + logERROR, + logWARNING, + logINFO, + logDEBUG, + logDEBUG1, + logDEBUG2, + logDEBUG3, + logDEBUG4 }; -inline Log::Log() -{ +class Log { + public: + Log(); + virtual ~Log(); + std::ostringstream& Get(TLogLevel level = logINFO); + + public: + static TLogLevel& ReportingLevel(); + static std::string ToString(TLogLevel level); + static TLogLevel FromString(const std::string& level); + + protected: + std::ostringstream os; + + private: + Log(const Log&); + Log& operator=(const Log&); +}; + +inline Log::Log() {} + +inline std::ostringstream& Log::Get(TLogLevel level) { + os << "- " << NowTime(); + os << " " << ToString(level) << ": "; + os << std::string(level > logDEBUG ? level - logDEBUG : 0, '\t'); + return os; } -inline std::ostringstream& Log::Get(TLogLevel level) -{ - os << "- " << NowTime(); - os << " " << ToString(level) << ": "; - os << std::string(level > logDEBUG ? level - logDEBUG : 0, '\t'); - return os; +inline Log::~Log() { + os << std::endl; + fprintf(stderr, "%s", os.str().c_str()); + fflush(stderr); } -inline Log::~Log() -{ - os << std::endl; - fprintf(stderr, "%s", os.str().c_str()); - fflush(stderr); +inline TLogLevel& Log::ReportingLevel() { + static TLogLevel reportingLevel = logDEBUG4; + return reportingLevel; } -inline TLogLevel& Log::ReportingLevel() -{ - static TLogLevel reportingLevel = logDEBUG4; - return reportingLevel; +inline std::string Log::ToString(TLogLevel level) { + static const char* const buffer[] = {"NONE", "ERROR", "WARNING", + "INFO", "DEBUG", "DEBUG1", + "DEBUG2", "DEBUG3", "DEBUG4"}; + return buffer[level]; } -inline std::string Log::ToString(TLogLevel level) -{ - static const char* const buffer[] = {"NONE", "ERROR", "WARNING", "INFO", "DEBUG", "DEBUG1", "DEBUG2", "DEBUG3", "DEBUG4"}; - return buffer[level]; -} - -inline TLogLevel Log::FromString(const std::string& level) -{ - if (level == "DEBUG4") - return logDEBUG4; - if (level == "DEBUG3") - return logDEBUG3; - if (level == "DEBUG2") - return logDEBUG2; - if (level == "DEBUG1") - return logDEBUG1; - if (level == "DEBUG") - return logDEBUG; - if (level == "INFO") - return logINFO; - if (level == "WARNING") - return logWARNING; - if (level == "ERROR") - return logERROR; - if (level == "NONE") - return logNONE; - Log().Get(logWARNING) << "Unknown logging level '" << level << "'. Using INFO level as default."; - return logINFO; +inline TLogLevel Log::FromString(const std::string& level) { + if (level == "DEBUG4") return logDEBUG4; + if (level == "DEBUG3") return logDEBUG3; + if (level == "DEBUG2") return logDEBUG2; + if (level == "DEBUG1") return logDEBUG1; + if (level == "DEBUG") return logDEBUG; + if (level == "INFO") return logINFO; + if (level == "WARNING") return logWARNING; + if (level == "ERROR") return logERROR; + if (level == "NONE") return logNONE; + Log().Get(logWARNING) << "Unknown logging level '" << level + << "'. Using INFO level as default."; + return logINFO; } typedef Log FILELog; -#define FILE_LOG(level) \ - if (level > FILELog::ReportingLevel()) ; \ - else Log().Get(level) - +#define FILE_LOG(level) \ + if (level > FILELog::ReportingLevel()) \ + ; \ + else \ + Log().Get(level) #ifdef _WIN32 -inline std::string NowTime() -{ - SYSTEMTIME st; - GetLocalTime(&st); - char result[100] = {0}; - sprintf(result, "%d:%d:%d.%d", st.wHour , st.wMinute , st.wSecond , st.wMilliseconds); - return result; +inline std::string NowTime() { + SYSTEMTIME st; + GetLocalTime(&st); + char result[100] = {0}; + sprintf(result, "%d:%d:%d.%d", st.wHour, st.wMinute, st.wSecond, + st.wMilliseconds); + return result; } #else -inline std::string NowTime() -{ - char buffer[11]; - time_t t; - time(&t); - tm * r = gmtime(&t); - strftime(buffer, sizeof(buffer), "%H:%M:%S", r); - struct timeval tv; - gettimeofday(&tv, 0); - char result[100] = {0}; - sprintf(result, "%s.%03ld", buffer, (long)tv.tv_usec / 1000); - return result; +inline std::string NowTime() { + char buffer[11]; + time_t t; + time(&t); + tm* r = gmtime(&t); + strftime(buffer, sizeof(buffer), "%H:%M:%S", r); + struct timeval tv; + gettimeofday(&tv, 0); + char result[100] = {0}; + sprintf(result, "%s.%03ld", buffer, (long)tv.tv_usec / 1000); + return result; } #endif diff --git a/hal/lib/Athena/Accelerometer.cpp b/hal/lib/Athena/Accelerometer.cpp index 315df1094f..bd0f203c55 100644 --- a/hal/lib/Athena/Accelerometer.cpp +++ b/hal/lib/Athena/Accelerometer.cpp @@ -7,10 +7,10 @@ #include "HAL/Accelerometer.hpp" -#include "ChipObject.h" +#include #include #include -#include +#include "ChipObject.h" // The 7-bit I2C address with a 0 "send" bit static const uint8_t kSendAddress = (0x1c << 1) | 0; @@ -22,56 +22,56 @@ static const uint8_t kControlTxRx = 1; static const uint8_t kControlStart = 2; static const uint8_t kControlStop = 4; -static tAccel *accel = 0; +static tAccel* accel = 0; static AccelerometerRange accelerometerRange; // Register addresses enum Register { - kReg_Status = 0x00, - kReg_OutXMSB = 0x01, - kReg_OutXLSB = 0x02, - kReg_OutYMSB = 0x03, - kReg_OutYLSB = 0x04, - kReg_OutZMSB = 0x05, - kReg_OutZLSB = 0x06, - kReg_Sysmod = 0x0B, - kReg_IntSource = 0x0C, - kReg_WhoAmI = 0x0D, - kReg_XYZDataCfg = 0x0E, - kReg_HPFilterCutoff = 0x0F, - kReg_PLStatus = 0x10, - kReg_PLCfg = 0x11, - kReg_PLCount = 0x12, - kReg_PLBfZcomp = 0x13, - kReg_PLThsReg = 0x14, - kReg_FFMtCfg = 0x15, - kReg_FFMtSrc = 0x16, - kReg_FFMtThs = 0x17, - kReg_FFMtCount = 0x18, - kReg_TransientCfg = 0x1D, - kReg_TransientSrc = 0x1E, - kReg_TransientThs = 0x1F, - kReg_TransientCount = 0x20, - kReg_PulseCfg = 0x21, - kReg_PulseSrc = 0x22, - kReg_PulseThsx = 0x23, - kReg_PulseThsy = 0x24, - kReg_PulseThsz = 0x25, - kReg_PulseTmlt = 0x26, - kReg_PulseLtcy = 0x27, - kReg_PulseWind = 0x28, - kReg_ASlpCount = 0x29, - kReg_CtrlReg1 = 0x2A, - kReg_CtrlReg2 = 0x2B, - kReg_CtrlReg3 = 0x2C, - kReg_CtrlReg4 = 0x2D, - kReg_CtrlReg5 = 0x2E, - kReg_OffX = 0x2F, - kReg_OffY = 0x30, - kReg_OffZ = 0x31 + kReg_Status = 0x00, + kReg_OutXMSB = 0x01, + kReg_OutXLSB = 0x02, + kReg_OutYMSB = 0x03, + kReg_OutYLSB = 0x04, + kReg_OutZMSB = 0x05, + kReg_OutZLSB = 0x06, + kReg_Sysmod = 0x0B, + kReg_IntSource = 0x0C, + kReg_WhoAmI = 0x0D, + kReg_XYZDataCfg = 0x0E, + kReg_HPFilterCutoff = 0x0F, + kReg_PLStatus = 0x10, + kReg_PLCfg = 0x11, + kReg_PLCount = 0x12, + kReg_PLBfZcomp = 0x13, + kReg_PLThsReg = 0x14, + kReg_FFMtCfg = 0x15, + kReg_FFMtSrc = 0x16, + kReg_FFMtThs = 0x17, + kReg_FFMtCount = 0x18, + kReg_TransientCfg = 0x1D, + kReg_TransientSrc = 0x1E, + kReg_TransientThs = 0x1F, + kReg_TransientCount = 0x20, + kReg_PulseCfg = 0x21, + kReg_PulseSrc = 0x22, + kReg_PulseThsx = 0x23, + kReg_PulseThsy = 0x24, + kReg_PulseThsz = 0x25, + kReg_PulseTmlt = 0x26, + kReg_PulseLtcy = 0x27, + kReg_PulseWind = 0x28, + kReg_ASlpCount = 0x29, + kReg_CtrlReg1 = 0x2A, + kReg_CtrlReg2 = 0x2B, + kReg_CtrlReg3 = 0x2C, + kReg_CtrlReg4 = 0x2D, + kReg_CtrlReg5 = 0x2E, + kReg_OffX = 0x2F, + kReg_OffY = 0x30, + kReg_OffZ = 0x31 }; -extern "C" uint32_t getFPGATime(int32_t *status); +extern "C" uint32_t getFPGATime(int32_t* status); static void writeRegister(Register reg, uint8_t data); static uint8_t readRegister(Register reg); @@ -80,83 +80,83 @@ static uint8_t readRegister(Register reg); * Initialize the accelerometer. */ static void initializeAccelerometer() { - int32_t status; + int32_t status; - if(!accel) { - accel = tAccel::create(&status); + if (!accel) { + accel = tAccel::create(&status); - // Enable I2C - accel->writeCNFG(1, &status); + // Enable I2C + accel->writeCNFG(1, &status); - // Set the counter to 100 kbps - accel->writeCNTR(213, &status); + // Set the counter to 100 kbps + accel->writeCNTR(213, &status); - // The device identification number should be 0x2a - assert(readRegister(kReg_WhoAmI) == 0x2a); - } + // The device identification number should be 0x2a + assert(readRegister(kReg_WhoAmI) == 0x2a); + } } static void writeRegister(Register reg, uint8_t data) { - int32_t status = 0; - uint32_t initialTime; + int32_t status = 0; + uint32_t initialTime; - accel->writeADDR(kSendAddress, &status); + accel->writeADDR(kSendAddress, &status); - // Send a start transmit/receive message with the register address - accel->writeCNTL(kControlStart | kControlTxRx, &status); - accel->writeDATO(reg, &status); - accel->strobeGO(&status); + // Send a start transmit/receive message with the register address + accel->writeCNTL(kControlStart | kControlTxRx, &status); + accel->writeDATO(reg, &status); + accel->strobeGO(&status); - // Execute and wait until it's done (up to a millisecond) - initialTime = getFPGATime(&status); - while(accel->readSTAT(&status) & 1) { - if(getFPGATime(&status) > initialTime + 1000) break; - } + // Execute and wait until it's done (up to a millisecond) + initialTime = getFPGATime(&status); + while (accel->readSTAT(&status) & 1) { + if (getFPGATime(&status) > initialTime + 1000) break; + } - // Send a stop transmit/receive message with the data - accel->writeCNTL(kControlStop | kControlTxRx, &status); - accel->writeDATO(data, &status); - accel->strobeGO(&status); + // Send a stop transmit/receive message with the data + accel->writeCNTL(kControlStop | kControlTxRx, &status); + accel->writeDATO(data, &status); + accel->strobeGO(&status); - // Execute and wait until it's done (up to a millisecond) - initialTime = getFPGATime(&status); - while(accel->readSTAT(&status) & 1) { - if(getFPGATime(&status) > initialTime + 1000) break; - } + // Execute and wait until it's done (up to a millisecond) + initialTime = getFPGATime(&status); + while (accel->readSTAT(&status) & 1) { + if (getFPGATime(&status) > initialTime + 1000) break; + } - fflush(stdout); + fflush(stdout); } static uint8_t readRegister(Register reg) { - int32_t status = 0; - uint32_t initialTime; + int32_t status = 0; + uint32_t initialTime; - // Send a start transmit/receive message with the register address - accel->writeADDR(kSendAddress, &status); - accel->writeCNTL(kControlStart | kControlTxRx, &status); - accel->writeDATO(reg, &status); - accel->strobeGO(&status); + // Send a start transmit/receive message with the register address + accel->writeADDR(kSendAddress, &status); + accel->writeCNTL(kControlStart | kControlTxRx, &status); + accel->writeDATO(reg, &status); + accel->strobeGO(&status); - // Execute and wait until it's done (up to a millisecond) - initialTime = getFPGATime(&status); - while(accel->readSTAT(&status) & 1) { - if(getFPGATime(&status) > initialTime + 1000) break; - } + // Execute and wait until it's done (up to a millisecond) + initialTime = getFPGATime(&status); + while (accel->readSTAT(&status) & 1) { + if (getFPGATime(&status) > initialTime + 1000) break; + } - // Receive a message with the data and stop - accel->writeADDR(kReceiveAddress, &status); - accel->writeCNTL(kControlStart | kControlStop | kControlTxRx, &status); - accel->strobeGO(&status); + // Receive a message with the data and stop + accel->writeADDR(kReceiveAddress, &status); + accel->writeCNTL(kControlStart | kControlStop | kControlTxRx, &status); + accel->strobeGO(&status); - // Execute and wait until it's done (up to a millisecond) - initialTime = getFPGATime(&status); - while(accel->readSTAT(&status) & 1) { - if(getFPGATime(&status) > initialTime + 1000) break; - } + // Execute and wait until it's done (up to a millisecond) + initialTime = getFPGATime(&status); + while (accel->readSTAT(&status) & 1) { + if (getFPGATime(&status) > initialTime + 1000) break; + } - fflush(stdout); + fflush(stdout); - return accel->readDATI(&status); + return accel->readDATI(&status); } /** @@ -164,18 +164,22 @@ static uint8_t readRegister(Register reg) { * 1 g-force, taking into account the accelerometer range. */ static double unpackAxis(int16_t raw) { - // The raw value is actually 12 bits, not 16, so we need to propogate the - // 2's complement sign bit to the unused 4 bits for this to work with - // negative numbers. - raw <<= 4; - raw >>= 4; + // The raw value is actually 12 bits, not 16, so we need to propogate the + // 2's complement sign bit to the unused 4 bits for this to work with + // negative numbers. + raw <<= 4; + raw >>= 4; - switch(accelerometerRange) { - case kRange_2G: return raw / 1024.0; - case kRange_4G: return raw / 512.0; - case kRange_8G: return raw / 256.0; - default: return 0.0; - } + switch (accelerometerRange) { + case kRange_2G: + return raw / 1024.0; + case kRange_4G: + return raw / 512.0; + case kRange_8G: + return raw / 256.0; + default: + return 0.0; + } } extern "C" { @@ -185,11 +189,11 @@ extern "C" { * mode to change any configuration. */ void setAccelerometerActive(bool active) { - initializeAccelerometer(); + initializeAccelerometer(); - uint8_t ctrlReg1 = readRegister(kReg_CtrlReg1); - ctrlReg1 &= ~1; // Clear the existing active bit - writeRegister(kReg_CtrlReg1, ctrlReg1 | (active? 1 : 0)); + uint8_t ctrlReg1 = readRegister(kReg_CtrlReg1); + ctrlReg1 &= ~1; // Clear the existing active bit + writeRegister(kReg_CtrlReg1, ctrlReg1 | (active ? 1 : 0)); } /** @@ -197,13 +201,13 @@ void setAccelerometerActive(bool active) { * The accelerometer should be in standby mode when this is called. */ void setAccelerometerRange(AccelerometerRange range) { - initializeAccelerometer(); + initializeAccelerometer(); - accelerometerRange = range; + accelerometerRange = range; - uint8_t xyzDataCfg = readRegister(kReg_XYZDataCfg); - xyzDataCfg &= ~3; // Clear the existing two range bits - writeRegister(kReg_XYZDataCfg, xyzDataCfg | range); + uint8_t xyzDataCfg = readRegister(kReg_XYZDataCfg); + xyzDataCfg &= ~3; // Clear the existing two range bits + writeRegister(kReg_XYZDataCfg, xyzDataCfg | range); } /** @@ -212,10 +216,11 @@ void setAccelerometerRange(AccelerometerRange range) { * This is a floating point value in units of 1 g-force */ double getAccelerometerX() { - initializeAccelerometer(); + initializeAccelerometer(); - int raw = (readRegister(kReg_OutXMSB) << 4) | (readRegister(kReg_OutXLSB) >> 4); - return unpackAxis(raw); + int raw = + (readRegister(kReg_OutXMSB) << 4) | (readRegister(kReg_OutXLSB) >> 4); + return unpackAxis(raw); } /** @@ -224,10 +229,11 @@ double getAccelerometerX() { * This is a floating point value in units of 1 g-force */ double getAccelerometerY() { - initializeAccelerometer(); + initializeAccelerometer(); - int raw = (readRegister(kReg_OutYMSB) << 4) | (readRegister(kReg_OutYLSB) >> 4); - return unpackAxis(raw); + int raw = + (readRegister(kReg_OutYMSB) << 4) | (readRegister(kReg_OutYLSB) >> 4); + return unpackAxis(raw); } /** @@ -236,10 +242,11 @@ double getAccelerometerY() { * This is a floating point value in units of 1 g-force */ double getAccelerometerZ() { - initializeAccelerometer(); + initializeAccelerometer(); - int raw = (readRegister(kReg_OutZMSB) << 4) | (readRegister(kReg_OutZLSB) >> 4); - return unpackAxis(raw); + int raw = + (readRegister(kReg_OutZMSB) << 4) | (readRegister(kReg_OutZLSB) >> 4); + return unpackAxis(raw); } } // extern "C" diff --git a/hal/lib/Athena/Analog.cpp b/hal/lib/Athena/Analog.cpp index a34eb9316d..2bf9d7846c 100644 --- a/hal/lib/Athena/Analog.cpp +++ b/hal/lib/Athena/Analog.cpp @@ -7,15 +7,15 @@ #include "HAL/Analog.hpp" -#include "HAL/cpp/priority_mutex.h" -#include "HAL/Port.h" -#include "HAL/HAL.hpp" #include "ChipObject.h" -#include "HAL/cpp/Resource.hpp" #include "FRC_NetworkCommunication/AICalibration.h" #include "FRC_NetworkCommunication/LoadOut.h" +#include "HAL/HAL.hpp" +#include "HAL/Port.h" +#include "HAL/cpp/Resource.hpp" +#include "HAL/cpp/priority_mutex.h" -static const long kTimebase = 40000000; ///< 40 MHz clock +static const long kTimebase = 40000000; ///< 40 MHz clock static const long kDefaultOversampleBits = 0; static const long kDefaultAverageBits = 7; static const float kDefaultSampleRate = 50000.0; @@ -27,7 +27,7 @@ static const uint32_t kAccumulatorChannels[] = {0, 1}; struct AnalogPort { Port port; - tAccumulator *accumulator; + tAccumulator* accumulator; }; static bool analogSampleRateSet = false; @@ -39,8 +39,8 @@ static uint32_t analogNumChannelsToActivate = 0; extern "C" { // Utility methods defined below. -static uint32_t getAnalogNumActiveChannels(int32_t *status); -static uint32_t getAnalogNumChannelsToActivate(int32_t *status); +static uint32_t getAnalogNumActiveChannels(int32_t* status); +static uint32_t getAnalogNumChannelsToActivate(int32_t* status); static void setAnalogNumChannelsToActivate(uint32_t channels); static bool analogSystemInitialized = false; @@ -48,7 +48,7 @@ static bool analogSystemInitialized = false; /** * Initialize the analog System. */ -void initializeAnalog(int32_t *status) { +void initializeAnalog(int32_t* status) { std::lock_guard sync(analogRegisterWindowMutex); if (analogSystemInitialized) return; analogInputSystem = tAI::create(status); @@ -61,16 +61,17 @@ void initializeAnalog(int32_t *status) { /** * Initialize the analog input port using the given port object. */ -void* initializeAnalogInputPort(void* port_pointer, int32_t *status) { +void* initializeAnalogInputPort(void* port_pointer, int32_t* status) { initializeAnalog(status); - Port* port = (Port*) port_pointer; + Port* port = (Port*)port_pointer; // Initialize port structure AnalogPort* analog_port = new AnalogPort(); analog_port->port = *port; if (isAccumulatorChannel(analog_port, status)) { analog_port->accumulator = tAccumulator::create(port->pin, status); - } else analog_port->accumulator = NULL; + } else + analog_port->accumulator = NULL; // Set default configuration analogInputSystem->writeScanList(port->pin, port->pin, status); @@ -80,7 +81,7 @@ void* initializeAnalogInputPort(void* port_pointer, int32_t *status) { } void freeAnalogInputPort(void* analog_port_pointer) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; + AnalogPort* port = (AnalogPort*)analog_port_pointer; if (!port) return; delete port->accumulator; delete port; @@ -89,9 +90,9 @@ void freeAnalogInputPort(void* analog_port_pointer) { /** * Initialize the analog output port using the given port object. */ -void* initializeAnalogOutputPort(void* port_pointer, int32_t *status) { +void* initializeAnalogOutputPort(void* port_pointer, int32_t* status) { initializeAnalog(status); - Port* port = (Port*) port_pointer; + Port* port = (Port*)port_pointer; // Initialize port structure AnalogPort* analog_port = new AnalogPort(); @@ -101,61 +102,59 @@ void* initializeAnalogOutputPort(void* port_pointer, int32_t *status) { } void freeAnalogOutputPort(void* analog_port_pointer) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; + AnalogPort* port = (AnalogPort*)analog_port_pointer; if (!port) return; delete port->accumulator; delete port; } - /** * Check that the analog module number is valid. * * @return Analog module is valid and present */ -bool checkAnalogModule(uint8_t module) { - return module == 1; -} +bool checkAnalogModule(uint8_t module) { return module == 1; } /** * Check that the analog output channel number is value. - * Verify that the analog channel number is one of the legal channel numbers. Channel numbers - * are 0-based. + * Verify that the analog channel number is one of the legal channel numbers. + * Channel numbers are 0-based. * * @return Analog channel is valid */ bool checkAnalogInputChannel(uint32_t pin) { - if (pin < kAnalogInputPins) - return true; + if (pin < kAnalogInputPins) return true; return false; } /** * Check that the analog output channel number is value. - * Verify that the analog channel number is one of the legal channel numbers. Channel numbers - * are 0-based. + * Verify that the analog channel number is one of the legal channel numbers. + * Channel numbers are 0-based. * * @return Analog channel is valid */ bool checkAnalogOutputChannel(uint32_t pin) { - if (pin < kAnalogOutputPins) - return true; + if (pin < kAnalogOutputPins) return true; return false; } -void setAnalogOutput(void* analog_port_pointer, double voltage, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +void setAnalogOutput(void* analog_port_pointer, double voltage, + int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; uint16_t rawValue = (uint16_t)(voltage / 5.0 * 0x1000); - if(voltage < 0.0) rawValue = 0; - else if(voltage > 5.0) rawValue = 0x1000; + if (voltage < 0.0) + rawValue = 0; + else if (voltage > 5.0) + rawValue = 0x1000; analogOutputSystem->writeMXP(port->port.pin, rawValue, status); } -double getAnalogOutput(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +double getAnalogOutput(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; uint16_t rawValue = analogOutputSystem->readMXP(port->port.pin, status); @@ -169,22 +168,24 @@ double getAnalogOutput(void* analog_port_pointer, int32_t *status) { * * @param samplesPerSecond The number of samples per channel per second. */ -void setAnalogSampleRate(double samplesPerSecond, int32_t *status) { +void setAnalogSampleRate(double samplesPerSecond, int32_t* status) { // TODO: This will change when variable size scan lists are implemented. // TODO: Need float comparison with epsilon. - //wpi_assert(!sampleRateSet || GetSampleRate() == samplesPerSecond); + // wpi_assert(!sampleRateSet || GetSampleRate() == samplesPerSecond); analogSampleRateSet = true; // Compute the convert rate uint32_t ticksPerSample = (uint32_t)((float)kTimebase / samplesPerSecond); - uint32_t ticksPerConversion = ticksPerSample / getAnalogNumChannelsToActivate(status); + uint32_t ticksPerConversion = + ticksPerSample / getAnalogNumChannelsToActivate(status); // ticksPerConversion must be at least 80 if (ticksPerConversion < 80) { if ((*status) >= 0) *status = SAMPLE_RATE_TOO_HIGH; ticksPerConversion = 80; } - // Atomically set the scan size and the convert rate so that the sample rate is constant + // Atomically set the scan size and the convert rate so that the sample rate + // is constant tAI::tConfig config; config.ScanSize = getAnalogNumChannelsToActivate(status); config.ConvertRate = ticksPerConversion; @@ -202,38 +203,40 @@ void setAnalogSampleRate(double samplesPerSecond, int32_t *status) { * * @return Sample rate. */ -float getAnalogSampleRate(int32_t *status) { +float getAnalogSampleRate(int32_t* status) { uint32_t ticksPerConversion = analogInputSystem->readLoopTiming(status); - uint32_t ticksPerSample = ticksPerConversion * getAnalogNumActiveChannels(status); + uint32_t ticksPerSample = + ticksPerConversion * getAnalogNumActiveChannels(status); return (float)kTimebase / (float)ticksPerSample; } /** * Set the number of averaging bits. * - * This sets the number of averaging bits. The actual number of averaged samples is 2**bits. - * Use averaging to improve the stability of your measurement at the expense of sampling rate. - * The averaging is done automatically in the FPGA. + * This sets the number of averaging bits. The actual number of averaged samples + * is 2**bits. Use averaging to improve the stability of your measurement at the + * expense of sampling rate. The averaging is done automatically in the FPGA. * * @param analog_port_pointer Pointer to the analog port to configure. * @param bits Number of bits to average. */ -void setAnalogAverageBits(void* analog_port_pointer, uint32_t bits, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +void setAnalogAverageBits(void* analog_port_pointer, uint32_t bits, + int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; analogInputSystem->writeAverageBits(port->port.pin, bits, status); } /** * Get the number of averaging bits. * - * This gets the number of averaging bits from the FPGA. The actual number of averaged samples is 2**bits. - * The averaging is done automatically in the FPGA. + * This gets the number of averaging bits from the FPGA. The actual number of + * averaged samples is 2**bits. The averaging is done automatically in the FPGA. * * @param analog_port_pointer Pointer to the analog port to use. * @return Bits to average. */ -uint32_t getAnalogAverageBits(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +uint32_t getAnalogAverageBits(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; uint32_t result = analogInputSystem->readAverageBits(port->port.pin, status); return result; } @@ -241,45 +244,49 @@ uint32_t getAnalogAverageBits(void* analog_port_pointer, int32_t *status) { /** * Set the number of oversample bits. * - * This sets the number of oversample bits. The actual number of oversampled values is 2**bits. - * Use oversampling to improve the resolution of your measurements at the expense of sampling rate. - * The oversampling is done automatically in the FPGA. + * This sets the number of oversample bits. The actual number of oversampled + * values is 2**bits. Use oversampling to improve the resolution of your + * measurements at the expense of sampling rate. The oversampling is done + * automatically in the FPGA. * * @param analog_port_pointer Pointer to the analog port to use. * @param bits Number of bits to oversample. */ -void setAnalogOversampleBits(void* analog_port_pointer, uint32_t bits, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +void setAnalogOversampleBits(void* analog_port_pointer, uint32_t bits, + int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; analogInputSystem->writeOversampleBits(port->port.pin, bits, status); } - /** * Get the number of oversample bits. * - * This gets the number of oversample bits from the FPGA. The actual number of oversampled values is - * 2**bits. The oversampling is done automatically in the FPGA. + * This gets the number of oversample bits from the FPGA. The actual number of + * oversampled values is 2**bits. The oversampling is done automatically in the + * FPGA. * * @param analog_port_pointer Pointer to the analog port to use. * @return Bits to oversample. */ -uint32_t getAnalogOversampleBits(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; - uint32_t result = analogInputSystem->readOversampleBits(port->port.pin, status); +uint32_t getAnalogOversampleBits(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; + uint32_t result = + analogInputSystem->readOversampleBits(port->port.pin, status); return result; } /** * Get a sample straight from the channel on this module. * - * The sample is a 12-bit value representing the 0V to 5V range of the A/D converter in the module. - * The units are in A/D converter codes. Use GetVoltage() to get the analog value in calibrated units. + * The sample is a 12-bit value representing the 0V to 5V range of the A/D + * converter in the module. The units are in A/D converter codes. Use + * GetVoltage() to get the analog value in calibrated units. * * @param analog_port_pointer Pointer to the analog port to use. * @return A sample straight from the channel on this module. */ -int16_t getAnalogValue(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +int16_t getAnalogValue(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; int16_t value; if (!checkAnalogInputChannel(port->port.pin)) { return 0; @@ -293,26 +300,28 @@ int16_t getAnalogValue(void* analog_port_pointer, int32_t *status) { std::lock_guard sync(analogRegisterWindowMutex); analogInputSystem->writeReadSelect(readSelect, status); analogInputSystem->strobeLatchOutput(status); - value = (int16_t) analogInputSystem->readOutput(status); + value = (int16_t)analogInputSystem->readOutput(status); } return value; } /** - * Get a sample from the output of the oversample and average engine for the channel. + * Get a sample from the output of the oversample and average engine for the + * channel. * * The sample is 12-bit + the value configured in SetOversampleBits(). - * The value configured in SetAverageBits() will cause this value to be averaged 2**bits number of samples. - * This is not a sliding window. The sample will not change until 2**(OversamplBits + AverageBits) samples - * have been acquired from the module on this channel. - * Use GetAverageVoltage() to get the analog value in calibrated units. + * The value configured in SetAverageBits() will cause this value to be averaged + * 2**bits number of samples. This is not a sliding window. The sample will not + * change until 2**(OversamplBits + AverageBits) samples have been acquired from + * the module on this channel. Use GetAverageVoltage() to get the analog value + * in calibrated units. * * @param analog_port_pointer Pointer to the analog port to use. * @return A sample from the oversample and average engine for the channel. */ -int32_t getAnalogAverageValue(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +int32_t getAnalogAverageValue(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; int32_t value; if (!checkAnalogInputChannel(port->port.pin)) { return 0; @@ -326,7 +335,7 @@ int32_t getAnalogAverageValue(void* analog_port_pointer, int32_t *status) { std::lock_guard sync(analogRegisterWindowMutex); analogInputSystem->writeReadSelect(readSelect, status); analogInputSystem->strobeLatchOutput(status); - value = (int32_t) analogInputSystem->readOutput(status); + value = (int32_t)analogInputSystem->readOutput(status); } return value; @@ -335,12 +344,13 @@ int32_t getAnalogAverageValue(void* analog_port_pointer, int32_t *status) { /** * Get a scaled sample straight from the channel on this module. * - * The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset(). + * The value is scaled to units of Volts using the calibrated scaling data from + * GetLSBWeight() and GetOffset(). * * @param analog_port_pointer Pointer to the analog port to use. * @return A scaled sample straight from the channel on this module. */ -float getAnalogVoltage(void* analog_port_pointer, int32_t *status) { +float getAnalogVoltage(void* analog_port_pointer, int32_t* status) { int16_t value = getAnalogValue(analog_port_pointer, status); uint32_t LSBWeight = getAnalogLSBWeight(analog_port_pointer, status); int32_t offset = getAnalogOffset(analog_port_pointer, status); @@ -349,21 +359,27 @@ float getAnalogVoltage(void* analog_port_pointer, int32_t *status) { } /** - * Get a scaled sample from the output of the oversample and average engine for the channel. + * Get a scaled sample from the output of the oversample and average engine for + * the channel. * - * The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset(). - * Using oversampling will cause this value to be higher resolution, but it will update more slowly. - * Using averaging will cause this value to be more stable, but it will update more slowly. + * The value is scaled to units of Volts using the calibrated scaling data from + * GetLSBWeight() and GetOffset(). Using oversampling will cause this value to + * be higher resolution, but it will update more slowly. Using averaging will + * cause this value to be more stable, but it will update more slowly. * * @param analog_port_pointer Pointer to the analog port to use. - * @return A scaled sample from the output of the oversample and average engine for the channel. + * @return A scaled sample from the output of the oversample and average engine + * for the channel. */ -float getAnalogAverageVoltage(void* analog_port_pointer, int32_t *status) { +float getAnalogAverageVoltage(void* analog_port_pointer, int32_t* status) { int32_t value = getAnalogAverageValue(analog_port_pointer, status); uint32_t LSBWeight = getAnalogLSBWeight(analog_port_pointer, status); int32_t offset = getAnalogOffset(analog_port_pointer, status); - uint32_t oversampleBits = getAnalogOversampleBits(analog_port_pointer, status); - float voltage = ((LSBWeight * 1.0e-9 * value) / (float)(1 << oversampleBits)) - offset * 1.0e-9; + uint32_t oversampleBits = + getAnalogOversampleBits(analog_port_pointer, status); + float voltage = + ((LSBWeight * 1.0e-9 * value) / (float)(1 << oversampleBits)) - + offset * 1.0e-9; return voltage; } @@ -379,7 +395,8 @@ float getAnalogAverageVoltage(void* analog_port_pointer, int32_t *status) { * @param voltage The voltage to convert. * @return The raw value for the channel. */ -int32_t getAnalogVoltsToValue(void* analog_port_pointer, double voltage, int32_t *status) { +int32_t getAnalogVoltsToValue(void* analog_port_pointer, double voltage, + int32_t* status) { if (voltage > 5.0) { voltage = 5.0; *status = VOLTAGE_OUT_OF_RANGE; @@ -390,52 +407,52 @@ int32_t getAnalogVoltsToValue(void* analog_port_pointer, double voltage, int32_t } uint32_t LSBWeight = getAnalogLSBWeight(analog_port_pointer, status); int32_t offset = getAnalogOffset(analog_port_pointer, status); - int32_t value = (int32_t) ((voltage + offset * 1.0e-9) / (LSBWeight * 1.0e-9)); + int32_t value = (int32_t)((voltage + offset * 1.0e-9) / (LSBWeight * 1.0e-9)); return value; } /** * Get the factory scaling least significant bit weight constant. - * The least significant bit weight constant for the channel that was calibrated in - * manufacturing and stored in an eeprom in the module. + * The least significant bit weight constant for the channel that was calibrated + * in manufacturing and stored in an eeprom in the module. * * Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9) * * @param analog_port_pointer Pointer to the analog port to use. * @return Least significant bit weight. */ -uint32_t getAnalogLSBWeight(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; - uint32_t lsbWeight = FRC_NetworkCommunication_nAICalibration_getLSBWeight(0, port->port.pin, status); // XXX: aiSystemIndex == 0? +uint32_t getAnalogLSBWeight(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; + uint32_t lsbWeight = FRC_NetworkCommunication_nAICalibration_getLSBWeight( + 0, port->port.pin, status); // XXX: aiSystemIndex == 0? return lsbWeight; } /** * Get the factory scaling offset constant. - * The offset constant for the channel that was calibrated in manufacturing and stored - * in an eeprom in the module. + * The offset constant for the channel that was calibrated in manufacturing and + * stored in an eeprom in the module. * * Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9) * * @param analog_port_pointer Pointer to the analog port to use. * @return Offset constant. */ -int32_t getAnalogOffset(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; - int32_t offset = FRC_NetworkCommunication_nAICalibration_getOffset(0, port->port.pin, status); // XXX: aiSystemIndex == 0? +int32_t getAnalogOffset(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; + int32_t offset = FRC_NetworkCommunication_nAICalibration_getOffset( + 0, port->port.pin, status); // XXX: aiSystemIndex == 0? return offset; } - /** * Return the number of channels on the module in use. * * @return Active channels. */ -static uint32_t getAnalogNumActiveChannels(int32_t *status) { +static uint32_t getAnalogNumActiveChannels(int32_t* status) { uint32_t scanSize = analogInputSystem->readConfig_ScanSize(status); - if (scanSize == 0) - return 8; + if (scanSize == 0) return 8; return scanSize; } @@ -450,15 +467,17 @@ static uint32_t getAnalogNumActiveChannels(int32_t *status) { * * @return Value to write to the active channels field. */ -static uint32_t getAnalogNumChannelsToActivate(int32_t *status) { - if(analogNumChannelsToActivate == 0) return getAnalogNumActiveChannels(status); +static uint32_t getAnalogNumChannelsToActivate(int32_t* status) { + if (analogNumChannelsToActivate == 0) + return getAnalogNumActiveChannels(status); return analogNumChannelsToActivate; } /** * Set the number of active channels. * - * Store the number of active channels to set. Don't actually commit to hardware + * Store the number of active channels to set. Don't actually commit to + * hardware * until SetSampleRate(). * * @param channels Number of active channels. @@ -474,9 +493,9 @@ static void setAnalogNumChannelsToActivate(uint32_t channels) { * * @return The analog channel is attached to an accumulator. */ -bool isAccumulatorChannel(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; - for (uint32_t i=0; i < kAccumulatorNumChannels; i++) { +bool isAccumulatorChannel(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; + for (uint32_t i = 0; i < kAccumulatorNumChannels; i++) { if (port->port.pin == kAccumulatorChannels[i]) return true; } return false; @@ -485,7 +504,7 @@ bool isAccumulatorChannel(void* analog_port_pointer, int32_t *status) { /** * Initialize the accumulator. */ -void initAccumulator(void* analog_port_pointer, int32_t *status) { +void initAccumulator(void* analog_port_pointer, int32_t* status) { setAccumulatorCenter(analog_port_pointer, 0, status); resetAccumulator(analog_port_pointer, status); } @@ -493,8 +512,8 @@ void initAccumulator(void* analog_port_pointer, int32_t *status) { /** * Resets the accumulator to the initial value. */ -void resetAccumulator(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +void resetAccumulator(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; if (port->accumulator == NULL) { *status = NULL_PARAMETER; return; @@ -505,15 +524,18 @@ void resetAccumulator(void* analog_port_pointer, int32_t *status) { /** * Set the center value of the accumulator. * - * The center value is subtracted from each A/D value before it is added to the accumulator. This - * is used for the center value of devices like gyros and accelerometers to make integration work - * and to take the device offset into account when integrating. + * The center value is subtracted from each A/D value before it is added to the + * accumulator. This is used for the center value of devices like gyros and + * accelerometers to make integration work and to take the device offset into + * account when integrating. * - * This center value is based on the output of the oversampled and averaged source from channel 1. - * Because of this, any non-zero oversample bits will affect the size of the value for this field. + * This center value is based on the output of the oversampled and averaged + * source from channel 1. Because of this, any non-zero oversample bits will + * affect the size of the value for this field. */ -void setAccumulatorCenter(void* analog_port_pointer, int32_t center, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +void setAccumulatorCenter(void* analog_port_pointer, int32_t center, + int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; if (port->accumulator == NULL) { *status = NULL_PARAMETER; return; @@ -524,8 +546,9 @@ void setAccumulatorCenter(void* analog_port_pointer, int32_t center, int32_t *st /** * Set the accumulator's deadband. */ -void setAccumulatorDeadband(void* analog_port_pointer, int32_t deadband, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +void setAccumulatorDeadband(void* analog_port_pointer, int32_t deadband, + int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; if (port->accumulator == NULL) { *status = NULL_PARAMETER; return; @@ -541,8 +564,8 @@ void setAccumulatorDeadband(void* analog_port_pointer, int32_t deadband, int32_t * * @return The 64-bit value accumulated since the last Reset(). */ -int64_t getAccumulatorValue(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +int64_t getAccumulatorValue(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; if (port->accumulator == NULL) { *status = NULL_PARAMETER; return 0; @@ -554,12 +577,13 @@ int64_t getAccumulatorValue(void* analog_port_pointer, int32_t *status) { /** * Read the number of accumulated values. * - * Read the count of the accumulated values since the accumulator was last Reset(). + * Read the count of the accumulated values since the accumulator was last + * Reset(). * * @return The number of times samples from the channel were accumulated. */ -uint32_t getAccumulatorCount(void* analog_port_pointer, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +uint32_t getAccumulatorCount(void* analog_port_pointer, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; if (port->accumulator == NULL) { *status = NULL_PARAMETER; return 0; @@ -576,8 +600,9 @@ uint32_t getAccumulatorCount(void* analog_port_pointer, int32_t *status) { * @param value Pointer to the 64-bit accumulated output. * @param count Pointer to the number of accumulation cycles. */ -void getAccumulatorOutput(void* analog_port_pointer, int64_t *value, uint32_t *count, int32_t *status) { - AnalogPort* port = (AnalogPort*) analog_port_pointer; +void getAccumulatorOutput(void* analog_port_pointer, int64_t* value, + uint32_t* count, int32_t* status) { + AnalogPort* port = (AnalogPort*)analog_port_pointer; if (port->accumulator == NULL) { *status = NULL_PARAMETER; return; @@ -593,7 +618,6 @@ void getAccumulatorOutput(void* analog_port_pointer, int64_t *value, uint32_t *c *count = output.Count; } - struct trigger_t { tAnalogTrigger* trigger; AnalogPort* port; @@ -601,14 +625,15 @@ struct trigger_t { }; typedef struct trigger_t AnalogTrigger; -static hal::Resource *triggers = NULL; +static hal::Resource* triggers = NULL; -void* initializeAnalogTrigger(void* port_pointer, uint32_t *index, int32_t *status) { - Port* port = (Port*) port_pointer; +void* initializeAnalogTrigger(void* port_pointer, uint32_t* index, + int32_t* status) { + Port* port = (Port*)port_pointer; hal::Resource::CreateResourceObject(&triggers, tAnalogTrigger::kNumSystems); AnalogTrigger* trigger = new AnalogTrigger(); - trigger->port = (AnalogPort*) initializeAnalogInputPort(port, status); + trigger->port = (AnalogPort*)initializeAnalogInputPort(port, status); trigger->index = triggers->Allocate("Analog Trigger"); *index = trigger->index; // TODO: if (index == ~0ul) { CloneError(triggers); return; } @@ -619,8 +644,8 @@ void* initializeAnalogTrigger(void* port_pointer, uint32_t *index, int32_t *stat return trigger; } -void cleanAnalogTrigger(void* analog_trigger_pointer, int32_t *status) { - AnalogTrigger* trigger = (AnalogTrigger*) analog_trigger_pointer; +void cleanAnalogTrigger(void* analog_trigger_pointer, int32_t* status) { + AnalogTrigger* trigger = (AnalogTrigger*)analog_trigger_pointer; if (!trigger) return; triggers->Free(trigger->index); delete trigger->trigger; @@ -628,10 +653,11 @@ void cleanAnalogTrigger(void* analog_trigger_pointer, int32_t *status) { delete trigger; } -void setAnalogTriggerLimitsRaw(void* analog_trigger_pointer, int32_t lower, int32_t upper, int32_t *status) { - AnalogTrigger* trigger = (AnalogTrigger*) analog_trigger_pointer; +void setAnalogTriggerLimitsRaw(void* analog_trigger_pointer, int32_t lower, + int32_t upper, int32_t* status) { + AnalogTrigger* trigger = (AnalogTrigger*)analog_trigger_pointer; if (lower > upper) { - *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR; + *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR; } trigger->trigger->writeLowerLimit(lower, status); trigger->trigger->writeUpperLimit(upper, status); @@ -641,40 +667,49 @@ void setAnalogTriggerLimitsRaw(void* analog_trigger_pointer, int32_t lower, int3 * Set the upper and lower limits of the analog trigger. * The limits are given as floating point voltage values. */ -void setAnalogTriggerLimitsVoltage(void* analog_trigger_pointer, double lower, double upper, int32_t *status) { - AnalogTrigger* trigger = (AnalogTrigger*) analog_trigger_pointer; +void setAnalogTriggerLimitsVoltage(void* analog_trigger_pointer, double lower, + double upper, int32_t* status) { + AnalogTrigger* trigger = (AnalogTrigger*)analog_trigger_pointer; if (lower > upper) { - *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR; + *status = ANALOG_TRIGGER_LIMIT_ORDER_ERROR; } - // TODO: This depends on the averaged setting. Only raw values will work as is. - trigger->trigger->writeLowerLimit(getAnalogVoltsToValue(trigger->port, lower, status), status); - trigger->trigger->writeUpperLimit(getAnalogVoltsToValue(trigger->port, upper, status), status); + // TODO: This depends on the averaged setting. Only raw values will work as + // is. + trigger->trigger->writeLowerLimit( + getAnalogVoltsToValue(trigger->port, lower, status), status); + trigger->trigger->writeUpperLimit( + getAnalogVoltsToValue(trigger->port, upper, status), status); } /** * Configure the analog trigger to use the averaged vs. raw values. - * If the value is true, then the averaged value is selected for the analog trigger, otherwise - * the immediate value is used. + * If the value is true, then the averaged value is selected for the analog + * trigger, otherwise the immediate value is used. */ -void setAnalogTriggerAveraged(void* analog_trigger_pointer, bool useAveragedValue, int32_t *status) { - AnalogTrigger* trigger = (AnalogTrigger*) analog_trigger_pointer; +void setAnalogTriggerAveraged(void* analog_trigger_pointer, + bool useAveragedValue, int32_t* status) { + AnalogTrigger* trigger = (AnalogTrigger*)analog_trigger_pointer; if (trigger->trigger->readSourceSelect_Filter(status) != 0) { - *status = INCOMPATIBLE_STATE; - // TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not support average and filtering at the same time."); + *status = INCOMPATIBLE_STATE; + // TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not + // support average and filtering at the same time."); } trigger->trigger->writeSourceSelect_Averaged(useAveragedValue, status); } /** * Configure the analog trigger to use a filtered value. - * The analog trigger will operate with a 3 point average rejection filter. This is designed to - * help with 360 degree pot applications for the period where the pot crosses through zero. + * The analog trigger will operate with a 3 point average rejection filter. This + * is designed to help with 360 degree pot applications for the period where the + * pot crosses through zero. */ -void setAnalogTriggerFiltered(void* analog_trigger_pointer, bool useFilteredValue, int32_t *status) { - AnalogTrigger* trigger = (AnalogTrigger*) analog_trigger_pointer; +void setAnalogTriggerFiltered(void* analog_trigger_pointer, + bool useFilteredValue, int32_t* status) { + AnalogTrigger* trigger = (AnalogTrigger*)analog_trigger_pointer; if (trigger->trigger->readSourceSelect_Averaged(status) != 0) { - *status = INCOMPATIBLE_STATE; - // TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not support average and filtering at the same time."); + *status = INCOMPATIBLE_STATE; + // TODO: wpi_setWPIErrorWithContext(IncompatibleMode, "Hardware does not " + // "support average and filtering at the same time."); } trigger->trigger->writeSourceSelect_Filter(useFilteredValue, status); } @@ -684,8 +719,8 @@ void setAnalogTriggerFiltered(void* analog_trigger_pointer, bool useFilteredValu * True if the analog input is between the upper and lower limits. * @return The InWindow output of the analog trigger. */ -bool getAnalogTriggerInWindow(void* analog_trigger_pointer, int32_t *status) { - AnalogTrigger* trigger = (AnalogTrigger*) analog_trigger_pointer; +bool getAnalogTriggerInWindow(void* analog_trigger_pointer, int32_t* status) { + AnalogTrigger* trigger = (AnalogTrigger*)analog_trigger_pointer; return trigger->trigger->readOutput_InHysteresis(trigger->index, status) != 0; } @@ -696,8 +731,9 @@ bool getAnalogTriggerInWindow(void* analog_trigger_pointer, int32_t *status) { * If in Hysteresis, maintain previous state. * @return The TriggerState output of the analog trigger. */ -bool getAnalogTriggerTriggerState(void* analog_trigger_pointer, int32_t *status) { - AnalogTrigger* trigger = (AnalogTrigger*) analog_trigger_pointer; +bool getAnalogTriggerTriggerState(void* analog_trigger_pointer, + int32_t* status) { + AnalogTrigger* trigger = (AnalogTrigger*)analog_trigger_pointer; return trigger->trigger->readOutput_OverLimit(trigger->index, status) != 0; } @@ -705,52 +741,56 @@ bool getAnalogTriggerTriggerState(void* analog_trigger_pointer, int32_t *status) * Get the state of the analog trigger output. * @return The state of the analog trigger output. */ -bool getAnalogTriggerOutput(void* analog_trigger_pointer, AnalogTriggerType type, int32_t *status) { - AnalogTrigger* trigger = (AnalogTrigger*) analog_trigger_pointer; +bool getAnalogTriggerOutput(void* analog_trigger_pointer, + AnalogTriggerType type, int32_t* status) { + AnalogTrigger* trigger = (AnalogTrigger*)analog_trigger_pointer; bool result = false; - switch(type) { - case kInWindow: - result = trigger->trigger->readOutput_InHysteresis(trigger->index, status); - break; // XXX: Backport - case kState: - result = trigger->trigger->readOutput_OverLimit(trigger->index, status); - break; // XXX: Backport - case kRisingPulse: - case kFallingPulse: - *status = ANALOG_TRIGGER_PULSE_OUTPUT_ERROR; - return false; + switch (type) { + case kInWindow: + result = + trigger->trigger->readOutput_InHysteresis(trigger->index, status); + break; // XXX: Backport + case kState: + result = trigger->trigger->readOutput_OverLimit(trigger->index, status); + break; // XXX: Backport + case kRisingPulse: + case kFallingPulse: + *status = ANALOG_TRIGGER_PULSE_OUTPUT_ERROR; + return false; } return result; } - - //// Float JNA Hack // Float -int getAnalogSampleRateIntHack(int32_t *status) { +int getAnalogSampleRateIntHack(int32_t* status) { return floatToInt(getAnalogSampleRate(status)); } -int getAnalogVoltageIntHack(void* analog_port_pointer, int32_t *status) { +int getAnalogVoltageIntHack(void* analog_port_pointer, int32_t* status) { return floatToInt(getAnalogVoltage(analog_port_pointer, status)); } -int getAnalogAverageVoltageIntHack(void* analog_port_pointer, int32_t *status) { +int getAnalogAverageVoltageIntHack(void* analog_port_pointer, int32_t* status) { return floatToInt(getAnalogAverageVoltage(analog_port_pointer, status)); } - // Doubles -void setAnalogSampleRateIntHack(int samplesPerSecond, int32_t *status) { +void setAnalogSampleRateIntHack(int samplesPerSecond, int32_t* status) { setAnalogSampleRate(intToFloat(samplesPerSecond), status); } -int32_t getAnalogVoltsToValueIntHack(void* analog_port_pointer, int voltage, int32_t *status) { - return getAnalogVoltsToValue(analog_port_pointer, intToFloat(voltage), status); +int32_t getAnalogVoltsToValueIntHack(void* analog_port_pointer, int voltage, + int32_t* status) { + return getAnalogVoltsToValue(analog_port_pointer, intToFloat(voltage), + status); } -void setAnalogTriggerLimitsVoltageIntHack(void* analog_trigger_pointer, int lower, int upper, int32_t *status) { - setAnalogTriggerLimitsVoltage(analog_trigger_pointer, intToFloat(lower), intToFloat(upper), status); +void setAnalogTriggerLimitsVoltageIntHack(void* analog_trigger_pointer, + int lower, int upper, + int32_t* status) { + setAnalogTriggerLimitsVoltage(analog_trigger_pointer, intToFloat(lower), + intToFloat(upper), status); } } // extern "C" diff --git a/hal/lib/Athena/ChipObject.h b/hal/lib/Athena/ChipObject.h index ac0fe71f91..cba5b0edaa 100644 --- a/hal/lib/Athena/ChipObject.h +++ b/hal/lib/Athena/ChipObject.h @@ -20,12 +20,12 @@ #include "FRC_FPGA_ChipObject/tSystemInterface.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/nInterfaceGlobals.h" +#include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAI.h" +#include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAO.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAccel.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAccumulator.h" -#include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAI.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAlarm.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAnalogTrigger.h" -#include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tAO.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tBIST.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tCounter.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tDIO.h" @@ -33,8 +33,8 @@ #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tEncoder.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tGlobal.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tInterrupt.h" -#include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tPower.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tPWM.h" +#include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tPower.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tRelay.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tSPI.h" #include "FRC_FPGA_ChipObject/nRoboRIO_FPGANamespace/tSysWatchdog.h" diff --git a/hal/lib/Athena/Compressor.cpp b/hal/lib/Athena/Compressor.cpp index 25986a4c9e..465a409f8b 100644 --- a/hal/lib/Athena/Compressor.cpp +++ b/hal/lib/Athena/Compressor.cpp @@ -6,122 +6,119 @@ /*----------------------------------------------------------------------------*/ #include "HAL/Compressor.hpp" -#include "ctre/PCM.h" #include +#include "ctre/PCM.h" static const int NUM_MODULE_NUMBERS = 63; -extern PCM *PCM_modules[NUM_MODULE_NUMBERS]; +extern PCM* PCM_modules[NUM_MODULE_NUMBERS]; extern void initializePCM(int module); extern "C" { -void *initializeCompressor(uint8_t module) { - initializePCM(module); - - return PCM_modules[module]; +void* initializeCompressor(uint8_t module) { + initializePCM(module); + + return PCM_modules[module]; } bool checkCompressorModule(uint8_t module) { - return module < NUM_MODULE_NUMBERS; + return module < NUM_MODULE_NUMBERS; } -bool getCompressor(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - bool value; - - *status = module->GetCompressor(value); - - return value; +bool getCompressor(void* pcm_pointer, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + bool value; + + *status = module->GetCompressor(value); + + return value; } +void setClosedLoopControl(void* pcm_pointer, bool value, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; -void setClosedLoopControl(void *pcm_pointer, bool value, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - - *status = module->SetClosedLoopControl(value); + *status = module->SetClosedLoopControl(value); } +bool getClosedLoopControl(void* pcm_pointer, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + bool value; -bool getClosedLoopControl(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - bool value; - - *status = module->GetClosedLoopControl(value); - - return value; + *status = module->GetClosedLoopControl(value); + + return value; } +bool getPressureSwitch(void* pcm_pointer, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + bool value; -bool getPressureSwitch(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - bool value; - - *status = module->GetPressure(value); - - return value; + *status = module->GetPressure(value); + + return value; } +float getCompressorCurrent(void* pcm_pointer, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + float value; -float getCompressorCurrent(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - float value; - - *status = module->GetCompressorCurrent(value); - - return value; + *status = module->GetCompressorCurrent(value); + + return value; } -bool getCompressorCurrentTooHighFault(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - bool value; - - *status = module->GetCompressorCurrentTooHighFault(value); - - return value; +bool getCompressorCurrentTooHighFault(void* pcm_pointer, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + bool value; + + *status = module->GetCompressorCurrentTooHighFault(value); + + return value; } -bool getCompressorCurrentTooHighStickyFault(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - bool value; - - *status = module->GetCompressorCurrentTooHighStickyFault(value); - - return value; +bool getCompressorCurrentTooHighStickyFault(void* pcm_pointer, + int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + bool value; + + *status = module->GetCompressorCurrentTooHighStickyFault(value); + + return value; } -bool getCompressorShortedStickyFault(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - bool value; - - *status = module->GetCompressorShortedStickyFault(value); - - return value; +bool getCompressorShortedStickyFault(void* pcm_pointer, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + bool value; + + *status = module->GetCompressorShortedStickyFault(value); + + return value; } -bool getCompressorShortedFault(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - bool value; - - *status = module->GetCompressorShortedFault(value); - - return value; +bool getCompressorShortedFault(void* pcm_pointer, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + bool value; + + *status = module->GetCompressorShortedFault(value); + + return value; } -bool getCompressorNotConnectedStickyFault(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - bool value; - - *status = module->GetCompressorNotConnectedStickyFault(value); - - return value; +bool getCompressorNotConnectedStickyFault(void* pcm_pointer, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + bool value; + + *status = module->GetCompressorNotConnectedStickyFault(value); + + return value; } -bool getCompressorNotConnectedFault(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - bool value; - - *status = module->GetCompressorNotConnectedFault(value); - - return value; +bool getCompressorNotConnectedFault(void* pcm_pointer, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + bool value; + + *status = module->GetCompressorNotConnectedFault(value); + + return value; } -void clearAllPCMStickyFaults(void *pcm_pointer, int32_t *status) { - PCM *module = (PCM *)pcm_pointer; - - *status = module->ClearStickyFaults(); +void clearAllPCMStickyFaults(void* pcm_pointer, int32_t* status) { + PCM* module = (PCM*)pcm_pointer; + + *status = module->ClearStickyFaults(); } } // extern "C" diff --git a/hal/lib/Athena/Digital.cpp b/hal/lib/Athena/Digital.cpp index 353933bbd8..ce9c13f8d8 100644 --- a/hal/lib/Athena/Digital.cpp +++ b/hal/lib/Athena/Digital.cpp @@ -7,41 +7,44 @@ #include "HAL/Digital.hpp" -#include "HAL/Port.h" -#include "HAL/HAL.hpp" +#include +#include +#include #include "ChipObject.h" +#include "FRC_NetworkCommunication/LoadOut.h" +#include "HAL/HAL.hpp" +#include "HAL/Port.h" #include "HAL/cpp/Resource.hpp" #include "HAL/cpp/priority_mutex.h" -#include "FRC_NetworkCommunication/LoadOut.h" -#include -#include -#include #include "i2clib/i2c-lib.h" #include "spilib/spi-lib.h" -static_assert(sizeof(uint32_t) <= sizeof(void *), "This file shoves uint32_ts into pointers."); +static_assert(sizeof(uint32_t) <= sizeof(void*), + "This file shoves uint32_ts into pointers."); static const uint32_t kExpectedLoopTiming = 40; static const uint32_t kDigitalPins = 26; static const uint32_t kPwmPins = 20; static const uint32_t kRelayPins = 8; -static const uint32_t kNumHeaders = 10; // Number of non-MXP pins +static const uint32_t kNumHeaders = 10; // Number of non-MXP pins /** * kDefaultPwmPeriod is in ms * - * - 20ms periods (50 Hz) are the "safest" setting in that this works for all devices + * - 20ms periods (50 Hz) are the "safest" setting in that this works for all + * devices * - 20ms periods seem to be desirable for Vex Motors - * - 20ms periods are the specified period for HS-322HD servos, but work reliably down - * to 10.0 ms; starting at about 8.5ms, the servo sometimes hums and get hot; - * by 5.0ms the hum is nearly continuous + * - 20ms periods are the specified period for HS-322HD servos, but work + * reliably down to 10.0 ms; starting at about 8.5ms, the servo sometimes hums + * and get hot; by 5.0ms the hum is nearly continuous * - 10ms periods work well for Victor 884 - * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed controllers. - * Due to the shipping firmware on the Jaguar, we can't run the update period less - * than 5.05 ms. + * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed + * controllers. Due to the shipping firmware on the Jaguar, we can't run the + * update period less than 5.05 ms. * - * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period scaling is implemented as an - * output squelch to get longer periods for old devices. + * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period + * scaling is implemented as an output squelch to get longer periods for old + * devices. */ static const float kDefaultPwmPeriod = 5.05; /** @@ -71,9 +74,9 @@ static priority_recursive_mutex digitalI2CMXPMutex; static tDIO* digitalSystem = NULL; static tRelay* relaySystem = NULL; static tPWM* pwmSystem = NULL; -static hal::Resource *DIOChannels = NULL; -static hal::Resource *DO_PWMGenerators = NULL; -static hal::Resource *PWMChannels = NULL; +static hal::Resource* DIOChannels = NULL; +static hal::Resource* DO_PWMGenerators = NULL; +static hal::Resource* PWMChannels = NULL; static bool digitalSystemsInitialized = false; @@ -89,44 +92,49 @@ static int32_t m_spiCS3Handle = 0; static int32_t m_spiMXPHandle = 0; static priority_recursive_mutex spiOnboardSemaphore; static priority_recursive_mutex spiMXPSemaphore; -static tSPI *spiSystem; +static tSPI* spiSystem; extern "C" { struct SPIAccumulator { - void* notifier = nullptr; - uint64_t triggerTime; - uint32_t period; + void* notifier = nullptr; + uint64_t triggerTime; + uint32_t period; - int64_t value = 0; - uint32_t count = 0; - int32_t last_value = 0; + int64_t value = 0; + uint32_t count = 0; + int32_t last_value = 0; - int32_t center = 0; - int32_t deadband = 0; + int32_t center = 0; + int32_t deadband = 0; - uint8_t cmd[4]; // command to send (up to 4 bytes) - uint32_t valid_mask; - uint32_t valid_value; - int32_t data_max; // one more than max data value - int32_t data_msb_mask; // data field MSB mask (for signed) - uint8_t data_shift; // data field shift right amount, in bits - uint8_t xfer_size; // SPI transfer size, in bytes (up to 4) - uint8_t port; - bool is_signed; // is data field signed? - bool big_endian; // is response big endian? + uint8_t cmd[4]; // command to send (up to 4 bytes) + uint32_t valid_mask; + uint32_t valid_value; + int32_t data_max; // one more than max data value + int32_t data_msb_mask; // data field MSB mask (for signed) + uint8_t data_shift; // data field shift right amount, in bits + uint8_t xfer_size; // SPI transfer size, in bytes (up to 4) + uint8_t port; + bool is_signed; // is data field signed? + bool big_endian; // is response big endian? }; -SPIAccumulator* spiAccumulators[5] = {nullptr, nullptr, nullptr, nullptr, nullptr}; +SPIAccumulator* spiAccumulators[5] = {nullptr, nullptr, nullptr, nullptr, + nullptr}; /** * Initialize the digital system. */ -void initializeDigital(int32_t *status) { +void initializeDigital(int32_t* status) { if (digitalSystemsInitialized) return; - hal::Resource::CreateResourceObject(&DIOChannels, tDIO::kNumSystems * kDigitalPins); - hal::Resource::CreateResourceObject(&DO_PWMGenerators, tDIO::kNumPWMDutyCycleAElements + tDIO::kNumPWMDutyCycleBElements); - hal::Resource::CreateResourceObject(&PWMChannels, tPWM::kNumSystems * kPwmPins); + hal::Resource::CreateResourceObject(&DIOChannels, + tDIO::kNumSystems * kDigitalPins); + hal::Resource::CreateResourceObject( + &DO_PWMGenerators, + tDIO::kNumPWMDutyCycleAElements + tDIO::kNumPWMDutyCycleBElements); + hal::Resource::CreateResourceObject(&PWMChannels, + tPWM::kNumSystems * kPwmPins); digitalSystem = tDIO::create(status); // Relay Setup @@ -139,22 +147,28 @@ void initializeDigital(int32_t *status) { // PWM Setup pwmSystem = tPWM::create(status); - // Make sure that the 9403 IONode has had a chance to initialize before continuing. - while(pwmSystem->readLoopTiming(status) == 0) delayTicks(1); + // Make sure that the 9403 IONode has had a chance to initialize before + // continuing. + while (pwmSystem->readLoopTiming(status) == 0) delayTicks(1); if (pwmSystem->readLoopTiming(status) != kExpectedLoopTiming) { - // TODO: char err[128]; - // TODO: sprintf(err, "DIO LoopTiming: %d, expecting: %lu\n", digitalModules[port->module-1]->readLoopTiming(status), kExpectedLoopTiming); - *status = LOOP_TIMING_ERROR; // NOTE: Doesn't display the error + // TODO: char err[128]; + // TODO: sprintf(err, "DIO LoopTiming: %d, expecting: %lu\n", + // digitalModules[port->module-1]->readLoopTiming(status), + // kExpectedLoopTiming); + *status = LOOP_TIMING_ERROR; // NOTE: Doesn't display the error } - //Calculate the length, in ms, of one DIO loop - double loopTime = pwmSystem->readLoopTiming(status)/(kSystemClockTicksPerMicrosecond*1e3); + // Calculate the length, in ms, of one DIO loop + double loopTime = pwmSystem->readLoopTiming(status) / + (kSystemClockTicksPerMicrosecond * 1e3); - pwmSystem->writeConfig_Period((uint16_t) (kDefaultPwmPeriod/loopTime + .5), status); - uint16_t minHigh = (uint16_t) ((kDefaultPwmCenter-kDefaultPwmStepsDown*loopTime)/loopTime + .5); + pwmSystem->writeConfig_Period((uint16_t)(kDefaultPwmPeriod / loopTime + .5), + status); + uint16_t minHigh = (uint16_t)( + (kDefaultPwmCenter - kDefaultPwmStepsDown * loopTime) / loopTime + .5); pwmSystem->writeConfig_MinHigh(minHigh, status); -// printf("MinHigh: %d\n", minHigh); + // printf("MinHigh: %d\n", minHigh); // Ensure that PWM output values are set to OFF for (uint32_t pwm_index = 0; pwm_index < kPwmPins; pwm_index++) { // Initialize port structure @@ -162,7 +176,7 @@ void initializeDigital(int32_t *status) { digital_port.port.pin = pwm_index; setPWM(&digital_port, kPwmDisabled, status); - setPWMPeriodScale(&digital_port, 3, status); // Set all to 4x by default. + setPWMPeriodScale(&digital_port, 3, status); // Set all to 4x by default. } digitalSystemsInitialized = true; @@ -171,9 +185,9 @@ void initializeDigital(int32_t *status) { /** * Create a new instance of a digital port. */ -void* initializeDigitalPort(void* port_pointer, int32_t *status) { +void* initializeDigitalPort(void* port_pointer, int32_t* status) { initializeDigital(status); - Port* port = (Port*) port_pointer; + Port* port = (Port*)port_pointer; // Initialize port structure DigitalPort* digital_port = new DigitalPort(); @@ -183,17 +197,17 @@ void* initializeDigitalPort(void* port_pointer, int32_t *status) { } void freeDigitalPort(void* digital_port_pointer) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; + DigitalPort* port = (DigitalPort*)digital_port_pointer; delete port; } bool checkPWMChannel(void* digital_port_pointer) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; + DigitalPort* port = (DigitalPort*)digital_port_pointer; return port->port.pin < kPwmPins; } bool checkRelayChannel(void* digital_port_pointer) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; + DigitalPort* port = (DigitalPort*)digital_port_pointer; return port->port.pin < kRelayPins; } @@ -204,7 +218,7 @@ bool checkRelayChannel(void* digital_port_pointer) { * * @return true if the port passed validation. */ -static bool verifyPWMChannel(DigitalPort *port, int32_t *status) { +static bool verifyPWMChannel(DigitalPort* port, int32_t* status) { if (port == NULL) { *status = NULL_PARAMETER; return false; @@ -223,7 +237,7 @@ static bool verifyPWMChannel(DigitalPort *port, int32_t *status) { * * @return true if the port passed validation. */ -static bool verifyRelayChannel(DigitalPort *port, int32_t *status) { +static bool verifyRelayChannel(DigitalPort* port, int32_t* status) { if (port == NULL) { *status = NULL_PARAMETER; return false; @@ -239,30 +253,31 @@ static bool verifyRelayChannel(DigitalPort *port, int32_t *status) { * Map DIO pin numbers from their physical number (10 to 26) to their position * in the bit field. */ -uint32_t remapMXPChannel(uint32_t pin) { - return pin - 10; -} +uint32_t remapMXPChannel(uint32_t pin) { return pin - 10; } uint32_t remapMXPPWMChannel(uint32_t pin) { - if(pin < 14) { - return pin - 10; //first block of 4 pwms (MXP 0-3) - } else { - return pin - 6; //block of PWMs after SPI - } + if (pin < 14) { + return pin - 10; // first block of 4 pwms (MXP 0-3) + } else { + return pin - 6; // block of PWMs after SPI + } } /** - * Set a PWM channel to the desired value. The values range from 0 to 255 and the period is controlled + * Set a PWM channel to the desired value. The values range from 0 to 255 and + * the period is controlled * by the PWM Period and MinHigh registers. * * @param channel The PWM channel to set. * @param value The PWM value to set. */ -void setPWM(void* digital_port_pointer, unsigned short value, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; - if (!verifyPWMChannel(port, status)) { return; } +void setPWM(void* digital_port_pointer, unsigned short value, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyPWMChannel(port, status)) { + return; + } - if(port->port.pin < tPWM::kNumHdrRegisters) { + if (port->port.pin < tPWM::kNumHdrRegisters) { pwmSystem->writeHdr(port->port.pin, value, status); } else { pwmSystem->writeMXP(port->port.pin - tPWM::kNumHdrRegisters, value, status); @@ -275,23 +290,27 @@ void setPWM(void* digital_port_pointer, unsigned short value, int32_t *status) { * @param channel The PWM channel to read from. * @return The raw PWM value. */ -unsigned short getPWM(void* digital_port_pointer, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; - if (!verifyPWMChannel(port, status)) { return 0; } +unsigned short getPWM(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyPWMChannel(port, status)) { + return 0; + } - if(port->port.pin < tPWM::kNumHdrRegisters) { + if (port->port.pin < tPWM::kNumHdrRegisters) { return pwmSystem->readHdr(port->port.pin, status); } else { return pwmSystem->readMXP(port->port.pin - tPWM::kNumHdrRegisters, status); } } -void latchPWMZero(void* digital_port_pointer, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; - if (!verifyPWMChannel(port, status)) { return; } +void latchPWMZero(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyPWMChannel(port, status)) { + return; + } - pwmSystem->writeZeroLatch(port->port.pin, true, status); - pwmSystem->writeZeroLatch(port->port.pin, false, status); + pwmSystem->writeZeroLatch(port->port.pin, true, status); + pwmSystem->writeZeroLatch(port->port.pin, false, status); } /** @@ -300,14 +319,18 @@ void latchPWMZero(void* digital_port_pointer, int32_t *status) { * @param channel The PWM channel to configure. * @param squelchMask The 2-bit mask of outputs to squelch. */ -void setPWMPeriodScale(void* digital_port_pointer, uint32_t squelchMask, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; - if (!verifyPWMChannel(port, status)) { return; } +void setPWMPeriodScale(void* digital_port_pointer, uint32_t squelchMask, + int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyPWMChannel(port, status)) { + return; + } - if(port->port.pin < tPWM::kNumPeriodScaleHdrElements) { + if (port->port.pin < tPWM::kNumPeriodScaleHdrElements) { pwmSystem->writePeriodScaleHdr(port->port.pin, squelchMask, status); } else { - pwmSystem->writePeriodScaleMXP(port->port.pin - tPWM::kNumPeriodScaleHdrElements, squelchMask, status); + pwmSystem->writePeriodScaleMXP( + port->port.pin - tPWM::kNumPeriodScaleHdrElements, squelchMask, status); } } @@ -317,17 +340,18 @@ void setPWMPeriodScale(void* digital_port_pointer, uint32_t squelchMask, int32_t * * @return PWM Generator refnum */ -void* allocatePWM(int32_t *status) { +void* allocatePWM(int32_t* status) { return (void*)DO_PWMGenerators->Allocate("DO_PWM"); } /** * Free the resource associated with a DO PWM generator. * - * @param pwmGenerator The pwmGen to free that was allocated with AllocateDO_PWM() + * @param pwmGenerator The pwmGen to free that was allocated with + * AllocateDO_PWM() */ -void freePWM(void* pwmGenerator, int32_t *status) { - uint32_t id = (uint32_t) pwmGenerator; +void freePWM(void* pwmGenerator, int32_t* status) { + uint32_t id = (uint32_t)pwmGenerator; if (id == ~0ul) return; DO_PWMGenerators->Free(id); } @@ -335,42 +359,47 @@ void freePWM(void* pwmGenerator, int32_t *status) { /** * Change the frequency of the DO PWM generator. * - * The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is logarithmic. + * The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is + * logarithmic. * * @param rate The frequency to output all digital output PWM signals. */ -void setPWMRate(double rate, int32_t *status) { - // Currently rounding in the log rate domain... heavy weight toward picking a higher freq. +void setPWMRate(double rate, int32_t* status) { + // Currently rounding in the log rate domain... heavy weight toward picking a + // higher freq. // TODO: Round in the linear rate domain. - uint8_t pwmPeriodPower = (uint8_t)(log(1.0 / (pwmSystem->readLoopTiming(status) * 0.25E-6 * rate))/log(2.0) + 0.5); + uint8_t pwmPeriodPower = (uint8_t)( + log(1.0 / (pwmSystem->readLoopTiming(status) * 0.25E-6 * rate)) / + log(2.0) + + 0.5); digitalSystem->writePWMPeriodPower(pwmPeriodPower, status); } - /** * Configure the duty-cycle of the PWM generator * * @param pwmGenerator The generator index reserved by AllocateDO_PWM() * @param dutyCycle The percent duty cycle to output [0..1]. */ -void setPWMDutyCycle(void* pwmGenerator, double dutyCycle, int32_t *status) { - uint32_t id = (uint32_t) pwmGenerator; +void setPWMDutyCycle(void* pwmGenerator, double dutyCycle, int32_t* status) { + uint32_t id = (uint32_t)pwmGenerator; if (id == ~0ul) return; if (dutyCycle > 1.0) dutyCycle = 1.0; if (dutyCycle < 0.0) dutyCycle = 0.0; float rawDutyCycle = 256.0 * dutyCycle; if (rawDutyCycle > 255.5) rawDutyCycle = 255.5; { - std::lock_guard sync(digitalPwmMutex); + std::lock_guard sync(digitalPwmMutex); uint8_t pwmPeriodPower = digitalSystem->readPWMPeriodPower(status); if (pwmPeriodPower < 4) { - // The resolution of the duty cycle drops close to the highest frequencies. - rawDutyCycle = rawDutyCycle / pow(2.0, 4 - pwmPeriodPower); - } - if(id < 4) - digitalSystem->writePWMDutyCycleA(id, (uint8_t)rawDutyCycle, status); - else - digitalSystem->writePWMDutyCycleB(id - 4, (uint8_t)rawDutyCycle, status); + // The resolution of the duty cycle drops close to the highest + // frequencies. + rawDutyCycle = rawDutyCycle / pow(2.0, 4 - pwmPeriodPower); + } + if (id < 4) + digitalSystem->writePWMDutyCycleA(id, (uint8_t)rawDutyCycle, status); + else + digitalSystem->writePWMDutyCycleB(id - 4, (uint8_t)rawDutyCycle, status); } } @@ -380,19 +409,20 @@ void setPWMDutyCycle(void* pwmGenerator, double dutyCycle, int32_t *status) { * @param pwmGenerator The generator index reserved by AllocateDO_PWM() * @param channel The Digital Output channel to output on */ -void setPWMOutputChannel(void* pwmGenerator, uint32_t pin, int32_t *status) { - uint32_t id = (uint32_t) pwmGenerator; +void setPWMOutputChannel(void* pwmGenerator, uint32_t pin, int32_t* status) { + uint32_t id = (uint32_t)pwmGenerator; if (id > 5) return; digitalSystem->writePWMOutputSelect(id, pin, status); } /** * Set the state of a relay. - * Set the state of a relay output to be forward. Relays have two outputs and each is + * Set the state of a relay output to be forward. Relays have two outputs and + * each is * independently set to 0v or 12v. */ -void setRelayForward(void* digital_port_pointer, bool on, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; +void setRelayForward(void* digital_port_pointer, bool on, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; if (!verifyRelayChannel(port, status)) { return; } @@ -410,11 +440,12 @@ void setRelayForward(void* digital_port_pointer, bool on, int32_t *status) { /** * Set the state of a relay. - * Set the state of a relay output to be reverse. Relays have two outputs and each is + * Set the state of a relay output to be reverse. Relays have two outputs and + * each is * independently set to 0v or 12v. */ -void setRelayReverse(void* digital_port_pointer, bool on, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; +void setRelayReverse(void* digital_port_pointer, bool on, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; if (!verifyRelayChannel(port, status)) { return; } @@ -433,9 +464,11 @@ void setRelayReverse(void* digital_port_pointer, bool on, int32_t *status) { /** * Get the current state of the forward relay channel */ -bool getRelayForward(void* digital_port_pointer, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; - if (!verifyRelayChannel(port, status)) { return false; } +bool getRelayForward(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyRelayChannel(port, status)) { + return false; + } uint8_t forwardRelays = relaySystem->readValue_Forward(status); return (forwardRelays & (1 << port->port.pin)) != 0; @@ -444,9 +477,11 @@ bool getRelayForward(void* digital_port_pointer, int32_t *status) { /** * Get the current state of the reverse relay channel */ -bool getRelayReverse(void* digital_port_pointer, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; - if (!verifyRelayChannel(port, status)) { return false; } +bool getRelayReverse(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyRelayChannel(port, status)) { + return false; + } uint8_t reverseRelays = relaySystem->readValue_Reverse(status); return (reverseRelays & (1 << port->port.pin)) != 0; @@ -454,15 +489,15 @@ bool getRelayReverse(void* digital_port_pointer, int32_t *status) { /** * Allocate Digital I/O channels. - * Allocate channels so that they are not accidently reused. Also the direction is set at the - * time of the allocation. + * Allocate channels so that they are not accidently reused. Also the direction + * is set at the time of the allocation. * * @param channel The Digital I/O channel * @param input If true open as input; if false open as output * @return Was successfully allocated */ -bool allocateDIO(void* digital_port_pointer, bool input, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; +bool allocateDIO(void* digital_port_pointer, bool input, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; char buf[64]; snprintf(buf, 64, "DIO %d", port->port.pin); if (DIOChannels->Allocate(port->port.pin, buf) == ~0ul) { @@ -475,24 +510,30 @@ bool allocateDIO(void* digital_port_pointer, bool input, int32_t *status) { tDIO::tOutputEnable outputEnable = digitalSystem->readOutputEnable(status); - if(port->port.pin < kNumHeaders) { + if (port->port.pin < kNumHeaders) { uint32_t bitToSet = 1 << port->port.pin; if (input) { - outputEnable.Headers = outputEnable.Headers & (~bitToSet); // clear the bit for read + outputEnable.Headers = + outputEnable.Headers & (~bitToSet); // clear the bit for read } else { - outputEnable.Headers = outputEnable.Headers | bitToSet; // set the bit for write + outputEnable.Headers = + outputEnable.Headers | bitToSet; // set the bit for write } } else { uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); // Disable special functions on this pin - short specialFunctions = digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, status); + short specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, + status); if (input) { - outputEnable.MXP = outputEnable.MXP & (~bitToSet); // clear the bit for read + outputEnable.MXP = + outputEnable.MXP & (~bitToSet); // clear the bit for read } else { - outputEnable.MXP = outputEnable.MXP | bitToSet; // set the bit for write + outputEnable.MXP = + outputEnable.MXP | bitToSet; // set the bit for write } } @@ -501,39 +542,50 @@ bool allocateDIO(void* digital_port_pointer, bool input, int32_t *status) { return true; } -bool allocatePWMChannel(void* digital_port_pointer, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; - if (!verifyPWMChannel(port, status)) { return false; } +bool allocatePWMChannel(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!verifyPWMChannel(port, status)) { + return false; + } - char buf[64]; - snprintf(buf, 64, "PWM %d", port->port.pin); - if (PWMChannels->Allocate(port->port.pin, buf) == ~0ul) { - *status = RESOURCE_IS_ALLOCATED; - return false; - } + char buf[64]; + snprintf(buf, 64, "PWM %d", port->port.pin); + if (PWMChannels->Allocate(port->port.pin, buf) == ~0ul) { + *status = RESOURCE_IS_ALLOCATED; + return false; + } - if (port->port.pin > tPWM::kNumHdrRegisters-1) { - snprintf(buf, 64, "PWM %d and DIO %d", port->port.pin, remapMXPPWMChannel(port->port.pin) + 10); - if (DIOChannels->Allocate(remapMXPPWMChannel(port->port.pin) + 10, buf) == ~0ul) return false; - uint32_t bitToSet = 1 << remapMXPPWMChannel(port->port.pin); - short specialFunctions = digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToSet, status); - } - return true; + if (port->port.pin > tPWM::kNumHdrRegisters - 1) { + snprintf(buf, 64, "PWM %d and DIO %d", port->port.pin, + remapMXPPWMChannel(port->port.pin) + 10); + if (DIOChannels->Allocate(remapMXPPWMChannel(port->port.pin) + 10, buf) == + ~0ul) + return false; + uint32_t bitToSet = 1 << remapMXPPWMChannel(port->port.pin); + short specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToSet, + status); + } + return true; } -void freePWMChannel(void* digital_port_pointer, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; - if (!port) return; - if (!verifyPWMChannel(port, status)) { return; } +void freePWMChannel(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + if (!port) return; + if (!verifyPWMChannel(port, status)) { + return; + } - PWMChannels->Free(port->port.pin); - if(port->port.pin > tPWM::kNumHdrRegisters-1) { - DIOChannels->Free(remapMXPPWMChannel(port->port.pin) + 10); - uint32_t bitToUnset = 1 << remapMXPPWMChannel(port->port.pin); - short specialFunctions = digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToUnset, status); - } + PWMChannels->Free(port->port.pin); + if (port->port.pin > tPWM::kNumHdrRegisters - 1) { + DIOChannels->Free(remapMXPPWMChannel(port->port.pin) + 10); + uint32_t bitToUnset = 1 << remapMXPPWMChannel(port->port.pin); + short specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToUnset, + status); + } } /** @@ -541,8 +593,8 @@ void freePWMChannel(void* digital_port_pointer, int32_t *status) { * * @param channel The Digital I/O channel to free */ -void freeDIO(void* digital_port_pointer, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; +void freeDIO(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; if (!port) return; DIOChannels->Free(port->port.pin); } @@ -552,34 +604,38 @@ void freeDIO(void* digital_port_pointer, int32_t *status) { * Set a single value on a digital I/O channel. * * @param channel The Digital I/O channel - * @param value The state to set the digital channel (if it is configured as an output) + * @param value The state to set the digital channel (if it is configured as an + * output) */ -void setDIO(void* digital_port_pointer, short value, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; +void setDIO(void* digital_port_pointer, short value, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; if (value != 0 && value != 1) { - if (value != 0) - value = 1; + if (value != 0) value = 1; } { std::lock_guard sync(digitalDIOMutex); tDIO::tDO currentDIO = digitalSystem->readDO(status); - if(port->port.pin < kNumHeaders) { - if(value == 0) { + if (port->port.pin < kNumHeaders) { + if (value == 0) { currentDIO.Headers = currentDIO.Headers & ~(1 << port->port.pin); } else if (value == 1) { currentDIO.Headers = currentDIO.Headers | (1 << port->port.pin); } } else { - if(value == 0) { - currentDIO.MXP = currentDIO.MXP & ~(1 << remapMXPChannel(port->port.pin)); + if (value == 0) { + currentDIO.MXP = + currentDIO.MXP & ~(1 << remapMXPChannel(port->port.pin)); } else if (value == 1) { - currentDIO.MXP = currentDIO.MXP | (1 << remapMXPChannel(port->port.pin)); + currentDIO.MXP = + currentDIO.MXP | (1 << remapMXPChannel(port->port.pin)); } uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); - short specialFunctions = digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, status); + short specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, + status); } digitalSystem->writeDO(currentDIO, status); } @@ -592,21 +648,23 @@ void setDIO(void* digital_port_pointer, short value, int32_t *status) { * @param channel The digital I/O channel * @return The state of the specified channel */ -bool getDIO(void* digital_port_pointer, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; +bool getDIO(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; tDIO::tDI currentDIO = digitalSystem->readDI(status); - //Shift 00000001 over channel-1 places. - //AND it against the currentDIO - //if it == 0, then return false - //else return true + // Shift 00000001 over channel-1 places. + // AND it against the currentDIO + // if it == 0, then return false + // else return true - if(port->port.pin < kNumHeaders) { + if (port->port.pin < kNumHeaders) { return ((currentDIO.Headers >> port->port.pin) & 1) != 0; } else { // Disable special functions uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); - short specialFunctions = digitalSystem->readEnableMXPSpecialFunction(status); - digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, status); + short specialFunctions = + digitalSystem->readEnableMXPSpecialFunction(status); + digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, + status); return ((currentDIO.MXP >> remapMXPChannel(port->port.pin)) & 1) != 0; } @@ -619,39 +677,44 @@ bool getDIO(void* digital_port_pointer, int32_t *status) { * @param channel The digital I/O channel * @return The direction of the specified channel */ -bool getDIODirection(void* digital_port_pointer, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; - tDIO::tOutputEnable currentOutputEnable = digitalSystem->readOutputEnable(status); - //Shift 00000001 over port->port.pin-1 places. - //AND it against the currentOutputEnable - //if it == 0, then return false - //else return true +bool getDIODirection(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; + tDIO::tOutputEnable currentOutputEnable = + digitalSystem->readOutputEnable(status); + // Shift 00000001 over port->port.pin-1 places. + // AND it against the currentOutputEnable + // if it == 0, then return false + // else return true - if(port->port.pin < kNumHeaders) { + if (port->port.pin < kNumHeaders) { return ((currentOutputEnable.Headers >> port->port.pin) & 1) != 0; } else { - return ((currentOutputEnable.MXP >> remapMXPChannel(port->port.pin)) & 1) != 0; + return ((currentOutputEnable.MXP >> remapMXPChannel(port->port.pin)) & 1) != + 0; } } /** * Generate a single pulse. - * Write a pulse to the specified digital output channel. There can only be a single pulse going at any time. + * Write a pulse to the specified digital output channel. There can only be a + * single pulse going at any time. * * @param channel The Digital Output channel that the pulse should be output on * @param pulseLength The active length of the pulse (in seconds) */ -void pulse(void* digital_port_pointer, double pulseLength, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; +void pulse(void* digital_port_pointer, double pulseLength, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; tDIO::tPulse pulse; - if(port->port.pin < kNumHeaders) { + if (port->port.pin < kNumHeaders) { pulse.Headers = 1 << port->port.pin; } else { pulse.MXP = 1 << remapMXPChannel(port->port.pin); } - digitalSystem->writePulseLength((uint8_t)(1.0e9 * pulseLength / (pwmSystem->readLoopTiming(status) * 25)), status); + digitalSystem->writePulseLength( + (uint8_t)(1.0e9 * pulseLength / (pwmSystem->readLoopTiming(status) * 25)), + status); digitalSystem->writePulse(pulse, status); } @@ -660,11 +723,11 @@ void pulse(void* digital_port_pointer, double pulseLength, int32_t *status) { * * @return A pulse is in progress */ -bool isPulsing(void* digital_port_pointer, int32_t *status) { - DigitalPort* port = (DigitalPort*) digital_port_pointer; +bool isPulsing(void* digital_port_pointer, int32_t* status) { + DigitalPort* port = (DigitalPort*)digital_port_pointer; tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); - if(port->port.pin < kNumHeaders) { + if (port->port.pin < kNumHeaders) { return (pulseRegister.Headers & (1 << port->port.pin)) != 0; } else { return (pulseRegister.MXP & (1 << remapMXPChannel(port->port.pin))) != 0; @@ -676,7 +739,7 @@ bool isPulsing(void* digital_port_pointer, int32_t *status) { * * @return A pulse on some line is in progress */ -bool isAnyPulsing(int32_t *status) { +bool isAnyPulsing(int32_t* status) { tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); return pulseRegister.Headers != 0 && pulseRegister.MXP != 0; } @@ -696,8 +759,7 @@ void setFilterSelect(void* digital_port_pointer, int filter_index, std::lock_guard sync(digitalDIOMutex); if (port->port.pin < kNumHeaders) { digitalSystem->writeFilterSelectHdr(port->port.pin, filter_index, status); - } - else { + } else { digitalSystem->writeFilterSelectMXP(remapMXPChannel(port->port.pin), filter_index, status); } @@ -717,8 +779,7 @@ int getFilterSelect(void* digital_port_pointer, int32_t* status) { std::lock_guard sync(digitalDIOMutex); if (port->port.pin < kNumHeaders) { return digitalSystem->readFilterSelectHdr(port->port.pin, status); - } - else { + } else { return digitalSystem->readFilterSelectMXP(remapMXPChannel(port->port.pin), status); } @@ -778,45 +839,47 @@ struct counter_t { }; typedef struct counter_t Counter; -static hal::Resource *counters = NULL; +static hal::Resource* counters = NULL; -void* initializeCounter(Mode mode, uint32_t *index, int32_t *status) { - hal::Resource::CreateResourceObject(&counters, tCounter::kNumSystems); - *index = counters->Allocate("Counter"); - if (*index == ~0ul) { - *status = NO_AVAILABLE_RESOURCES; - return NULL; - } - Counter* counter = new Counter(); - counter->counter = tCounter::create(*index, status); - counter->counter->writeConfig_Mode(mode, status); - counter->counter->writeTimerConfig_AverageSize(1, status); - counter->index = *index; - return counter; +void* initializeCounter(Mode mode, uint32_t* index, int32_t* status) { + hal::Resource::CreateResourceObject(&counters, tCounter::kNumSystems); + *index = counters->Allocate("Counter"); + if (*index == ~0ul) { + *status = NO_AVAILABLE_RESOURCES; + return NULL; + } + Counter* counter = new Counter(); + counter->counter = tCounter::create(*index, status); + counter->counter->writeConfig_Mode(mode, status); + counter->counter->writeTimerConfig_AverageSize(1, status); + counter->index = *index; + return counter; } -void freeCounter(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; - if (!counter) return; - delete counter->counter; - counters->Free(counter->index); +void freeCounter(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + if (!counter) return; + delete counter->counter; + counters->Free(counter->index); } -void setCounterAverageSize(void* counter_pointer, int32_t size, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterAverageSize(void* counter_pointer, int32_t size, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->writeTimerConfig_AverageSize(size, status); } /** * remap the digital source pin and set the module. - * If it's an analog trigger, determine the module from the high order routing channel - * else do normal digital input remapping based on pin number (MXP) + * If it's an analog trigger, determine the module from the high order routing + * channel else do normal digital input remapping based on pin number (MXP) */ -extern "C++" void remapDigitalSource(bool analogTrigger, uint32_t &pin, uint8_t &module) { +extern "C++" void remapDigitalSource(bool analogTrigger, uint32_t& pin, + uint8_t& module) { if (analogTrigger) { module = pin >> 4; } else { - if(pin >= kNumHeaders) { + if (pin >= kNumHeaders) { pin = remapMXPChannel(pin); module = 1; } else { @@ -829,8 +892,9 @@ extern "C++" void remapDigitalSource(bool analogTrigger, uint32_t &pin, uint8_t * Set the source object that causes the counter to count up. * Set the up counting DigitalSource. */ -void setCounterUpSource(void* counter_pointer, uint32_t pin, bool analogTrigger, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterUpSource(void* counter_pointer, uint32_t pin, bool analogTrigger, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; uint8_t module; @@ -840,9 +904,9 @@ void setCounterUpSource(void* counter_pointer, uint32_t pin, bool analogTrigger, counter->counter->writeConfig_UpSource_Channel(pin, status); counter->counter->writeConfig_UpSource_AnalogTrigger(analogTrigger, status); - if(counter->counter->readConfig_Mode(status) == kTwoPulse || - counter->counter->readConfig_Mode(status) == kExternalDirection) { - setCounterUpSourceEdge(counter_pointer, true, false, status); + if (counter->counter->readConfig_Mode(status) == kTwoPulse || + counter->counter->readConfig_Mode(status) == kExternalDirection) { + setCounterUpSourceEdge(counter_pointer, true, false, status); } counter->counter->strobeReset(status); } @@ -851,8 +915,9 @@ void setCounterUpSource(void* counter_pointer, uint32_t pin, bool analogTrigger, * Set the edge sensitivity on an up counting source. * Set the up source to either detect rising edges or falling edges. */ -void setCounterUpSourceEdge(void* counter_pointer, bool risingEdge, bool fallingEdge, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterUpSourceEdge(void* counter_pointer, bool risingEdge, + bool fallingEdge, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->writeConfig_UpRisingEdge(risingEdge, status); counter->counter->writeConfig_UpFallingEdge(fallingEdge, status); } @@ -860,8 +925,8 @@ void setCounterUpSourceEdge(void* counter_pointer, bool risingEdge, bool falling /** * Disable the up counting source to the counter. */ -void clearCounterUpSource(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void clearCounterUpSource(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->writeConfig_UpFallingEdge(false, status); counter->counter->writeConfig_UpRisingEdge(false, status); // Index 0 of digital is always 0. @@ -873,13 +938,15 @@ void clearCounterUpSource(void* counter_pointer, int32_t *status) { * Set the source object that causes the counter to count down. * Set the down counting DigitalSource. */ -void setCounterDownSource(void* counter_pointer, uint32_t pin, bool analogTrigger, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterDownSource(void* counter_pointer, uint32_t pin, + bool analogTrigger, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; unsigned char mode = counter->counter->readConfig_Mode(status); if (mode != kTwoPulse && mode != kExternalDirection) { - // TODO: wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only supports DownSource in TwoPulse and ExternalDirection modes."); - *status = PARAMETER_OUT_OF_RANGE; - return; + // TODO: wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only + // supports DownSource in TwoPulse and ExternalDirection modes."); + *status = PARAMETER_OUT_OF_RANGE; + return; } uint8_t module; @@ -898,8 +965,9 @@ void setCounterDownSource(void* counter_pointer, uint32_t pin, bool analogTrigge * Set the edge sensitivity on a down counting source. * Set the down source to either detect rising edges or falling edges. */ -void setCounterDownSourceEdge(void* counter_pointer, bool risingEdge, bool fallingEdge, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterDownSourceEdge(void* counter_pointer, bool risingEdge, + bool fallingEdge, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->writeConfig_DownRisingEdge(risingEdge, status); counter->counter->writeConfig_DownFallingEdge(fallingEdge, status); } @@ -907,8 +975,8 @@ void setCounterDownSourceEdge(void* counter_pointer, bool risingEdge, bool falli /** * Disable the down counting source to the counter. */ -void clearCounterDownSource(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void clearCounterDownSource(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->writeConfig_DownFallingEdge(false, status); counter->counter->writeConfig_DownRisingEdge(false, status); // Index 0 of digital is always 0. @@ -920,8 +988,8 @@ void clearCounterDownSource(void* counter_pointer, int32_t *status) { * Set standard up / down counting mode on this counter. * Up and down counts are sourced independently from two inputs. */ -void setCounterUpDownMode(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterUpDownMode(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->writeConfig_Mode(kTwoPulse, status); } @@ -930,8 +998,8 @@ void setCounterUpDownMode(void* counter_pointer, int32_t *status) { * Counts are sourced on the Up counter input. * The Down counter input represents the direction to count. */ -void setCounterExternalDirectionMode(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterExternalDirectionMode(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->writeConfig_Mode(kExternalDirection, status); } @@ -939,130 +1007,143 @@ void setCounterExternalDirectionMode(void* counter_pointer, int32_t *status) { * Set Semi-period mode on this counter. * Counts up on both rising and falling edges. */ -void setCounterSemiPeriodMode(void* counter_pointer, bool highSemiPeriod, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterSemiPeriodMode(void* counter_pointer, bool highSemiPeriod, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->writeConfig_Mode(kSemiperiod, status); counter->counter->writeConfig_UpRisingEdge(highSemiPeriod, status); setCounterUpdateWhenEmpty(counter_pointer, false, status); } /** - * Configure the counter to count in up or down based on the length of the input pulse. + * Configure the counter to count in up or down based on the length of the input + * pulse. * This mode is most useful for direction sensitive gear tooth sensors. - * @param threshold The pulse length beyond which the counter counts the opposite direction. Units are seconds. + * @param threshold The pulse length beyond which the counter counts the + * opposite direction. Units are seconds. */ -void setCounterPulseLengthMode(void* counter_pointer, double threshold, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterPulseLengthMode(void* counter_pointer, double threshold, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->writeConfig_Mode(kPulseLength, status); - counter->counter->writeConfig_PulseLengthThreshold((uint32_t)(threshold * 1.0e6) * kSystemClockTicksPerMicrosecond, status); + counter->counter->writeConfig_PulseLengthThreshold( + (uint32_t)(threshold * 1.0e6) * kSystemClockTicksPerMicrosecond, status); } /** - * Get the Samples to Average which specifies the number of samples of the timer to + * Get the Samples to Average which specifies the number of samples of the timer + * to * average when calculating the period. Perform averaging to account for * mechanical imperfections or as oversampling to increase resolution. * @return SamplesToAverage The number of samples being averaged (from 1 to 127) */ -int32_t getCounterSamplesToAverage(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +int32_t getCounterSamplesToAverage(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; return counter->counter->readTimerConfig_AverageSize(status); } /** - * Set the Samples to Average which specifies the number of samples of the timer to - * average when calculating the period. Perform averaging to account for + * Set the Samples to Average which specifies the number of samples of the timer + * to average when calculating the period. Perform averaging to account for * mechanical imperfections or as oversampling to increase resolution. * @param samplesToAverage The number of samples to average from 1 to 127. */ -void setCounterSamplesToAverage(void* counter_pointer, int samplesToAverage, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterSamplesToAverage(void* counter_pointer, int samplesToAverage, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; if (samplesToAverage < 1 || samplesToAverage > 127) { - *status = PARAMETER_OUT_OF_RANGE; + *status = PARAMETER_OUT_OF_RANGE; } counter->counter->writeTimerConfig_AverageSize(samplesToAverage, status); } /** * Reset the Counter to zero. - * Set the counter value to zero. This doesn't effect the running state of the counter, just sets - * the current value to zero. + * Set the counter value to zero. This doesn't effect the running state of the + * counter, just sets the current value to zero. */ -void resetCounter(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void resetCounter(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->strobeReset(status); } /** * Read the current counter value. - * Read the value at this instant. It may still be running, so it reflects the current value. Next - * time it is read, it might have a different value. + * Read the value at this instant. It may still be running, so it reflects the + * current value. Next time it is read, it might have a different value. */ -int32_t getCounter(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +int32_t getCounter(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; int32_t value = counter->counter->readOutput_Value(status); return value; } /* * Get the Period of the most recent count. - * Returns the time interval of the most recent count. This can be used for velocity calculations - * to determine shaft speed. + * Returns the time interval of the most recent count. This can be used for + * velocity calculations to determine shaft speed. * @returns The period of the last two pulses in units of seconds. */ -double getCounterPeriod(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +double getCounterPeriod(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; tCounter::tTimerOutput output = counter->counter->readTimerOutput(status); double period; - if (output.Stalled) { - // Return infinity - double zero = 0.0; - period = 1.0 / zero; + if (output.Stalled) { + // Return infinity + double zero = 0.0; + period = 1.0 / zero; } else { - // output.Period is a fixed point number that counts by 2 (24 bits, 25 integer bits) - period = (double)(output.Period << 1) / (double)output.Count; + // output.Period is a fixed point number that counts by 2 (24 bits, 25 + // integer bits) + period = (double)(output.Period << 1) / (double)output.Count; } return period * 2.5e-8; // result * timebase (currently 40ns) } /** * Set the maximum period where the device is still considered "moving". - * Sets the maximum period where the device is considered moving. This value is used to determine - * the "stopped" state of the counter using the GetStopped method. - * @param maxPeriod The maximum period where the counted device is considered moving in - * seconds. + * Sets the maximum period where the device is considered moving. This value is + * used to determine the "stopped" state of the counter using the GetStopped + * method. + * @param maxPeriod The maximum period where the counted device is considered + * moving in seconds. */ -void setCounterMaxPeriod(void* counter_pointer, double maxPeriod, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; - counter->counter->writeTimerConfig_StallPeriod((uint32_t)(maxPeriod * 4.0e8), status); +void setCounterMaxPeriod(void* counter_pointer, double maxPeriod, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; + counter->counter->writeTimerConfig_StallPeriod((uint32_t)(maxPeriod * 4.0e8), + status); } /** - * Select whether you want to continue updating the event timer output when there are no samples captured. - * The output of the event timer has a buffer of periods that are averaged and posted to - * a register on the FPGA. When the timer detects that the event source has stopped - * (based on the MaxPeriod) the buffer of samples to be averaged is emptied. If you - * enable the update when empty, you will be notified of the stopped source and the event - * time will report 0 samples. If you disable update when empty, the most recent average - * will remain on the output until a new sample is acquired. You will never see 0 samples - * output (except when there have been no events since an FPGA reset) and you will likely not - * see the stopped bit become true (since it is updated at the end of an average and there are - * no samples to average). + * Select whether you want to continue updating the event timer output when + * there are no samples captured. The output of the event timer has a buffer of + * periods that are averaged and posted to a register on the FPGA. When the + * timer detects that the event source has stopped (based on the MaxPeriod) the + * buffer of samples to be averaged is emptied. If you enable the update when + * empty, you will be notified of the stopped source and the event time will + * report 0 samples. If you disable update when empty, the most recent average + * will remain on the output until a new sample is acquired. You will never see + * 0 samples output (except when there have been no events since an FPGA reset) + * and you will likely not see the stopped bit become true (since it is updated + * at the end of an average and there are no samples to average). */ -void setCounterUpdateWhenEmpty(void* counter_pointer, bool enabled, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterUpdateWhenEmpty(void* counter_pointer, bool enabled, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; counter->counter->writeTimerConfig_UpdateWhenEmpty(enabled, status); } /** * Determine if the clock is stopped. - * Determine if the clocked input is stopped based on the MaxPeriod value set using the - * SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the device (and counter) are - * assumed to be stopped and it returns true. - * @return Returns true if the most recent counter period exceeds the MaxPeriod value set by - * SetMaxPeriod. + * Determine if the clocked input is stopped based on the MaxPeriod value set + * using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the + * device (and counter) are assumed to be stopped and it returns true. + * @return Returns true if the most recent counter period exceeds the MaxPeriod + * value set by SetMaxPeriod. */ -bool getCounterStopped(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +bool getCounterStopped(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; return counter->counter->readTimerOutput_Stalled(status); } @@ -1070,25 +1151,26 @@ bool getCounterStopped(void* counter_pointer, int32_t *status) { * The last direction the counter value changed. * @return The last direction the counter value changed. */ -bool getCounterDirection(void* counter_pointer, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +bool getCounterDirection(void* counter_pointer, int32_t* status) { + Counter* counter = (Counter*)counter_pointer; bool value = counter->counter->readOutput_Direction(status); return value; } /** * Set the Counter to return reversed sensing on the direction. - * This allows counters to change the direction they are counting in the case of 1X and 2X - * quadrature encoding only. Any other counter mode isn't supported. + * This allows counters to change the direction they are counting in the case of + * 1X and 2X quadrature encoding only. Any other counter mode isn't supported. * @param reverseDirection true if the value counted should be negated. */ -void setCounterReverseDirection(void* counter_pointer, bool reverseDirection, int32_t *status) { - Counter* counter = (Counter*) counter_pointer; +void setCounterReverseDirection(void* counter_pointer, bool reverseDirection, + int32_t* status) { + Counter* counter = (Counter*)counter_pointer; if (counter->counter->readConfig_Mode(status) == kExternalDirection) { - if (reverseDirection) - setCounterDownSourceEdge(counter_pointer, true, true, status); - else - setCounterDownSourceEdge(counter_pointer, false, true, status); + if (reverseDirection) + setCounterDownSourceEdge(counter_pointer, true, true, status); + else + setCounterDownSourceEdge(counter_pointer, false, true, status); } } @@ -1099,12 +1181,13 @@ struct encoder_t { typedef struct encoder_t Encoder; static const double DECODING_SCALING_FACTOR = 0.25; -static hal::Resource *quadEncoders = NULL; - -void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, bool port_a_analog_trigger, - uint8_t port_b_module, uint32_t port_b_pin, bool port_b_analog_trigger, - bool reverseDirection, int32_t *index, int32_t *status) { +static hal::Resource* quadEncoders = NULL; +void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, + bool port_a_analog_trigger, uint8_t port_b_module, + uint32_t port_b_pin, bool port_b_analog_trigger, + bool reverseDirection, int32_t* index, + int32_t* status) { // Initialize encoder structure Encoder* encoder = new Encoder(); @@ -1118,10 +1201,12 @@ void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, bool port_a_ encoder->encoder = tEncoder::create(encoder->index, status); encoder->encoder->writeConfig_ASource_Module(port_a_module, status); encoder->encoder->writeConfig_ASource_Channel(port_a_pin, status); - encoder->encoder->writeConfig_ASource_AnalogTrigger(port_a_analog_trigger, status); + encoder->encoder->writeConfig_ASource_AnalogTrigger(port_a_analog_trigger, + status); encoder->encoder->writeConfig_BSource_Module(port_b_module, status); encoder->encoder->writeConfig_BSource_Channel(port_b_pin, status); - encoder->encoder->writeConfig_BSource_AnalogTrigger(port_b_analog_trigger, status); + encoder->encoder->writeConfig_BSource_AnalogTrigger(port_b_analog_trigger, + status); encoder->encoder->strobeReset(status); encoder->encoder->writeConfig_Reverse(reverseDirection, status); encoder->encoder->writeTimerConfig_AverageSize(4, status); @@ -1129,8 +1214,8 @@ void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, bool port_a_ return encoder; } -void freeEncoder(void* encoder_pointer, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; +void freeEncoder(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; if (!encoder) return; quadEncoders->Free(encoder->index); delete encoder->encoder; @@ -1140,8 +1225,8 @@ void freeEncoder(void* encoder_pointer, int32_t *status) { * Reset the Encoder distance to zero. * Resets the current count to zero on the encoder. */ -void resetEncoder(void* encoder_pointer, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; +void resetEncoder(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; encoder->encoder->strobeReset(status); } @@ -1151,8 +1236,8 @@ void resetEncoder(void* encoder_pointer, int32_t *status) { * factor. * @return Current raw count from the encoder */ -int32_t getEncoder(void* encoder_pointer, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; +int32_t getEncoder(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; return encoder->encoder->readOutput_Value(status); } @@ -1161,21 +1246,23 @@ int32_t getEncoder(void* encoder_pointer, int32_t *status) { * Returns the period of the most recent Encoder pulse in seconds. * This method compenstates for the decoding type. * - * @deprecated Use GetRate() in favor of this method. This returns unscaled periods and GetRate() scales using value from SetDistancePerPulse(). + * @deprecated Use GetRate() in favor of this method. This returns unscaled + * periods and GetRate() scales using value from SetDistancePerPulse(). * * @return Period in seconds of the most recent pulse. */ -double getEncoderPeriod(void* encoder_pointer, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; +double getEncoderPeriod(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; tEncoder::tTimerOutput output = encoder->encoder->readTimerOutput(status); double value; if (output.Stalled) { - // Return infinity - double zero = 0.0; - value = 1.0 / zero; + // Return infinity + double zero = 0.0; + value = 1.0 / zero; } else { - // output.Period is a fixed point number that counts by 2 (24 bits, 25 integer bits) - value = (double)(output.Period << 1) / (double)output.Count; + // output.Period is a fixed point number that counts by 2 (24 bits, 25 + // integer bits) + value = (double)(output.Period << 1) / (double)output.Count; } double measuredPeriod = value * 2.5e-8; return measuredPeriod / DECODING_SCALING_FACTOR; @@ -1183,30 +1270,34 @@ double getEncoderPeriod(void* encoder_pointer, int32_t *status) { /** * Sets the maximum period for stopped detection. - * Sets the value that represents the maximum period of the Encoder before it will assume - * that the attached device is stopped. This timeout allows users to determine if the wheels or - * other shaft has stopped rotating. + * Sets the value that represents the maximum period of the Encoder before it + * will assume that the attached device is stopped. This timeout allows users + * to determine if the wheels or other shaft has stopped rotating. * This method compensates for the decoding type. * - * @deprecated Use SetMinRate() in favor of this method. This takes unscaled periods and SetMinRate() scales using value from SetDistancePerPulse(). + * @deprecated Use SetMinRate() in favor of this method. This takes unscaled + * periods and SetMinRate() scales using value from SetDistancePerPulse(). * - * @param maxPeriod The maximum time between rising and falling edges before the FPGA will + * @param maxPeriod The maximum time between rising and falling edges before the + * FPGA will * report the device stopped. This is expressed in seconds. */ -void setEncoderMaxPeriod(void* encoder_pointer, double maxPeriod, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; - encoder->encoder->writeTimerConfig_StallPeriod((uint32_t)(maxPeriod * 4.0e8 * DECODING_SCALING_FACTOR), status); +void setEncoderMaxPeriod(void* encoder_pointer, double maxPeriod, + int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; + encoder->encoder->writeTimerConfig_StallPeriod( + (uint32_t)(maxPeriod * 4.0e8 * DECODING_SCALING_FACTOR), status); } /** * Determine if the encoder is stopped. - * Using the MaxPeriod value, a boolean is returned that is true if the encoder is considered - * stopped and false if it is still moving. A stopped encoder is one where the most recent pulse - * width exceeds the MaxPeriod. + * Using the MaxPeriod value, a boolean is returned that is true if the encoder + * is considered stopped and false if it is still moving. A stopped encoder is + * one where the most recent pulse width exceeds the MaxPeriod. * @return True if the encoder is considered stopped. */ -bool getEncoderStopped(void* encoder_pointer, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; +bool getEncoderStopped(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; return encoder->encoder->readTimerOutput_Stalled(status) != 0; } @@ -1214,44 +1305,46 @@ bool getEncoderStopped(void* encoder_pointer, int32_t *status) { * The last direction the encoder value changed. * @return The last direction the encoder value changed. */ -bool getEncoderDirection(void* encoder_pointer, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; +bool getEncoderDirection(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; return encoder->encoder->readOutput_Direction(status); } /** * Set the direction sensing for this encoder. - * This sets the direction sensing on the encoder so that it could count in the correct - * software direction regardless of the mounting. + * This sets the direction sensing on the encoder so that it could count in the + * correct software direction regardless of the mounting. * @param reverseDirection true if the encoder direction should be reversed */ -void setEncoderReverseDirection(void* encoder_pointer, bool reverseDirection, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; +void setEncoderReverseDirection(void* encoder_pointer, bool reverseDirection, + int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; encoder->encoder->writeConfig_Reverse(reverseDirection, status); } /** - * Set the Samples to Average which specifies the number of samples of the timer to - * average when calculating the period. Perform averaging to account for + * Set the Samples to Average which specifies the number of samples of the timer + * to average when calculating the period. Perform averaging to account for * mechanical imperfections or as oversampling to increase resolution. * @param samplesToAverage The number of samples to average from 1 to 127. */ -void setEncoderSamplesToAverage(void* encoder_pointer, uint32_t samplesToAverage, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; +void setEncoderSamplesToAverage(void* encoder_pointer, + uint32_t samplesToAverage, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; if (samplesToAverage < 1 || samplesToAverage > 127) { - *status = PARAMETER_OUT_OF_RANGE; + *status = PARAMETER_OUT_OF_RANGE; } encoder->encoder->writeTimerConfig_AverageSize(samplesToAverage, status); } /** - * Get the Samples to Average which specifies the number of samples of the timer to - * average when calculating the period. Perform averaging to account for + * Get the Samples to Average which specifies the number of samples of the timer + * to average when calculating the period. Perform averaging to account for * mechanical imperfections or as oversampling to increase resolution. * @return SamplesToAverage The number of samples being averaged (from 1 to 127) */ -uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; +uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; return encoder->encoder->readTimerConfig_AverageSize(status); } @@ -1259,12 +1352,14 @@ uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t *status) { * Set an index source for an encoder, which is an input that resets the * encoder's count. */ -void setEncoderIndexSource(void *encoder_pointer, uint32_t pin, bool analogTrigger, bool activeHigh, - bool edgeSensitive, int32_t *status) { - Encoder* encoder = (Encoder*) encoder_pointer; +void setEncoderIndexSource(void* encoder_pointer, uint32_t pin, + bool analogTrigger, bool activeHigh, + bool edgeSensitive, int32_t* status) { + Encoder* encoder = (Encoder*)encoder_pointer; encoder->encoder->writeConfig_IndexSource_Channel((unsigned char)pin, status); encoder->encoder->writeConfig_IndexSource_Module((unsigned char)0, status); - encoder->encoder->writeConfig_IndexSource_AnalogTrigger(analogTrigger, status); + encoder->encoder->writeConfig_IndexSource_AnalogTrigger(analogTrigger, + status); encoder->encoder->writeConfig_IndexActiveHigh(activeHigh, status); encoder->encoder->writeConfig_IndexEdgeSensitive(edgeSensitive, status); } @@ -1274,7 +1369,7 @@ void setEncoderIndexSource(void *encoder_pointer, uint32_t pin, bool analogTrigg * * @return The loop time */ -uint16_t getLoopTiming(int32_t *status) { +uint16_t getLoopTiming(int32_t* status) { return pwmSystem->readLoopTiming(status); } @@ -1283,42 +1378,55 @@ uint16_t getLoopTiming(int32_t *status) { * If opening the MXP port, also sets up the pin functions appropriately * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP */ -void spiInitialize(uint8_t port, int32_t *status) { - if(spiSystem == NULL) - spiSystem = tSPI::create(status); - if(spiGetHandle(port) !=0 ) return; - switch(port){ - case 0: - spiSetHandle(0, spilib_open("/dev/spidev0.0")); - break; - case 1: - spiSetHandle(1, spilib_open("/dev/spidev0.1")); - break; - case 2: - spiSetHandle(2, spilib_open("/dev/spidev0.2")); - break; - case 3: - spiSetHandle(3, spilib_open("/dev/spidev0.3")); - break; - case 4: - initializeDigital(status); - if(!allocateDIO(getPort(14), false, status)){printf("Failed to allocate DIO 14\n"); return;} - if(!allocateDIO(getPort(15), false, status)) {printf("Failed to allocate DIO 15\n"); return;} - if(!allocateDIO(getPort(16), true, status)) {printf("Failed to allocate DIO 16\n"); return;} - if(!allocateDIO(getPort(17), false, status)) {printf("Failed to allocate DIO 17\n"); return;} - digitalSystem->writeEnableMXPSpecialFunction(digitalSystem->readEnableMXPSpecialFunction(status)|0x00F0, status); - spiSetHandle(4, spilib_open("/dev/spidev1.0")); - break; - default: - break; - } - return; +void spiInitialize(uint8_t port, int32_t* status) { + if (spiSystem == NULL) spiSystem = tSPI::create(status); + if (spiGetHandle(port) != 0) return; + switch (port) { + case 0: + spiSetHandle(0, spilib_open("/dev/spidev0.0")); + break; + case 1: + spiSetHandle(1, spilib_open("/dev/spidev0.1")); + break; + case 2: + spiSetHandle(2, spilib_open("/dev/spidev0.2")); + break; + case 3: + spiSetHandle(3, spilib_open("/dev/spidev0.3")); + break; + case 4: + initializeDigital(status); + if (!allocateDIO(getPort(14), false, status)) { + printf("Failed to allocate DIO 14\n"); + return; + } + if (!allocateDIO(getPort(15), false, status)) { + printf("Failed to allocate DIO 15\n"); + return; + } + if (!allocateDIO(getPort(16), true, status)) { + printf("Failed to allocate DIO 16\n"); + return; + } + if (!allocateDIO(getPort(17), false, status)) { + printf("Failed to allocate DIO 17\n"); + return; + } + digitalSystem->writeEnableMXPSpecialFunction( + digitalSystem->readEnableMXPSpecialFunction(status) | 0x00F0, status); + spiSetHandle(4, spilib_open("/dev/spidev1.0")); + break; + default: + break; + } + return; } /** * Generic transaction. * - * This is a lower-level interface to the spi hardware giving you more control over each transaction. + * This is a lower-level interface to the spi hardware giving you more control + * over each transaction. * * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP * @param dataToSend Buffer of data to send as part of the transaction. @@ -1326,10 +1434,11 @@ void spiInitialize(uint8_t port, int32_t *status) { * @param size Number of bytes to transfer. [0..7] * @return Number of bytes transferred, -1 for error */ -int32_t spiTransaction(uint8_t port, uint8_t *dataToSend, uint8_t *dataReceived, uint8_t size) -{ - std::lock_guard sync(spiGetSemaphore(port)); - return spilib_writeread(spiGetHandle(port), (const char*) dataToSend, (char*) dataReceived, (int32_t) size); +int32_t spiTransaction(uint8_t port, uint8_t* dataToSend, uint8_t* dataReceived, + uint8_t size) { + std::lock_guard sync(spiGetSemaphore(port)); + return spilib_writeread(spiGetHandle(port), (const char*)dataToSend, + (char*)dataReceived, (int32_t)size); } /** @@ -1342,28 +1451,28 @@ int32_t spiTransaction(uint8_t port, uint8_t *dataToSend, uint8_t *dataReceived, * @param sendSize The number of bytes to be written * @return The number of bytes written. -1 for an error */ -int32_t spiWrite(uint8_t port, uint8_t* dataToSend, uint8_t sendSize) -{ - std::lock_guard sync(spiGetSemaphore(port)); - return spilib_write(spiGetHandle(port), (const char*) dataToSend, (int32_t) sendSize); +int32_t spiWrite(uint8_t port, uint8_t* dataToSend, uint8_t sendSize) { + std::lock_guard sync(spiGetSemaphore(port)); + return spilib_write(spiGetHandle(port), (const char*)dataToSend, + (int32_t)sendSize); } /** * Execute a read from the device. * - * This methdod does not write any data out to the device + * This method does not write any data out to the device * Most spi devices will require a register address to be written before * they begin returning data * * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP - * @param buffer A pointer to the array of bytes to store the data read from the device. + * @param buffer A pointer to the array of bytes to store the data read from the + * device. * @param count The number of bytes to read in the transaction. [1..7] * @return Number of bytes read. -1 for error. */ -int32_t spiRead(uint8_t port, uint8_t *buffer, uint8_t count) -{ - std::lock_guard sync(spiGetSemaphore(port)); - return spilib_read(spiGetHandle(port), (char*) buffer, (int32_t) count); +int32_t spiRead(uint8_t port, uint8_t* buffer, uint8_t count) { + std::lock_guard sync(spiGetSemaphore(port)); + return spilib_read(spiGetHandle(port), (char*)buffer, (int32_t)count); } /** @@ -1372,14 +1481,14 @@ int32_t spiRead(uint8_t port, uint8_t *buffer, uint8_t count) * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP */ void spiClose(uint8_t port) { - std::lock_guard sync(spiGetSemaphore(port)); - if (spiAccumulators[port]) { - int32_t status = 0; - spiFreeAccumulator(port, &status); - } - spilib_close(spiGetHandle(port)); - spiSetHandle(port, 0); - return; + std::lock_guard sync(spiGetSemaphore(port)); + if (spiAccumulators[port]) { + int32_t status = 0; + spiFreeAccumulator(port, &status); + } + spilib_close(spiGetHandle(port)); + spiSetHandle(port, 0); + return; } /** @@ -1389,8 +1498,8 @@ void spiClose(uint8_t port) { * @param speed The speed in Hz (0-1MHz) */ void spiSetSpeed(uint8_t port, uint32_t speed) { - std::lock_guard sync(spiGetSemaphore(port)); - spilib_setspeed(spiGetHandle(port), speed); + std::lock_guard sync(spiGetSemaphore(port)); + spilib_setspeed(spiGetHandle(port), speed); } /** @@ -1398,12 +1507,16 @@ void spiSetSpeed(uint8_t port, uint32_t speed) { * * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP * @param msb_first True to write the MSB first, False for LSB first - * @param sample_on_trailing True to sample on the trailing edge, False to sample on the leading edge - * @param clk_idle_high True to set the clock to active low, False to set the clock active high + * @param sample_on_trailing True to sample on the trailing edge, False to + * sample on the leading edge + * @param clk_idle_high True to set the clock to active low, False to set the + * clock active high */ -void spiSetOpts(uint8_t port, int msb_first, int sample_on_trailing, int clk_idle_high) { - std::lock_guard sync(spiGetSemaphore(port)); - spilib_setopts(spiGetHandle(port), msb_first, sample_on_trailing, clk_idle_high); +void spiSetOpts(uint8_t port, int msb_first, int sample_on_trailing, + int clk_idle_high) { + std::lock_guard sync(spiGetSemaphore(port)); + spilib_setopts(spiGetHandle(port), msb_first, sample_on_trailing, + clk_idle_high); } /** @@ -1411,16 +1524,14 @@ void spiSetOpts(uint8_t port, int msb_first, int sample_on_trailing, int clk_idl * * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP */ -void spiSetChipSelectActiveHigh(uint8_t port, int32_t *status){ - std::lock_guard sync(spiGetSemaphore(port)); - if(port < 4) - { - spiSystem->writeChipSelectActiveHigh_Hdr(spiSystem->readChipSelectActiveHigh_Hdr(status) | (1<writeChipSelectActiveHigh_MXP(1, status); - } +void spiSetChipSelectActiveHigh(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + if (port < 4) { + spiSystem->writeChipSelectActiveHigh_Hdr( + spiSystem->readChipSelectActiveHigh_Hdr(status) | (1 << port), status); + } else { + spiSystem->writeChipSelectActiveHigh_MXP(1, status); + } } /** @@ -1428,16 +1539,14 @@ void spiSetChipSelectActiveHigh(uint8_t port, int32_t *status){ * * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP */ -void spiSetChipSelectActiveLow(uint8_t port, int32_t *status){ - std::lock_guard sync(spiGetSemaphore(port)); - if(port < 4) - { - spiSystem->writeChipSelectActiveHigh_Hdr(spiSystem->readChipSelectActiveHigh_Hdr(status) & ~(1<writeChipSelectActiveHigh_MXP(0, status); - } +void spiSetChipSelectActiveLow(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + if (port < 4) { + spiSystem->writeChipSelectActiveHigh_Hdr( + spiSystem->readChipSelectActiveHigh_Hdr(status) & ~(1 << port), status); + } else { + spiSystem->writeChipSelectActiveHigh_MXP(0, status); + } } /** @@ -1446,51 +1555,52 @@ void spiSetChipSelectActiveLow(uint8_t port, int32_t *status){ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP * @return The stored handle for the SPI port. 0 represents no stored handle. */ -int32_t spiGetHandle(uint8_t port){ - std::lock_guard sync(spiGetSemaphore(port)); - switch(port){ - case 0: - return m_spiCS0Handle; - case 1: - return m_spiCS1Handle; - case 2: - return m_spiCS2Handle; - case 3: - return m_spiCS3Handle; - case 4: - return m_spiMXPHandle; - default: - return 0; - } +int32_t spiGetHandle(uint8_t port) { + std::lock_guard sync(spiGetSemaphore(port)); + switch (port) { + case 0: + return m_spiCS0Handle; + case 1: + return m_spiCS1Handle; + case 2: + return m_spiCS2Handle; + case 3: + return m_spiCS3Handle; + case 4: + return m_spiMXPHandle; + default: + return 0; + } } /** * Set the stored handle for a SPI port * - * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP. + * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for + * MXP. * @param handle The value of the handle for the port. */ -void spiSetHandle(uint8_t port, int32_t handle){ - std::lock_guard sync(spiGetSemaphore(port)); - switch(port){ - case 0: - m_spiCS0Handle = handle; - break; - case 1: - m_spiCS1Handle = handle; - break; - case 2: - m_spiCS2Handle = handle; - break; - case 3: - m_spiCS3Handle = handle; - break; - case 4: - m_spiMXPHandle = handle; - break; - default: - break; - } +void spiSetHandle(uint8_t port, int32_t handle) { + std::lock_guard sync(spiGetSemaphore(port)); + switch (port) { + case 0: + m_spiCS0Handle = handle; + break; + case 1: + m_spiCS1Handle = handle; + break; + case 2: + m_spiCS2Handle = handle; + break; + case 3: + m_spiCS3Handle = handle; + break; + case 4: + m_spiMXPHandle = handle; + break; + default: + break; + } } /** @@ -1500,61 +1610,61 @@ void spiSetHandle(uint8_t port, int32_t handle){ * @return The semaphore for the SPI port. */ extern "C++" priority_recursive_mutex& spiGetSemaphore(uint8_t port) { - if(port < 4) - return spiOnboardSemaphore; - else - return spiMXPSemaphore; + if (port < 4) + return spiOnboardSemaphore; + else + return spiMXPSemaphore; } -static void spiAccumulatorProcess(uint64_t currentTime, void *param) { - SPIAccumulator* accum = (SPIAccumulator*)param; +static void spiAccumulatorProcess(uint64_t currentTime, void* param) { + SPIAccumulator* accum = (SPIAccumulator*)param; - // perform SPI transaction - uint8_t resp_b[4]; - std::lock_guard sync(spiGetSemaphore(accum->port)); - spilib_writeread(spiGetHandle(accum->port), (const char*) accum->cmd, (char*) resp_b, (int32_t) accum->xfer_size); + // perform SPI transaction + uint8_t resp_b[4]; + std::lock_guard sync(spiGetSemaphore(accum->port)); + spilib_writeread(spiGetHandle(accum->port), (const char*)accum->cmd, + (char*)resp_b, (int32_t)accum->xfer_size); - // convert from bytes - uint32_t resp = 0; - if (accum->big_endian) { - for (int i=0; i < accum->xfer_size; ++i) { - resp <<= 8; - resp |= resp_b[i] & 0xff; - } - } else { - for (int i = accum->xfer_size - 1; i >= 0; --i) { - resp <<= 8; - resp |= resp_b[i] & 0xff; - } - } + // convert from bytes + uint32_t resp = 0; + if (accum->big_endian) { + for (int i = 0; i < accum->xfer_size; ++i) { + resp <<= 8; + resp |= resp_b[i] & 0xff; + } + } else { + for (int i = accum->xfer_size - 1; i >= 0; --i) { + resp <<= 8; + resp |= resp_b[i] & 0xff; + } + } - // process response - if ((resp & accum->valid_mask) == accum->valid_value) { - // valid sensor data; extract data field - int32_t data = (int32_t)(resp >> accum->data_shift); - data &= accum->data_max - 1; - // 2s complement conversion if signed MSB is set - if (accum->is_signed && (data & accum->data_msb_mask) != 0) - data -= accum->data_max; - // center offset - data -= accum->center; - // only accumulate if outside deadband - if (data < -accum->deadband || data > accum->deadband) - accum->value += data; - ++accum->count; - accum->last_value = data; - } else { - // no data from the sensor; just clear the last value - accum->last_value = 0; - } + // process response + if ((resp & accum->valid_mask) == accum->valid_value) { + // valid sensor data; extract data field + int32_t data = (int32_t)(resp >> accum->data_shift); + data &= accum->data_max - 1; + // 2s complement conversion if signed MSB is set + if (accum->is_signed && (data & accum->data_msb_mask) != 0) + data -= accum->data_max; + // center offset + data -= accum->center; + // only accumulate if outside deadband + if (data < -accum->deadband || data > accum->deadband) accum->value += data; + ++accum->count; + accum->last_value = data; + } else { + // no data from the sensor; just clear the last value + accum->last_value = 0; + } - // reschedule timer - accum->triggerTime += accum->period; - // handle timer slip - if (accum->triggerTime < currentTime) - accum->triggerTime = currentTime + accum->period; - int32_t status = 0; - updateNotifierAlarm(accum->notifier, accum->triggerTime, &status); + // reschedule timer + accum->triggerTime += accum->period; + // handle timer slip + if (accum->triggerTime < currentTime) + accum->triggerTime = currentTime + accum->period; + int32_t status = 0; + updateNotifierAlarm(accum->notifier, accum->triggerTime, &status); } /** @@ -1574,113 +1684,119 @@ static void spiAccumulatorProcess(uint64_t currentTime, void *param) { * @param big_endian Is device big endian? */ void spiInitAccumulator(uint8_t port, uint32_t period, uint32_t cmd, - uint8_t xfer_size, uint32_t valid_mask, uint32_t valid_value, - uint8_t data_shift, uint8_t data_size, bool is_signed, - bool big_endian, int32_t *status) { - std::lock_guard sync(spiGetSemaphore(port)); - if (port > 4) return; - if (!spiAccumulators[port]) - spiAccumulators[port] = new SPIAccumulator(); - SPIAccumulator* accum = spiAccumulators[port]; - if (big_endian) { - for (int i = xfer_size - 1; i >= 0; --i) { - accum->cmd[i] = cmd & 0xff; - cmd >>= 8; - } - } else { - accum->cmd[0] = cmd & 0xff; cmd >>= 8; - accum->cmd[1] = cmd & 0xff; cmd >>= 8; - accum->cmd[2] = cmd & 0xff; cmd >>= 8; - accum->cmd[3] = cmd & 0xff; - } - accum->period = period; - accum->xfer_size = xfer_size; - accum->valid_mask = valid_mask; - accum->valid_value = valid_value; - accum->data_shift = data_shift; - accum->data_max = (1 << data_size); - accum->data_msb_mask = (1 << (data_size - 1)); - accum->is_signed = is_signed; - accum->big_endian = big_endian; - if (!accum->notifier) { - accum->notifier = initializeNotifier(spiAccumulatorProcess, accum, status); - accum->triggerTime = getFPGATime(status) + period; - if (*status != 0) return; - updateNotifierAlarm(accum->notifier, accum->triggerTime, status); - } + uint8_t xfer_size, uint32_t valid_mask, + uint32_t valid_value, uint8_t data_shift, + uint8_t data_size, bool is_signed, bool big_endian, + int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + if (port > 4) return; + if (!spiAccumulators[port]) spiAccumulators[port] = new SPIAccumulator(); + SPIAccumulator* accum = spiAccumulators[port]; + if (big_endian) { + for (int i = xfer_size - 1; i >= 0; --i) { + accum->cmd[i] = cmd & 0xff; + cmd >>= 8; + } + } else { + accum->cmd[0] = cmd & 0xff; + cmd >>= 8; + accum->cmd[1] = cmd & 0xff; + cmd >>= 8; + accum->cmd[2] = cmd & 0xff; + cmd >>= 8; + accum->cmd[3] = cmd & 0xff; + } + accum->period = period; + accum->xfer_size = xfer_size; + accum->valid_mask = valid_mask; + accum->valid_value = valid_value; + accum->data_shift = data_shift; + accum->data_max = (1 << data_size); + accum->data_msb_mask = (1 << (data_size - 1)); + accum->is_signed = is_signed; + accum->big_endian = big_endian; + if (!accum->notifier) { + accum->notifier = initializeNotifier(spiAccumulatorProcess, accum, status); + accum->triggerTime = getFPGATime(status) + period; + if (*status != 0) return; + updateNotifierAlarm(accum->notifier, accum->triggerTime, status); + } } /** * Frees a SPI accumulator. */ -void spiFreeAccumulator(uint8_t port, int32_t *status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return; - } - cleanNotifier(accum->notifier, status); - delete accum; - spiAccumulators[port] = nullptr; +void spiFreeAccumulator(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + cleanNotifier(accum->notifier, status); + delete accum; + spiAccumulators[port] = nullptr; } /** * Resets the accumulator to zero. */ -void spiResetAccumulator(uint8_t port, int32_t *status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return; - } - accum->value = 0; - accum->count = 0; - accum->last_value = 0; +void spiResetAccumulator(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + accum->value = 0; + accum->count = 0; + accum->last_value = 0; } /** * Set the center value of the accumulator. * - * The center value is subtracted from each value before it is added to the accumulator. This - * is used for the center value of devices like gyros and accelerometers to make integration work + * The center value is subtracted from each value before it is added to the + * accumulator. This + * is used for the center value of devices like gyros and accelerometers to make + * integration work * and to take the device offset into account when integrating. */ -void spiSetAccumulatorCenter(uint8_t port, int32_t center, int32_t *status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return; - } - accum->center = center; +void spiSetAccumulatorCenter(uint8_t port, int32_t center, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + accum->center = center; } /** * Set the accumulator's deadband. */ -void spiSetAccumulatorDeadband(uint8_t port, int32_t deadband, int32_t *status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return; - } - accum->deadband = deadband; +void spiSetAccumulatorDeadband(uint8_t port, int32_t deadband, + int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return; + } + accum->deadband = deadband; } /** * Read the last value read by the accumulator engine. */ -int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t *status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return 0; - } - return accum->last_value; +int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return 0; + } + return accum->last_value; } /** @@ -1688,31 +1804,32 @@ int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t *status) { * * @return The 64-bit value accumulated since the last Reset(). */ -int64_t spiGetAccumulatorValue(uint8_t port, int32_t *status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return 0; - } - return accum->value; +int64_t spiGetAccumulatorValue(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return 0; + } + return accum->value; } /** * Read the number of accumulated values. * - * Read the count of the accumulated values since the accumulator was last Reset(). + * Read the count of the accumulated values since the accumulator was last + * Reset(). * * @return The number of times samples from the channel were accumulated. */ -uint32_t spiGetAccumulatorCount(uint8_t port, int32_t *status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - return 0; - } - return accum->count; +uint32_t spiGetAccumulatorCount(uint8_t port, int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + return 0; + } + return accum->count; } /** @@ -1720,12 +1837,12 @@ uint32_t spiGetAccumulatorCount(uint8_t port, int32_t *status) { * * @return The accumulated average value (value / count). */ -double spiGetAccumulatorAverage(uint8_t port, int32_t *status) { - int64_t value; - uint32_t count; - spiGetAccumulatorOutput(port, &value, &count, status); - if (count == 0) return 0.0; - return ((double)value) / count; +double spiGetAccumulatorAverage(uint8_t port, int32_t* status) { + int64_t value; + uint32_t count; + spiGetAccumulatorOutput(port, &value, &count, status); + if (count == 0) return 0.0; + return ((double)value) / count; } /** @@ -1737,18 +1854,18 @@ double spiGetAccumulatorAverage(uint8_t port, int32_t *status) { * @param value Pointer to the 64-bit accumulated output. * @param count Pointer to the number of accumulation cycles. */ -void spiGetAccumulatorOutput(uint8_t port, int64_t *value, uint32_t *count, - int32_t *status) { - std::lock_guard sync(spiGetSemaphore(port)); - SPIAccumulator* accum = spiAccumulators[port]; - if (!accum) { - *status = NULL_PARAMETER; - *value = 0; - *count = 0; - return; - } - *value = accum->value; - *count = accum->count; +void spiGetAccumulatorOutput(uint8_t port, int64_t* value, uint32_t* count, + int32_t* status) { + std::lock_guard sync(spiGetSemaphore(port)); + SPIAccumulator* accum = spiAccumulators[port]; + if (!accum) { + *status = NULL_PARAMETER; + *value = 0; + *count = 0; + return; + } + *value = accum->value; + *count = accum->count; } /* @@ -1756,38 +1873,40 @@ void spiGetAccumulatorOutput(uint8_t port, int64_t *value, uint32_t *count, * If opening the MXP port, also sets up the pin functions appropriately * @param port The port to open, 0 for the on-board, 1 for the MXP. */ -void i2CInitialize(uint8_t port, int32_t *status) { - initializeDigital(status); +void i2CInitialize(uint8_t port, int32_t* status) { + initializeDigital(status); - if(port > 1) - { - //Set port out of range error here - return; - } + if (port > 1) { + // Set port out of range error here + return; + } - priority_recursive_mutex &lock = port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; - { - std::lock_guard sync(lock); - if(port == 0) { - i2COnboardObjCount++; - if (i2COnBoardHandle > 0) return; - i2COnBoardHandle = i2clib_open("/dev/i2c-2"); - } else if(port == 1) { - i2CMXPObjCount++; - if (i2CMXPHandle > 0) return; - if(!allocateDIO(getPort(24), false, status)) return; - if(!allocateDIO(getPort(25), false, status)) return; - digitalSystem->writeEnableMXPSpecialFunction(digitalSystem->readEnableMXPSpecialFunction(status)|0xC000, status); - i2CMXPHandle = i2clib_open("/dev/i2c-1"); - } - return; - } + priority_recursive_mutex& lock = + port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; + { + std::lock_guard sync(lock); + if (port == 0) { + i2COnboardObjCount++; + if (i2COnBoardHandle > 0) return; + i2COnBoardHandle = i2clib_open("/dev/i2c-2"); + } else if (port == 1) { + i2CMXPObjCount++; + if (i2CMXPHandle > 0) return; + if (!allocateDIO(getPort(24), false, status)) return; + if (!allocateDIO(getPort(25), false, status)) return; + digitalSystem->writeEnableMXPSpecialFunction( + digitalSystem->readEnableMXPSpecialFunction(status) | 0xC000, status); + i2CMXPHandle = i2clib_open("/dev/i2c-1"); + } + return; + } } /** * Generic transaction. * - * This is a lower-level interface to the I2C hardware giving you more control over each transaction. + * This is a lower-level interface to the I2C hardware giving you more control + * over each transaction. * * @param dataToSend Buffer of data to send as part of the transaction. * @param sendSize Number of bytes to send as part of the transaction. @@ -1795,20 +1914,24 @@ void i2CInitialize(uint8_t port, int32_t *status) { * @param receiveSize Number of bytes to read from the device. * @return The number of bytes read (>= 0) or -1 on transfer abort. */ -int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend, uint8_t sendSize, uint8_t *dataReceived, uint8_t receiveSize) -{ - if(port > 1) { - //Set port out of range error here - return -1; - } +int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, + uint8_t sendSize, uint8_t* dataReceived, + uint8_t receiveSize) { + if (port > 1) { + // Set port out of range error here + return -1; + } - int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle; - priority_recursive_mutex &lock = port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; + int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; + priority_recursive_mutex& lock = + port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; - { - std::lock_guard sync(lock); - return i2clib_writeread(handle, deviceAddress, (const char*) dataToSend, (int32_t) sendSize, (char*) dataReceived, (int32_t) receiveSize); - } + { + std::lock_guard sync(lock); + return i2clib_writeread(handle, deviceAddress, (const char*)dataToSend, + (int32_t)sendSize, (char*)dataReceived, + (int32_t)receiveSize); + } } /** @@ -1817,23 +1940,26 @@ int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend, * Write a single byte to a register on a device and wait until the * transaction is complete. * - * @param registerAddress The address of the register on the device to be written. + * @param registerAddress The address of the register on the device to be + * written. * @param data The byte to write to the register on the device. * @return The number of bytes written (>= 0) or -1 on transfer abort. */ -int32_t i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, uint8_t sendSize) -{ - if(port > 1) { - //Set port out of range error here - return -1; - } +int32_t i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, + uint8_t sendSize) { + if (port > 1) { + // Set port out of range error here + return -1; + } - int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle; - priority_recursive_mutex &lock = port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; - { - std::lock_guard sync(lock); - return i2clib_write(handle, deviceAddress, (const char*) dataToSend, (int32_t) sendSize); - } + int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; + priority_recursive_mutex& lock = + port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; + { + std::lock_guard sync(lock); + return i2clib_write(handle, deviceAddress, (const char*)dataToSend, + (int32_t)sendSize); + } } /** @@ -1845,39 +1971,41 @@ int32_t i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, uint8 * * @param registerAddress The register to read first in the transaction. * @param count The number of bytes to read in the transaction. - * @param buffer A pointer to the array of bytes to store the data read from the device. + * @param buffer A pointer to the array of bytes to store the data read from the + * device. * @return The number of bytes read (>= 0) or -1 on transfer abort. */ -int32_t i2CRead(uint8_t port, uint8_t deviceAddress, uint8_t *buffer, uint8_t count) -{ - if(port > 1) { - //Set port out of range error here - return -1; - } - - int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle; - priority_recursive_mutex &lock = port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; - { - std::lock_guard sync(lock); - return i2clib_read(handle, deviceAddress, (char*) buffer, (int32_t) count); - } +int32_t i2CRead(uint8_t port, uint8_t deviceAddress, uint8_t* buffer, + uint8_t count) { + if (port > 1) { + // Set port out of range error here + return -1; + } + int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; + priority_recursive_mutex& lock = + port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; + { + std::lock_guard sync(lock); + return i2clib_read(handle, deviceAddress, (char*)buffer, (int32_t)count); + } } void i2CClose(uint8_t port) { - if(port > 1) { - //Set port out of range error here - return; - } - priority_recursive_mutex &lock = port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; - { - std::lock_guard sync(lock); - if((port == 0 ? i2COnboardObjCount--:i2CMXPObjCount--) == 0) { - int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle; - i2clib_close(handle); - } - } - return; + if (port > 1) { + // Set port out of range error here + return; + } + priority_recursive_mutex& lock = + port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex; + { + std::lock_guard sync(lock); + if ((port == 0 ? i2COnboardObjCount-- : i2CMXPObjCount--) == 0) { + int32_t handle = port == 0 ? i2COnBoardHandle : i2CMXPHandle; + i2clib_close(handle); + } + } + return; } } // extern "C" diff --git a/hal/lib/Athena/HALAthena.cpp b/hal/lib/Athena/HALAthena.cpp index 3a392f189e..27f57430fd 100644 --- a/hal/lib/Athena/HALAthena.cpp +++ b/hal/lib/Athena/HALAthena.cpp @@ -7,29 +7,29 @@ #include "HAL/HAL.hpp" -#include "HAL/Port.h" -#include "HAL/Errors.hpp" -#include "ctre/ctre.h" -#include "visa/visa.h" -#include "ChipObject.h" -#include "FRC_NetworkCommunication/FRCComm.h" -#include "FRC_NetworkCommunication/UsageReporting.h" -#include "FRC_NetworkCommunication/LoadOut.h" -#include "FRC_NetworkCommunication/CANSessionMux.h" +#include // linux for kill +#include +#include #include #include #include #include -#include -#include -#include // linux for kill +#include "ChipObject.h" +#include "FRC_NetworkCommunication/CANSessionMux.h" +#include "FRC_NetworkCommunication/FRCComm.h" +#include "FRC_NetworkCommunication/LoadOut.h" +#include "FRC_NetworkCommunication/UsageReporting.h" +#include "HAL/Errors.hpp" +#include "HAL/Port.h" +#include "ctre/ctre.h" +#include "visa/visa.h" const uint32_t solenoid_kNumDO7_0Elements = 8; const uint32_t dio_kNumSystems = tDIO::kNumSystems; const uint32_t interrupt_kNumSystems = tInterrupt::kNumSystems; const uint32_t kSystemClockTicksPerMicrosecond = 40; -static tGlobal *global = nullptr; -static tSysWatchdog *watchdog = nullptr; +static tGlobal* global = nullptr; +static tSysWatchdog* watchdog = nullptr; static priority_mutex timeMutex; static priority_mutex msgMutex; @@ -39,129 +39,125 @@ static void* rolloverNotifier = nullptr; extern "C" { -void* getPort(uint8_t pin) -{ - Port* port = new Port(); - port->pin = pin; - port->module = 1; - return port; +void* getPort(uint8_t pin) { + Port* port = new Port(); + port->pin = pin; + port->module = 1; + return port; } /** * @deprecated Uses module numbers */ -void* getPortWithModule(uint8_t module, uint8_t pin) -{ - Port* port = new Port(); - port->pin = pin; - port->module = module; - return port; +void* getPortWithModule(uint8_t module, uint8_t pin) { + Port* port = new Port(); + port->pin = pin; + port->module = module; + return port; } -void freePort(void* port_pointer) -{ - Port* port = (Port*) port_pointer; - delete port; +void freePort(void* port_pointer) { + Port* port = (Port*)port_pointer; + delete port; } -const char* getHALErrorMessage(int32_t code) -{ - switch(code) { - case 0: - return ""; - case CTR_RxTimeout: - return CTR_RxTimeout_MESSAGE; - case CTR_TxTimeout: - return CTR_TxTimeout_MESSAGE; - case CTR_InvalidParamValue: - return CTR_InvalidParamValue_MESSAGE; - case CTR_UnexpectedArbId: - return CTR_UnexpectedArbId_MESSAGE; - case CTR_TxFailed: - return CTR_TxFailed_MESSAGE; - case CTR_SigNotUpdated: - return CTR_SigNotUpdated_MESSAGE; - case NiFpga_Status_FifoTimeout: - return NiFpga_Status_FifoTimeout_MESSAGE; - case NiFpga_Status_TransferAborted: - return NiFpga_Status_TransferAborted_MESSAGE; - case NiFpga_Status_MemoryFull: - return NiFpga_Status_MemoryFull_MESSAGE; - case NiFpga_Status_SoftwareFault: - return NiFpga_Status_SoftwareFault_MESSAGE; - case NiFpga_Status_InvalidParameter: - return NiFpga_Status_InvalidParameter_MESSAGE; - case NiFpga_Status_ResourceNotFound: - return NiFpga_Status_ResourceNotFound_MESSAGE; - case NiFpga_Status_ResourceNotInitialized: - return NiFpga_Status_ResourceNotInitialized_MESSAGE; - case NiFpga_Status_HardwareFault: - return NiFpga_Status_HardwareFault_MESSAGE; - case NiFpga_Status_IrqTimeout: - return NiFpga_Status_IrqTimeout_MESSAGE; - case SAMPLE_RATE_TOO_HIGH: - return SAMPLE_RATE_TOO_HIGH_MESSAGE; - case VOLTAGE_OUT_OF_RANGE: - return VOLTAGE_OUT_OF_RANGE_MESSAGE; - case LOOP_TIMING_ERROR: - return LOOP_TIMING_ERROR_MESSAGE; - case SPI_WRITE_NO_MOSI: - return SPI_WRITE_NO_MOSI_MESSAGE; - case SPI_READ_NO_MISO: - return SPI_READ_NO_MISO_MESSAGE; - case SPI_READ_NO_DATA: - return SPI_READ_NO_DATA_MESSAGE; - case INCOMPATIBLE_STATE: - return INCOMPATIBLE_STATE_MESSAGE; - case NO_AVAILABLE_RESOURCES: - return NO_AVAILABLE_RESOURCES_MESSAGE; - case RESOURCE_IS_ALLOCATED: - return RESOURCE_IS_ALLOCATED_MESSAGE; - case NULL_PARAMETER: - return NULL_PARAMETER_MESSAGE; - case ANALOG_TRIGGER_LIMIT_ORDER_ERROR: - return ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE; - case ANALOG_TRIGGER_PULSE_OUTPUT_ERROR: - return ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE; - case PARAMETER_OUT_OF_RANGE: - return PARAMETER_OUT_OF_RANGE_MESSAGE; - case ERR_CANSessionMux_InvalidBuffer: - return ERR_CANSessionMux_InvalidBuffer_MESSAGE; - case ERR_CANSessionMux_MessageNotFound: - return ERR_CANSessionMux_MessageNotFound_MESSAGE; - case WARN_CANSessionMux_NoToken: - return WARN_CANSessionMux_NoToken_MESSAGE; - case ERR_CANSessionMux_NotAllowed: - return ERR_CANSessionMux_NotAllowed_MESSAGE; - case ERR_CANSessionMux_NotInitialized: - return ERR_CANSessionMux_NotInitialized_MESSAGE; - case VI_ERROR_SYSTEM_ERROR: - return VI_ERROR_SYSTEM_ERROR_MESSAGE; - case VI_ERROR_INV_OBJECT: - return VI_ERROR_INV_OBJECT_MESSAGE; - case VI_ERROR_RSRC_LOCKED: - return VI_ERROR_RSRC_LOCKED_MESSAGE; - case VI_ERROR_RSRC_NFOUND: - return VI_ERROR_RSRC_NFOUND_MESSAGE; - case VI_ERROR_INV_RSRC_NAME: - return VI_ERROR_INV_RSRC_NAME_MESSAGE; - case VI_ERROR_QUEUE_OVERFLOW: - return VI_ERROR_QUEUE_OVERFLOW_MESSAGE; - case VI_ERROR_IO: - return VI_ERROR_IO_MESSAGE; - case VI_ERROR_ASRL_PARITY: - return VI_ERROR_ASRL_PARITY_MESSAGE; - case VI_ERROR_ASRL_FRAMING: - return VI_ERROR_ASRL_FRAMING_MESSAGE; - case VI_ERROR_ASRL_OVERRUN: - return VI_ERROR_ASRL_OVERRUN_MESSAGE; - case VI_ERROR_RSRC_BUSY: - return VI_ERROR_RSRC_BUSY_MESSAGE; - case VI_ERROR_INV_PARAMETER: - return VI_ERROR_INV_PARAMETER_MESSAGE; - default: - return "Unknown error status"; - } +const char* getHALErrorMessage(int32_t code) { + switch (code) { + case 0: + return ""; + case CTR_RxTimeout: + return CTR_RxTimeout_MESSAGE; + case CTR_TxTimeout: + return CTR_TxTimeout_MESSAGE; + case CTR_InvalidParamValue: + return CTR_InvalidParamValue_MESSAGE; + case CTR_UnexpectedArbId: + return CTR_UnexpectedArbId_MESSAGE; + case CTR_TxFailed: + return CTR_TxFailed_MESSAGE; + case CTR_SigNotUpdated: + return CTR_SigNotUpdated_MESSAGE; + case NiFpga_Status_FifoTimeout: + return NiFpga_Status_FifoTimeout_MESSAGE; + case NiFpga_Status_TransferAborted: + return NiFpga_Status_TransferAborted_MESSAGE; + case NiFpga_Status_MemoryFull: + return NiFpga_Status_MemoryFull_MESSAGE; + case NiFpga_Status_SoftwareFault: + return NiFpga_Status_SoftwareFault_MESSAGE; + case NiFpga_Status_InvalidParameter: + return NiFpga_Status_InvalidParameter_MESSAGE; + case NiFpga_Status_ResourceNotFound: + return NiFpga_Status_ResourceNotFound_MESSAGE; + case NiFpga_Status_ResourceNotInitialized: + return NiFpga_Status_ResourceNotInitialized_MESSAGE; + case NiFpga_Status_HardwareFault: + return NiFpga_Status_HardwareFault_MESSAGE; + case NiFpga_Status_IrqTimeout: + return NiFpga_Status_IrqTimeout_MESSAGE; + case SAMPLE_RATE_TOO_HIGH: + return SAMPLE_RATE_TOO_HIGH_MESSAGE; + case VOLTAGE_OUT_OF_RANGE: + return VOLTAGE_OUT_OF_RANGE_MESSAGE; + case LOOP_TIMING_ERROR: + return LOOP_TIMING_ERROR_MESSAGE; + case SPI_WRITE_NO_MOSI: + return SPI_WRITE_NO_MOSI_MESSAGE; + case SPI_READ_NO_MISO: + return SPI_READ_NO_MISO_MESSAGE; + case SPI_READ_NO_DATA: + return SPI_READ_NO_DATA_MESSAGE; + case INCOMPATIBLE_STATE: + return INCOMPATIBLE_STATE_MESSAGE; + case NO_AVAILABLE_RESOURCES: + return NO_AVAILABLE_RESOURCES_MESSAGE; + case RESOURCE_IS_ALLOCATED: + return RESOURCE_IS_ALLOCATED_MESSAGE; + case NULL_PARAMETER: + return NULL_PARAMETER_MESSAGE; + case ANALOG_TRIGGER_LIMIT_ORDER_ERROR: + return ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE; + case ANALOG_TRIGGER_PULSE_OUTPUT_ERROR: + return ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE; + case PARAMETER_OUT_OF_RANGE: + return PARAMETER_OUT_OF_RANGE_MESSAGE; + case ERR_CANSessionMux_InvalidBuffer: + return ERR_CANSessionMux_InvalidBuffer_MESSAGE; + case ERR_CANSessionMux_MessageNotFound: + return ERR_CANSessionMux_MessageNotFound_MESSAGE; + case WARN_CANSessionMux_NoToken: + return WARN_CANSessionMux_NoToken_MESSAGE; + case ERR_CANSessionMux_NotAllowed: + return ERR_CANSessionMux_NotAllowed_MESSAGE; + case ERR_CANSessionMux_NotInitialized: + return ERR_CANSessionMux_NotInitialized_MESSAGE; + case VI_ERROR_SYSTEM_ERROR: + return VI_ERROR_SYSTEM_ERROR_MESSAGE; + case VI_ERROR_INV_OBJECT: + return VI_ERROR_INV_OBJECT_MESSAGE; + case VI_ERROR_RSRC_LOCKED: + return VI_ERROR_RSRC_LOCKED_MESSAGE; + case VI_ERROR_RSRC_NFOUND: + return VI_ERROR_RSRC_NFOUND_MESSAGE; + case VI_ERROR_INV_RSRC_NAME: + return VI_ERROR_INV_RSRC_NAME_MESSAGE; + case VI_ERROR_QUEUE_OVERFLOW: + return VI_ERROR_QUEUE_OVERFLOW_MESSAGE; + case VI_ERROR_IO: + return VI_ERROR_IO_MESSAGE; + case VI_ERROR_ASRL_PARITY: + return VI_ERROR_ASRL_PARITY_MESSAGE; + case VI_ERROR_ASRL_FRAMING: + return VI_ERROR_ASRL_FRAMING_MESSAGE; + case VI_ERROR_ASRL_OVERRUN: + return VI_ERROR_ASRL_OVERRUN_MESSAGE; + case VI_ERROR_RSRC_BUSY: + return VI_ERROR_RSRC_BUSY_MESSAGE; + case VI_ERROR_INV_PARAMETER: + return VI_ERROR_INV_PARAMETER_MESSAGE; + default: + return "Unknown error status"; + } } /** @@ -169,13 +165,12 @@ const char* getHALErrorMessage(int32_t code) * For now, expect this to be competition year. * @return FPGA Version number. */ -uint16_t getFPGAVersion(int32_t *status) -{ - if (!global) { - *status = NiFpga_Status_ResourceNotInitialized; - return 0; - } - return global->readVersion(status); +uint16_t getFPGAVersion(int32_t* status) { + if (!global) { + *status = NiFpga_Status_ResourceNotInitialized; + return 0; + } + return global->readVersion(status); } /** @@ -186,243 +181,210 @@ uint16_t getFPGAVersion(int32_t *status) * The 12 least significant bits are the Build Number. * @return FPGA Revision number. */ -uint32_t getFPGARevision(int32_t *status) -{ - if (!global) { - *status = NiFpga_Status_ResourceNotInitialized; - return 0; - } - return global->readRevision(status); +uint32_t getFPGARevision(int32_t* status) { + if (!global) { + *status = NiFpga_Status_ResourceNotInitialized; + return 0; + } + return global->readRevision(status); } /** * Read the microsecond-resolution timer on the FPGA. * - * @return The current time in microseconds according to the FPGA (since FPGA reset). + * @return The current time in microseconds according to the FPGA (since FPGA + * reset). */ -uint64_t getFPGATime(int32_t *status) -{ - if (!global) { - *status = NiFpga_Status_ResourceNotInitialized; - return 0; - } - std::lock_guard lock(timeMutex); - uint32_t fpgaTime = global->readLocalTime(status); - if (*status != 0) return 0; - // check for rollover - if (fpgaTime < prevFPGATime) ++timeEpoch; - prevFPGATime = fpgaTime; - return (((uint64_t)timeEpoch) << 32) | ((uint64_t)fpgaTime); +uint64_t getFPGATime(int32_t* status) { + if (!global) { + *status = NiFpga_Status_ResourceNotInitialized; + return 0; + } + std::lock_guard lock(timeMutex); + uint32_t fpgaTime = global->readLocalTime(status); + if (*status != 0) return 0; + // check for rollover + if (fpgaTime < prevFPGATime) ++timeEpoch; + prevFPGATime = fpgaTime; + return (((uint64_t)timeEpoch) << 32) | ((uint64_t)fpgaTime); } /** * Get the state of the "USER" button on the RoboRIO * @return true if the button is currently pressed down */ -bool getFPGAButton(int32_t *status) -{ - if (!global) { - *status = NiFpga_Status_ResourceNotInitialized; - return false; - } - return global->readUserButton(status); +bool getFPGAButton(int32_t* status) { + if (!global) { + *status = NiFpga_Status_ResourceNotInitialized; + return false; + } + return global->readUserButton(status); } -int HALSetErrorData(const char *errors, int errorsLength, int wait_ms) -{ - return setErrorData(errors, errorsLength, wait_ms); +int HALSetErrorData(const char* errors, int errorsLength, int wait_ms) { + return setErrorData(errors, errorsLength, wait_ms); } int HALSendError(int isError, int32_t errorCode, int isLVCode, - const char *details, const char *location, const char *callStack, - int printMsg) -{ - // Avoid flooding console by keeping track of previous 5 error - // messages and only printing again if they're longer than 1 second old. - static constexpr int KEEP_MSGS = 5; - std::lock_guard lock(msgMutex); - static std::string prev_msg[KEEP_MSGS]; - static uint64_t prev_msg_time[KEEP_MSGS] = { 0, 0, 0 }; + const char* details, const char* location, + const char* callStack, int printMsg) { + // Avoid flooding console by keeping track of previous 5 error + // messages and only printing again if they're longer than 1 second old. + static constexpr int KEEP_MSGS = 5; + std::lock_guard lock(msgMutex); + static std::string prev_msg[KEEP_MSGS]; + static uint64_t prev_msg_time[KEEP_MSGS] = {0, 0, 0}; - int32_t status = 0; - uint64_t curTime = getFPGATime(&status); - int i; - for (i=0; i= 1000000) { - retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode, details, location, callStack); - if (printMsg) { - if (location && location[0] != '\0') { - fprintf(stderr, "%s at %s: ", - isError ? "Error" : "Warning", - location); - } - fprintf(stderr, "%s\n", details); - if (callStack && callStack[0] != '\0') { - fprintf(stderr, "%s\n", callStack); - } - } - if (i == KEEP_MSGS) { - // replace the oldest one - i = 0; - uint64_t first = prev_msg_time[0]; - for (int j=1; j= 1000000) { + retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode, + details, location, callStack); + if (printMsg) { + if (location && location[0] != '\0') { + fprintf(stderr, "%s at %s: ", isError ? "Error" : "Warning", location); + } + fprintf(stderr, "%s\n", details); + if (callStack && callStack[0] != '\0') { + fprintf(stderr, "%s\n", callStack); + } + } + if (i == KEEP_MSGS) { + // replace the oldest one + i = 0; + uint64_t first = prev_msg_time[0]; + for (int j = 1; j < KEEP_MSGS; ++j) { + if (prev_msg_time[j] < first) { + first = prev_msg_time[j]; + i = j; + } + } + prev_msg[i] = details; + } + prev_msg_time[i] = curTime; + } + return retval; } - -bool HALGetSystemActive(int32_t *status) -{ - if (!watchdog) { - *status = NiFpga_Status_ResourceNotInitialized; - return false; - } - return watchdog->readStatus_SystemActive(status); +bool HALGetSystemActive(int32_t* status) { + if (!watchdog) { + *status = NiFpga_Status_ResourceNotInitialized; + return false; + } + return watchdog->readStatus_SystemActive(status); } -bool HALGetBrownedOut(int32_t *status) -{ - if (!watchdog) { - *status = NiFpga_Status_ResourceNotInitialized; - return false; - } - return !(watchdog->readStatus_PowerAlive(status)); +bool HALGetBrownedOut(int32_t* status) { + if (!watchdog) { + *status = NiFpga_Status_ResourceNotInitialized; + return false; + } + return !(watchdog->readStatus_PowerAlive(status)); } static void HALCleanupAtExit() { - global = nullptr; - watchdog = nullptr; + global = nullptr; + watchdog = nullptr; } static void timerRollover(uint64_t currentTime, void*) { - // reschedule timer for next rollover - int32_t status = 0; - updateNotifierAlarm(rolloverNotifier, currentTime + 0x80000000ULL, &status); + // reschedule timer for next rollover + int32_t status = 0; + updateNotifierAlarm(rolloverNotifier, currentTime + 0x80000000ULL, &status); } /** * Call this to start up HAL. This is required for robot programs. */ -int HALInitialize(int mode) -{ - setlinebuf(stdin); - setlinebuf(stdout); +int HALInitialize(int mode) { + setlinebuf(stdin); + setlinebuf(stdout); - prctl(PR_SET_PDEATHSIG, SIGTERM); + prctl(PR_SET_PDEATHSIG, SIGTERM); - FRC_NetworkCommunication_Reserve(nullptr); - // image 4; Fixes errors caused by multiple processes. Talk to NI about this - nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass = - nLoadOut::kTargetClass_RoboRIO; + FRC_NetworkCommunication_Reserve(nullptr); + // image 4; Fixes errors caused by multiple processes. Talk to NI about this + nFPGA::nRoboRIO_FPGANamespace::g_currentTargetClass = + nLoadOut::kTargetClass_RoboRIO; - int32_t status = 0; - global = tGlobal::create(&status); - watchdog = tSysWatchdog::create(&status); + int32_t status = 0; + global = tGlobal::create(&status); + watchdog = tSysWatchdog::create(&status); - std::atexit(HALCleanupAtExit); + std::atexit(HALCleanupAtExit); - if (!rolloverNotifier) - rolloverNotifier = initializeNotifier(timerRollover, nullptr, &status); - if (status == 0) { - uint64_t curTime = getFPGATime(&status); - if (status == 0) - updateNotifierAlarm(rolloverNotifier, curTime + 0x80000000ULL, &status); - } + if (!rolloverNotifier) + rolloverNotifier = initializeNotifier(timerRollover, nullptr, &status); + if (status == 0) { + uint64_t curTime = getFPGATime(&status); + if (status == 0) + updateNotifierAlarm(rolloverNotifier, curTime + 0x80000000ULL, &status); + } - // Kill any previous robot programs - std::fstream fs; - // By making this both in/out, it won't give us an error if it doesnt exist - fs.open("/var/lock/frc.pid", std::fstream::in | std::fstream::out); - if (fs.bad()) - return 0; + // Kill any previous robot programs + std::fstream fs; + // By making this both in/out, it won't give us an error if it doesnt exist + fs.open("/var/lock/frc.pid", std::fstream::in | std::fstream::out); + if (fs.bad()) return 0; - pid_t pid = 0; - if (!fs.eof() && !fs.fail()) - { - fs >> pid; - //see if the pid is around, but we don't want to mess with init id=1, or ourselves - if (pid >= 2 && kill(pid, 0) == 0 && pid != getpid()) - { - std::cout << "Killing previously running FRC program..." - << std::endl; - kill(pid, SIGTERM); // try to kill it - delayMillis(100); - if (kill(pid, 0) == 0) - { - // still not successfull - if (mode == 0) - { - std::cout << "FRC pid " << pid - << " did not die within 110ms. Aborting" - << std::endl; - return 0; // just fail - } - else if (mode == 1) // kill -9 it - kill(pid, SIGKILL); - else - { - std::cout << "WARNING: FRC pid " << pid - << " did not die within 110ms." << std::endl; - } - } + pid_t pid = 0; + if (!fs.eof() && !fs.fail()) { + fs >> pid; + // see if the pid is around, but we don't want to mess with init id=1, or + // ourselves + if (pid >= 2 && kill(pid, 0) == 0 && pid != getpid()) { + std::cout << "Killing previously running FRC program..." << std::endl; + kill(pid, SIGTERM); // try to kill it + delayMillis(100); + if (kill(pid, 0) == 0) { + // still not successfull + if (mode == 0) { + std::cout << "FRC pid " << pid + << " did not die within 110ms. Aborting" << std::endl; + return 0; // just fail + } else if (mode == 1) // kill -9 it + kill(pid, SIGKILL); + else { + std::cout << "WARNING: FRC pid " << pid + << " did not die within 110ms." << std::endl; + } + } + } + } + fs.close(); + // we will re-open it write only to truncate the file + fs.open("/var/lock/frc.pid", std::fstream::out | std::fstream::trunc); + fs.seekp(0); + pid = getpid(); + fs << pid << std::endl; + fs.close(); - } - } - fs.close(); - // we will re-open it write only to truncate the file - fs.open("/var/lock/frc.pid", std::fstream::out | std::fstream::trunc); - fs.seekp(0); - pid = getpid(); - fs << pid << std::endl; - fs.close(); - - return 1; + return 1; } uint32_t HALReport(uint8_t resource, uint8_t instanceNumber, uint8_t context, - const char *feature) -{ - if(feature == NULL) - { - feature = ""; - } + const char* feature) { + if (feature == NULL) { + feature = ""; + } - return FRC_NetworkCommunication_nUsageReporting_report(resource, instanceNumber, context, feature); + return FRC_NetworkCommunication_nUsageReporting_report( + resource, instanceNumber, context, feature); } // TODO: HACKS -void NumericArrayResize() -{ -} -void RTSetCleanupProc() -{ -} -void EDVR_CreateReference() -{ -} -void Occur() -{ -} +void NumericArrayResize() {} +void RTSetCleanupProc() {} +void EDVR_CreateReference() {} +void Occur() {} -void imaqGetErrorText() -{ -} -void imaqGetLastError() -{ -} -void niTimestamp64() -{ -} +void imaqGetErrorText() {} +void imaqGetLastError() {} +void niTimestamp64() {} } // extern "C" diff --git a/hal/lib/Athena/Interrupts.cpp b/hal/lib/Athena/Interrupts.cpp index 55c4660f8e..b461548234 100644 --- a/hal/lib/Athena/Interrupts.cpp +++ b/hal/lib/Athena/Interrupts.cpp @@ -8,34 +8,34 @@ #include "HAL/Interrupts.hpp" #include "ChipObject.h" -extern void remapDigitalSource(bool analogTrigger, uint32_t &pin, uint8_t &module); +extern void remapDigitalSource(bool analogTrigger, uint32_t& pin, + uint8_t& module); -struct Interrupt // FIXME: why is this internal? +struct Interrupt // FIXME: why is this internal? { - tInterrupt *anInterrupt; - tInterruptManager *manager; + tInterrupt* anInterrupt; + tInterruptManager* manager; }; extern "C" { -void* initializeInterrupts(uint32_t interruptIndex, bool watcher, int32_t *status) -{ - Interrupt* anInterrupt = new Interrupt(); - // Expects the calling leaf class to allocate an interrupt index. - anInterrupt->anInterrupt = tInterrupt::create(interruptIndex, status); - anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status); - anInterrupt->manager = new tInterruptManager( - (1 << interruptIndex) | (1 << (interruptIndex + 8)), watcher, status); - return anInterrupt; +void* initializeInterrupts(uint32_t interruptIndex, bool watcher, + int32_t* status) { + Interrupt* anInterrupt = new Interrupt(); + // Expects the calling leaf class to allocate an interrupt index. + anInterrupt->anInterrupt = tInterrupt::create(interruptIndex, status); + anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status); + anInterrupt->manager = new tInterruptManager( + (1 << interruptIndex) | (1 << (interruptIndex + 8)), watcher, status); + return anInterrupt; } -void cleanInterrupts(void* interrupt_pointer, int32_t *status) -{ - Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; - delete anInterrupt->anInterrupt; - delete anInterrupt->manager; - anInterrupt->anInterrupt = NULL; - anInterrupt->manager = NULL; +void cleanInterrupts(void* interrupt_pointer, int32_t* status) { + Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; + delete anInterrupt->anInterrupt; + delete anInterrupt->manager; + anInterrupt->anInterrupt = NULL; + anInterrupt->manager = NULL; } /** @@ -45,40 +45,40 @@ void cleanInterrupts(void* interrupt_pointer, int32_t *status) * waitForInterrupt was called. * @return The mask of interrupts that fired. */ -uint32_t waitForInterrupt(void* interrupt_pointer, double timeout, bool ignorePrevious, int32_t *status) -{ - uint32_t result; - Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; +uint32_t waitForInterrupt(void* interrupt_pointer, double timeout, + bool ignorePrevious, int32_t* status) { + uint32_t result; + Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; - result = anInterrupt->manager->watch((int32_t)(timeout * 1e3), ignorePrevious, status); + result = anInterrupt->manager->watch((int32_t)(timeout * 1e3), ignorePrevious, + status); - // Don't report a timeout as an error - the return code is enough to tell - // that a timeout happened. - if(*status == -NiFpga_Status_IrqTimeout) { - *status = NiFpga_Status_Success; - } + // Don't report a timeout as an error - the return code is enough to tell + // that a timeout happened. + if (*status == -NiFpga_Status_IrqTimeout) { + *status = NiFpga_Status_Success; + } - return result; + return result; } /** * Enable interrupts to occur on this input. - * Interrupts are disabled when the RequestInterrupt call is made. This gives time to do the - * setup of the other options before starting to field interrupts. + * Interrupts are disabled when the RequestInterrupt call is made. This gives + * time to do the setup of the other options before starting to field + * interrupts. */ -void enableInterrupts(void* interrupt_pointer, int32_t *status) -{ - Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; - anInterrupt->manager->enable(status); +void enableInterrupts(void* interrupt_pointer, int32_t* status) { + Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; + anInterrupt->manager->enable(status); } /** * Disable Interrupts without without deallocating structures. */ -void disableInterrupts(void* interrupt_pointer, int32_t *status) -{ - Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; - anInterrupt->manager->disable(status); +void disableInterrupts(void* interrupt_pointer, int32_t* status) { + Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; + anInterrupt->manager->disable(status); } /** @@ -86,11 +86,10 @@ void disableInterrupts(void* interrupt_pointer, int32_t *status) * This is in the same time domain as GetClock(). * @return Timestamp in seconds since boot. */ -double readRisingTimestamp(void* interrupt_pointer, int32_t *status) -{ - Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; - uint32_t timestamp = anInterrupt->anInterrupt->readRisingTimeStamp(status); - return timestamp * 1e-6; +double readRisingTimestamp(void* interrupt_pointer, int32_t* status) { + Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; + uint32_t timestamp = anInterrupt->anInterrupt->readRisingTimeStamp(status); + return timestamp * 1e-6; } /** @@ -98,37 +97,36 @@ double readRisingTimestamp(void* interrupt_pointer, int32_t *status) * This is in the same time domain as GetClock(). * @return Timestamp in seconds since boot. */ -double readFallingTimestamp(void* interrupt_pointer, int32_t *status) -{ - Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; - uint32_t timestamp = anInterrupt->anInterrupt->readFallingTimeStamp(status); - return timestamp * 1e-6; +double readFallingTimestamp(void* interrupt_pointer, int32_t* status) { + Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; + uint32_t timestamp = anInterrupt->anInterrupt->readFallingTimeStamp(status); + return timestamp * 1e-6; } -void requestInterrupts(void* interrupt_pointer, uint8_t routing_module, uint32_t routing_pin, - bool routing_analog_trigger, int32_t *status) -{ - Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; - anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status); - remapDigitalSource(routing_analog_trigger, routing_pin, routing_module); - anInterrupt->anInterrupt->writeConfig_Source_AnalogTrigger(routing_analog_trigger, status); - anInterrupt->anInterrupt->writeConfig_Source_Channel(routing_pin, status); - anInterrupt->anInterrupt->writeConfig_Source_Module(routing_module, status); +void requestInterrupts(void* interrupt_pointer, uint8_t routing_module, + uint32_t routing_pin, bool routing_analog_trigger, + int32_t* status) { + Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; + anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status); + remapDigitalSource(routing_analog_trigger, routing_pin, routing_module); + anInterrupt->anInterrupt->writeConfig_Source_AnalogTrigger( + routing_analog_trigger, status); + anInterrupt->anInterrupt->writeConfig_Source_Channel(routing_pin, status); + anInterrupt->anInterrupt->writeConfig_Source_Module(routing_module, status); } -void attachInterruptHandler(void* interrupt_pointer, InterruptHandlerFunction handler, void* param, - int32_t *status) -{ - Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; - anInterrupt->manager->registerHandler(handler, param, status); +void attachInterruptHandler(void* interrupt_pointer, + InterruptHandlerFunction handler, void* param, + int32_t* status) { + Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; + anInterrupt->manager->registerHandler(handler, param, status); } -void setInterruptUpSourceEdge(void* interrupt_pointer, bool risingEdge, bool fallingEdge, - int32_t *status) -{ - Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; - anInterrupt->anInterrupt->writeConfig_RisingEdge(risingEdge, status); - anInterrupt->anInterrupt->writeConfig_FallingEdge(fallingEdge, status); +void setInterruptUpSourceEdge(void* interrupt_pointer, bool risingEdge, + bool fallingEdge, int32_t* status) { + Interrupt* anInterrupt = (Interrupt*)interrupt_pointer; + anInterrupt->anInterrupt->writeConfig_RisingEdge(risingEdge, status); + anInterrupt->anInterrupt->writeConfig_FallingEdge(fallingEdge, status); } } // extern "C" diff --git a/hal/lib/Athena/Notifier.cpp b/hal/lib/Athena/Notifier.cpp index 9679979890..95e9aee20b 100644 --- a/hal/lib/Athena/Notifier.cpp +++ b/hal/lib/Athena/Notifier.cpp @@ -6,163 +6,159 @@ /*----------------------------------------------------------------------------*/ #include "HAL/Notifier.hpp" -#include "ChipObject.h" -#include "HAL/HAL.hpp" -#include "HAL/cpp/priority_mutex.h" #include #include #include +#include "ChipObject.h" +#include "HAL/HAL.hpp" +#include "HAL/cpp/priority_mutex.h" static const uint32_t kTimerInterruptNumber = 28; static priority_mutex notifierInterruptMutex; static priority_recursive_mutex notifierMutex; -static tAlarm *notifierAlarm = nullptr; -static tInterruptManager *notifierManager = nullptr; +static tAlarm* notifierAlarm = nullptr; +static tInterruptManager* notifierManager = nullptr; static uint64_t closestTrigger = UINT64_MAX; struct Notifier { - Notifier *prev, *next; - void *param; - void (*process)(uint64_t, void*); - uint64_t triggerTime = UINT64_MAX; + Notifier *prev, *next; + void* param; + void (*process)(uint64_t, void*); + uint64_t triggerTime = UINT64_MAX; }; -static Notifier *notifiers = nullptr; +static Notifier* notifiers = nullptr; static std::atomic_flag notifierAtexitRegistered = ATOMIC_FLAG_INIT; static std::atomic_int notifierRefCount{0}; -static void alarmCallback(uint32_t, void*) -{ - std::unique_lock sync(notifierMutex); +static void alarmCallback(uint32_t, void*) { + std::unique_lock sync(notifierMutex); - int32_t status = 0; - uint64_t currentTime = 0; + int32_t status = 0; + uint64_t currentTime = 0; - // the hardware disables itself after each alarm - closestTrigger = UINT64_MAX; + // the hardware disables itself after each alarm + closestTrigger = UINT64_MAX; - // process all notifiers - Notifier *notifier = notifiers; - while (notifier) { - if (notifier->triggerTime != UINT64_MAX) { - if (currentTime == 0) - currentTime = getFPGATime(&status); - if (notifier->triggerTime < currentTime) { - notifier->triggerTime = UINT64_MAX; - auto process = notifier->process; - auto param = notifier->param; - sync.unlock(); - process(currentTime, param); - sync.lock(); - } else if (notifier->triggerTime < closestTrigger) { - updateNotifierAlarm(notifier, notifier->triggerTime, &status); - } - } - notifier = notifier->next; - } + // process all notifiers + Notifier* notifier = notifiers; + while (notifier) { + if (notifier->triggerTime != UINT64_MAX) { + if (currentTime == 0) currentTime = getFPGATime(&status); + if (notifier->triggerTime < currentTime) { + notifier->triggerTime = UINT64_MAX; + auto process = notifier->process; + auto param = notifier->param; + sync.unlock(); + process(currentTime, param); + sync.lock(); + } else if (notifier->triggerTime < closestTrigger) { + updateNotifierAlarm(notifier, notifier->triggerTime, &status); + } + } + notifier = notifier->next; + } } static void cleanupNotifierAtExit() { - notifierAlarm = nullptr; - notifierManager = nullptr; + notifierAlarm = nullptr; + notifierManager = nullptr; } extern "C" { -void* initializeNotifier(void (*process)(uint64_t, void*), void *param, int32_t *status) -{ - if (!process) { - *status = NULL_PARAMETER; - return nullptr; - } - if (!notifierAtexitRegistered.test_and_set()) - std::atexit(cleanupNotifierAtExit); - if (notifierRefCount.fetch_add(1) == 0) { - std::lock_guard sync(notifierInterruptMutex); - // create manager and alarm if not already created - if (!notifierManager) { - notifierManager = new tInterruptManager(1 << kTimerInterruptNumber, false, status); - notifierManager->registerHandler(alarmCallback, NULL, status); - notifierManager->enable(status); - } - if (!notifierAlarm) notifierAlarm = tAlarm::create(status); - } +void* initializeNotifier(void (*process)(uint64_t, void*), void* param, + int32_t* status) { + if (!process) { + *status = NULL_PARAMETER; + return nullptr; + } + if (!notifierAtexitRegistered.test_and_set()) + std::atexit(cleanupNotifierAtExit); + if (notifierRefCount.fetch_add(1) == 0) { + std::lock_guard sync(notifierInterruptMutex); + // create manager and alarm if not already created + if (!notifierManager) { + notifierManager = + new tInterruptManager(1 << kTimerInterruptNumber, false, status); + notifierManager->registerHandler(alarmCallback, NULL, status); + notifierManager->enable(status); + } + if (!notifierAlarm) notifierAlarm = tAlarm::create(status); + } - std::lock_guard sync(notifierMutex); - // create notifier structure and add to list - Notifier* notifier = new Notifier(); - notifier->prev = nullptr; - notifier->next = notifiers; - if (notifier->next) notifier->next->prev = notifier; - notifier->param = param; - notifier->process = process; - notifiers = notifier; - return notifier; + std::lock_guard sync(notifierMutex); + // create notifier structure and add to list + Notifier* notifier = new Notifier(); + notifier->prev = nullptr; + notifier->next = notifiers; + if (notifier->next) notifier->next->prev = notifier; + notifier->param = param; + notifier->process = process; + notifiers = notifier; + return notifier; } -void cleanNotifier(void* notifier_pointer, int32_t *status) -{ - { - std::lock_guard sync(notifierMutex); - Notifier* notifier = (Notifier*)notifier_pointer; +void cleanNotifier(void* notifier_pointer, int32_t* status) { + { + std::lock_guard sync(notifierMutex); + Notifier* notifier = (Notifier*)notifier_pointer; - // remove from list and delete - if (notifier->prev) notifier->prev->next = notifier->next; - if (notifier->next) notifier->next->prev = notifier->prev; - if (notifiers == notifier) notifiers = notifier->next; - delete notifier; - } + // remove from list and delete + if (notifier->prev) notifier->prev->next = notifier->next; + if (notifier->next) notifier->next->prev = notifier->prev; + if (notifiers == notifier) notifiers = notifier->next; + delete notifier; + } - if (notifierRefCount.fetch_sub(1) == 1) { - std::lock_guard sync(notifierInterruptMutex); - // if this was the last notifier, clean up alarm and manager - if (notifierAlarm) { - notifierAlarm->writeEnable(false, status); - delete notifierAlarm; - notifierAlarm = nullptr; - } - if (notifierManager) { - notifierManager->disable(status); - delete notifierManager; - notifierManager = nullptr; - } - closestTrigger = UINT64_MAX; - } + if (notifierRefCount.fetch_sub(1) == 1) { + std::lock_guard sync(notifierInterruptMutex); + // if this was the last notifier, clean up alarm and manager + if (notifierAlarm) { + notifierAlarm->writeEnable(false, status); + delete notifierAlarm; + notifierAlarm = nullptr; + } + if (notifierManager) { + notifierManager->disable(status); + delete notifierManager; + notifierManager = nullptr; + } + closestTrigger = UINT64_MAX; + } } -void* getNotifierParam(void* notifier_pointer, int32_t *status) -{ - return ((Notifier*)notifier_pointer)->param; +void* getNotifierParam(void* notifier_pointer, int32_t* status) { + return ((Notifier*)notifier_pointer)->param; } -void updateNotifierAlarm(void* notifier_pointer, uint64_t triggerTime, int32_t *status) -{ - std::lock_guard sync(notifierMutex); +void updateNotifierAlarm(void* notifier_pointer, uint64_t triggerTime, + int32_t* status) { + std::lock_guard sync(notifierMutex); - Notifier* notifier = (Notifier*)notifier_pointer; - notifier->triggerTime = triggerTime; - bool wasActive = (closestTrigger != UINT64_MAX); + Notifier* notifier = (Notifier*)notifier_pointer; + notifier->triggerTime = triggerTime; + bool wasActive = (closestTrigger != UINT64_MAX); - if (!notifierInterruptMutex.try_lock() || notifierRefCount == 0 || - !notifierAlarm) - return; + if (!notifierInterruptMutex.try_lock() || notifierRefCount == 0 || + !notifierAlarm) + return; - // Update alarm time if closer than current. - if (triggerTime < closestTrigger) { - closestTrigger = triggerTime; - // Simply truncate the hardware trigger time to 32-bit. - notifierAlarm->writeTriggerTime((uint32_t)triggerTime, status); - } - // Enable the alarm. The hardware disables itself after each alarm. - if (!wasActive) notifierAlarm->writeEnable(true, status); + // Update alarm time if closer than current. + if (triggerTime < closestTrigger) { + closestTrigger = triggerTime; + // Simply truncate the hardware trigger time to 32-bit. + notifierAlarm->writeTriggerTime((uint32_t)triggerTime, status); + } + // Enable the alarm. The hardware disables itself after each alarm. + if (!wasActive) notifierAlarm->writeEnable(true, status); - notifierInterruptMutex.unlock(); + notifierInterruptMutex.unlock(); } -void stopNotifierAlarm(void* notifier_pointer, int32_t *status) -{ - std::lock_guard sync(notifierMutex); - Notifier* notifier = (Notifier*)notifier_pointer; - notifier->triggerTime = UINT64_MAX; +void stopNotifierAlarm(void* notifier_pointer, int32_t* status) { + std::lock_guard sync(notifierMutex); + Notifier* notifier = (Notifier*)notifier_pointer; + notifier->triggerTime = UINT64_MAX; } } // extern "C" diff --git a/hal/lib/Athena/PDP.cpp b/hal/lib/Athena/PDP.cpp index 334e8d4ea3..7df073db1d 100644 --- a/hal/lib/Athena/PDP.cpp +++ b/hal/lib/Athena/PDP.cpp @@ -7,74 +7,74 @@ #include "HAL/PDP.hpp" #include "ctre/PDP.h" -//static PDP pdp; +// static PDP pdp; static const int NUM_MODULE_NUMBERS = 63; -static PDP *pdp[NUM_MODULE_NUMBERS] = { NULL }; +static PDP* pdp[NUM_MODULE_NUMBERS] = {NULL}; extern "C" { void initializePDP(uint8_t module) { - if(!pdp[module]) { - pdp[module] = new PDP(module); - } + if (!pdp[module]) { + pdp[module] = new PDP(module); + } } -double getPDPTemperature(uint8_t module, int32_t *status) { - double temperature; +double getPDPTemperature(uint8_t module, int32_t* status) { + double temperature; - *status = pdp[module]->GetTemperature(temperature); + *status = pdp[module]->GetTemperature(temperature); - return temperature; + return temperature; } -double getPDPVoltage(uint8_t module, int32_t *status) { - double voltage; +double getPDPVoltage(uint8_t module, int32_t* status) { + double voltage; - *status = pdp[module]->GetVoltage(voltage); + *status = pdp[module]->GetVoltage(voltage); - return voltage; + return voltage; } -double getPDPChannelCurrent(uint8_t module, uint8_t channel, int32_t *status) { - double current; +double getPDPChannelCurrent(uint8_t module, uint8_t channel, int32_t* status) { + double current; - *status = pdp[module]->GetChannelCurrent(channel, current); + *status = pdp[module]->GetChannelCurrent(channel, current); - return current; + return current; } -double getPDPTotalCurrent(uint8_t module, int32_t *status) { - double current; +double getPDPTotalCurrent(uint8_t module, int32_t* status) { + double current; - *status = pdp[module]->GetTotalCurrent(current); + *status = pdp[module]->GetTotalCurrent(current); - return current; + return current; } -double getPDPTotalPower(uint8_t module, int32_t *status) { - double power; +double getPDPTotalPower(uint8_t module, int32_t* status) { + double power; - *status = pdp[module]->GetTotalPower(power); + *status = pdp[module]->GetTotalPower(power); - return power; + return power; } -double getPDPTotalEnergy(uint8_t module, int32_t *status) { - double energy; +double getPDPTotalEnergy(uint8_t module, int32_t* status) { + double energy; - *status = pdp[module]->GetTotalEnergy(energy); + *status = pdp[module]->GetTotalEnergy(energy); - return energy; + return energy; } -void resetPDPTotalEnergy(uint8_t module, int32_t *status) { - *status = pdp[module]->ResetEnergy(); +void resetPDPTotalEnergy(uint8_t module, int32_t* status) { + *status = pdp[module]->ResetEnergy(); } -void clearPDPStickyFaults(uint8_t module, int32_t *status) { - *status = pdp[module]->ClearStickyFaults(); +void clearPDPStickyFaults(uint8_t module, int32_t* status) { + *status = pdp[module]->ClearStickyFaults(); } } // extern "C" diff --git a/hal/lib/Athena/Power.cpp b/hal/lib/Athena/Power.cpp index 47da91860e..0a6f373cf5 100644 --- a/hal/lib/Athena/Power.cpp +++ b/hal/lib/Athena/Power.cpp @@ -8,12 +8,12 @@ #include "HAL/Power.hpp" #include "ChipObject.h" -static tPower *power = NULL; +static tPower* power = NULL; -static void initializePower(int32_t *status) { - if(power == NULL) { - power = tPower::create(status); - } +static void initializePower(int32_t* status) { + if (power == NULL) { + power = tPower::create(status); + } } extern "C" { @@ -21,118 +21,118 @@ extern "C" { /** * Get the roboRIO input voltage */ -float getVinVoltage(int32_t *status) { - initializePower(status); - return power->readVinVoltage(status) / 4.096f * 0.025733f - 0.029f; +float getVinVoltage(int32_t* status) { + initializePower(status); + return power->readVinVoltage(status) / 4.096f * 0.025733f - 0.029f; } /** * Get the roboRIO input current */ -float getVinCurrent(int32_t *status) { - initializePower(status); - return power->readVinCurrent(status) / 4.096f * 0.017042 - 0.071f; +float getVinCurrent(int32_t* status) { + initializePower(status); + return power->readVinCurrent(status) / 4.096f * 0.017042 - 0.071f; } /** * Get the 6V rail voltage */ -float getUserVoltage6V(int32_t *status) { - initializePower(status); - return power->readUserVoltage6V(status) / 4.096f * 0.007019f - 0.014f; +float getUserVoltage6V(int32_t* status) { + initializePower(status); + return power->readUserVoltage6V(status) / 4.096f * 0.007019f - 0.014f; } /** * Get the 6V rail current */ -float getUserCurrent6V(int32_t *status) { - initializePower(status); - return power->readUserCurrent6V(status) / 4.096f * 0.005566f - 0.009f; +float getUserCurrent6V(int32_t* status) { + initializePower(status); + return power->readUserCurrent6V(status) / 4.096f * 0.005566f - 0.009f; } /** * Get the active state of the 6V rail */ -bool getUserActive6V(int32_t *status) { - initializePower(status); - return power->readStatus_User6V(status) == 4; +bool getUserActive6V(int32_t* status) { + initializePower(status); + return power->readStatus_User6V(status) == 4; } /** * Get the fault count for the 6V rail */ -int getUserCurrentFaults6V(int32_t *status) { - initializePower(status); - return (int)power->readFaultCounts_OverCurrentFaultCount6V(status); +int getUserCurrentFaults6V(int32_t* status) { + initializePower(status); + return (int)power->readFaultCounts_OverCurrentFaultCount6V(status); } /** * Get the 5V rail voltage */ -float getUserVoltage5V(int32_t *status) { - initializePower(status); - return power->readUserVoltage5V(status) / 4.096f * 0.005962f - 0.013f; +float getUserVoltage5V(int32_t* status) { + initializePower(status); + return power->readUserVoltage5V(status) / 4.096f * 0.005962f - 0.013f; } /** * Get the 5V rail current */ -float getUserCurrent5V(int32_t *status) { - initializePower(status); - return power->readUserCurrent5V(status) / 4.096f * 0.001996f - 0.002f; +float getUserCurrent5V(int32_t* status) { + initializePower(status); + return power->readUserCurrent5V(status) / 4.096f * 0.001996f - 0.002f; } /** * Get the active state of the 5V rail */ -bool getUserActive5V(int32_t *status) { - initializePower(status); - return power->readStatus_User5V(status) == 4; +bool getUserActive5V(int32_t* status) { + initializePower(status); + return power->readStatus_User5V(status) == 4; } /** * Get the fault count for the 5V rail */ -int getUserCurrentFaults5V(int32_t *status) { - initializePower(status); - return (int)power->readFaultCounts_OverCurrentFaultCount5V(status); +int getUserCurrentFaults5V(int32_t* status) { + initializePower(status); + return (int)power->readFaultCounts_OverCurrentFaultCount5V(status); } -unsigned char getUserStatus5V(int32_t *status) { - initializePower(status); - return power->readStatus_User5V(status); +unsigned char getUserStatus5V(int32_t* status) { + initializePower(status); + return power->readStatus_User5V(status); } /** * Get the 3.3V rail voltage */ -float getUserVoltage3V3(int32_t *status) { - initializePower(status); - return power->readUserVoltage3V3(status) / 4.096f * 0.004902f - 0.01f; +float getUserVoltage3V3(int32_t* status) { + initializePower(status); + return power->readUserVoltage3V3(status) / 4.096f * 0.004902f - 0.01f; } /** * Get the 3.3V rail current */ -float getUserCurrent3V3(int32_t *status) { - initializePower(status); - return power->readUserCurrent3V3(status) / 4.096f * 0.002486f - 0.003f; +float getUserCurrent3V3(int32_t* status) { + initializePower(status); + return power->readUserCurrent3V3(status) / 4.096f * 0.002486f - 0.003f; } /** * Get the active state of the 3.3V rail */ -bool getUserActive3V3(int32_t *status) { - initializePower(status); - return power->readStatus_User3V3(status) == 4; +bool getUserActive3V3(int32_t* status) { + initializePower(status); + return power->readStatus_User3V3(status) == 4; } /** * Get the fault count for the 3.3V rail */ -int getUserCurrentFaults3V3(int32_t *status) { - initializePower(status); - return (int)power->readFaultCounts_OverCurrentFaultCount3V3(status); +int getUserCurrentFaults3V3(int32_t* status) { + initializePower(status); + return (int)power->readFaultCounts_OverCurrentFaultCount3V3(status); } } // extern "C" diff --git a/hal/lib/Athena/Semaphore.cpp b/hal/lib/Athena/Semaphore.cpp index 8403438118..d1b44d58b9 100644 --- a/hal/lib/Athena/Semaphore.cpp +++ b/hal/lib/Athena/Semaphore.cpp @@ -12,9 +12,11 @@ // set the logging level TLogLevel semaphoreLogLevel = logDEBUG; -#define SEMAPHORE_LOG(level) \ - if (level > semaphoreLogLevel) ; \ - else Log().Get(level) +#define SEMAPHORE_LOG(level) \ + if (level > semaphoreLogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { diff --git a/hal/lib/Athena/SerialPort.cpp b/hal/lib/Athena/SerialPort.cpp index 5e8d065fcb..e667dbfa4d 100644 --- a/hal/lib/Athena/SerialPort.cpp +++ b/hal/lib/Athena/SerialPort.cpp @@ -8,150 +8,140 @@ #include "HAL/SerialPort.hpp" #include "visa/visa.h" - static uint32_t m_resourceManagerHandle; static uint32_t m_portHandle[2]; extern "C" { -void serialInitializePort(uint8_t port, int32_t *status) { - char const * portName; +void serialInitializePort(uint8_t port, int32_t* status) { + char const* portName; - if (m_resourceManagerHandle ==0) - viOpenDefaultRM((ViSession*)&m_resourceManagerHandle); - - if(port == 0) - portName = "ASRL1::INSTR"; - else if (port == 1) - portName = "ASRL2::INSTR"; - else - portName = "ASRL3::INSTR"; + if (m_resourceManagerHandle == 0) + viOpenDefaultRM((ViSession*)&m_resourceManagerHandle); - *status = viOpen(m_resourceManagerHandle, const_cast(portName), VI_NULL, VI_NULL, (ViSession*)&m_portHandle[port]); - if(*status > 0) - *status = 0; + if (port == 0) + portName = "ASRL1::INSTR"; + else if (port == 1) + portName = "ASRL2::INSTR"; + else + portName = "ASRL3::INSTR"; + + *status = viOpen(m_resourceManagerHandle, const_cast(portName), + VI_NULL, VI_NULL, (ViSession*)&m_portHandle[port]); + if (*status > 0) *status = 0; } -void serialSetBaudRate(uint8_t port, uint32_t baud, int32_t *status) { - *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_BAUD, baud); - if(*status > 0) - *status = 0; -} - -void serialSetDataBits(uint8_t port, uint8_t bits, int32_t *status) { - *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_DATA_BITS, bits); - if(*status > 0) - *status = 0; +void serialSetBaudRate(uint8_t port, uint32_t baud, int32_t* status) { + *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_BAUD, baud); + if (*status > 0) *status = 0; } -void serialSetParity(uint8_t port, uint8_t parity, int32_t *status) { - *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_PARITY, parity); - if(*status > 0) - *status = 0; -} - -void serialSetStopBits(uint8_t port, uint8_t stopBits, int32_t *status) { - *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_STOP_BITS, stopBits); - if(*status > 0) - *status = 0; +void serialSetDataBits(uint8_t port, uint8_t bits, int32_t* status) { + *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_DATA_BITS, bits); + if (*status > 0) *status = 0; } -void serialSetWriteMode(uint8_t port, uint8_t mode, int32_t *status) { - *status = viSetAttribute(m_portHandle[port], VI_ATTR_WR_BUF_OPER_MODE, mode); - if(*status > 0) - *status = 0; -} - -void serialSetFlowControl(uint8_t port, uint8_t flow, int32_t *status) { - *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_FLOW_CNTRL, flow); - if(*status > 0) - *status = 0; +void serialSetParity(uint8_t port, uint8_t parity, int32_t* status) { + *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_PARITY, parity); + if (*status > 0) *status = 0; } -void serialSetTimeout(uint8_t port, float timeout, int32_t *status) { - *status = viSetAttribute(m_portHandle[port], VI_ATTR_TMO_VALUE, (uint32_t)(timeout * 1e3)); - if(*status > 0) - *status = 0; -} - -void serialEnableTermination(uint8_t port, char terminator, int32_t *status) { - viSetAttribute(m_portHandle[port], VI_ATTR_TERMCHAR_EN, VI_TRUE); - viSetAttribute(m_portHandle[port], VI_ATTR_TERMCHAR, terminator); - *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_END_IN, VI_ASRL_END_TERMCHAR); - if(*status > 0) - *status = 0; +void serialSetStopBits(uint8_t port, uint8_t stopBits, int32_t* status) { + *status = + viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_STOP_BITS, stopBits); + if (*status > 0) *status = 0; } -void serialDisableTermination(uint8_t port, int32_t *status) { - viSetAttribute(m_portHandle[port], VI_ATTR_TERMCHAR_EN, VI_FALSE); - *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_END_IN, VI_ASRL_END_NONE); - if(*status > 0) - *status = 0; +void serialSetWriteMode(uint8_t port, uint8_t mode, int32_t* status) { + *status = viSetAttribute(m_portHandle[port], VI_ATTR_WR_BUF_OPER_MODE, mode); + if (*status > 0) *status = 0; } -void serialSetReadBufferSize(uint8_t port, uint32_t size, int32_t *status) { - *status = viSetBuf(m_portHandle[port], VI_READ_BUF, size); - if(*status > 0) - *status = 0; -} - -void serialSetWriteBufferSize(uint8_t port, uint32_t size, int32_t *status) { - *status = viSetBuf(m_portHandle[port], VI_WRITE_BUF, size); - if(*status > 0) - *status = 0; +void serialSetFlowControl(uint8_t port, uint8_t flow, int32_t* status) { + *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_FLOW_CNTRL, flow); + if (*status > 0) *status = 0; } -int32_t serialGetBytesReceived(uint8_t port, int32_t *status) { - int32_t bytes = 0; - - *status = viGetAttribute(m_portHandle[port], VI_ATTR_ASRL_AVAIL_NUM, &bytes); - if(*status > 0) - *status = 0; - return bytes; -} - -uint32_t serialRead(uint8_t port, char* buffer, int32_t count, int32_t *status) { - uint32_t retCount = 0; - - *status = viRead(m_portHandle[port], (ViPBuf)buffer, count, (ViPUInt32)&retCount); - - if(*status == VI_ERROR_IO || *status == VI_ERROR_ASRL_OVERRUN || *status == VI_ERROR_ASRL_FRAMING || *status == VI_ERROR_ASRL_PARITY) - { - int32_t localStatus = 0; - serialClear(port, &localStatus); - } - - if(*status == VI_ERROR_TMO || *status > 0) - *status = 0; - return retCount; -} - -uint32_t serialWrite(uint8_t port, const char *buffer, int32_t count, int32_t *status) { - uint32_t retCount = 0; - - *status = viWrite(m_portHandle[port], (ViPBuf)buffer, count, (ViPUInt32)&retCount); - - if(*status > 0) - *status = 0; - return retCount; +void serialSetTimeout(uint8_t port, float timeout, int32_t* status) { + *status = viSetAttribute(m_portHandle[port], VI_ATTR_TMO_VALUE, + (uint32_t)(timeout * 1e3)); + if (*status > 0) *status = 0; } -void serialFlush(uint8_t port, int32_t *status) { - *status = viFlush(m_portHandle[port], VI_WRITE_BUF); - if(*status > 0) - *status = 0; +void serialEnableTermination(uint8_t port, char terminator, int32_t* status) { + viSetAttribute(m_portHandle[port], VI_ATTR_TERMCHAR_EN, VI_TRUE); + viSetAttribute(m_portHandle[port], VI_ATTR_TERMCHAR, terminator); + *status = viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_END_IN, + VI_ASRL_END_TERMCHAR); + if (*status > 0) *status = 0; } -void serialClear(uint8_t port, int32_t *status) { - *status = viClear(m_portHandle[port]); - if(*status > 0) - *status = 0; +void serialDisableTermination(uint8_t port, int32_t* status) { + viSetAttribute(m_portHandle[port], VI_ATTR_TERMCHAR_EN, VI_FALSE); + *status = + viSetAttribute(m_portHandle[port], VI_ATTR_ASRL_END_IN, VI_ASRL_END_NONE); + if (*status > 0) *status = 0; } -void serialClose(uint8_t port, int32_t *status) { - *status = viClose(m_portHandle[port]); - if(*status > 0) - *status = 0; +void serialSetReadBufferSize(uint8_t port, uint32_t size, int32_t* status) { + *status = viSetBuf(m_portHandle[port], VI_READ_BUF, size); + if (*status > 0) *status = 0; +} + +void serialSetWriteBufferSize(uint8_t port, uint32_t size, int32_t* status) { + *status = viSetBuf(m_portHandle[port], VI_WRITE_BUF, size); + if (*status > 0) *status = 0; +} + +int32_t serialGetBytesReceived(uint8_t port, int32_t* status) { + int32_t bytes = 0; + + *status = viGetAttribute(m_portHandle[port], VI_ATTR_ASRL_AVAIL_NUM, &bytes); + if (*status > 0) *status = 0; + return bytes; +} + +uint32_t serialRead(uint8_t port, char* buffer, int32_t count, + int32_t* status) { + uint32_t retCount = 0; + + *status = + viRead(m_portHandle[port], (ViPBuf)buffer, count, (ViPUInt32)&retCount); + + if (*status == VI_ERROR_IO || *status == VI_ERROR_ASRL_OVERRUN || + *status == VI_ERROR_ASRL_FRAMING || *status == VI_ERROR_ASRL_PARITY) { + int32_t localStatus = 0; + serialClear(port, &localStatus); + } + + if (*status == VI_ERROR_TMO || *status > 0) *status = 0; + return retCount; +} + +uint32_t serialWrite(uint8_t port, const char* buffer, int32_t count, + int32_t* status) { + uint32_t retCount = 0; + + *status = + viWrite(m_portHandle[port], (ViPBuf)buffer, count, (ViPUInt32)&retCount); + + if (*status > 0) *status = 0; + return retCount; +} + +void serialFlush(uint8_t port, int32_t* status) { + *status = viFlush(m_portHandle[port], VI_WRITE_BUF); + if (*status > 0) *status = 0; +} + +void serialClear(uint8_t port, int32_t* status) { + *status = viClear(m_portHandle[port]); + if (*status > 0) *status = 0; +} + +void serialClose(uint8_t port, int32_t* status) { + *status = viClose(m_portHandle[port]); + if (*status > 0) *status = 0; } } // extern "C" diff --git a/hal/lib/Athena/Solenoid.cpp b/hal/lib/Athena/Solenoid.cpp index 6df930a2e5..57a1fb6b47 100644 --- a/hal/lib/Athena/Solenoid.cpp +++ b/hal/lib/Athena/Solenoid.cpp @@ -7,101 +7,100 @@ #include "HAL/Solenoid.hpp" -#include "HAL/Port.h" -#include "HAL/Errors.hpp" #include "ChipObject.h" #include "FRC_NetworkCommunication/LoadOut.h" +#include "HAL/Errors.hpp" +#include "HAL/Port.h" #include "ctre/PCM.h" static const int NUM_MODULE_NUMBERS = 63; -PCM *PCM_modules[NUM_MODULE_NUMBERS] = { NULL }; +PCM* PCM_modules[NUM_MODULE_NUMBERS] = {NULL}; struct solenoid_port_t { - PCM *module; - uint32_t pin; + PCM* module; + uint32_t pin; }; void initializePCM(int module) { - if(!PCM_modules[module]) { - PCM_modules[module] = new PCM(module); - } + if (!PCM_modules[module]) { + PCM_modules[module] = new PCM(module); + } } extern "C" { -void* initializeSolenoidPort(void *port_pointer, int32_t *status) { - Port* port = (Port*) port_pointer; - initializePCM(port->module); - - solenoid_port_t *solenoid_port = new solenoid_port_t; - solenoid_port->module = PCM_modules[port->module]; - solenoid_port->pin = port->pin; +void* initializeSolenoidPort(void* port_pointer, int32_t* status) { + Port* port = (Port*)port_pointer; + initializePCM(port->module); - return solenoid_port; + solenoid_port_t* solenoid_port = new solenoid_port_t; + solenoid_port->module = PCM_modules[port->module]; + solenoid_port->pin = port->pin; + + return solenoid_port; } void freeSolenoidPort(void* solenoid_port_pointer) { - solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer; - delete port; + solenoid_port_t* port = (solenoid_port_t*)solenoid_port_pointer; + delete port; } -bool checkSolenoidModule(uint8_t module) { - return module < NUM_MODULE_NUMBERS; +bool checkSolenoidModule(uint8_t module) { return module < NUM_MODULE_NUMBERS; } + +bool getSolenoid(void* solenoid_port_pointer, int32_t* status) { + solenoid_port_t* port = (solenoid_port_t*)solenoid_port_pointer; + bool value; + + *status = port->module->GetSolenoid(port->pin, value); + + return value; } -bool getSolenoid(void* solenoid_port_pointer, int32_t *status) { - solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer; - bool value; +uint8_t getAllSolenoids(void* solenoid_port_pointer, int32_t* status) { + solenoid_port_t* port = (solenoid_port_t*)solenoid_port_pointer; + uint8_t value; - *status = port->module->GetSolenoid(port->pin, value); + *status = port->module->GetAllSolenoids(value); - return value; + return value; } -uint8_t getAllSolenoids(void* solenoid_port_pointer, int32_t *status) { - solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer; - uint8_t value; +void setSolenoid(void* solenoid_port_pointer, bool value, int32_t* status) { + solenoid_port_t* port = (solenoid_port_t*)solenoid_port_pointer; - *status = port->module->GetAllSolenoids(value); - - return value; + *status = port->module->SetSolenoid(port->pin, value); } -void setSolenoid(void* solenoid_port_pointer, bool value, int32_t *status) { - solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer; +int getPCMSolenoidBlackList(void* solenoid_port_pointer, int32_t* status) { + solenoid_port_t* port = (solenoid_port_t*)solenoid_port_pointer; + UINT8 value; - *status = port->module->SetSolenoid(port->pin, value); + *status = port->module->GetSolenoidBlackList(value); + + return value; } +bool getPCMSolenoidVoltageStickyFault(void* solenoid_port_pointer, + int32_t* status) { + solenoid_port_t* port = (solenoid_port_t*)solenoid_port_pointer; + bool value; -int getPCMSolenoidBlackList(void* solenoid_port_pointer, int32_t *status){ - solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer; - UINT8 value; - - *status = port->module->GetSolenoidBlackList(value); + *status = port->module->GetSolenoidStickyFault(value); - return value; + return value; } -bool getPCMSolenoidVoltageStickyFault(void* solenoid_port_pointer, int32_t *status){ - solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer; - bool value; +bool getPCMSolenoidVoltageFault(void* solenoid_port_pointer, int32_t* status) { + solenoid_port_t* port = (solenoid_port_t*)solenoid_port_pointer; + bool value; - *status = port->module->GetSolenoidStickyFault(value); + *status = port->module->GetSolenoidFault(value); - return value; + return value; } -bool getPCMSolenoidVoltageFault(void* solenoid_port_pointer, int32_t *status){ - solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer; - bool value; +void clearAllPCMStickyFaults_sol(void* solenoid_port_pointer, int32_t* status) { + solenoid_port_t* port = (solenoid_port_t*)solenoid_port_pointer; - *status = port->module->GetSolenoidFault(value); - - return value; -} -void clearAllPCMStickyFaults_sol(void *solenoid_port_pointer, int32_t *status){ - solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer; - - *status = port->module->ClearStickyFaults(); + *status = port->module->ClearStickyFaults(); } } // extern "C" diff --git a/hal/lib/Athena/Task.cpp b/hal/lib/Athena/Task.cpp index 3536544b22..3c78c676fd 100644 --- a/hal/lib/Athena/Task.cpp +++ b/hal/lib/Athena/Task.cpp @@ -8,10 +8,10 @@ #include "HAL/Task.hpp" #ifndef OK -#define OK 0 +#define OK 0 #endif /* OK */ #ifndef ERROR -#define ERROR (-1) +#define ERROR (-1) #endif /* ERROR */ #include @@ -20,9 +20,9 @@ extern "C" { STATUS verifyTaskID(TASK task) { if (task != nullptr && pthread_kill(*task, 0) == 0) { - return OK; + return OK; } else { - return ERROR; + return ERROR; } } @@ -35,12 +35,10 @@ STATUS setTaskPriority(TASK task, int priority) { param.sched_priority = priority; if (pthread_setschedparam(*task, SCHED_FIFO, ¶m) == 0) { return OK; - } - else { + } else { return ERROR; } - } - else { + } else { return ERROR; } } @@ -50,11 +48,10 @@ STATUS getTaskPriority(TASK task, int* priority) { struct sched_param param; if (verifyTaskID(task) == OK && - pthread_getschedparam(*task, &policy, ¶m) == 0) { + pthread_getschedparam(*task, &policy, ¶m) == 0) { *priority = param.sched_priority; return OK; - } - else { + } else { return ERROR; } } diff --git a/hal/lib/Athena/Utilities.cpp b/hal/lib/Athena/Utilities.cpp index 5c22ea82b7..3338a4133e 100644 --- a/hal/lib/Athena/Utilities.cpp +++ b/hal/lib/Athena/Utilities.cpp @@ -13,43 +13,40 @@ const int32_t HAL_WAIT_FOREVER = -1; extern "C" { -void delayTicks(int32_t ticks) -{ - struct timespec test, remaining; - test.tv_sec = 0; - test.tv_nsec = ticks * 3; +void delayTicks(int32_t ticks) { + struct timespec test, remaining; + test.tv_sec = 0; + test.tv_nsec = ticks * 3; - /* Sleep until the requested number of ticks has passed, with additional - time added if nanosleep is interrupted. */ - while(nanosleep(&test, &remaining) == -1) { - test = remaining; - } + /* Sleep until the requested number of ticks has passed, with additional + time added if nanosleep is interrupted. */ + while (nanosleep(&test, &remaining) == -1) { + test = remaining; + } } -void delayMillis(double ms) -{ - struct timespec test, remaining; - test.tv_sec = ms / 1000; - test.tv_nsec = 1000 * (((uint64_t)ms) % 1000000); +void delayMillis(double ms) { + struct timespec test, remaining; + test.tv_sec = ms / 1000; + test.tv_nsec = 1000 * (((uint64_t)ms) % 1000000); - /* Sleep until the requested number of milliseconds has passed, with - additional time added if nanosleep is interrupted. */ - while(nanosleep(&test, &remaining) == -1) { - test = remaining; - } + /* Sleep until the requested number of milliseconds has passed, with + additional time added if nanosleep is interrupted. */ + while (nanosleep(&test, &remaining) == -1) { + test = remaining; + } } -void delaySeconds(double s) -{ - struct timespec test, remaining; - test.tv_sec = (int)s; - test.tv_nsec = (s - (int)s) * 1000000000.0; +void delaySeconds(double s) { + struct timespec test, remaining; + test.tv_sec = (int)s; + test.tv_nsec = (s - (int)s) * 1000000000.0; - /* Sleep until the requested number of seconds has passed, with additional - time added if nanosleep is interrupted. */ - while(nanosleep(&test, &remaining) == -1) { - test = remaining; - } + /* Sleep until the requested number of seconds has passed, with additional + time added if nanosleep is interrupted. */ + while (nanosleep(&test, &remaining) == -1) { + test = remaining; + } } } // extern "C" diff --git a/hal/lib/Athena/cpp/Resource.cpp b/hal/lib/Athena/cpp/Resource.cpp index 3e13d6369d..b3c8ca69fe 100644 --- a/hal/lib/Athena/cpp/Resource.cpp +++ b/hal/lib/Athena/cpp/Resource.cpp @@ -6,8 +6,8 @@ /*----------------------------------------------------------------------------*/ #include "HAL/cpp/Resource.hpp" -#include "HAL/Errors.hpp" #include +#include "HAL/Errors.hpp" #include "HAL/cpp/priority_mutex.h" namespace hal { @@ -16,8 +16,9 @@ priority_recursive_mutex Resource::m_createLock; /** * Allocate storage for a new instance of Resource. - * Allocate a bool array of values that will get initialized to indicate that no resources - * have been allocated yet. The indicies of the resources are [0 .. elements - 1]. + * Allocate a bool array of values that will get initialized to indicate that no + * resources have been allocated yet. The indicies of the resources are [0 .. + * elements - 1]. */ Resource::Resource(uint32_t elements) { std::lock_guard sync(m_createLock); @@ -34,78 +35,69 @@ Resource::Resource(uint32_t elements) { * track, that is, it will allocate resource numbers in the range * [0 .. elements - 1]. */ -/*static*/ void Resource::CreateResourceObject(Resource **r, uint32_t elements) -{ - std::lock_guard sync(m_createLock); - if (*r == NULL) - { - *r = new Resource(elements); - } +/*static*/ void Resource::CreateResourceObject(Resource** r, + uint32_t elements) { + std::lock_guard sync(m_createLock); + if (*r == NULL) { + *r = new Resource(elements); + } } /** * Allocate a resource. - * When a resource is requested, mark it allocated. In this case, a free resource value + * When a resource is requested, mark it allocated. In this case, a free + * resource value * within the range is located and returned after it is marked allocated. */ -uint32_t Resource::Allocate(const char *resourceDesc) -{ - std::lock_guard sync(m_allocateLock); - for (uint32_t i=0; i < m_isAllocated.size(); i++) - { - if (!m_isAllocated[i]) - { - m_isAllocated[i] = true; - return i; - } - } - // TODO: wpi_setWPIErrorWithContext(NoAvailableResources, resourceDesc); - return ~0ul; +uint32_t Resource::Allocate(const char* resourceDesc) { + std::lock_guard sync(m_allocateLock); + for (uint32_t i = 0; i < m_isAllocated.size(); i++) { + if (!m_isAllocated[i]) { + m_isAllocated[i] = true; + return i; + } + } + // TODO: wpi_setWPIErrorWithContext(NoAvailableResources, resourceDesc); + return ~0ul; } /** * Allocate a specific resource value. - * The user requests a specific resource value, i.e. channel number and it is verified - * unallocated, then returned. + * The user requests a specific resource value, i.e. channel number and it is + * verified unallocated, then returned. */ -uint32_t Resource::Allocate(uint32_t index, const char *resourceDesc) -{ - std::lock_guard sync(m_allocateLock); - if (index >= m_isAllocated.size()) - { - // TODO: wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc); - return ~0ul; - } - if ( m_isAllocated[index] ) - { - // TODO: wpi_setWPIErrorWithContext(ResourceAlreadyAllocated, resourceDesc); - return ~0ul; - } - m_isAllocated[index] = true; - return index; +uint32_t Resource::Allocate(uint32_t index, const char* resourceDesc) { + std::lock_guard sync(m_allocateLock); + if (index >= m_isAllocated.size()) { + // TODO: wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc); + return ~0ul; + } + if (m_isAllocated[index]) { + // TODO: wpi_setWPIErrorWithContext(ResourceAlreadyAllocated, resourceDesc); + return ~0ul; + } + m_isAllocated[index] = true; + return index; } - /** * Free an allocated resource. - * After a resource is no longer needed, for example a destructor is called for a channel assignment - * class, Free will release the resource value so it can be reused somewhere else in the program. + * After a resource is no longer needed, for example a destructor is called for + * a channel assignment class, Free will release the resource value so it can + * be reused somewhere else in the program. */ -void Resource::Free(uint32_t index) -{ - std::lock_guard sync(m_allocateLock); - if (index == ~0ul) return; - if (index >= m_isAllocated.size()) - { - // TODO: wpi_setWPIError(NotAllocated); - return; - } - if (!m_isAllocated[index]) - { - // TODO: wpi_setWPIError(NotAllocated); - return; - } - m_isAllocated[index] = false; +void Resource::Free(uint32_t index) { + std::lock_guard sync(m_allocateLock); + if (index == ~0ul) return; + if (index >= m_isAllocated.size()) { + // TODO: wpi_setWPIError(NotAllocated); + return; + } + if (!m_isAllocated[index]) { + // TODO: wpi_setWPIError(NotAllocated); + return; + } + m_isAllocated[index] = false; } } // namespace hal diff --git a/hal/lib/Athena/cpp/Semaphore.cpp b/hal/lib/Athena/cpp/Semaphore.cpp index 56349e3294..61e354167a 100644 --- a/hal/lib/Athena/cpp/Semaphore.cpp +++ b/hal/lib/Athena/cpp/Semaphore.cpp @@ -7,9 +7,7 @@ #include "HAL/cpp/Semaphore.hpp" -Semaphore::Semaphore(uint32_t count) { - m_count = count; -} +Semaphore::Semaphore(uint32_t count) { m_count = count; } void Semaphore::give() { std::lock_guard lock(m_mutex); @@ -19,7 +17,7 @@ void Semaphore::give() { void Semaphore::take() { std::unique_lock lock(m_mutex); - m_condition.wait(lock, [this] { return m_count; } ); + m_condition.wait(lock, [this] { return m_count; }); --m_count; } @@ -28,8 +26,7 @@ bool Semaphore::tryTake() { if (m_count) { --m_count; return true; - } - else { + } else { return false; } } diff --git a/hal/lib/Athena/cpp/priority_mutex.cpp b/hal/lib/Athena/cpp/priority_mutex.cpp index 5aed98260c..7843dfa17a 100644 --- a/hal/lib/Athena/cpp/priority_mutex.cpp +++ b/hal/lib/Athena/cpp/priority_mutex.cpp @@ -7,34 +7,22 @@ #include "HAL/cpp/priority_mutex.h" -void priority_recursive_mutex::lock() { - pthread_mutex_lock(&m_mutex); -} +void priority_recursive_mutex::lock() { pthread_mutex_lock(&m_mutex); } -void priority_recursive_mutex::unlock() { - pthread_mutex_unlock(&m_mutex); -} +void priority_recursive_mutex::unlock() { pthread_mutex_unlock(&m_mutex); } bool priority_recursive_mutex::try_lock() noexcept { return !pthread_mutex_trylock(&m_mutex); } -pthread_mutex_t* priority_recursive_mutex::native_handle() { - return &m_mutex; -} +pthread_mutex_t* priority_recursive_mutex::native_handle() { return &m_mutex; } -void priority_mutex::lock() { - pthread_mutex_lock(&m_mutex); -} +void priority_mutex::lock() { pthread_mutex_lock(&m_mutex); } -void priority_mutex::unlock() { - pthread_mutex_unlock(&m_mutex); -} +void priority_mutex::unlock() { pthread_mutex_unlock(&m_mutex); } bool priority_mutex::try_lock() noexcept { return !pthread_mutex_trylock(&m_mutex); } -pthread_mutex_t* priority_mutex::native_handle() { - return &m_mutex; -} +pthread_mutex_t* priority_mutex::native_handle() { return &m_mutex; } diff --git a/hal/lib/Desktop/HALDesktop.cpp b/hal/lib/Desktop/HALDesktop.cpp index 7860708750..a7398025ef 100644 --- a/hal/lib/Desktop/HALDesktop.cpp +++ b/hal/lib/Desktop/HALDesktop.cpp @@ -1 +1 @@ -//nothing here yet! \ No newline at end of file +// nothing here yet! \ No newline at end of file diff --git a/hal/lib/Shared/HAL.cpp b/hal/lib/Shared/HAL.cpp index 42582a9176..4f89206c02 100644 --- a/hal/lib/Shared/HAL.cpp +++ b/hal/lib/Shared/HAL.cpp @@ -1,153 +1,137 @@ -//This file must compile on ALL PLATFORMS. Be very careful what you put in here. +// This file must compile on ALL PLATFORMS. Be very careful what you put in +// here. #include "HAL/HAL.hpp" -#include "FRC_NetworkCommunication/FRCComm.h" #include +#include "FRC_NetworkCommunication/FRCComm.h" extern "C" { -int HALGetControlWord(HALControlWord *data) -{ - return FRC_NetworkCommunication_getControlWord((ControlWord_t*) data); +int HALGetControlWord(HALControlWord* data) { + return FRC_NetworkCommunication_getControlWord((ControlWord_t*)data); } -void HALSetNewDataSem(MULTIWAIT_ID sem) -{ - setNewDataSem(sem->native_handle()); +void HALSetNewDataSem(MULTIWAIT_ID sem) { setNewDataSem(sem->native_handle()); } + +int HALGetAllianceStation(enum HALAllianceStationID* allianceStation) { + return FRC_NetworkCommunication_getAllianceStation( + (AllianceStationID_t*)allianceStation); } -int HALGetAllianceStation(enum HALAllianceStationID *allianceStation) -{ - return FRC_NetworkCommunication_getAllianceStation((AllianceStationID_t*) allianceStation); +int HALGetJoystickAxes(uint8_t joystickNum, HALJoystickAxes* axes) { + return FRC_NetworkCommunication_getJoystickAxes( + joystickNum, (JoystickAxes_t*)axes, kMaxJoystickAxes); } -int HALGetJoystickAxes(uint8_t joystickNum, HALJoystickAxes *axes) -{ - return FRC_NetworkCommunication_getJoystickAxes(joystickNum, (JoystickAxes_t*) axes, kMaxJoystickAxes); +int HALGetJoystickPOVs(uint8_t joystickNum, HALJoystickPOVs* povs) { + return FRC_NetworkCommunication_getJoystickPOVs( + joystickNum, (JoystickPOV_t*)povs, kMaxJoystickPOVs); } -int HALGetJoystickPOVs(uint8_t joystickNum, HALJoystickPOVs *povs) -{ - return FRC_NetworkCommunication_getJoystickPOVs(joystickNum, (JoystickPOV_t*) povs, kMaxJoystickPOVs); -} - -int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons *buttons) -{ - return FRC_NetworkCommunication_getJoystickButtons(joystickNum, &buttons->buttons, &buttons->count); +int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons* buttons) { + return FRC_NetworkCommunication_getJoystickButtons( + joystickNum, &buttons->buttons, &buttons->count); } /** * Retrieve the Joystick Descriptor for particular slot - * @param desc [out] descriptor (data transfer object) to fill in. desc is filled in regardless of success. - * In other words, if descriptor is not available, desc is filled in with default - * values matching the init-values in Java and C++ Driverstation for when caller - * requests a too-large joystick index. + * @param desc [out] descriptor (data transfer object) to fill in. desc is + * filled in regardless of success. In other words, if descriptor is not + * available, desc is filled in with default values matching the init-values in + * Java and C++ Driverstation for when caller requests a too-large joystick + * index. * - * @return error code reported from Network Comm back-end. Zero is good, nonzero is bad. + * @return error code reported from Network Comm back-end. Zero is good, + * nonzero is bad. */ -int HALGetJoystickDescriptor(uint8_t joystickNum, HALJoystickDescriptor *desc) -{ - desc->isXbox = 0; - desc->type = -1; - desc->name[0] = '\0'; - desc->axisCount = kMaxJoystickAxes; /* set to the desc->axisTypes's capacity */ - desc->buttonCount = 0; - desc->povCount = 0; - int retval = FRC_NetworkCommunication_getJoystickDesc(joystickNum, &desc->isXbox, &desc->type, (char *)(&desc->name), - &desc->axisCount, (uint8_t *)&desc->axisTypes, &desc->buttonCount, &desc->povCount); - /* check the return, if there is an error and the RIOimage predates FRC2017, then axisCount needs to be cleared */ - if(retval != 0) - { - /* set count to zero so downstream code doesn't decode invalid axisTypes. */ - desc->axisCount = 0; - } - return retval; +int HALGetJoystickDescriptor(uint8_t joystickNum, HALJoystickDescriptor* desc) { + desc->isXbox = 0; + desc->type = -1; + desc->name[0] = '\0'; + desc->axisCount = + kMaxJoystickAxes; /* set to the desc->axisTypes's capacity */ + desc->buttonCount = 0; + desc->povCount = 0; + int retval = FRC_NetworkCommunication_getJoystickDesc( + joystickNum, &desc->isXbox, &desc->type, (char*)(&desc->name), + &desc->axisCount, (uint8_t*)&desc->axisTypes, &desc->buttonCount, + &desc->povCount); + /* check the return, if there is an error and the RIOimage predates FRC2017, + * then axisCount needs to be cleared */ + if (retval != 0) { + /* set count to zero so downstream code doesn't decode invalid axisTypes. */ + desc->axisCount = 0; + } + return retval; } -int HALGetJoystickIsXbox(uint8_t joystickNum) -{ - HALJoystickDescriptor joystickDesc; - if(HALGetJoystickDescriptor(joystickNum, &joystickDesc)<0) - { - return 0; - }else - { - return joystickDesc.isXbox; - } +int HALGetJoystickIsXbox(uint8_t joystickNum) { + HALJoystickDescriptor joystickDesc; + if (HALGetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + return 0; + } else { + return joystickDesc.isXbox; + } } -int HALGetJoystickType(uint8_t joystickNum) -{ - HALJoystickDescriptor joystickDesc; - if(HALGetJoystickDescriptor(joystickNum, &joystickDesc)<0) - { - return -1; - } else - { - return joystickDesc.type; - } +int HALGetJoystickType(uint8_t joystickNum) { + HALJoystickDescriptor joystickDesc; + if (HALGetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + return -1; + } else { + return joystickDesc.type; + } } -char* HALGetJoystickName(uint8_t joystickNum) -{ - HALJoystickDescriptor joystickDesc; - if(HALGetJoystickDescriptor(joystickNum, &joystickDesc)<0) - { - char* name = (char*)std::malloc(1); - name[0] = '\0'; - return name; - } else - { - size_t len = std::strlen(joystickDesc.name); - char* name = (char*)std::malloc(len+1); - std::strcpy(name, joystickDesc.name); - return name; - } +char* HALGetJoystickName(uint8_t joystickNum) { + HALJoystickDescriptor joystickDesc; + if (HALGetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + char* name = (char*)std::malloc(1); + name[0] = '\0'; + return name; + } else { + size_t len = std::strlen(joystickDesc.name); + char* name = (char*)std::malloc(len + 1); + std::strcpy(name, joystickDesc.name); + return name; + } } -int HALGetJoystickAxisType(uint8_t joystickNum, uint8_t axis) -{ - HALJoystickDescriptor joystickDesc; - if(HALGetJoystickDescriptor(joystickNum, &joystickDesc)<0) - { - return -1; - } else - { - return joystickDesc.axisTypes[axis]; - } +int HALGetJoystickAxisType(uint8_t joystickNum, uint8_t axis) { + HALJoystickDescriptor joystickDesc; + if (HALGetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + return -1; + } else { + return joystickDesc.axisTypes[axis]; + } } -int HALSetJoystickOutputs(uint8_t joystickNum, uint32_t outputs, uint16_t leftRumble, uint16_t rightRumble) -{ - return FRC_NetworkCommunication_setJoystickOutputs(joystickNum, outputs, leftRumble, rightRumble); +int HALSetJoystickOutputs(uint8_t joystickNum, uint32_t outputs, + uint16_t leftRumble, uint16_t rightRumble) { + return FRC_NetworkCommunication_setJoystickOutputs(joystickNum, outputs, + leftRumble, rightRumble); } -int HALGetMatchTime(float *matchTime) -{ - return FRC_NetworkCommunication_getMatchTime(matchTime); +int HALGetMatchTime(float* matchTime) { + return FRC_NetworkCommunication_getMatchTime(matchTime); } -void HALNetworkCommunicationObserveUserProgramStarting(void) -{ - FRC_NetworkCommunication_observeUserProgramStarting(); +void HALNetworkCommunicationObserveUserProgramStarting(void) { + FRC_NetworkCommunication_observeUserProgramStarting(); } -void HALNetworkCommunicationObserveUserProgramDisabled(void) -{ - FRC_NetworkCommunication_observeUserProgramDisabled(); +void HALNetworkCommunicationObserveUserProgramDisabled(void) { + FRC_NetworkCommunication_observeUserProgramDisabled(); } -void HALNetworkCommunicationObserveUserProgramAutonomous(void) -{ - FRC_NetworkCommunication_observeUserProgramAutonomous(); +void HALNetworkCommunicationObserveUserProgramAutonomous(void) { + FRC_NetworkCommunication_observeUserProgramAutonomous(); } -void HALNetworkCommunicationObserveUserProgramTeleop(void) -{ - FRC_NetworkCommunication_observeUserProgramTeleop(); +void HALNetworkCommunicationObserveUserProgramTeleop(void) { + FRC_NetworkCommunication_observeUserProgramTeleop(); } -void HALNetworkCommunicationObserveUserProgramTest(void) -{ - FRC_NetworkCommunication_observeUserProgramTest(); +void HALNetworkCommunicationObserveUserProgramTest(void) { + FRC_NetworkCommunication_observeUserProgramTest(); } } // extern "C" diff --git a/simulation/frc_gazebo_plugins/clock/src/clock.cpp b/simulation/frc_gazebo_plugins/clock/src/clock.cpp index 0297943da6..1e3268461e 100644 --- a/simulation/frc_gazebo_plugins/clock/src/clock.cpp +++ b/simulation/frc_gazebo_plugins/clock/src/clock.cpp @@ -20,13 +20,14 @@ void Clock::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { if (sdf->HasElement("topic")) { topic = sdf->Get("topic"); } else { - topic = "~/"+sdf->GetAttribute("name")->GetAsString(); + topic = "~/" + sdf->GetAttribute("name")->GetAsString(); } gzmsg << "Initializing clock: " << topic << std::endl; // Connect to Gazebo transport for messaging - std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName(); + std::string scoped_name = + model->GetWorld()->GetName() + "::" + model->GetScopedName(); boost::replace_all(scoped_name, "::", "/"); node = transport::NodePtr(new transport::Node()); node->Init(scoped_name); @@ -34,10 +35,11 @@ void Clock::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { // Connect to the world update event. // This will trigger the Update function every Gazebo iteration - updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Clock::Update, this, _1)); + updateConn = event::Events::ConnectWorldUpdateBegin( + boost::bind(&Clock::Update, this, _1)); } -void Clock::Update(const common::UpdateInfo &info) { +void Clock::Update(const common::UpdateInfo& info) { msgs::Float64 msg; msg.set_data(info.simTime.Double()); pub->Publish(msg); diff --git a/simulation/frc_gazebo_plugins/clock/src/clock.h b/simulation/frc_gazebo_plugins/clock/src/clock.h index 403fcb16d7..0287ed15de 100644 --- a/simulation/frc_gazebo_plugins/clock/src/clock.h +++ b/simulation/frc_gazebo_plugins/clock/src/clock.h @@ -7,13 +7,11 @@ #pragma once - #include "simulation/gz_msgs/msgs.h" +#include #include #include -#include - using namespace gazebo; @@ -34,8 +32,8 @@ using namespace gazebo; * * \todo Make WorldPlugin? */ -class Clock: public ModelPlugin { -public: +class Clock : public ModelPlugin { + public: Clock(); ~Clock(); @@ -43,13 +41,13 @@ public: void Load(physics::ModelPtr model, sdf::ElementPtr sdf); /// \brief Sends out time each timestep. - void Update(const common::UpdateInfo &info); + void Update(const common::UpdateInfo& info); -private: - std::string topic; ///< \brief Publish the time on this topic. - physics::ModelPtr model; ///< \brief The model that this is attached to. - event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function. - transport::NodePtr node; ///< \brief The node we're advertising on. - transport::PublisherPtr pub; ///< \brief Publisher handle. + private: + std::string topic; ///< \brief Publish the time on this topic. + physics::ModelPtr model; ///< \brief The model that this is attached to. + event::ConnectionPtr + updateConn; ///< \brief Pointer to the world update function. + transport::NodePtr node; ///< \brief The node we're advertising on. + transport::PublisherPtr pub; ///< \brief Publisher handle. }; - diff --git a/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.cpp b/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.cpp index 1074d7014d..b510983f59 100644 --- a/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.cpp +++ b/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.cpp @@ -22,7 +22,7 @@ void DCMotor::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { if (sdf->HasElement("topic")) { topic = sdf->Get("topic"); } else { - topic = "~/"+sdf->GetAttribute("name")->GetAsString(); + topic = "~/" + sdf->GetAttribute("name")->GetAsString(); } if (sdf->HasElement("multiplier")) { @@ -35,7 +35,8 @@ void DCMotor::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { << " multiplier=" << multiplier << std::endl; // Connect to Gazebo transport for messaging - std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName(); + std::string scoped_name = + model->GetWorld()->GetName() + "::" + model->GetScopedName(); boost::replace_all(scoped_name, "::", "/"); node = transport::NodePtr(new transport::Node()); node->Init(scoped_name); @@ -43,15 +44,19 @@ void DCMotor::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { // Connect to the world update event. // This will trigger the Update function every Gazebo iteration - updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&DCMotor::Update, this, _1)); + updateConn = event::Events::ConnectWorldUpdateBegin( + boost::bind(&DCMotor::Update, this, _1)); } -void DCMotor::Update(const common::UpdateInfo &info) { - joint->SetForce(0, signal*multiplier); +void DCMotor::Update(const common::UpdateInfo& info) { + joint->SetForce(0, signal * multiplier); } -void DCMotor::Callback(const msgs::ConstFloat64Ptr &msg) { +void DCMotor::Callback(const msgs::ConstFloat64Ptr& msg) { signal = msg->data(); - if (signal < -1) { signal = -1; } - else if (signal > 1) { signal = 1; } + if (signal < -1) { + signal = -1; + } else if (signal > 1) { + signal = 1; + } } diff --git a/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h b/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h index d162ff8358..0a089dd1cb 100644 --- a/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h +++ b/simulation/frc_gazebo_plugins/dc_motor/src/dc_motor.h @@ -9,12 +9,9 @@ #include "simulation/gz_msgs/msgs.h" +#include #include #include -#include - - - using namespace gazebo; @@ -38,8 +35,8 @@ using namespace gazebo; * - `topic`: Optional. Message type should be gazebo.msgs.Float64. * - `multiplier`: Optional. Defaults to 1. */ -class DCMotor: public ModelPlugin { -public: +class DCMotor : public ModelPlugin { + public: DCMotor(); ~DCMotor(); @@ -47,9 +44,9 @@ public: void Load(physics::ModelPtr model, sdf::ElementPtr sdf); /// \brief Update the torque on the joint from the dc motor each timestep. - void Update(const common::UpdateInfo &info); + void Update(const common::UpdateInfo& info); -private: + private: /// \brief Topic to read control signal from. std::string topic; @@ -63,11 +60,11 @@ private: physics::JointPtr joint; /// \brief Callback for receiving msgs and storing the signal. - void Callback(const msgs::ConstFloat64Ptr &msg); + void Callback(const msgs::ConstFloat64Ptr& msg); - - physics::ModelPtr model; ///< \brief The model that this is attached to. - event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function. - transport::NodePtr node; ///< \brief The node we're advertising on. - transport::SubscriberPtr sub; ///< \brief Subscriber handle. + physics::ModelPtr model; ///< \brief The model that this is attached to. + event::ConnectionPtr + updateConn; ///< \brief Pointer to the world update function. + transport::NodePtr node; ///< \brief The node we're advertising on. + transport::SubscriberPtr sub; ///< \brief Subscriber handle. }; diff --git a/simulation/frc_gazebo_plugins/encoder/src/encoder.cpp b/simulation/frc_gazebo_plugins/encoder/src/encoder.cpp index 0742edd885..e731c201f1 100644 --- a/simulation/frc_gazebo_plugins/encoder/src/encoder.cpp +++ b/simulation/frc_gazebo_plugins/encoder/src/encoder.cpp @@ -21,7 +21,7 @@ void Encoder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { if (sdf->HasElement("topic")) { topic = sdf->Get("topic"); } else { - topic = "~/"+sdf->GetAttribute("name")->GetAsString(); + topic = "~/" + sdf->GetAttribute("name")->GetAsString(); } if (sdf->HasElement("units")) { @@ -37,20 +37,22 @@ void Encoder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { << " radians=" << radians << std::endl; // Connect to Gazebo transport for messaging - std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName(); + std::string scoped_name = + model->GetWorld()->GetName() + "::" + model->GetScopedName(); boost::replace_all(scoped_name, "::", "/"); node = transport::NodePtr(new transport::Node()); node->Init(scoped_name); - command_sub = node->Subscribe(topic+"/control", &Encoder::Callback, this); - pos_pub = node->Advertise(topic+"/position"); - vel_pub = node->Advertise(topic+"/velocity"); + command_sub = node->Subscribe(topic + "/control", &Encoder::Callback, this); + pos_pub = node->Advertise(topic + "/position"); + vel_pub = node->Advertise(topic + "/velocity"); // Connect to the world update event. // This will trigger the Update function every Gazebo iteration - updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Encoder::Update, this, _1)); + updateConn = event::Events::ConnectWorldUpdateBegin( + boost::bind(&Encoder::Update, this, _1)); } -void Encoder::Update(const common::UpdateInfo &info) { +void Encoder::Update(const common::UpdateInfo& info) { msgs::Float64 pos_msg, vel_msg; if (stopped) { pos_msg.set_data(stop_value); @@ -65,18 +67,19 @@ void Encoder::Update(const common::UpdateInfo &info) { } } -void Encoder::Callback(const msgs::ConstStringPtr &msg) { +void Encoder::Callback(const msgs::ConstStringPtr& msg) { std::string command = msg->data(); if (command == "reset") { zero = GetAngle(); } else if (command == "start") { stopped = false; zero = (GetAngle() - stop_value); - } else if (command == "stop") { + } else if (command == "stop") { stopped = true; stop_value = GetAngle(); } else { - gzerr << "WARNING: Encoder got unknown command '" << command << "'." << std::endl; + gzerr << "WARNING: Encoder got unknown command '" << command << "'." + << std::endl; } } @@ -95,4 +98,3 @@ double Encoder::GetVelocity() { return joint->GetVelocity(0) * (180.0 / M_PI); } } - diff --git a/simulation/frc_gazebo_plugins/encoder/src/encoder.h b/simulation/frc_gazebo_plugins/encoder/src/encoder.h index f3d3ea1324..3b223f8afe 100644 --- a/simulation/frc_gazebo_plugins/encoder/src/encoder.h +++ b/simulation/frc_gazebo_plugins/encoder/src/encoder.h @@ -9,10 +9,9 @@ #include "simulation/gz_msgs/msgs.h" +#include #include #include -#include - using namespace gazebo; @@ -38,12 +37,14 @@ using namespace gazebo; * * * - `joint`: Name of the joint this encoder is attached to. - * - `topic`: Optional. Used as the root for subtopics. `topic`/position (gazebo.msgs.Float64), - * `topic`/velocity (gazebo.msgs.Float64), `topic`/control (gazebo.msgs.String) + * - `topic`: Optional. Used as the root for subtopics. `topic`/position + * (gazebo.msgs.Float64), + * `topic`/velocity (gazebo.msgs.Float64), `topic`/control + * (gazebo.msgs.String) * - `units`: Optional. Defaults to radians. */ -class Encoder: public ModelPlugin { -public: +class Encoder : public ModelPlugin { + public: Encoder(); ~Encoder(); @@ -51,9 +52,9 @@ public: void Load(physics::ModelPtr model, sdf::ElementPtr sdf); /// \brief Sends out the encoder reading each timestep. - void Update(const common::UpdateInfo &info); + void Update(const common::UpdateInfo& info); -private: + private: /// \brief Root topic for subtopics on this topic. std::string topic; @@ -73,7 +74,7 @@ private: physics::JointPtr joint; /// \brief Callback for handling control data - void Callback(const msgs::ConstStringPtr &msg); + void Callback(const msgs::ConstStringPtr& msg); /// \brief Gets the current angle, taking into account whether to /// return radians or degrees. @@ -83,9 +84,10 @@ private: /// return radians/second or degrees/second. double GetVelocity(); - physics::ModelPtr model; ///< \brief The model that this is attached to. - event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function. - transport::NodePtr node; ///< \brief The node we're advertising on. - transport::SubscriberPtr command_sub; ///< \brief Subscriber handle. - transport::PublisherPtr pos_pub, vel_pub; ///< \brief Publisher handles. + physics::ModelPtr model; ///< \brief The model that this is attached to. + event::ConnectionPtr + updateConn; ///< \brief Pointer to the world update function. + transport::NodePtr node; ///< \brief The node we're advertising on. + transport::SubscriberPtr command_sub; ///< \brief Subscriber handle. + transport::PublisherPtr pos_pub, vel_pub; ///< \brief Publisher handles. }; diff --git a/simulation/frc_gazebo_plugins/gyro/src/gyro.cpp b/simulation/frc_gazebo_plugins/gyro/src/gyro.cpp index 0895c6a150..c81b62e5f9 100644 --- a/simulation/frc_gazebo_plugins/gyro/src/gyro.cpp +++ b/simulation/frc_gazebo_plugins/gyro/src/gyro.cpp @@ -7,7 +7,6 @@ #include "gyro.h" - GZ_REGISTER_MODEL_PLUGIN(Gyro) Gyro::Gyro() {} @@ -22,7 +21,7 @@ void Gyro::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { if (sdf->HasElement("topic")) { topic = sdf->Get("topic"); } else { - topic = "~/"+sdf->GetAttribute("name")->GetAsString(); + topic = "~/" + sdf->GetAttribute("name")->GetAsString(); } std::string axisString = sdf->Get("axis"); @@ -41,20 +40,22 @@ void Gyro::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { << " axis=" << axis << " radians=" << radians << std::endl; // Connect to Gazebo transport for messaging - std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName(); + std::string scoped_name = + model->GetWorld()->GetName() + "::" + model->GetScopedName(); boost::replace_all(scoped_name, "::", "/"); node = transport::NodePtr(new transport::Node()); node->Init(scoped_name); - command_sub = node->Subscribe(topic+"/control", &Gyro::Callback, this); - pos_pub = node->Advertise(topic+"/position"); - vel_pub = node->Advertise(topic+"/velocity"); + command_sub = node->Subscribe(topic + "/control", &Gyro::Callback, this); + pos_pub = node->Advertise(topic + "/position"); + vel_pub = node->Advertise(topic + "/velocity"); // Connect to the world update event. // This will trigger the Update function every Gazebo iteration - updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Gyro::Update, this, _1)); + updateConn = event::Events::ConnectWorldUpdateBegin( + boost::bind(&Gyro::Update, this, _1)); } -void Gyro::Update(const common::UpdateInfo &info) { +void Gyro::Update(const common::UpdateInfo& info) { msgs::Float64 pos_msg, vel_msg; pos_msg.set_data(Limit(GetAngle() - zero)); pos_pub->Publish(pos_msg); @@ -62,12 +63,13 @@ void Gyro::Update(const common::UpdateInfo &info) { vel_pub->Publish(vel_msg); } -void Gyro::Callback(const msgs::ConstStringPtr &msg) { +void Gyro::Callback(const msgs::ConstStringPtr& msg) { std::string command = msg->data(); if (command == "reset") { zero = GetAngle(); } else { - gzerr << "WARNING: Gyro got unknown command '" << command << "'." << std::endl; + gzerr << "WARNING: Gyro got unknown command '" << command << "'." + << std::endl; } } @@ -90,15 +92,21 @@ double Gyro::GetVelocity() { double Gyro::Limit(double value) { if (radians) { while (true) { - if (value < -M_PI) value += 2*M_PI; - else if (value > M_PI) value -= 2*M_PI; - else break; + if (value < -M_PI) + value += 2 * M_PI; + else if (value > M_PI) + value -= 2 * M_PI; + else + break; } } else { while (true) { - if (value < -180) value += 360; - else if (value > 180) value -= 360; - else break; + if (value < -180) + value += 360; + else if (value > 180) + value -= 360; + else + break; } } return value; diff --git a/simulation/frc_gazebo_plugins/gyro/src/gyro.h b/simulation/frc_gazebo_plugins/gyro/src/gyro.h index 29f62a9b40..bdd2db2124 100644 --- a/simulation/frc_gazebo_plugins/gyro/src/gyro.h +++ b/simulation/frc_gazebo_plugins/gyro/src/gyro.h @@ -9,16 +9,14 @@ #include "simulation/gz_msgs/msgs.h" +#include #include #include -#include - - using namespace gazebo; /// \brief The axis about which to measure rotation. -typedef enum {Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/} ROTATION; +typedef enum { Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/ } ROTATION; /** * \brief Plugin for reading the speed and relative angle of a link. @@ -38,12 +36,14 @@ typedef enum {Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/} ROTATION; * * * - `link`: Name of the link this potentiometer is attached to. - * - `topic`: Optional. Used as the root for subtopics. `topic`/position (gazebo.msgs.Float64), - * `topic`/velocity (gazebo.msgs.Float64), `topic`/control (gazebo.msgs.String) + * - `topic`: Optional. Used as the root for subtopics. `topic`/position + * (gazebo.msgs.Float64), + * `topic`/velocity (gazebo.msgs.Float64), `topic`/control + * (gazebo.msgs.String) * - `units`; Optional, defaults to radians. */ -class Gyro: public ModelPlugin { -public: +class Gyro : public ModelPlugin { + public: Gyro(); ~Gyro(); @@ -51,9 +51,9 @@ public: void Load(physics::ModelPtr model, sdf::ElementPtr sdf); /// \brief Sends out the gyro reading each timestep. - void Update(const common::UpdateInfo &info); + void Update(const common::UpdateInfo& info); -private: + private: /// \brief Publish the angle on this topic. std::string topic; @@ -70,7 +70,7 @@ private: physics::LinkPtr link; /// \brief Callback for handling control data - void Callback(const msgs::ConstStringPtr &msg); + void Callback(const msgs::ConstStringPtr& msg); /// \brief Gets the current angle, taking into account whether to /// return radians or degrees. @@ -84,9 +84,10 @@ private: /// depending on whether or radians or degrees are being used. double Limit(double value); - physics::ModelPtr model; ///< \brief The model that this is attached to. - event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function. - transport::NodePtr node; ///< \brief The node we're advertising on. - transport::SubscriberPtr command_sub; ///< \brief Subscriber handle. - transport::PublisherPtr pos_pub, vel_pub; ///< \brief Publisher handles. + physics::ModelPtr model; ///< \brief The model that this is attached to. + event::ConnectionPtr + updateConn; ///< \brief Pointer to the world update function. + transport::NodePtr node; ///< \brief The node we're advertising on. + transport::SubscriberPtr command_sub; ///< \brief Subscriber handle. + transport::PublisherPtr pos_pub, vel_pub; ///< \brief Publisher handles. }; diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.cpp b/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.cpp index c871212395..e58af2b67f 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.cpp +++ b/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.cpp @@ -7,12 +7,12 @@ #include "external_limit_switch.h" - ExternalLimitSwitch::ExternalLimitSwitch(sdf::ElementPtr sdf) { sensor = boost::dynamic_pointer_cast( - sensors::get_sensor(sdf->Get("sensor"))); + sensors::get_sensor(sdf->Get("sensor"))); - gzmsg << "\texternal limit switch: " << " sensor=" << sensor->GetName() << std::endl; + gzmsg << "\texternal limit switch: " + << " sensor=" << sensor->GetName() << std::endl; } ExternalLimitSwitch::~ExternalLimitSwitch() {} diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.h b/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.h index 08e9198833..f662e1adaa 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.h +++ b/simulation/frc_gazebo_plugins/limit_switch/src/external_limit_switch.h @@ -10,23 +10,23 @@ #include "switch.h" #ifdef _WIN32 - #include +#include #endif -#include #include #include +#include using namespace gazebo; class ExternalLimitSwitch : public Switch { -public: + public: ExternalLimitSwitch(sdf::ElementPtr sdf); ~ExternalLimitSwitch(); /// \brief Returns true when the switch is triggered. virtual bool Get(); -private: + private: sensors::ContactSensorPtr sensor; }; diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.cpp b/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.cpp index b410ca562e..daa30ea978 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.cpp +++ b/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.cpp @@ -7,7 +7,8 @@ #include "internal_limit_switch.h" -InternalLimitSwitch::InternalLimitSwitch(physics::ModelPtr model, sdf::ElementPtr sdf) { +InternalLimitSwitch::InternalLimitSwitch(physics::ModelPtr model, + sdf::ElementPtr sdf) { joint = model->GetJoint(sdf->Get("joint")); min = sdf->Get("min"); max = sdf->Get("max"); @@ -18,8 +19,9 @@ InternalLimitSwitch::InternalLimitSwitch(physics::ModelPtr model, sdf::ElementPt radians = true; } - gzmsg << "\tinternal limit switch: " << " type=" << joint->GetName() - << " range=[" << min << "," << max << "] radians=" << radians << std::endl; + gzmsg << "\tinternal limit switch: " + << " type=" << joint->GetName() << " range=[" << min << "," << max + << "] radians=" << radians << std::endl; } InternalLimitSwitch::~InternalLimitSwitch() {} diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.h b/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.h index b7b007ab49..5fdfb5eb52 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.h +++ b/simulation/frc_gazebo_plugins/limit_switch/src/internal_limit_switch.h @@ -10,22 +10,22 @@ #include "switch.h" #ifdef _WIN32 - #include +#include #endif -#include #include +#include using namespace gazebo; class InternalLimitSwitch : public Switch { -public: + public: InternalLimitSwitch(physics::ModelPtr model, sdf::ElementPtr sdf); ~InternalLimitSwitch(); /// \brief Returns true when the switch is triggered. virtual bool Get(); -private: + private: physics::JointPtr joint; double min, max; bool radians; diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.cpp b/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.cpp index 88f8846461..d65580e5f2 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.cpp +++ b/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.cpp @@ -20,11 +20,12 @@ void LimitSwitch::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { if (sdf->HasElement("topic")) { topic = sdf->Get("topic"); } else { - topic = "~/"+sdf->GetAttribute("name")->GetAsString(); + topic = "~/" + sdf->GetAttribute("name")->GetAsString(); } std::string type = sdf->Get("type"); - gzmsg << "Initializing limit switch: " << topic << " type=" << type << std::endl; + gzmsg << "Initializing limit switch: " << topic << " type=" << type + << std::endl; if (type == "internal") { ls = new InternalLimitSwitch(model, sdf); } else if (type == "external") { @@ -34,7 +35,8 @@ void LimitSwitch::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { } // Connect to Gazebo transport for messaging - std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName(); + std::string scoped_name = + model->GetWorld()->GetName() + "::" + model->GetScopedName(); boost::replace_all(scoped_name, "::", "/"); node = transport::NodePtr(new transport::Node()); node->Init(scoped_name); @@ -42,10 +44,11 @@ void LimitSwitch::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { // Connect to the world update event. // This will trigger the Update function every Gazebo iteration - updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&LimitSwitch::Update, this, _1)); + updateConn = event::Events::ConnectWorldUpdateBegin( + boost::bind(&LimitSwitch::Update, this, _1)); } -void LimitSwitch::Update(const common::UpdateInfo &info) { +void LimitSwitch::Update(const common::UpdateInfo& info) { msgs::Bool msg; msg.set_data(ls->Get()); pub->Publish(msg); diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.h b/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.h index 17ae8cf343..6f5f561634 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.h +++ b/simulation/frc_gazebo_plugins/limit_switch/src/limit_switch.h @@ -11,12 +11,12 @@ #include "switch.h" -#include "internal_limit_switch.h" #include "external_limit_switch.h" +#include "internal_limit_switch.h" +#include #include #include -#include using namespace gazebo; @@ -66,8 +66,8 @@ using namespace gazebo; * External * - `sensor`: Name of the contact sensor that this limit switch uses. */ -class LimitSwitch: public ModelPlugin { -public: +class LimitSwitch : public ModelPlugin { + public: LimitSwitch(); ~LimitSwitch(); @@ -75,18 +75,18 @@ public: void Load(physics::ModelPtr model, sdf::ElementPtr sdf); /// \brief Sends out the limit switch reading each timestep. - void Update(const common::UpdateInfo &info); + void Update(const common::UpdateInfo& info); -private: + private: /// \brief Publish the limit switch value on this topic. std::string topic; /// \brief LimitSwitch object, currently internal or external. Switch* ls; - - physics::ModelPtr model; ///< \brief The model that this is attached to. - event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function. - transport::NodePtr node; ///< \brief The node we're advertising on. - transport::PublisherPtr pub; ///< \brief Publisher handle. + physics::ModelPtr model; ///< \brief The model that this is attached to. + event::ConnectionPtr + updateConn; ///< \brief Pointer to the world update function. + transport::NodePtr node; ///< \brief The node we're advertising on. + transport::PublisherPtr pub; ///< \brief Publisher handle. }; diff --git a/simulation/frc_gazebo_plugins/limit_switch/src/switch.h b/simulation/frc_gazebo_plugins/limit_switch/src/switch.h index 8c204a3963..d2b07ba440 100644 --- a/simulation/frc_gazebo_plugins/limit_switch/src/switch.h +++ b/simulation/frc_gazebo_plugins/limit_switch/src/switch.h @@ -8,7 +8,7 @@ #pragma once class Switch { -public: + public: virtual ~Switch() {} /// \brief Returns true when the switch is triggered. diff --git a/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.cpp b/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.cpp index 4dbe57db2f..b28a226048 100644 --- a/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.cpp +++ b/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.cpp @@ -6,9 +6,9 @@ /*----------------------------------------------------------------------------*/ #ifdef _WIN32 - // Ensure that Winsock2.h is included before Windows.h, which can get - // pulled in by anybody (e.g., Boost). - #include +// Ensure that Winsock2.h is included before Windows.h, which can get +// pulled in by anybody (e.g., Boost). +#include #endif #include "pneumatic_piston.h" @@ -31,23 +31,25 @@ void PneumaticPiston::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { if (sdf->HasElement("topic")) { topic = sdf->Get("topic"); } else { - topic = "~/"+sdf->GetAttribute("name")->GetAsString(); + topic = "~/" + sdf->GetAttribute("name")->GetAsString(); } forward_force = sdf->Get("forward-force"); reverse_force = sdf->Get("reverse-force"); - if (sdf->HasElement("direction") && sdf->Get("direction") == "reversed") { + if (sdf->HasElement("direction") && + sdf->Get("direction") == "reversed") { forward_force = -forward_force; reverse_force = -reverse_force; } gzmsg << "Initializing piston: " << topic << " joint=" << joint->GetName() << " forward_force=" << forward_force - << " reverse_force=" << reverse_force<< std::endl; + << " reverse_force=" << reverse_force << std::endl; // Connect to Gazebo transport for messaging - std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName(); + std::string scoped_name = + model->GetWorld()->GetName() + "::" + model->GetScopedName(); boost::replace_all(scoped_name, "::", "/"); node = transport::NodePtr(new transport::Node()); node->Init(scoped_name); @@ -55,14 +57,18 @@ void PneumaticPiston::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { // Connect to the world update event. // This will trigger the Update function every Gazebo iteration - updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&PneumaticPiston::Update, this, _1)); + updateConn = event::Events::ConnectWorldUpdateBegin( + boost::bind(&PneumaticPiston::Update, this, _1)); } -void PneumaticPiston::Update(const common::UpdateInfo &info) { +void PneumaticPiston::Update(const common::UpdateInfo& info) { joint->SetForce(0, signal); } -void PneumaticPiston::Callback(const msgs::ConstFloat64Ptr &msg) { - if (msg->data() < -0.001) { signal = -reverse_force; } - else if (msg->data() > 0.001) { signal = forward_force; } +void PneumaticPiston::Callback(const msgs::ConstFloat64Ptr& msg) { + if (msg->data() < -0.001) { + signal = -reverse_force; + } else if (msg->data() > 0.001) { + signal = forward_force; + } } diff --git a/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.h b/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.h index 488d9f32c1..75fab23b38 100644 --- a/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.h +++ b/simulation/frc_gazebo_plugins/pneumatic_piston/src/pneumatic_piston.h @@ -47,8 +47,8 @@ using namespace gazebo; * * \todo Signal should probably be made a tri-state message. */ -class PneumaticPiston: public ModelPlugin { -public: +class PneumaticPiston : public ModelPlugin { + public: PneumaticPiston(); ~PneumaticPiston(); @@ -56,9 +56,9 @@ public: void Load(physics::ModelPtr model, sdf::ElementPtr sdf); /// \brief Updat the force the piston applies on the joint. - void Update(const common::UpdateInfo &info); + void Update(const common::UpdateInfo& info); -private: + private: /// \brief Topic to read control signal from. std::string topic; @@ -72,11 +72,11 @@ private: physics::JointPtr joint; /// \brief Callback for receiving msgs and updating the torque. - void Callback(const msgs::ConstFloat64Ptr &msg); + void Callback(const msgs::ConstFloat64Ptr& msg); - - physics::ModelPtr model; ///< \brief The model that this is attached to. - event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function. - transport::NodePtr node; ///< \brief The node we're advertising on. - transport::SubscriberPtr sub; ///< \brief Subscriber handle. + physics::ModelPtr model; ///< \brief The model that this is attached to. + event::ConnectionPtr + updateConn; ///< \brief Pointer to the world update function. + transport::NodePtr node; ///< \brief The node we're advertising on. + transport::SubscriberPtr sub; ///< \brief Subscriber handle. }; diff --git a/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.cpp b/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.cpp index 0b0ad54221..4bc1e380e5 100644 --- a/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.cpp +++ b/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.cpp @@ -6,17 +6,16 @@ /*----------------------------------------------------------------------------*/ #ifdef _WIN32 - // Ensure that Winsock2.h is included before Windows.h, which can get - // pulled in by anybody (e.g., Boost). - #include +// Ensure that Winsock2.h is included before Windows.h, which can get +// pulled in by anybody (e.g., Boost). +#include #endif #include "potentiometer.h" +#include #include #include -#include - GZ_REGISTER_MODEL_PLUGIN(Potentiometer) @@ -32,7 +31,7 @@ void Potentiometer::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { if (sdf->HasElement("topic")) { topic = sdf->Get("topic"); } else { - topic = "~/"+sdf->GetAttribute("name")->GetAsString(); + topic = "~/" + sdf->GetAttribute("name")->GetAsString(); } if (sdf->HasElement("units")) { @@ -41,11 +40,12 @@ void Potentiometer::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { radians = true; } - gzmsg << "Initializing potentiometer: " << topic << " joint=" << joint->GetName() - << " radians=" << radians << std::endl; + gzmsg << "Initializing potentiometer: " << topic + << " joint=" << joint->GetName() << " radians=" << radians << std::endl; // Connect to Gazebo transport for messaging - std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName(); + std::string scoped_name = + model->GetWorld()->GetName() + "::" + model->GetScopedName(); boost::replace_all(scoped_name, "::", "/"); node = transport::NodePtr(new transport::Node()); node->Init(scoped_name); @@ -53,10 +53,11 @@ void Potentiometer::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { // Connect to the world update event. // This will trigger the Update function every Gazebo iteration - updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Potentiometer::Update, this, _1)); + updateConn = event::Events::ConnectWorldUpdateBegin( + boost::bind(&Potentiometer::Update, this, _1)); } -void Potentiometer::Update(const common::UpdateInfo &info) { +void Potentiometer::Update(const common::UpdateInfo& info) { joint->GetAngle(0).Normalize(); msgs::Float64 msg; if (radians) { diff --git a/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.h b/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.h index 5cb96cdbbd..3a5945e9cc 100644 --- a/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.h +++ b/simulation/frc_gazebo_plugins/potentiometer/src/potentiometer.h @@ -32,8 +32,8 @@ using namespace gazebo; * - `topic`: Optional. Message will be published as a gazebo.msgs.Float64. * - `units`: Optional. Defaults to radians. */ -class Potentiometer: public ModelPlugin { -public: +class Potentiometer : public ModelPlugin { + public: Potentiometer(); ~Potentiometer(); @@ -41,9 +41,9 @@ public: void Load(physics::ModelPtr model, sdf::ElementPtr sdf); /// \brief Sends out the potentiometer reading each timestep. - void Update(const common::UpdateInfo &info); + void Update(const common::UpdateInfo& info); -private: + private: /// \brief Publish the angle on this topic. std::string topic; @@ -53,9 +53,9 @@ private: /// \brief The joint that this potentiometer measures physics::JointPtr joint; - - physics::ModelPtr model; ///< \brief The model that this is attached to. - event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function. - transport::NodePtr node; ///< \brief The node we're advertising on. - transport::PublisherPtr pub; ///< \brief Publisher handle. + physics::ModelPtr model; ///< \brief The model that this is attached to. + event::ConnectionPtr + updateConn; ///< \brief Pointer to the world update function. + transport::NodePtr node; ///< \brief The node we're advertising on. + transport::PublisherPtr pub; ///< \brief Publisher handle. }; diff --git a/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.cpp b/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.cpp index dd76107319..81f61136b8 100644 --- a/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.cpp +++ b/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.cpp @@ -6,16 +6,16 @@ /*----------------------------------------------------------------------------*/ #ifdef _WIN32 - // Ensure that Winsock2.h is included before Windows.h, which can get - // pulled in by anybody (e.g., Boost). - #include +// Ensure that Winsock2.h is included before Windows.h, which can get +// pulled in by anybody (e.g., Boost). +#include #endif #include "rangefinder.h" #include -#include #include +#include #include @@ -30,17 +30,19 @@ void Rangefinder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { // Parse SDF properties sensor = boost::dynamic_pointer_cast( - sensors::get_sensor(sdf->Get("sensor"))); + sensors::get_sensor(sdf->Get("sensor"))); if (sdf->HasElement("topic")) { topic = sdf->Get("topic"); } else { - topic = "~/"+sdf->GetAttribute("name")->GetAsString(); + topic = "~/" + sdf->GetAttribute("name")->GetAsString(); } - gzmsg << "Initializing rangefinder: " << topic << " sensor=" << sensor->GetName() << std::endl; + gzmsg << "Initializing rangefinder: " << topic + << " sensor=" << sensor->GetName() << std::endl; // Connect to Gazebo transport for messaging - std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName(); + std::string scoped_name = + model->GetWorld()->GetName() + "::" + model->GetScopedName(); boost::replace_all(scoped_name, "::", "/"); node = transport::NodePtr(new transport::Node()); node->Init(scoped_name); @@ -48,10 +50,11 @@ void Rangefinder::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { // Connect to the world update event. // This will trigger the Update function every Gazebo iteration - updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Rangefinder::Update, this, _1)); + updateConn = event::Events::ConnectWorldUpdateBegin( + boost::bind(&Rangefinder::Update, this, _1)); } -void Rangefinder::Update(const common::UpdateInfo &info) { +void Rangefinder::Update(const common::UpdateInfo& info) { msgs::Float64 msg; msg.set_data(sensor->GetRange()); pub->Publish(msg); diff --git a/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.h b/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.h index a205368118..3adfc5eed5 100644 --- a/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.h +++ b/simulation/frc_gazebo_plugins/rangefinder/src/rangefinder.h @@ -30,8 +30,8 @@ using namespace gazebo; * - `sensor`: Name of the sonar sensor that this rangefinder uses. * - `topic`: Optional. Message will be published as a gazebo.msgs.Float64. */ -class Rangefinder: public ModelPlugin { -public: +class Rangefinder : public ModelPlugin { + public: Rangefinder(); ~Rangefinder(); @@ -39,17 +39,18 @@ public: void Load(physics::ModelPtr model, sdf::ElementPtr sdf); /// \brief Sends out the rangefinder reading each timestep. - void Update(const common::UpdateInfo &info); + void Update(const common::UpdateInfo& info); -private: + private: /// \brief Publish the range on this topic. std::string topic; /// \brief The sonar sensor that this rangefinder uses sensors::SonarSensorPtr sensor; - physics::ModelPtr model; ///< \brief The model that this is attached to. - event::ConnectionPtr updateConn; ///< \brief Pointer to the world update function. - transport::NodePtr node; ///< \brief The node we're advertising on. - transport::PublisherPtr pub; ///< \brief Publisher handle. + physics::ModelPtr model; ///< \brief The model that this is attached to. + event::ConnectionPtr + updateConn; ///< \brief Pointer to the world update function. + transport::NodePtr node; ///< \brief The node we're advertising on. + transport::PublisherPtr pub; ///< \brief Publisher handle. }; diff --git a/simulation/frc_gazebo_plugins/servo/src/servo.cpp b/simulation/frc_gazebo_plugins/servo/src/servo.cpp index e1b61f3ff1..95839daa7e 100644 --- a/simulation/frc_gazebo_plugins/servo/src/servo.cpp +++ b/simulation/frc_gazebo_plugins/servo/src/servo.cpp @@ -6,9 +6,9 @@ /*----------------------------------------------------------------------------*/ #ifdef _WIN32 - // Ensure that Winsock2.h is included before Windows.h, which can get - // pulled in by anybody (e.g., Boost). - #include +// Ensure that Winsock2.h is included before Windows.h, which can get +// pulled in by anybody (e.g., Boost). +#include #endif #include "servo.h" @@ -22,56 +22,57 @@ Servo::Servo() {} Servo::~Servo() {} -void Servo::Load(physics::ModelPtr model, sdf::ElementPtr sdf){ - this->model = model; - signal = 0; +void Servo::Load(physics::ModelPtr model, sdf::ElementPtr sdf) { + this->model = model; + signal = 0; - //parse SDF Properries - joint = model->GetJoint(sdf->Get("joint")); - if (sdf->HasElement("topic")) { - topic = sdf->Get("topic"); - } - else { - topic = "~/"+sdf->GetAttribute("name")->GetAsString(); - } - - if (sdf->HasElement("torque")) { - torque = sdf->Get("torque"); - } - else { - torque = 5; - } - - gzmsg << "initializing awesome servo: " << topic - << " joint=" << joint->GetName() - << " torque=" << torque << std::endl; - - //Connect to Gazebo transport for messaging - std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName(); - boost::replace_all(scoped_name, "::","/"); - node = transport::NodePtr(new transport::Node()); - node->Init(scoped_name); - sub = node->Subscribe(topic, &Servo::Callback, this); - - //connect to the world update event - //this will call update every iteration - updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Servo::Update, this, _1)); -} - -void Servo::Update(const common::UpdateInfo &info){ - //torque is in kg*cm - //joint->SetAngle(0,signal*180); - if (joint->GetAngle(0) < signal){ - joint->SetForce(0,torque); + // parse SDF Properries + joint = model->GetJoint(sdf->Get("joint")); + if (sdf->HasElement("topic")) { + topic = sdf->Get("topic"); + } else { + topic = "~/" + sdf->GetAttribute("name")->GetAsString(); } - else if (joint->GetAngle(0) > signal){ - joint->SetForce(0,torque); + + if (sdf->HasElement("torque")) { + torque = sdf->Get("torque"); + } else { + torque = 5; } - joint->SetForce(0,0); + + gzmsg << "initializing awesome servo: " << topic + << " joint=" << joint->GetName() << " torque=" << torque << std::endl; + + // Connect to Gazebo transport for messaging + std::string scoped_name = + model->GetWorld()->GetName() + "::" + model->GetScopedName(); + boost::replace_all(scoped_name, "::", "/"); + node = transport::NodePtr(new transport::Node()); + node->Init(scoped_name); + sub = node->Subscribe(topic, &Servo::Callback, this); + + // connect to the world update event + // this will call update every iteration + updateConn = event::Events::ConnectWorldUpdateBegin( + boost::bind(&Servo::Update, this, _1)); } -void Servo::Callback(const msgs::ConstFloat64Ptr &msg){ - signal = msg->data(); - if (signal < -1) { signal = -1; } - else if (signal > 1) { signal = 1; } +void Servo::Update(const common::UpdateInfo& info) { + // torque is in kg*cm + // joint->SetAngle(0,signal*180); + if (joint->GetAngle(0) < signal) { + joint->SetForce(0, torque); + } else if (joint->GetAngle(0) > signal) { + joint->SetForce(0, torque); + } + joint->SetForce(0, 0); +} + +void Servo::Callback(const msgs::ConstFloat64Ptr& msg) { + signal = msg->data(); + if (signal < -1) { + signal = -1; + } else if (signal > 1) { + signal = 1; + } } diff --git a/simulation/frc_gazebo_plugins/servo/src/servo.h b/simulation/frc_gazebo_plugins/servo/src/servo.h index dd6d18b1eb..528f5b1675 100644 --- a/simulation/frc_gazebo_plugins/servo/src/servo.h +++ b/simulation/frc_gazebo_plugins/servo/src/servo.h @@ -31,36 +31,36 @@ using namespace gazebo; * - `link`: Name of the link the servo is attached to. * - `topic`: Optional. Message type should be gazebo.msgs.Float64. */ -class Servo: public ModelPlugin { -public: - Servo(); - ~Servo(); +class Servo : public ModelPlugin { + public: + Servo(); + ~Servo(); - /// \brief load the servo and configure it according to the sdf - void Load(physics::ModelPtr model, sdf::ElementPtr sdf); + /// \brief load the servo and configure it according to the sdf + void Load(physics::ModelPtr model, sdf::ElementPtr sdf); - /// \brief Update the torque on the joint from the dc motor each timestep. - void Update(const common::UpdateInfo &info); + /// \brief Update the torque on the joint from the dc motor each timestep. + void Update(const common::UpdateInfo& info); -private: - /// \brief Topic to read control signal from. - std::string topic; + private: + /// \brief Topic to read control signal from. + std::string topic; - /// \brief the pwm signal limited to the range [-1,1] - double signal; + /// \brief the pwm signal limited to the range [-1,1] + double signal; - /// \brief the torque of the motor in kg/cm - double torque; + /// \brief the torque of the motor in kg/cm + double torque; - /// \brief the joint that this servo moves - physics::JointPtr joint; + /// \brief the joint that this servo moves + physics::JointPtr joint; - /// \brief Callback for receiving msgs and storing the signal - void Callback(const msgs::ConstFloat64Ptr &msg); - - physics::ModelPtr model; ///< \brief The model that this is attached to - event::ConnectionPtr updateConn; ///< \brief The Pointer to the world update function - transport::NodePtr node; ///< \brief The node we're advertising torque on - transport::SubscriberPtr sub; ///< \brief the Subscriber for the pwm signal + /// \brief Callback for receiving msgs and storing the signal + void Callback(const msgs::ConstFloat64Ptr& msg); + physics::ModelPtr model; ///< \brief The model that this is attached to + event::ConnectionPtr + updateConn; ///< \brief The Pointer to the world update function + transport::NodePtr node; ///< \brief The node we're advertising torque on + transport::SubscriberPtr sub; ///< \brief the Subscriber for the pwm signal }; diff --git a/styleguide/format.py b/styleguide/format.py new file mode 100755 index 0000000000..7639a26b7b --- /dev/null +++ b/styleguide/format.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +from subprocess import call +import os +import re +import sys + +sep = os.sep +# If directory separator is backslash, escape it for regexes +if sep == "\\": + sep += "\\" + +# Files and directories which should be included in or excluded from the update +regexInclude = re.compile("\.cpp$|\.h$|\.hpp$|\.inc$") +folderExclude = "build" + sep + "|\.git" + sep + "|gradle" + sep + \ + "|\.gradle" + sep + "|ni-libraries" + sep + "|ctre" + sep + \ + "|frccansae" + sep + "|FRC_FPGA_ChipObject" + sep + \ + "|gtest" + sep + "|i2clib" + sep + \ + "|NetworkCommunication" + sep + "|spilib" + sep + \ + "|visa" + sep + "|wpilibj" + sep +regexExclude = re.compile(folderExclude + + "|NIIMAQdx\.h$|nivision\.h$|can_proto\.h$|" + "CanTalonSRX\.h$") + +# Handle running in either the root or styleguide directories +configPath = "" +if os.getcwd().rpartition("/")[2] == "styleguide": + configPath = ".." +else: + configPath = "." + +# Recursively create list of files in given directory +files = [os.path.join(dp, f) for dp, dn, fn in + os.walk(os.path.expanduser(configPath)) for f in fn] + +# Apply regex filters to list +files = [f for f in files if regexInclude.search(f)] +files = [f for f in files if not regexExclude.search(f)] + +# Set clang-format name for platform +clangExec = "clang-format" +if sys.platform.startswith("win32"): + clangExec += ".exe" + +for name in files: + print("Processing", name,) + call([clangExec, "-i", "-style=file", name]) diff --git a/wpilibc/Athena/include/ADXL345_I2C.h b/wpilibc/Athena/include/ADXL345_I2C.h index e0327f78a4..175196bc70 100644 --- a/wpilibc/Athena/include/ADXL345_I2C.h +++ b/wpilibc/Athena/include/ADXL345_I2C.h @@ -7,10 +7,10 @@ #pragma once -#include "interfaces/Accelerometer.h" +#include #include "I2C.h" #include "LiveWindow/LiveWindowSendable.h" -#include +#include "interfaces/Accelerometer.h" /** * ADXL345 Accelerometer on I2C. @@ -52,7 +52,8 @@ class ADXL345_I2C : public Accelerometer, }; public: - explicit ADXL345_I2C(Port port, Range range = kRange_2G, int deviceAddress = kAddress); + explicit ADXL345_I2C(Port port, Range range = kRange_2G, + int deviceAddress = kAddress); virtual ~ADXL345_I2C() = default; ADXL345_I2C(const ADXL345_I2C&) = delete; diff --git a/wpilibc/Athena/include/ADXL345_SPI.h b/wpilibc/Athena/include/ADXL345_SPI.h index ceb28cd137..24df32e4f1 100644 --- a/wpilibc/Athena/include/ADXL345_SPI.h +++ b/wpilibc/Athena/include/ADXL345_SPI.h @@ -7,10 +7,10 @@ #pragma once -#include "interfaces/Accelerometer.h" -#include "SensorBase.h" -#include "SPI.h" #include "LiveWindow/LiveWindowSendable.h" +#include "SPI.h" +#include "SensorBase.h" +#include "interfaces/Accelerometer.h" #include diff --git a/wpilibc/Athena/include/ADXL362.h b/wpilibc/Athena/include/ADXL362.h index d4c2e0b0da..7266278ef3 100644 --- a/wpilibc/Athena/include/ADXL362.h +++ b/wpilibc/Athena/include/ADXL362.h @@ -7,10 +7,10 @@ #pragma once -#include "interfaces/Accelerometer.h" -#include "SensorBase.h" -#include "SPI.h" #include "LiveWindow/LiveWindowSendable.h" +#include "SPI.h" +#include "SensorBase.h" +#include "interfaces/Accelerometer.h" #include diff --git a/wpilibc/Athena/include/ADXRS450_Gyro.h b/wpilibc/Athena/include/ADXRS450_Gyro.h index 7717f578fd..36daaa75d8 100644 --- a/wpilibc/Athena/include/ADXRS450_Gyro.h +++ b/wpilibc/Athena/include/ADXRS450_Gyro.h @@ -8,9 +8,9 @@ #pragma once #include "GyroBase.h" +#include "HAL/cpp/priority_mutex.h" #include "Notifier.h" #include "SPI.h" -#include "HAL/cpp/priority_mutex.h" #include diff --git a/wpilibc/Athena/include/AnalogAccelerometer.h b/wpilibc/Athena/include/AnalogAccelerometer.h index 86f7019770..faa3c3a56b 100644 --- a/wpilibc/Athena/include/AnalogAccelerometer.h +++ b/wpilibc/Athena/include/AnalogAccelerometer.h @@ -8,26 +8,24 @@ #pragma once #include "AnalogInput.h" -#include "SensorBase.h" -#include "PIDSource.h" #include "LiveWindow/LiveWindowSendable.h" +#include "PIDSource.h" +#include "SensorBase.h" #include /** * Handle operation of an analog accelerometer. * The accelerometer reads acceleration directly through the sensor. Many - * sensors have - * multiple axis and can be treated as multiple devices. Each is calibrated by - * finding - * the center value over a period of time. + * sensors have multiple axis and can be treated as multiple devices. Each is + * calibrated by finding the center value over a period of time. */ class AnalogAccelerometer : public SensorBase, public PIDSource, public LiveWindowSendable { public: explicit AnalogAccelerometer(int32_t channel); - explicit AnalogAccelerometer(AnalogInput *channel); + explicit AnalogAccelerometer(AnalogInput* channel); explicit AnalogAccelerometer(std::shared_ptr channel); virtual ~AnalogAccelerometer() = default; diff --git a/wpilibc/Athena/include/AnalogGyro.h b/wpilibc/Athena/include/AnalogGyro.h index acb5b0f4c1..0f45c44a66 100644 --- a/wpilibc/Athena/include/AnalogGyro.h +++ b/wpilibc/Athena/include/AnalogGyro.h @@ -14,18 +14,13 @@ class AnalogInput; /** * Use a rate gyro to return the robots heading relative to a starting position. * The Gyro class tracks the robots heading based on the starting position. As - * the robot - * rotates the new heading is computed by integrating the rate of rotation - * returned - * by the sensor. When the class is instantiated, it does a short calibration - * routine - * where it samples the gyro while at rest to determine the default offset. This - * is - * subtracted from each sample to determine the heading. This gyro class must be - * used - * with a channel that is assigned one of the Analog accumulators from the FPGA. - * See - * AnalogInput for the current accumulator assignments. + * the robot rotates the new heading is computed by integrating the rate of + * rotation returned by the sensor. When the class is instantiated, it does a + * short calibration routine where it samples the gyro while at rest to + * determine the default offset. This is subtracted from each sample to + * determine the heading. This gyro class must be used with a channel that is + * assigned one of the Analog accumulators from the FPGA. See AnalogInput for + * the current accumulator assignments. * * This class is for gyro sensors that connect to an analog input. */ @@ -38,10 +33,11 @@ class AnalogGyro : public GyroBase { static constexpr float kDefaultVoltsPerDegreePerSecond = 0.007; explicit AnalogGyro(int32_t channel); - explicit AnalogGyro(AnalogInput *channel); + explicit AnalogGyro(AnalogInput* channel); explicit AnalogGyro(std::shared_ptr channel); AnalogGyro(int32_t channel, uint32_t center, float offset); - AnalogGyro(std::shared_ptr channel, uint32_t center, float offset); + AnalogGyro(std::shared_ptr channel, uint32_t center, + float offset); virtual ~AnalogGyro() = default; float GetAngle() const override; diff --git a/wpilibc/Athena/include/AnalogInput.h b/wpilibc/Athena/include/AnalogInput.h index 6217e2d0df..e7cb88507f 100644 --- a/wpilibc/Athena/include/AnalogInput.h +++ b/wpilibc/Athena/include/AnalogInput.h @@ -8,9 +8,9 @@ #pragma once #include "HAL/HAL.hpp" -#include "SensorBase.h" -#include "PIDSource.h" #include "LiveWindow/LiveWindowSendable.h" +#include "PIDSource.h" +#include "SensorBase.h" #include @@ -18,16 +18,13 @@ * Analog input class. * * Connected to each analog channel is an averaging and oversampling engine. - * This engine accumulates - * the specified ( by SetAverageBits() and SetOversampleBits() ) number of - * samples before returning a new - * value. This is not a sliding window average. The only difference between - * the oversampled samples and - * the averaged samples is that the oversampled samples are simply accumulated - * effectively increasing the - * resolution, while the averaged samples are divided by the number of samples - * to retain the resolution, - * but get more stable values. + * This engine accumulates the specified ( by SetAverageBits() and + * SetOversampleBits() ) number of samples before returning a new value. This + * is not a sliding window average. The only difference between the oversampled + * samples and the averaged samples is that the oversampled samples are simply + * accumulated effectively increasing the resolution, while the averaged samples + * are divided by the number of samples to retain the resolution, but get more + * stable values. */ class AnalogInput : public SensorBase, public PIDSource, @@ -64,7 +61,7 @@ class AnalogInput : public SensorBase, void SetAccumulatorDeadband(int32_t deadband); int64_t GetAccumulatorValue() const; uint32_t GetAccumulatorCount() const; - void GetAccumulatorOutput(int64_t &value, uint32_t &count) const; + void GetAccumulatorOutput(int64_t& value, uint32_t& count) const; static void SetSampleRate(float samplesPerSecond); static float GetSampleRate(); @@ -80,8 +77,8 @@ class AnalogInput : public SensorBase, private: uint32_t m_channel; - //TODO: Adjust HAL to avoid use of raw pointers. - void *m_port; + // TODO: Adjust HAL to avoid use of raw pointers. + void* m_port; int64_t m_accumulatorOffset; std::shared_ptr m_table; diff --git a/wpilibc/Athena/include/AnalogOutput.h b/wpilibc/Athena/include/AnalogOutput.h index d7bd30efdd..9f4fac87e7 100644 --- a/wpilibc/Athena/include/AnalogOutput.h +++ b/wpilibc/Athena/include/AnalogOutput.h @@ -7,11 +7,11 @@ #pragma once -#include "HAL/HAL.hpp" -#include "SensorBase.h" -#include "LiveWindow/LiveWindowSendable.h" -#include #include +#include +#include "HAL/HAL.hpp" +#include "LiveWindow/LiveWindowSendable.h" +#include "SensorBase.h" /** * MXP analog output class. @@ -33,7 +33,7 @@ class AnalogOutput : public SensorBase, public LiveWindowSendable { protected: uint32_t m_channel; - void *m_port; + void* m_port; std::shared_ptr m_table; }; diff --git a/wpilibc/Athena/include/AnalogPotentiometer.h b/wpilibc/Athena/include/AnalogPotentiometer.h index 3c6daa558d..4c9a98b9c2 100644 --- a/wpilibc/Athena/include/AnalogPotentiometer.h +++ b/wpilibc/Athena/include/AnalogPotentiometer.h @@ -6,8 +6,8 @@ /*----------------------------------------------------------------------------*/ #include "AnalogInput.h" -#include "interfaces/Potentiometer.h" #include "LiveWindow/LiveWindowSendable.h" +#include "interfaces/Potentiometer.h" #include @@ -43,7 +43,7 @@ class AnalogPotentiometer : public Potentiometer, public LiveWindowSendable { explicit AnalogPotentiometer(int channel, double fullRange = 1.0, double offset = 0.0); - explicit AnalogPotentiometer(AnalogInput *input, double fullRange = 1.0, + explicit AnalogPotentiometer(AnalogInput* input, double fullRange = 1.0, double offset = 0.0); explicit AnalogPotentiometer(std::shared_ptr input, diff --git a/wpilibc/Athena/include/AnalogTrigger.h b/wpilibc/Athena/include/AnalogTrigger.h index 811cc07743..245258d732 100644 --- a/wpilibc/Athena/include/AnalogTrigger.h +++ b/wpilibc/Athena/include/AnalogTrigger.h @@ -7,8 +7,8 @@ #pragma once -#include "HAL/HAL.hpp" #include "AnalogTriggerOutput.h" +#include "HAL/HAL.hpp" #include "SensorBase.h" class AnalogInput; @@ -18,7 +18,7 @@ class AnalogTrigger : public SensorBase { public: explicit AnalogTrigger(int32_t channel); - explicit AnalogTrigger(AnalogInput *channel); + explicit AnalogTrigger(AnalogInput* channel); virtual ~AnalogTrigger(); void SetLimitsVoltage(float lower, float upper); @@ -28,9 +28,10 @@ class AnalogTrigger : public SensorBase { uint32_t GetIndex() const; bool GetInWindow(); bool GetTriggerState(); - std::shared_ptr CreateOutput(AnalogTriggerType type) const; + std::shared_ptr CreateOutput( + AnalogTriggerType type) const; private: uint8_t m_index; - void *m_trigger; + void* m_trigger; }; diff --git a/wpilibc/Athena/include/AnalogTriggerOutput.h b/wpilibc/Athena/include/AnalogTriggerOutput.h index 976d30cc6a..c9a9916dfe 100644 --- a/wpilibc/Athena/include/AnalogTriggerOutput.h +++ b/wpilibc/Athena/include/AnalogTriggerOutput.h @@ -11,48 +11,34 @@ class AnalogTrigger; -/** - * Class to represent a specific output from an analog trigger. - * This class is used to get the current output value and also as a - * DigitalSource - * to provide routing of an output to digital subsystems on the FPGA such as - * Counter, Encoder, and Interrupt. +/** Class to represent a specific output from an analog trigger. + * This class is used to get the current output value and also as a + * DigitalSource to provide routing of an output to digital subsystems on the + * FPGA such as Counter, Encoder, and Interrupt. * * The TriggerState output indicates the primary output value of the trigger. - * If the analog - * signal is less than the lower limit, the output is false. If the analog - * value is greater - * than the upper limit, then the output is true. If the analog value is in - * between, then - * the trigger output state maintains its most recent value. + * If the analog signal is less than the lower limit, the output is false. If + * the analog value is greater than the upper limit, then the output is true. + * If the analog value is in between, then the trigger output state maintains + * its most recent value. * * The InWindow output indicates whether or not the analog signal is inside the - * range defined - * by the limits. + * range defined by the limits. * * The RisingPulse and FallingPulse outputs detect an instantaneous transition - * from above the - * upper limit to below the lower limit, and vise versa. These pulses represent - * a rollover - * condition of a sensor and can be routed to an up / down couter or to - * interrupts. Because - * the outputs generate a pulse, they cannot be read directly. To help ensure - * that a rollover - * condition is not missed, there is an average rejection filter available that - * operates on the + * from above the upper limit to below the lower limit, and vise versa. These + * pulses represent a rollover condition of a sensor and can be routed to an up + * / down couter or to interrupts. Because the outputs generate a pulse, they + * cannot be read directly. To help ensure that a rollover condition is not + * missed, there is an average rejection filter available that operates on the * upper 8 bits of a 12 bit number and selects the nearest outlyer of 3 samples. - * This will reject - * a sample that is (due to averaging or sampling) errantly between the two - * limits. This filter - * will fail if more than one sample in a row is errantly in between the two - * limits. You may see - * this problem if attempting to use this feature with a mechanical rollover - * sensor, such as a + * This will reject a sample that is (due to averaging or sampling) errantly + * between the two limits. This filter will fail if more than one sample in a + * row is errantly in between the two limits. You may see this problem if + * attempting to use this feature with a mechanical rollover sensor, such as a * 360 degree no-stop potentiometer without signal conditioning, because the - * rollover transition - * is not sharp / clean enough. Using the averaging engine may help with this, - * but rotational speeds of - * the sensor will then be limited. + * rollover transition is not sharp / clean enough. Using the averaging engine + * may help with this, but rotational speeds of the sensor will then be limited. */ class AnalogTriggerOutput : public DigitalSource { friend class AnalogTrigger; @@ -67,12 +53,13 @@ class AnalogTriggerOutput : public DigitalSource { virtual bool GetAnalogTriggerForRouting() const override; protected: - AnalogTriggerOutput(const AnalogTrigger &trigger, AnalogTriggerType outputType); + AnalogTriggerOutput(const AnalogTrigger& trigger, + AnalogTriggerType outputType); private: // Uses reference rather than smart pointer because a user can not construct // an AnalogTriggerOutput themselves and because the AnalogTriggerOutput // should always be in scope at the same time as an AnalogTrigger. - const AnalogTrigger &m_trigger; + const AnalogTrigger& m_trigger; AnalogTriggerType m_outputType; }; diff --git a/wpilibc/Athena/include/BuiltInAccelerometer.h b/wpilibc/Athena/include/BuiltInAccelerometer.h index d201b613d8..a652cbf01c 100644 --- a/wpilibc/Athena/include/BuiltInAccelerometer.h +++ b/wpilibc/Athena/include/BuiltInAccelerometer.h @@ -7,9 +7,9 @@ #pragma once -#include "interfaces/Accelerometer.h" -#include "SensorBase.h" #include "LiveWindow/LiveWindowSendable.h" +#include "SensorBase.h" +#include "interfaces/Accelerometer.h" #include diff --git a/wpilibc/Athena/include/CANJaguar.h b/wpilibc/Athena/include/CANJaguar.h index c456e2aa69..132799b577 100644 --- a/wpilibc/Athena/include/CANJaguar.h +++ b/wpilibc/Athena/include/CANJaguar.h @@ -7,24 +7,24 @@ #pragma once -#include "ErrorBase.h" -#include "MotorSafety.h" -#include "Resource.h" -#include "MotorSafetyHelper.h" -#include "PIDOutput.h" -#include "CANSpeedController.h" -#include "HAL/cpp/Semaphore.hpp" -#include "HAL/HAL.hpp" -#include "LiveWindow/LiveWindowSendable.h" -#include "tables/ITableListener.h" -#include "NetworkCommunication/CANSessionMux.h" #include "CAN/can_proto.h" +#include "CANSpeedController.h" +#include "ErrorBase.h" +#include "HAL/HAL.hpp" +#include "HAL/cpp/Semaphore.hpp" +#include "LiveWindow/LiveWindowSendable.h" +#include "MotorSafety.h" +#include "MotorSafetyHelper.h" +#include "NetworkCommunication/CANSessionMux.h" +#include "PIDOutput.h" +#include "Resource.h" +#include "tables/ITableListener.h" #include -#include "HAL/cpp/priority_mutex.h" #include -#include #include +#include +#include "HAL/cpp/priority_mutex.h" /** * Luminary Micro / Vex Robotics Jaguar Speed Control @@ -152,23 +152,23 @@ class CANJaguar : public MotorSafety, void SetPositionReference(uint8_t reference); uint8_t GetPositionReference() const; - uint8_t packPercentage(uint8_t *buffer, double value); - uint8_t packFXP8_8(uint8_t *buffer, double value); - uint8_t packFXP16_16(uint8_t *buffer, double value); - uint8_t packint16_t(uint8_t *buffer, int16_t value); - uint8_t packint32_t(uint8_t *buffer, int32_t value); - double unpackPercentage(uint8_t *buffer) const; - double unpackFXP8_8(uint8_t *buffer) const; - double unpackFXP16_16(uint8_t *buffer) const; - int16_t unpackint16_t(uint8_t *buffer) const; - int32_t unpackint32_t(uint8_t *buffer) const; + uint8_t packPercentage(uint8_t* buffer, double value); + uint8_t packFXP8_8(uint8_t* buffer, double value); + uint8_t packFXP16_16(uint8_t* buffer, double value); + uint8_t packint16_t(uint8_t* buffer, int16_t value); + uint8_t packint32_t(uint8_t* buffer, int32_t value); + double unpackPercentage(uint8_t* buffer) const; + double unpackFXP8_8(uint8_t* buffer) const; + double unpackFXP16_16(uint8_t* buffer) const; + int16_t unpackint16_t(uint8_t* buffer) const; + int32_t unpackint32_t(uint8_t* buffer) const; - void sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize, + void sendMessage(uint32_t messageID, const uint8_t* data, uint8_t dataSize, int32_t period = CAN_SEND_PERIOD_NO_REPEAT); void requestMessage(uint32_t messageID, int32_t period = CAN_SEND_PERIOD_NO_REPEAT); - bool getMessage(uint32_t messageID, uint32_t mask, uint8_t *data, - uint8_t *dataSize) const; + bool getMessage(uint32_t messageID, uint32_t mask, uint8_t* data, + uint8_t* dataSize) const; void setupPeriodicStatus(); void updatePeriodicStatus() const; @@ -196,7 +196,8 @@ class CANJaguar : public MotorSafety, float m_faultTime = 0.0f; // Which parameters have been verified since they were last set? - bool m_controlModeVerified = false; // Needs to be verified because it's set in the constructor + bool m_controlModeVerified = + false; // Needs to be verified because it's set in the constructor bool m_speedRefVerified = true; bool m_posRefVerified = true; bool m_pVerified = true; diff --git a/wpilibc/Athena/include/CANSpeedController.h b/wpilibc/Athena/include/CANSpeedController.h index 5d7602639f..20aae9c065 100644 --- a/wpilibc/Athena/include/CANSpeedController.h +++ b/wpilibc/Athena/include/CANSpeedController.h @@ -22,8 +22,8 @@ class CANSpeedController : public SpeedController { kSpeed = 2, kPosition = 3, kVoltage = 4, - kFollower = 5, // Not supported in Jaguar. - kMotionProfile = 6, // Not supported in Jaguar. + kFollower = 5, // Not supported in Jaguar. + kMotionProfile = 6, // Not supported in Jaguar. }; // Helper function for the ControlMode enum @@ -97,6 +97,6 @@ class CANSpeedController : public SpeedController { virtual void ConfigMaxOutputVoltage(double voltage) = 0; virtual void ConfigFaultTime(float faultTime) = 0; // Hold off on interface until we figure out ControlMode enums. - // virtual void SetControlMode(ControlMode mode) = 0; - // virtual ControlMode GetControlMode() const = 0; + // virtual void SetControlMode(ControlMode mode) = 0; + // virtual ControlMode GetControlMode() const = 0; }; diff --git a/wpilibc/Athena/include/CANTalon.h b/wpilibc/Athena/include/CANTalon.h index 8ea9ae169c..8463217c62 100644 --- a/wpilibc/Athena/include/CANTalon.h +++ b/wpilibc/Athena/include/CANTalon.h @@ -7,14 +7,14 @@ #pragma once -#include "SafePWM.h" #include "CANSpeedController.h" +#include "HAL/CanTalonSRX.h" +#include "LiveWindow/LiveWindowSendable.h" +#include "MotorSafetyHelper.h" +#include "PIDInterface.h" #include "PIDOutput.h" #include "PIDSource.h" -#include "PIDInterface.h" -#include "HAL/CanTalonSRX.h" -#include "MotorSafetyHelper.h" -#include "LiveWindow/LiveWindowSendable.h" +#include "SafePWM.h" #include "tables/ITable.h" #include @@ -36,17 +36,22 @@ class CANTalon : public MotorSafety, AnalogEncoder = 3, EncRising = 4, EncFalling = 5, - CtreMagEncoder_Relative = 6, //!< Cross The Road Electronics Magnetic Encoder in Absolute/PulseWidth Mode - CtreMagEncoder_Absolute = 7, //!< Cross The Road Electronics Magnetic Encoder in Relative/Quadrature Mode + CtreMagEncoder_Relative = 6, //!< Cross The Road Electronics Magnetic + //! Encoder in Absolute/PulseWidth Mode + CtreMagEncoder_Absolute = 7, //!< Cross The Road Electronics Magnetic + //! Encoder in Relative/Quadrature Mode PulseWidth = 8, }; /** - * Depending on the sensor type, Talon can determine if sensor is plugged in ot not. + * Depending on the sensor type, Talon can determine if sensor is plugged in + * ot not. */ enum FeedbackDeviceStatus { - FeedbackStatusUnknown = 0, //!< Sensor status could not be determined. Not all sensors can do this. - FeedbackStatusPresent = 1, //!< Sensor is present and working okay. - FeedbackStatusNotPresent = 2, //!< Sensor is not present, not plugged in, not powered, etc... + FeedbackStatusUnknown = 0, //!< Sensor status could not be determined. Not + //! all sensors can do this. + FeedbackStatusPresent = 1, //!< Sensor is present and working okay. + FeedbackStatusNotPresent = + 2, //!< Sensor is not present, not plugged in, not powered, etc... }; enum StatusFrameRate { StatusFrameRateGeneral = 0, @@ -59,27 +64,28 @@ class CANTalon : public MotorSafety, * Enumerated types for Motion Control Set Values. * When in Motion Profile control mode, these constants are paseed * into set() to manipulate the motion profile executer. - * When changing modes, be sure to read the value back using getMotionProfileStatus() - * to ensure changes in output take effect before performing buffering actions. + * When changing modes, be sure to read the value back using + * getMotionProfileStatus() to ensure changes in output take effect before + * performing buffering actions. * Disable will signal Talon to put motor output into neutral drive. - * Talon will stop processing motion profile points. This means the buffer is - * effectively disconnected from the executer, allowing the robot to gracefully - * clear and push new traj points. isUnderrun will get cleared. + * Talon will stop processing motion profile points. This means the buffer + * is effectively disconnected from the executer, allowing the robot to + * gracefully clear and push new traj points. isUnderrun will get cleared. * The active trajectory is also cleared. - * Enable will signal Talon to pop a trajectory point from it's buffer and process it. - * If the active trajectory is empty, Talon will shift in the next point. - * If the active traj is empty, and so is the buffer, the motor drive is neutral and - * isUnderrun is set. When active traj times out, and buffer has at least one point, - * Talon shifts in next one, and isUnderrun is cleared. When active traj times out, - * and buffer is empty, Talon keeps processing active traj and sets IsUnderrun. + * Enable will signal Talon to pop a trajectory point from it's buffer and + * process it. If the active trajectory is empty, Talon will shift in the + * next point. If the active traj is empty, and so is the buffer, the motor + * drive is neutral and isUnderrun is set. When active traj times out, and + * buffer has at least one point, Talon shifts in next one, and isUnderrun + * is cleared. When active traj times out, and buffer is empty, Talon + * keeps processing active traj and sets IsUnderrun. * Hold will signal Talon keep processing the active trajectory indefinitely. * If the active traj is cleared, Talon will neutral motor drive. Otherwise - * Talon will keep processing the active traj but it will not shift in - * points from the buffer. This means the buffer is effectively disconnected - * from the executer, allowing the robot to gracefully clear and push - * new traj points. - * isUnderrun is set if active traj is empty, otherwise it is cleared. - * isLast signal is also cleared. + * Talon will keep processing the active traj but it will not shift in + * points from the buffer. This means the buffer is effectively + * disconnected from the executer, allowing the robot to gracefully clear + * and push new traj points. isUnderrun is set if active traj is empty, + * otherwise it is cleared. isLast signal is also cleared. * * Typical workflow: * set(Disable), @@ -87,8 +93,10 @@ class CANTalon : public MotorSafety, * clear buffer and push buffer points, * set(Enable) when enough points have been pushed to ensure no underruns, * wait for MP to finish or decide abort, - * If MP finished gracefully set(Hold) to hold position servo and disconnect buffer, - * If MP is being aborted set(Disable) to neutral the motor and disconnect buffer, + * If MP finished gracefully set(Hold) to hold position servo and disconnect + * buffer, + * If MP is being aborted set(Disable) to neutral the motor and disconnect + * buffer, * Confirm mode takes effect, * clear buffer and push buffer points, and rinse-repeat. */ @@ -102,8 +110,8 @@ class CANTalon : public MotorSafety, * This is simply a data transer object. */ struct TrajectoryPoint { - double position; //!< The position to servo to. - double velocity; //!< The velocity to feed-forward. + double position; //!< The position to servo to. + double velocity; //!< The velocity to feed-forward. /** * Time in milliseconds to process this point. * Value should be between 1ms and 255ms. If value is zero @@ -122,12 +130,15 @@ class CANTalon : public MotorSafety, * Set to true to only perform the velocity feed-forward and not perform * position servo. This is useful when learning how the position servo * changes the motor response. The same could be accomplish by clearing the - * PID gains, however this is synchronous the streaming, and doesn't require restoing + * PID gains, however this is synchronous the streaming, and doesn't require + * restoing * gains when finished. * - * Additionaly setting this basically gives you direct control of the motor output + * Additionaly setting this basically gives you direct control of the motor + * output * since motor output = targetVelocity X Kv, where Kv is our Fgain. - * This means you can also scheduling straight-throttle curves without relying on + * This means you can also scheduling straight-throttle curves without + * relying on * a sensor. */ bool velocityOnly; @@ -135,17 +146,22 @@ class CANTalon : public MotorSafety, * Set to true to signal Talon that this is the final point, so do not * attempt to pop another trajectory point from out of the Talon buffer. * Instead continue processing this way point. Typically the velocity - * member variable should be zero so that the motor doesn't spin indefinitely. + * member variable should be zero so that the motor doesn't spin + * indefinitely. */ bool isLastPoint; - /** + /** * Set to true to signal Talon to zero the selected sensor. - * When generating MPs, one simple method is to make the first target position zero, - * and the final target position the target distance from the current position. + * When generating MPs, one simple method is to make the first target + * position zero, + * and the final target position the target distance from the current + * position. * Then when you fire the MP, the current position gets set to zero. - * If this is the intent, you can set zeroPos on the first trajectory point. + * If this is the intent, you can set zeroPos on the first trajectory + * point. * - * Otherwise you can leave this false for all points, and offset the positions + * Otherwise you can leave this false for all points, and offset the + * positions * of all trajectory points so they are correct. */ bool zeroPos; @@ -158,9 +174,10 @@ class CANTalon : public MotorSafety, /** * The available empty slots in the trajectory buffer. * - * The robot API holds a "top buffer" of trajectory points, so your applicaion - * can dump several points at once. The API will then stream them into the Talon's - * low-level buffer, allowing the Talon to act on them. + * The robot API holds a "top buffer" of trajectory points, so your + * applicaion can dump several points at once. The API will then stream + * them into the Talon's low-level buffer, allowing the Talon to act on + * them. */ unsigned int topBufferRem; /** @@ -180,8 +197,8 @@ class CANTalon : public MotorSafety, bool hasUnderrun; /** * This is set if Talon needs to shift a point from its buffer into - * the active trajectory point however the buffer is empty. This gets cleared - * automatically when is resolved. + * the active trajectory point however the buffer is empty. This gets + * cleared automatically when is resolved. */ bool isUnderrun; /** @@ -194,9 +211,11 @@ class CANTalon : public MotorSafety, */ TrajectoryPoint activePoint; /** - * The current output mode of the motion profile executer (disabled, enabled, or hold). - * When changing the set() value in MP mode, it's important to check this signal to - * confirm the change takes effect before interacting with the top buffer. + * The current output mode of the motion profile executer (disabled, + * enabled, or hold). + * When changing the set() value in MP mode, it's important to check this + * signal to confirm the change takes effect before interacting with the + * top buffer. */ SetValueMotionProfile outputEnable; }; @@ -268,7 +287,8 @@ class CANTalon : public MotorSafety, virtual int GetPulseWidthVelocity() const; virtual int GetPulseWidthRiseToFallUs() const; virtual int GetPulseWidthRiseToRiseUs() const; - virtual FeedbackDeviceStatus IsSensorPresent(FeedbackDevice feedbackDevice)const; + virtual FeedbackDeviceStatus IsSensorPresent( + FeedbackDevice feedbackDevice) const; virtual bool GetForwardLimitOK() const override; virtual bool GetReverseLimitOK() const override; virtual uint16_t GetFaults() const override; @@ -286,7 +306,8 @@ class CANTalon : public MotorSafety, virtual void ConfigLimitMode(LimitMode mode) override; virtual void ConfigForwardLimit(double forwardLimitPosition) override; virtual void ConfigReverseLimit(double reverseLimitPosition) override; - void ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, bool bReverseLimitSwitchEn); + void ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, + bool bReverseLimitSwitchEn); void ConfigForwardSoftLimitEnable(bool bForwardSoftLimitEn); void ConfigReverseSoftLimitEnable(bool bReverseSoftLimitEn); /** @@ -312,18 +333,20 @@ class CANTalon : public MotorSafety, */ void ConfigRevLimitSwitchNormallyOpen(bool normallyOpen); virtual void ConfigMaxOutputVoltage(double voltage) override; - void ConfigPeakOutputVoltage(double forwardVoltage,double reverseVoltage); - void ConfigNominalOutputVoltage(double forwardVoltage,double reverseVoltage); + void ConfigPeakOutputVoltage(double forwardVoltage, double reverseVoltage); + void ConfigNominalOutputVoltage(double forwardVoltage, double reverseVoltage); /** * Enables Talon SRX to automatically zero the Sensor Position whenever an * edge is detected on the index signal. - * @param enable boolean input, pass true to enable feature or false to disable. - * @param risingEdge boolean input, pass true to clear the position on rising edge, - * pass false to clear the position on falling edge. + * + * @param enable boolean input, pass true to enable feature or false to + * disable. + * @param risingEdge boolean input, pass true to clear the position on rising + * edge, pass false to clear the position on falling edge. */ void EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge); void ConfigSetParameter(uint32_t paramEnum, double value); - bool GetParameter(uint32_t paramEnum, double & dvalue) const; + bool GetParameter(uint32_t paramEnum, double& dvalue) const; virtual void ConfigFaultTime(float faultTime) override; virtual void SetControlMode(ControlMode mode); @@ -343,37 +366,39 @@ class CANTalon : public MotorSafety, bool IsEnabled() const override; double GetSetpoint() const override; - /** - * Calling application can opt to speed up the handshaking between the robot API and the Talon to increase the - * download rate of the Talon's Motion Profile. Ideally the period should be no more than half the period - * of a trajectory point. + * Calling application can opt to speed up the handshaking between the robot + * API and the Talon to increase the download rate of the Talon's Motion + * Profile. Ideally the period should be no more than half the period of a + * trajectory point. */ void ChangeMotionControlFramePeriod(int periodMs); /** - * Clear the buffered motion profile in both Talon RAM (bottom), and in the API (top). - * Be sure to check GetMotionProfileStatus() to know when the buffer is actually cleared. + * Clear the buffered motion profile in both Talon RAM (bottom), and in the + * API (top). Be sure to check GetMotionProfileStatus() to know when the + * buffer is actually cleared. */ void ClearMotionProfileTrajectories(); /** * Retrieve just the buffer count for the api-level (top) buffer. - * This routine performs no CAN or data structure lookups, so its fast and ideal - * if caller needs to quickly poll the progress of trajectory points being emptied - * into Talon's RAM. Otherwise just use GetMotionProfileStatus. + * This routine performs no CAN or data structure lookups, so its fast and + * ideal if caller needs to quickly poll the progress of trajectory points + * being emptied into Talon's RAM. Otherwise just use GetMotionProfileStatus. * @return number of trajectory points in the top buffer. */ int GetMotionProfileTopLevelBufferCount(); /** - * Push another trajectory point into the top level buffer (which is emptied into - * the Talon's bottom buffer as room allows). + * Push another trajectory point into the top level buffer (which is emptied + * into the Talon's bottom buffer as room allows). + * * @param trajPt the trajectory point to insert into buffer. - * @return true if trajectory point push ok. CTR_BufferFull if buffer is full - * due to kMotionProfileTopBufferCapacity. + * @return true if trajectory point push ok. CTR_BufferFull if buffer is full + * due to kMotionProfileTopBufferCapacity. */ - bool PushMotionProfileTrajectory(const TrajectoryPoint & trajPt); + bool PushMotionProfileTrajectory(const TrajectoryPoint& trajPt); /** * @return true if api-level (top) buffer is full. @@ -381,28 +406,34 @@ class CANTalon : public MotorSafety, bool IsMotionProfileTopLevelBufferFull(); /** - * This must be called periodically to funnel the trajectory points from the API's top level buffer to - * the Talon's bottom level buffer. Recommendation is to call this twice as fast as the executation rate of the motion profile. - * So if MP is running with 20ms trajectory points, try calling this routine every 10ms. All motion profile functions are thread-safe - * through the use of a mutex, so there is no harm in having the caller utilize threading. + * This must be called periodically to funnel the trajectory points from the + * API's top level buffer to the Talon's bottom level buffer. Recommendation + * is to call this twice as fast as the executation rate of the motion + * profile. So if MP is running with 20ms trajectory points, try calling this + * routine every 10ms. All motion profile functions are thread-safe through + * the use of a mutex, so there is no harm in having the caller utilize + * threading. */ void ProcessMotionProfileBuffer(); /** * Retrieve all status information. - * Since this all comes from one CAN frame, its ideal to have one routine to retrieve the frame once and decode everything. - * @param [out] motionProfileStatus contains all progress information on the currently running MP. + * Since this all comes from one CAN frame, its ideal to have one routine to + * retrieve the frame once and decode everything. + * @param [out] motionProfileStatus contains all progress information on the + * currently running MP. */ - void GetMotionProfileStatus(MotionProfileStatus & motionProfileStatus); + void GetMotionProfileStatus(MotionProfileStatus& motionProfileStatus); /** - * Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is ready for another point, - * but the low level buffer is empty. + * Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is + * ready for another point, but the low level buffer is empty. * - * Once the Motion Profile Executer sets the hasUnderrun flag, it stays set until - * Robot Application clears it with this routine, which ensures Robot Application - * gets a chance to instrument or react. Caller could also check the isUnderrun flag - * which automatically clears when fault condition is removed. + * Once the Motion Profile Executer sets the hasUnderrun flag, it stays set + * until Robot Application clears it with this routine, which ensures Robot + * Application gets a chance to instrument or react. Caller could also check + * the isUnderrun flag which automatically clears when fault condition is + * removed. */ void ClearMotionProfileHasUnderrun(); @@ -437,7 +468,7 @@ class CANTalon : public MotorSafety, std::unique_ptr m_impl; std::unique_ptr m_safetyHelper; int m_profile = 0; // Profile from CANTalon to use. Set to zero until we can - // actually test this. + // actually test this. bool m_controlEnabled = true; bool m_stopped = false; @@ -447,37 +478,38 @@ class CANTalon : public MotorSafety, double m_setPoint = 0; /** * Encoder CPR, counts per rotations, also called codes per revoluion. - * Default value of zero means the API behaves as it did during the 2015 season, each position - * unit is a single pulse and there are four pulses per count (4X). - * Caller can use ConfigEncoderCodesPerRev to set the quadrature encoder CPR. + * Default value of zero means the API behaves as it did during the 2015 + * season, each position unit is a single pulse and there are four pulses per + * count (4X). Caller can use ConfigEncoderCodesPerRev to set the quadrature + * encoder CPR. */ uint32_t m_codesPerRev = 0; /** - * Number of turns per rotation. For example, a 10-turn pot spins ten full rotations from - * a wiper voltage of zero to 3.3 volts. Therefore knowing the - * number of turns a full voltage sweep represents is necessary for calculating rotations - * and velocity. - * A default value of zero means the API behaves as it did during the 2015 season, there are 1024 - * position units from zero to 3.3V. + * Number of turns per rotation. For example, a 10-turn pot spins ten full + * rotations from a wiper voltage of zero to 3.3 volts. Therefore knowing + * the number of turns a full voltage sweep represents is necessary for + * calculating rotations and velocity. A default value of zero means the API + * behaves as it did during the 2015 season, there are 1024 position units + * from zero to 3.3V. */ uint32_t m_numPotTurns = 0; /** - * Although the Talon handles feedback selection, caching the feedback selection is helpful at the API level - * for scaling into rotations and RPM. + * Although the Talon handles feedback selection, caching the feedback + * selection is helpful at the API level for scaling into rotations and RPM. */ FeedbackDevice m_feedbackDevice = QuadEncoder; static const unsigned int kDelayForSolicitedSignalsUs = 4000; /** * @param devToLookup FeedbackDevice to lookup the scalar for. Because Talon - * allows multiple sensors to be attached simultaneously, caller must - * specify which sensor to lookup. - * @return The number of native Talon units per rotation of the selected sensor. - * Zero if the necessary sensor information is not available. + * allows multiple sensors to be attached simultaneously, + * caller must specify which sensor to lookup. + * @return The number of native Talon units per rotation of the selected + * sensor. Zero if the necessary sensor information is not available. * @see ConfigEncoderCodesPerRev * @see ConfigPotentiometerTurns */ - double GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup)const; + double GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup) const; /** * Fixup the sendMode so Set() serializes the correct demand value. * Also fills the modeSelecet in the control frame to disabled. @@ -486,43 +518,58 @@ class CANTalon : public MotorSafety, */ void ApplyControlMode(CANSpeedController::ControlMode mode); /** - * @param fullRotations double precision value representing number of rotations of selected feedback sensor. - * If user has never called the config routine for the selected sensor, then the caller - * is likely passing rotations in engineering units already, in which case it is returned - * as is. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return fullRotations in native engineering units of the Talon SRX firmware. + * @param fullRotations double precision value representing number of + * rotations of selected feedback sensor. If user has + * never called the config routine for the selected + * sensor, then the caller is likely passing rotations + * in engineering units already, in which case it is + * returned as is. + * @see ConfigPotentiometerTurns + * @see ConfigEncoderCodesPerRev + * @return fullRotations in native engineering units of the Talon SRX + * firmware. */ - int32_t ScaleRotationsToNativeUnits(FeedbackDevice devToLookup, double fullRotations) const; + int32_t ScaleRotationsToNativeUnits(FeedbackDevice devToLookup, + double fullRotations) const; /** - * @param rpm double precision value representing number of rotations per minute of selected feedback sensor. - * If user has never called the config routine for the selected sensor, then the caller - * is likely passing rotations in engineering units already, in which case it is returned - * as is. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return sensor velocity in native engineering units of the Talon SRX firmware. + * @param rpm double precision value representing number of rotations per + * minute of selected feedback sensor. If user has never called + * the config routine for the selected sensor, then the caller is + * likely passing rotations in engineering units already, in which + * case it is returned as is. + * @see ConfigPotentiometerTurns + * @see ConfigEncoderCodesPerRev + * @return sensor velocity in native engineering units of the Talon SRX + * firmware. */ - int32_t ScaleVelocityToNativeUnits(FeedbackDevice devToLookup, double rpm) const; + int32_t ScaleVelocityToNativeUnits(FeedbackDevice devToLookup, + double rpm) const; /** - * @param nativePos integral position of the feedback sensor in native Talon SRX units. - * If user has never called the config routine for the selected sensor, then the return - * will be in TALON SRX units as well to match the behavior in the 2015 season. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return double precision number of rotations, unless config was never performed. + * @param nativePos integral position of the feedback sensor in native + * Talon SRX units. If user has never called the config + * routine for the selected sensor, then the return will be + * in TALON SRX units as well to match the behavior in the + * 2015 season. + * @see ConfigPotentiometerTurns + * @see ConfigEncoderCodesPerRev + * @return double precision number of rotations, unless config was never + * performed. */ - double ScaleNativeUnitsToRotations(FeedbackDevice devToLookup, int32_t nativePos) const; + double ScaleNativeUnitsToRotations(FeedbackDevice devToLookup, + int32_t nativePos) const; /** - * @param nativeVel integral velocity of the feedback sensor in native Talon SRX units. - * If user has never called the config routine for the selected sensor, then the return - * will be in TALON SRX units as well to match the behavior in the 2015 season. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return double precision of sensor velocity in RPM, unless config was never performed. + * @param nativeVel integral velocity of the feedback sensor in native + * Talon SRX units. If user has never called the config + * routine for the selected sensor, then the return will be + * in TALON SRX units as well to match the behavior in the + * 2015 season. + * @see ConfigPotentiometerTurns + * @see ConfigEncoderCodesPerRev + * @return double precision of sensor velocity in RPM, unless config was never + * performed. */ - double ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, int32_t nativeVel) const; + double ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, + int32_t nativeVel) const; // LiveWindow stuff. std::shared_ptr m_table; diff --git a/wpilibc/Athena/include/CameraServer.h b/wpilibc/Athena/include/CameraServer.h index bb0c314acf..c9a4869a71 100644 --- a/wpilibc/Athena/include/CameraServer.h +++ b/wpilibc/Athena/include/CameraServer.h @@ -7,17 +7,17 @@ #pragma once -#include "USBCamera.h" #include "ErrorBase.h" -#include "nivision.h" #include "NIIMAQdx.h" +#include "USBCamera.h" +#include "nivision.h" -#include "HAL/cpp/priority_mutex.h" -#include -#include #include +#include +#include #include #include +#include "HAL/cpp/priority_mutex.h" class CameraServer : public ErrorBase { private: diff --git a/wpilibc/Athena/include/Compressor.h b/wpilibc/Athena/include/Compressor.h index 0d56ef7213..0a8ba64c13 100644 --- a/wpilibc/Athena/include/Compressor.h +++ b/wpilibc/Athena/include/Compressor.h @@ -9,9 +9,9 @@ #define Compressor_H_ #include "HAL/HAL.hpp" +#include "LiveWindow/LiveWindowSendable.h" #include "SensorBase.h" #include "tables/ITableListener.h" -#include "LiveWindow/LiveWindowSendable.h" #include @@ -55,7 +55,7 @@ class Compressor : public SensorBase, std::shared_ptr value, bool isNew) override; protected: - void *m_pcm_pointer; + void* m_pcm_pointer; private: void SetCompressor(bool on); diff --git a/wpilibc/Athena/include/Counter.h b/wpilibc/Athena/include/Counter.h index 3cb55b5d19..a679aeea69 100644 --- a/wpilibc/Athena/include/Counter.h +++ b/wpilibc/Athena/include/Counter.h @@ -7,11 +7,11 @@ #pragma once -#include "HAL/HAL.hpp" #include "AnalogTriggerOutput.h" #include "CounterBase.h" -#include "SensorBase.h" +#include "HAL/HAL.hpp" #include "LiveWindow/LiveWindowSendable.h" +#include "SensorBase.h" #include @@ -20,10 +20,8 @@ class DigitalGlitchFilter; /** * Class for counting the number of ticks on a digital input channel. * This is a general purpose class for counting repetitive events. It can return - * the number - * of counts, the period of the most recent cycle, and detect when the signal - * being counted - * has stopped by supplying a maximum cycle time. + * the number of counts, the period of the most recent cycle, and detect when + * the signal being counted has stopped by supplying a maximum cycle time. * * All counters will immediately start counting - Reset() them if you need them * to be zeroed before use. @@ -34,35 +32,35 @@ class Counter : public SensorBase, public: explicit Counter(Mode mode = kTwoPulse); explicit Counter(int32_t channel); - explicit Counter(DigitalSource *source); + explicit Counter(DigitalSource* source); explicit Counter(std::shared_ptr source); DEPRECATED("Use pass-by-reference instead.") - explicit Counter(AnalogTrigger *trigger); - explicit Counter(const AnalogTrigger &trigger); - Counter(EncodingType encodingType, DigitalSource *upSource, - DigitalSource *downSource, bool inverted); + explicit Counter(AnalogTrigger* trigger); + explicit Counter(const AnalogTrigger& trigger); + Counter(EncodingType encodingType, DigitalSource* upSource, + DigitalSource* downSource, bool inverted); Counter(EncodingType encodingType, std::shared_ptr upSource, std::shared_ptr downSource, bool inverted); virtual ~Counter(); void SetUpSource(int32_t channel); - void SetUpSource(AnalogTrigger *analogTrigger, AnalogTriggerType triggerType); + void SetUpSource(AnalogTrigger* analogTrigger, AnalogTriggerType triggerType); void SetUpSource(std::shared_ptr analogTrigger, AnalogTriggerType triggerType); - void SetUpSource(DigitalSource *source); + void SetUpSource(DigitalSource* source); void SetUpSource(std::shared_ptr source); - void SetUpSource(DigitalSource &source); + void SetUpSource(DigitalSource& source); void SetUpSourceEdge(bool risingEdge, bool fallingEdge); void ClearUpSource(); void SetDownSource(int32_t channel); - void SetDownSource(AnalogTrigger *analogTrigger, + void SetDownSource(AnalogTrigger* analogTrigger, AnalogTriggerType triggerType); void SetDownSource(std::shared_ptr analogTrigger, AnalogTriggerType triggerType); - void SetDownSource(DigitalSource *source); + void SetDownSource(DigitalSource* source); void SetDownSource(std::shared_ptr source); - void SetDownSource(DigitalSource &source); + void SetDownSource(DigitalSource& source); void SetDownSourceEdge(bool risingEdge, bool fallingEdge); void ClearDownSource(); @@ -99,9 +97,9 @@ class Counter : public SensorBase, // Makes the counter count down. std::shared_ptr m_downSource; // The FPGA counter object - void *m_counter = nullptr; ///< The FPGA counter object. + void* m_counter = nullptr; ///< The FPGA counter object. private: - uint32_t m_index = 0; ///< The index of this counter. + uint32_t m_index = 0; ///< The index of this counter. std::shared_ptr m_table; friend class DigitalGlitchFilter; diff --git a/wpilibc/Athena/include/DigitalGlitchFilter.h b/wpilibc/Athena/include/DigitalGlitchFilter.h index 0f9e6769d9..a84a42785e 100644 --- a/wpilibc/Athena/include/DigitalGlitchFilter.h +++ b/wpilibc/Athena/include/DigitalGlitchFilter.h @@ -9,8 +9,8 @@ #include -#include "HAL/cpp/priority_mutex.h" #include "DigitalSource.h" +#include "HAL/cpp/priority_mutex.h" class Encoder; class Counter; @@ -26,13 +26,13 @@ class DigitalGlitchFilter : public SensorBase { DigitalGlitchFilter(); ~DigitalGlitchFilter(); - void Add(DigitalSource *input); - void Add(Encoder *input); - void Add(Counter *input); + void Add(DigitalSource* input); + void Add(Encoder* input); + void Add(Counter* input); - void Remove(DigitalSource *input); - void Remove(Encoder *input); - void Remove(Counter *input); + void Remove(DigitalSource* input); + void Remove(Encoder* input); + void Remove(Counter* input); void SetPeriodCycles(uint32_t fpga_cycles); void SetPeriodNanoSeconds(uint64_t nanoseconds); @@ -44,7 +44,7 @@ class DigitalGlitchFilter : public SensorBase { // Sets the filter for the input to be the requested index. A value of 0 // disables the filter, and the filter value must be between 1 and 3, // inclusive. - void DoAdd(DigitalSource *input, int requested_index); + void DoAdd(DigitalSource* input, int requested_index); int m_channelIndex = -1; static priority_mutex m_mutex; diff --git a/wpilibc/Athena/include/DigitalInput.h b/wpilibc/Athena/include/DigitalInput.h index 96a70a0862..81f7cbc5c8 100644 --- a/wpilibc/Athena/include/DigitalInput.h +++ b/wpilibc/Athena/include/DigitalInput.h @@ -10,20 +10,18 @@ #include "DigitalSource.h" #include "LiveWindow/LiveWindowSendable.h" -#include #include +#include class DigitalGlitchFilter; /** * Class to read a digital input. * This class will read digital inputs and return the current value on the - * channel. Other devices - * such as encoders, gear tooth sensors, etc. that are implemented elsewhere - * will automatically - * allocate digital inputs and outputs as required. This class is only for - * devices like switches - * etc. that aren't implemented anywhere else. + * channel. Other devices such as encoders, gear tooth sensors, etc. that are + * implemented elsewhere will automatically allocate digital inputs and outputs + * as required. This class is only for devices like switches etc. that aren't + * implemented anywhere else. */ class DigitalInput : public DigitalSource, public LiveWindowSendable { public: diff --git a/wpilibc/Athena/include/DigitalOutput.h b/wpilibc/Athena/include/DigitalOutput.h index e8bb5140d1..4b6d3493e4 100644 --- a/wpilibc/Athena/include/DigitalOutput.h +++ b/wpilibc/Athena/include/DigitalOutput.h @@ -16,8 +16,8 @@ /** * Class to write to digital outputs. * Write values to the digital output channels. Other devices implemented - * elsewhere will allocate - * channels automatically so for those devices it shouldn't be done here. + * elsewhere will allocate channels automatically so for those devices it + * shouldn't be done here. */ class DigitalOutput : public DigitalSource, public ITableListener, @@ -50,7 +50,7 @@ class DigitalOutput : public DigitalSource, private: uint32_t m_channel; - void *m_pwmGenerator; + void* m_pwmGenerator; std::shared_ptr m_table; }; diff --git a/wpilibc/Athena/include/DoubleSolenoid.h b/wpilibc/Athena/include/DoubleSolenoid.h index 9d1f033819..ddf44a7e45 100644 --- a/wpilibc/Athena/include/DoubleSolenoid.h +++ b/wpilibc/Athena/include/DoubleSolenoid.h @@ -7,8 +7,8 @@ #pragma once -#include "SolenoidBase.h" #include "LiveWindow/LiveWindowSendable.h" +#include "SolenoidBase.h" #include "tables/ITableListener.h" #include diff --git a/wpilibc/Athena/include/DriverStation.h b/wpilibc/Athena/include/DriverStation.h index b98c6ec089..0fc2b886fa 100644 --- a/wpilibc/Athena/include/DriverStation.h +++ b/wpilibc/Athena/include/DriverStation.h @@ -7,15 +7,15 @@ #pragma once -#include "SensorBase.h" -#include "RobotState.h" -#include "Task.h" +#include +#include #include "HAL/HAL.hpp" #include "HAL/cpp/Semaphore.hpp" -#include "HAL/cpp/priority_mutex.h" #include "HAL/cpp/priority_condition_variable.h" -#include -#include +#include "HAL/cpp/priority_mutex.h" +#include "RobotState.h" +#include "SensorBase.h" +#include "Task.h" struct HALControlWord; class AnalogInput; @@ -29,7 +29,7 @@ class DriverStation : public SensorBase, public RobotStateInterface { enum Alliance { kRed, kBlue, kInvalid }; virtual ~DriverStation(); - static DriverStation &GetInstance(); + static DriverStation& GetInstance(); static void ReportError(std::string error); static void ReportWarning(std::string error); static void ReportError(bool is_error, int32_t code, const std::string& error, @@ -70,26 +70,22 @@ class DriverStation : public SensorBase, public RobotStateInterface { float GetBatteryVoltage() const; /** Only to be used to tell the Driver Station what code you claim to be - * executing - * for diagnostic purposes only + * executing for diagnostic purposes only * @param entering If true, starting disabled code; if false, leaving disabled * code */ void InDisabled(bool entering) { m_userInDisabled = entering; } /** Only to be used to tell the Driver Station what code you claim to be - * executing - * for diagnostic purposes only + * executing for diagnostic purposes only * @param entering If true, starting autonomous code; if false, leaving * autonomous code */ void InAutonomous(bool entering) { m_userInAutonomous = entering; } /** Only to be used to tell the Driver Station what code you claim to be - * executing - * for diagnostic purposes only + * executing for diagnostic purposes only * @param entering If true, starting teleop code; if false, leaving teleop * code */ void InOperatorControl(bool entering) { m_userInTeleop = entering; } /** Only to be used to tell the Driver Station what code you claim to be - * executing - * for diagnostic purposes only + * executing for diagnostic purposes only * @param entering If true, starting test code; if false, leaving test code */ void InTest(bool entering) { m_userInTest = entering; } @@ -99,7 +95,7 @@ class DriverStation : public SensorBase, public RobotStateInterface { void GetData(); private: - static DriverStation *m_instance; + static DriverStation* m_instance; void ReportJoystickUnpluggedError(std::string message); void ReportJoystickUnpluggedWarning(std::string message); void Run(); diff --git a/wpilibc/Athena/include/Encoder.h b/wpilibc/Athena/include/Encoder.h index 5c692f0e48..9312d8b812 100644 --- a/wpilibc/Athena/include/Encoder.h +++ b/wpilibc/Athena/include/Encoder.h @@ -7,12 +7,12 @@ #pragma once -#include "HAL/HAL.hpp" -#include "CounterBase.h" -#include "SensorBase.h" #include "Counter.h" -#include "PIDSource.h" +#include "CounterBase.h" +#include "HAL/HAL.hpp" #include "LiveWindow/LiveWindowSendable.h" +#include "PIDSource.h" +#include "SensorBase.h" #include @@ -22,17 +22,13 @@ class DigitalGlitchFilter; /** * Class to read quad encoders. * Quadrature encoders are devices that count shaft rotation and can sense - * direction. The output of - * the QuadEncoder class is an integer that can count either up or down, and can - * go negative for - * reverse direction counting. When creating QuadEncoders, a direction is - * supplied that changes the - * sense of the output to make code more readable if the encoder is mounted such - * that forward movement - * generates negative values. Quadrature encoders have two digital outputs, an A - * Channel and a B Channel - * that are out of phase with each other to allow the FPGA to do direction - * sensing. + * direction. The output of the QuadEncoder class is an integer that can count + * either up or down, and can go negative for reverse direction counting. When + * creating QuadEncoders, a direction is supplied that changes the sense of the + * output to make code more readable if the encoder is mounted such that forward + * movement generates negative values. Quadrature encoders have two digital + * outputs, an A Channel and a B Channel that are out of phase with each other + * to allow the FPGA to do direction sensing. * * All encoders will immediately start counting - Reset() them if you need them * to be zeroed before use. @@ -52,11 +48,11 @@ class Encoder : public SensorBase, Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection = false, EncodingType encodingType = k4X); Encoder(std::shared_ptr aSource, - std::shared_ptr bSource, + std::shared_ptr bSource, bool reverseDirection = false, + EncodingType encodingType = k4X); + Encoder(DigitalSource* aSource, DigitalSource* bSource, bool reverseDirection = false, EncodingType encodingType = k4X); - Encoder(DigitalSource *aSource, DigitalSource *bSource, - bool reverseDirection = false, EncodingType encodingType = k4X); - Encoder(DigitalSource &aSource, DigitalSource &bSource, + Encoder(DigitalSource& aSource, DigitalSource& bSource, bool reverseDirection = false, EncodingType encodingType = k4X); virtual ~Encoder(); @@ -81,9 +77,9 @@ class Encoder : public SensorBase, void SetIndexSource(uint32_t channel, IndexingType type = kResetOnRisingEdge); DEPRECATED("Use pass-by-reference instead.") - void SetIndexSource(DigitalSource *source, + void SetIndexSource(DigitalSource* source, IndexingType type = kResetOnRisingEdge); - void SetIndexSource(const DigitalSource &source, + void SetIndexSource(const DigitalSource& source, IndexingType type = kResetOnRisingEdge); void UpdateTable() override; @@ -99,15 +95,15 @@ class Encoder : public SensorBase, void InitEncoder(bool _reverseDirection, EncodingType encodingType); double DecodingScaleFactor() const; - std::shared_ptr m_aSource; // the A phase of the quad encoder - std::shared_ptr m_bSource; // the B phase of the quad encoder - void *m_encoder = nullptr; - int32_t m_index = 0; // The encoder's FPGA index. - double m_distancePerPulse = 1.0; // distance of travel for each encoder tick + std::shared_ptr m_aSource; // the A phase of the quad encoder + std::shared_ptr m_bSource; // the B phase of the quad encoder + void* m_encoder = nullptr; + int32_t m_index = 0; // The encoder's FPGA index. + double m_distancePerPulse = 1.0; // distance of travel for each encoder tick std::unique_ptr m_counter = - nullptr; // Counter object for 1x and 2x encoding - EncodingType m_encodingType; // Encoding type - int32_t m_encodingScale; // 1x, 2x, or 4x, per the encodingType + nullptr; // Counter object for 1x and 2x encoding + EncodingType m_encodingType; // Encoding type + int32_t m_encodingScale; // 1x, 2x, or 4x, per the encodingType std::shared_ptr m_table; friend class DigitalGlitchFilter; diff --git a/wpilibc/Athena/include/GearTooth.h b/wpilibc/Athena/include/GearTooth.h index 070f3972d1..e70da710fd 100644 --- a/wpilibc/Athena/include/GearTooth.h +++ b/wpilibc/Athena/include/GearTooth.h @@ -7,23 +7,21 @@ #pragma once -#include "Counter.h" #include +#include "Counter.h" /** * Alias for counter class. * Implement the gear tooth sensor supplied by FIRST. Currently there is no - * reverse sensing on - * the gear tooth sensor, but in future versions we might implement the - * necessary timing in the - * FPGA to sense direction. + * reverse sensing on the gear tooth sensor, but in future versions we might + * implement the necessary timing in the FPGA to sense direction. */ class GearTooth : public Counter { public: /// 55 uSec for threshold static constexpr double kGearToothThreshold = 55e-6; GearTooth(uint32_t channel, bool directionSensitive = false); - GearTooth(DigitalSource *source, bool directionSensitive = false); + GearTooth(DigitalSource* source, bool directionSensitive = false); GearTooth(std::shared_ptr source, bool directionSensitive = false); virtual ~GearTooth() = default; diff --git a/wpilibc/Athena/include/I2C.h b/wpilibc/Athena/include/I2C.h index 3267032de6..8542db307d 100644 --- a/wpilibc/Athena/include/I2C.h +++ b/wpilibc/Athena/include/I2C.h @@ -26,16 +26,16 @@ class I2C : SensorBase { I2C(const I2C&) = delete; I2C& operator=(const I2C&) = delete; - bool Transaction(uint8_t *dataToSend, uint8_t sendSize, uint8_t *dataReceived, + bool Transaction(uint8_t* dataToSend, uint8_t sendSize, uint8_t* dataReceived, uint8_t receiveSize); bool AddressOnly(); bool Write(uint8_t registerAddress, uint8_t data); - bool WriteBulk(uint8_t *data, uint8_t count); - bool Read(uint8_t registerAddress, uint8_t count, uint8_t *data); - bool ReadOnly(uint8_t count, uint8_t *buffer); + bool WriteBulk(uint8_t* data, uint8_t count); + bool Read(uint8_t registerAddress, uint8_t count, uint8_t* data); + bool ReadOnly(uint8_t count, uint8_t* buffer); void Broadcast(uint8_t registerAddress, uint8_t data); bool VerifySensor(uint8_t registerAddress, uint8_t count, - const uint8_t *expected); + const uint8_t* expected); private: Port m_port; diff --git a/wpilibc/Athena/include/InterruptableSensorBase.h b/wpilibc/Athena/include/InterruptableSensorBase.h index 708089be72..45d04dc450 100644 --- a/wpilibc/Athena/include/InterruptableSensorBase.h +++ b/wpilibc/Athena/include/InterruptableSensorBase.h @@ -8,8 +8,8 @@ #pragma once #include "HAL/HAL.hpp" -#include "SensorBase.h" #include "Resource.h" +#include "SensorBase.h" #include @@ -29,23 +29,24 @@ class InterruptableSensorBase : public SensorBase { virtual bool GetAnalogTriggerForRouting() const = 0; virtual void RequestInterrupts( InterruptHandlerFunction handler, - void *param); ///< Asynchronus handler version. + void* param); ///< Asynchronus handler version. virtual void RequestInterrupts(); ///< Synchronus Wait version. virtual void CancelInterrupts(); ///< Free up the underlying chipobject functions. virtual WaitResult WaitForInterrupt( - float timeout, bool ignorePrevious = true); ///< Synchronus version. + float timeout, + bool ignorePrevious = true); ///< Synchronus version. virtual void EnableInterrupts(); ///< Enable interrupts - after finishing setup. virtual void DisableInterrupts(); ///< Disable, but don't deallocate. virtual double ReadRisingTimestamp(); ///< Return the timestamp for the - ///rising interrupt that occurred. + /// rising interrupt that occurred. virtual double ReadFallingTimestamp(); ///< Return the timestamp for the - ///falling interrupt that occurred. + /// falling interrupt that occurred. virtual void SetUpSourceEdge(bool risingEdge, bool fallingEdge); protected: - void *m_interrupt = nullptr; + void* m_interrupt = nullptr; uint32_t m_interruptIndex = std::numeric_limits::max(); void AllocateInterrupts(bool watcher); diff --git a/wpilibc/Athena/include/IterativeRobot.h b/wpilibc/Athena/include/IterativeRobot.h index acba2350cf..634e258ad9 100644 --- a/wpilibc/Athena/include/IterativeRobot.h +++ b/wpilibc/Athena/include/IterativeRobot.h @@ -7,8 +7,8 @@ #pragma once -#include "Timer.h" #include "RobotBase.h" +#include "Timer.h" /** * IterativeRobot implements a specific type of Robot Program framework, @@ -18,8 +18,7 @@ * robot program. * * This class is intended to implement the "old style" default code, by - * providing - * the following functions which are called by the main loop, + * providing the following functions which are called by the main loop, * StartCompetition(), at the appropriate times: * * RobotInit() -- provide for initialization at robot power-on @@ -28,19 +27,17 @@ * appropriate mode is entered: * - DisabledInit() -- called only when first disabled * - AutonomousInit() -- called each and every time autonomous is entered from - * another mode + * another mode * - TeleopInit() -- called each and every time teleop is entered from - * another mode + * another mode * - TestInit() -- called each and every time test is entered from - * another mode + * another mode * * Periodic() functions -- each of these functions is called iteratively at the * appropriate periodic rate (aka the "slow loop"). The - * default period of - * the iterative robot is synced to the driver station - * control packets, - * giving a periodic frequency of about 50Hz (50 times - * per second). + * default period of the iterative robot is synced to + * the driver station control packets, giving a periodic + * frequency of about 50Hz (50 times per second). * - DisabledPeriodic() * - AutonomousPeriodic() * - TeleopPeriodic() diff --git a/wpilibc/Athena/include/Joystick.h b/wpilibc/Athena/include/Joystick.h index fdd4d7d56b..634f099ac1 100644 --- a/wpilibc/Athena/include/Joystick.h +++ b/wpilibc/Athena/include/Joystick.h @@ -11,18 +11,17 @@ #include #include #include -#include "GenericHID.h" #include "ErrorBase.h" +#include "GenericHID.h" class DriverStation; /** * Handle input from standard Joysticks connected to the Driver Station. * This class handles standard input that comes from the Driver Station. Each - * time a value is requested - * the most recent value is returned. There is a single class instance for each - * joystick and the mapping - * of ports to hardware buttons depends on the code in the driver station. + * time a value is requested the most recent value is returned. There is a + * single class instance for each joystick and the mapping of ports to hardware + * buttons depends on the code in the driver station. */ class Joystick : public GenericHID, public ErrorBase { public: @@ -86,7 +85,7 @@ class Joystick : public GenericHID, public ErrorBase { virtual bool GetRawButton(uint32_t button) const override; virtual int GetPOV(uint32_t pov = 0) const override; bool GetButton(ButtonType button) const; - static Joystick *GetStickForPort(uint32_t port); + static Joystick* GetStickForPort(uint32_t port); virtual float GetMagnitude() const; virtual float GetDirectionRadians() const; @@ -106,7 +105,7 @@ class Joystick : public GenericHID, public ErrorBase { void SetOutputs(uint32_t value); private: - DriverStation &m_ds; + DriverStation& m_ds; uint32_t m_port; std::vector m_axes; std::vector m_buttons; diff --git a/wpilibc/Athena/include/MotorSafetyHelper.h b/wpilibc/Athena/include/MotorSafetyHelper.h index f124605df4..161c072dd4 100644 --- a/wpilibc/Athena/include/MotorSafetyHelper.h +++ b/wpilibc/Athena/include/MotorSafetyHelper.h @@ -16,7 +16,7 @@ class MotorSafety; class MotorSafetyHelper : public ErrorBase { public: - MotorSafetyHelper(MotorSafety *safeObject); + MotorSafetyHelper(MotorSafety* safeObject); ~MotorSafetyHelper(); void Feed(); void SetExpiration(float expirationTime); @@ -28,14 +28,18 @@ class MotorSafetyHelper : public ErrorBase { static void CheckMotors(); private: - double m_expiration; // the expiration time for this object - bool m_enabled; // true if motor safety is enabled for this motor - double m_stopTime; // the FPGA clock value when this motor has expired - mutable priority_recursive_mutex - m_syncMutex; // protect accesses to the state for this object - MotorSafety *m_safeObject; // the object that is using the helper + // the expiration time for this object + double m_expiration; + // true if motor safety is enabled for this motor + bool m_enabled; + // the FPGA clock value when this motor has expired + double m_stopTime; + // protect accesses to the state for this object + mutable priority_recursive_mutex m_syncMutex; + // the object that is using the helper + MotorSafety* m_safeObject; // List of all existing MotorSafetyHelper objects. static std::set m_helperList; - static priority_recursive_mutex - m_listMutex; // protect accesses to the list of helpers + // protect accesses to the list of helpers + static priority_recursive_mutex m_listMutex; }; diff --git a/wpilibc/Athena/include/Notifier.h b/wpilibc/Athena/include/Notifier.h index 070853f3ac..d4f6ac94c2 100644 --- a/wpilibc/Athena/include/Notifier.h +++ b/wpilibc/Athena/include/Notifier.h @@ -20,8 +20,7 @@ class Notifier : public ErrorBase { template Notifier(Callable&& f, Arg&& arg, Args&&... args) - : Notifier(std::bind(std::forward(f), - std::forward(arg), + : Notifier(std::bind(std::forward(f), std::forward(arg), std::forward(args)...)) {} virtual ~Notifier(); @@ -37,12 +36,12 @@ class Notifier : public ErrorBase { // update the HAL alarm void UpdateAlarm(); // HAL callback - static void Notify(uint64_t currentTimeInt, void *param); + static void Notify(uint64_t currentTimeInt, void* param); // held while updating process information priority_mutex m_processMutex; // HAL handle - void *m_notifier; + void* m_notifier; // address of the handler TimerEventHandler m_handler; // the absolute expiration time diff --git a/wpilibc/Athena/include/PWM.h b/wpilibc/Athena/include/PWM.h index 8f9976adc4..a803dbcdfc 100644 --- a/wpilibc/Athena/include/PWM.h +++ b/wpilibc/Athena/include/PWM.h @@ -7,8 +7,8 @@ #pragma once -#include "SensorBase.h" #include "LiveWindow/LiveWindowSendable.h" +#include "SensorBase.h" #include "tables/ITableListener.h" #include @@ -17,10 +17,9 @@ * Class implements the PWM generation in the FPGA. * * The values supplied as arguments for PWM outputs range from -1.0 to 1.0. They - * are mapped - * to the hardware dependent values, in this case 0-2000 for the FPGA. - * Changes are immediately sent to the FPGA, and the update occurs at the next - * FPGA cycle. There is no delay. + * are mapped to the hardware dependent values, in this case 0-2000 for the + * FPGA. Changes are immediately sent to the FPGA, and the update occurs at the + * next FPGA cycle. There is no delay. * * As of revision 0.1.10 of the FPGA, the FPGA interprets the 0-2000 values as * follows: @@ -59,23 +58,19 @@ class PWM : public SensorBase, * kDefaultPwmPeriod is in ms * * - 20ms periods (50 Hz) are the "safest" setting in that this works for all - * devices + * devices * - 20ms periods seem to be desirable for Vex Motors * - 20ms periods are the specified period for HS-322HD servos, but work - * reliably down - * to 10.0 ms; starting at about 8.5ms, the servo sometimes hums and get - *hot; - * by 5.0ms the hum is nearly continuous + * reliably down to 10.0 ms; starting at about 8.5ms, the servo sometimes + * hums and get hot; by 5.0ms the hum is nearly continuous * - 10ms periods work well for Victor 884 * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed - * controllers. - * Due to the shipping firmware on the Jaguar, we can't run the update - * period less - * than 5.05 ms. + * controllers. Due to the shipping firmware on the Jaguar, we can't run the + * update period less than 5.05 ms. * * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period - * scaling is implemented as an - * output squelch to get longer periods for old devices. + * scaling is implemented as an output squelch to get longer periods for old + * devices. */ static constexpr float kDefaultPwmPeriod = 5.05; /** diff --git a/wpilibc/Athena/include/PWMSpeedController.h b/wpilibc/Athena/include/PWMSpeedController.h index 5b10972804..363f72f4ff 100644 --- a/wpilibc/Athena/include/PWMSpeedController.h +++ b/wpilibc/Athena/include/PWMSpeedController.h @@ -25,8 +25,10 @@ class PWMSpeedController : public SafePWM, public SpeedController { virtual void SetInverted(bool isInverted) override; virtual bool GetInverted() const override; + protected: explicit PWMSpeedController(uint32_t channel); + private: bool m_isInverted = false; }; \ No newline at end of file diff --git a/wpilibc/Athena/include/PowerDistributionPanel.h b/wpilibc/Athena/include/PowerDistributionPanel.h index b9769ecbe9..c9295856f1 100644 --- a/wpilibc/Athena/include/PowerDistributionPanel.h +++ b/wpilibc/Athena/include/PowerDistributionPanel.h @@ -9,8 +9,8 @@ #ifndef __WPILIB_POWER_DISTRIBUTION_PANEL_H__ #define __WPILIB_POWER_DISTRIBUTION_PANEL_H__ -#include "SensorBase.h" #include "LiveWindow/LiveWindowSendable.h" +#include "SensorBase.h" #include diff --git a/wpilibc/Athena/include/Preferences.h b/wpilibc/Athena/include/Preferences.h index 1e1beff13b..ba5001adb4 100644 --- a/wpilibc/Athena/include/Preferences.h +++ b/wpilibc/Athena/include/Preferences.h @@ -7,25 +7,23 @@ #pragma once -#include "ErrorBase.h" -#include "Task.h" #include #include #include +#include "ErrorBase.h" #include "HAL/cpp/Semaphore.hpp" -#include "tables/ITableListener.h" +#include "Task.h" #include "networktables/NetworkTable.h" +#include "tables/ITableListener.h" /** * The preferences class provides a relatively simple way to save important - * values to - * the RoboRIO to access the next time the RoboRIO is booted. + * values to the RoboRIO to access the next time the RoboRIO is booted. * - *

This class loads and saves from a file - * inside the RoboRIO. The user can not access the file directly, but may - * modify values at specific - * fields which will then be automatically periodically saved to the file - * by the NetworkTable server.

+ *

This class loads and saves from a file inside the RoboRIO. The user can + * not access the file directly, but may modify values at specific fields which + * will then be automatically periodically saved to the file by the NetworkTable + * server.

* *

This class is thread safe.

* @@ -34,7 +32,7 @@ */ class Preferences : public ErrorBase { public: - static Preferences *GetInstance(); + static Preferences* GetInstance(); std::vector GetKeys(); std::string GetString(llvm::StringRef key, llvm::StringRef defaultValue = ""); diff --git a/wpilibc/Athena/include/Relay.h b/wpilibc/Athena/include/Relay.h index a9bd26a375..ba1ecacff4 100644 --- a/wpilibc/Athena/include/Relay.h +++ b/wpilibc/Athena/include/Relay.h @@ -7,11 +7,11 @@ #pragma once +#include "LiveWindow/LiveWindowSendable.h" #include "MotorSafety.h" #include "SensorBase.h" -#include "tables/ITableListener.h" -#include "LiveWindow/LiveWindowSendable.h" #include "tables/ITable.h" +#include "tables/ITableListener.h" #include @@ -20,16 +20,13 @@ class MotorSafetyHelper; /** * Class for Spike style relay outputs. * Relays are intended to be connected to spikes or similar relays. The relay - * channels controls - * a pair of pins that are either both off, one on, the other on, or both on. - * This translates into - * two spike outputs at 0v, one at 12v and one at 0v, one at 0v and the other at - * 12v, or two - * spike outputs at 12V. This allows off, full forward, or full reverse control - * of motors without - * variable speed. It also allows the two channels (forward and reverse) to be - * used independently - * for something that does not care about voltage polatiry (like a solenoid). + * channels controls a pair of pins that are either both off, one on, the other + * on, or both on. This translates into two spike outputs at 0v, one at 12v and + * one at 0v, one at 0v and the other at 12v, or two spike outputs at 12V. This + * allows off, full forward, or full reverse control of motors without variable + * speed. It also allows the two channels (forward and reverse) to be used + * independently for something that does not care about voltage polarity (like + * a solenoid). */ class Relay : public MotorSafety, public SensorBase, diff --git a/wpilibc/Athena/include/RobotBase.h b/wpilibc/Athena/include/RobotBase.h index b6626b96f0..724dae135d 100644 --- a/wpilibc/Athena/include/RobotBase.h +++ b/wpilibc/Athena/include/RobotBase.h @@ -20,7 +20,7 @@ class DriverStation; } \ HALReport(HALUsageReporting::kResourceType_Language, \ HALUsageReporting::kLanguage_CPlusPlus); \ - _ClassName_ *robot = new _ClassName_(); \ + _ClassName_* robot = new _ClassName_(); \ RobotBase::robotSetup(robot); \ return 0; \ } @@ -28,21 +28,18 @@ class DriverStation; /** * Implement a Robot Program framework. * The RobotBase class is intended to be subclassed by a user creating a robot - * program. - * Overridden Autonomous() and OperatorControl() methods are called at the - * appropriate time - * as the match proceeds. In the current implementation, the Autonomous code - * will run to - * completion before the OperatorControl code could start. In the future the - * Autonomous code - * might be spawned as a task, then killed at the end of the Autonomous period. + * program. Overridden Autonomous() and OperatorControl() methods are called at + * the appropriate time as the match proceeds. In the current implementation, + * the Autonomous code will run to completion before the OperatorControl code + * could start. In the future the Autonomous code might be spawned as a task, + * then killed at the end of the Autonomous period. */ class RobotBase { friend class RobotDeleter; public: - static RobotBase &getInstance(); - static void setInstance(RobotBase *robot); + static RobotBase& getInstance(); + static void setInstance(RobotBase* robot); bool IsEnabled() const; bool IsDisabled() const; @@ -51,10 +48,10 @@ class RobotBase { bool IsTest() const; bool IsNewDataAvailable() const; static void startRobotTask(FUNCPTR factory); - static void robotTask(FUNCPTR factory, Task *task); + static void robotTask(FUNCPTR factory, Task* task); virtual void StartCompetition() = 0; - static void robotSetup(RobotBase *robot); + static void robotSetup(RobotBase* robot); protected: RobotBase(); @@ -63,9 +60,9 @@ class RobotBase { RobotBase(const RobotBase&) = delete; RobotBase& operator=(const RobotBase&) = delete; - Task *m_task = nullptr; - DriverStation &m_ds; + Task* m_task = nullptr; + DriverStation& m_ds; private: - static RobotBase *m_instance; + static RobotBase* m_instance; }; diff --git a/wpilibc/Athena/include/RobotDrive.h b/wpilibc/Athena/include/RobotDrive.h index 547d06a3b1..29810bfd5e 100644 --- a/wpilibc/Athena/include/RobotDrive.h +++ b/wpilibc/Athena/include/RobotDrive.h @@ -7,10 +7,10 @@ #pragma once -#include "ErrorBase.h" #include #include #include +#include "ErrorBase.h" #include "HAL/HAL.hpp" #include "MotorSafety.h" #include "MotorSafetyHelper.h" @@ -22,16 +22,11 @@ class GenericHID; * Utility class for handling Robot drive based on a definition of the motor * configuration. * The robot drive class handles basic driving for a robot. Currently, 2 and 4 - * motor tank and - * mecanum drive trains are supported. In the future other drive types like - * swerve might be - * implemented. Motor channel numbers are passed supplied on creation of the - * class. Those - * are used for either the Drive function (intended for hand created drive code, - * such as - * autonomous) or with the Tank/Arcade functions intended to be used for - * Operator Control - * driving. + * motor tank and mecanum drive trains are supported. In the future other drive + * types like swerve might be implemented. Motor channel numbers are passed + * supplied on creation of the class. Those are used for either the Drive + * function (intended for hand created drive code, such as autonomous) or with + * the Tank/Arcade functions intended to be used for Operator Control driving. */ class RobotDrive : public MotorSafety, public ErrorBase { public: @@ -45,14 +40,14 @@ class RobotDrive : public MotorSafety, public ErrorBase { RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel); RobotDrive(uint32_t frontLeftMotorChannel, uint32_t rearLeftMotorChannel, uint32_t frontRightMotorChannel, uint32_t rearRightMotorChannel); - RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor); - RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor); + RobotDrive(SpeedController* leftMotor, SpeedController* rightMotor); + RobotDrive(SpeedController& leftMotor, SpeedController& rightMotor); RobotDrive(std::shared_ptr leftMotor, std::shared_ptr rightMotor); - RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor, - SpeedController *frontRightMotor, SpeedController *rearRightMotor); - RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor, - SpeedController &frontRightMotor, SpeedController &rearRightMotor); + RobotDrive(SpeedController* frontLeftMotor, SpeedController* rearLeftMotor, + SpeedController* frontRightMotor, SpeedController* rearRightMotor); + RobotDrive(SpeedController& frontLeftMotor, SpeedController& rearLeftMotor, + SpeedController& frontRightMotor, SpeedController& rearRightMotor); RobotDrive(std::shared_ptr frontLeftMotor, std::shared_ptr rearLeftMotor, std::shared_ptr frontRightMotor, @@ -63,24 +58,24 @@ class RobotDrive : public MotorSafety, public ErrorBase { RobotDrive& operator=(const RobotDrive&) = delete; void Drive(float outputMagnitude, float curve); - void TankDrive(GenericHID *leftStick, GenericHID *rightStick, + void TankDrive(GenericHID* leftStick, GenericHID* rightStick, bool squaredInputs = true); - void TankDrive(GenericHID &leftStick, GenericHID &rightStick, + void TankDrive(GenericHID& leftStick, GenericHID& rightStick, bool squaredInputs = true); - void TankDrive(GenericHID *leftStick, uint32_t leftAxis, - GenericHID *rightStick, uint32_t rightAxis, + void TankDrive(GenericHID* leftStick, uint32_t leftAxis, + GenericHID* rightStick, uint32_t rightAxis, bool squaredInputs = true); - void TankDrive(GenericHID &leftStick, uint32_t leftAxis, - GenericHID &rightStick, uint32_t rightAxis, + void TankDrive(GenericHID& leftStick, uint32_t leftAxis, + GenericHID& rightStick, uint32_t rightAxis, bool squaredInputs = true); void TankDrive(float leftValue, float rightValue, bool squaredInputs = true); - void ArcadeDrive(GenericHID *stick, bool squaredInputs = true); - void ArcadeDrive(GenericHID &stick, bool squaredInputs = true); - void ArcadeDrive(GenericHID *moveStick, uint32_t moveChannel, - GenericHID *rotateStick, uint32_t rotateChannel, + void ArcadeDrive(GenericHID* stick, bool squaredInputs = true); + void ArcadeDrive(GenericHID& stick, bool squaredInputs = true); + void ArcadeDrive(GenericHID* moveStick, uint32_t moveChannel, + GenericHID* rotateStick, uint32_t rotateChannel, bool squaredInputs = true); - void ArcadeDrive(GenericHID &moveStick, uint32_t moveChannel, - GenericHID &rotateStick, uint32_t rotateChannel, + void ArcadeDrive(GenericHID& moveStick, uint32_t moveChannel, + GenericHID& rotateStick, uint32_t rotateChannel, bool squaredInputs = true); void ArcadeDrive(float moveValue, float rotateValue, bool squaredInputs = true); @@ -105,8 +100,8 @@ class RobotDrive : public MotorSafety, public ErrorBase { protected: void InitRobotDrive(); float Limit(float num); - void Normalize(double *wheelSpeeds); - void RotateVector(double &x, double &y, double angle); + void Normalize(double* wheelSpeeds); + void RotateVector(double& x, double& y, double angle); static const int32_t kMaxNumberOfMotors = 4; float m_sensitivity = 0.5; diff --git a/wpilibc/Athena/include/SPI.h b/wpilibc/Athena/include/SPI.h index b6d68f3859..b7ea949606 100644 --- a/wpilibc/Athena/include/SPI.h +++ b/wpilibc/Athena/include/SPI.h @@ -60,13 +60,13 @@ class SPI : public SensorBase { int64_t GetAccumulatorValue() const; uint32_t GetAccumulatorCount() const; double GetAccumulatorAverage() const; - void GetAccumulatorOutput(int64_t &value, uint32_t &count) const; + void GetAccumulatorOutput(int64_t& value, uint32_t& count) const; protected: uint8_t m_port; - bool m_msbFirst = false; // default little-endian - bool m_sampleOnTrailing = false; // default data updated on falling edge - bool m_clk_idle_high = false; // default clock active high + bool m_msbFirst = false; // default little-endian + bool m_sampleOnTrailing = false; // default data updated on falling edge + bool m_clk_idle_high = false; // default clock active high private: void Init(); diff --git a/wpilibc/Athena/include/SafePWM.h b/wpilibc/Athena/include/SafePWM.h index 3c908c7e17..5ee4ad0bf0 100644 --- a/wpilibc/Athena/include/SafePWM.h +++ b/wpilibc/Athena/include/SafePWM.h @@ -7,20 +7,18 @@ #pragma once -#include "MotorSafety.h" -#include "PWM.h" -#include "MotorSafetyHelper.h" #include #include +#include "MotorSafety.h" +#include "MotorSafetyHelper.h" +#include "PWM.h" /** * A safe version of the PWM class. * It is safe because it implements the MotorSafety interface that provides - * timeouts - * in the event that the motor value is not updated before the expiration time. - * This delegates the actual work to a MotorSafetyHelper object that is used for - * all - * objects that implement MotorSafety. + * timeouts in the event that the motor value is not updated before the + * expiration time. This delegates the actual work to a MotorSafetyHelper + * object that is used for all objects that implement MotorSafety. */ class SafePWM : public PWM, public MotorSafety { public: diff --git a/wpilibc/Athena/include/SerialPort.h b/wpilibc/Athena/include/SerialPort.h index e94af8748d..ea7d676473 100644 --- a/wpilibc/Athena/include/SerialPort.h +++ b/wpilibc/Athena/include/SerialPort.h @@ -14,9 +14,8 @@ * Driver for the RS-232 serial port on the RoboRIO. * * The current implementation uses the VISA formatted I/O mode. This means that - * all traffic goes through the fomatted buffers. This allows the - * intermingled - * use of Printf(), Scanf(), and the raw buffer accessors Read() and Write(). + * all traffic goes through the fomatted buffers. This allows the intermingled + * use of Printf(), Scanf(), and the raw buffer accessors Read() and Write(). * * More information can be found in the NI-VISA User Manual here: * http://www.ni.com/pdf/manuals/370423a.pdf @@ -57,8 +56,8 @@ class SerialPort : public ErrorBase { void EnableTermination(char terminator = '\n'); void DisableTermination(); int32_t GetBytesReceived(); - uint32_t Read(char *buffer, int32_t count); - uint32_t Write(const std::string &buffer, int32_t count); + uint32_t Read(char* buffer, int32_t count); + uint32_t Write(const std::string& buffer, int32_t count); void SetTimeout(float timeout); void SetReadBufferSize(uint32_t size); void SetWriteBufferSize(uint32_t size); diff --git a/wpilibc/Athena/include/Solenoid.h b/wpilibc/Athena/include/Solenoid.h index d032930c79..82e2ecb89e 100644 --- a/wpilibc/Athena/include/Solenoid.h +++ b/wpilibc/Athena/include/Solenoid.h @@ -7,8 +7,8 @@ #pragma once -#include "SolenoidBase.h" #include "LiveWindow/LiveWindowSendable.h" +#include "SolenoidBase.h" #include "tables/ITableListener.h" #include @@ -17,8 +17,7 @@ * Solenoid class for running high voltage Digital Output (PCM). * * The Solenoid class is typically used for pneumatics solenoids, but could be - * used - * for any device within the current spec of the PCM. + * used for any device within the current spec of the PCM. */ class Solenoid : public SolenoidBase, public LiveWindowSendable, diff --git a/wpilibc/Athena/include/SolenoidBase.h b/wpilibc/Athena/include/SolenoidBase.h index 0a71de2c45..4116f93569 100644 --- a/wpilibc/Athena/include/SolenoidBase.h +++ b/wpilibc/Athena/include/SolenoidBase.h @@ -7,10 +7,10 @@ #pragma once -#include "Resource.h" -#include "SensorBase.h" #include "HAL/HAL.hpp" #include "HAL/Port.h" +#include "Resource.h" +#include "SensorBase.h" #include @@ -35,6 +35,6 @@ class SolenoidBase : public SensorBase { const static int m_maxPorts = 8; static void* m_ports[m_maxModules][m_maxPorts]; uint32_t m_moduleNumber; ///< Slot number where the module is plugged into - ///the chassis. + /// the chassis. static std::unique_ptr m_allocated; }; diff --git a/wpilibc/Athena/include/USBCamera.h b/wpilibc/Athena/include/USBCamera.h index 522e889449..0024a13e10 100644 --- a/wpilibc/Athena/include/USBCamera.h +++ b/wpilibc/Athena/include/USBCamera.h @@ -8,9 +8,9 @@ #pragma once #include "ErrorBase.h" -#include "nivision.h" -#include "NIIMAQdx.h" #include "HAL/cpp/priority_mutex.h" +#include "NIIMAQdx.h" +#include "nivision.h" #include @@ -24,17 +24,17 @@ typedef enum whiteBalance_enum { class USBCamera : public ErrorBase { private: - static constexpr char const *ATTR_WB_MODE = + static constexpr char const* ATTR_WB_MODE = "CameraAttributes::WhiteBalance::Mode"; - static constexpr char const *ATTR_WB_VALUE = + static constexpr char const* ATTR_WB_VALUE = "CameraAttributes::WhiteBalance::Value"; - static constexpr char const *ATTR_EX_MODE = + static constexpr char const* ATTR_EX_MODE = "CameraAttributes::Exposure::Mode"; - static constexpr char const *ATTR_EX_VALUE = + static constexpr char const* ATTR_EX_VALUE = "CameraAttributes::Exposure::Value"; - static constexpr char const *ATTR_BR_MODE = + static constexpr char const* ATTR_BR_MODE = "CameraAttributes::Brightness::Mode"; - static constexpr char const *ATTR_BR_VALUE = + static constexpr char const* ATTR_BR_VALUE = "CameraAttributes::Brightness::Value"; // Constants for the manual and auto types @@ -62,10 +62,10 @@ class USBCamera : public ErrorBase { unsigned int m_brightness = 80; bool m_needSettingsUpdate = true; - unsigned int GetJpegSize(void *buffer, unsigned int buffSize); + unsigned int GetJpegSize(void* buffer, unsigned int buffSize); public: - static constexpr char const *kDefaultCameraName = "cam0"; + static constexpr char const* kDefaultCameraName = "cam0"; USBCamera(std::string name, bool useJpeg); @@ -117,6 +117,6 @@ class USBCamera : public ErrorBase { */ void SetExposureManual(unsigned int expValue); - void GetImage(Image *image); - unsigned int GetImageData(void *buffer, unsigned int bufferSize); + void GetImage(Image* image); + unsigned int GetImageData(void* buffer, unsigned int bufferSize); }; diff --git a/wpilibc/Athena/include/Ultrasonic.h b/wpilibc/Athena/include/Ultrasonic.h index d586173d32..08c6d3456a 100644 --- a/wpilibc/Athena/include/Ultrasonic.h +++ b/wpilibc/Athena/include/Ultrasonic.h @@ -7,14 +7,14 @@ #pragma once -#include "SensorBase.h" -#include "Counter.h" -#include "Task.h" -#include "PIDSource.h" -#include "LiveWindow/LiveWindowSendable.h" #include #include #include +#include "Counter.h" +#include "LiveWindow/LiveWindowSendable.h" +#include "PIDSource.h" +#include "SensorBase.h" +#include "Task.h" class DigitalInput; class DigitalOutput; @@ -22,17 +22,12 @@ class DigitalOutput; /** * Ultrasonic rangefinder class. * The Ultrasonic rangefinder measures absolute distance based on the round-trip - * time - * of a ping generated by the controller. These sensors use two transducers, a - * speaker and - * a microphone both tuned to the ultrasonic range. A common ultrasonic sensor, - * the Daventech SRF04 - * requires a short pulse to be generated on a digital channel. This causes the - * chirp to be - * emmitted. A second line becomes high as the ping is transmitted and goes low - * when - * the echo is received. The time that the line is high determines the round - * trip distance + * time of a ping generated by the controller. These sensors use two + * transducers, a speaker and a microphone both tuned to the ultrasonic range. A + * common ultrasonic sensor, the Daventech SRF04 requires a short pulse to be + * generated on a digital channel. This causes the chirp to be emitted. A second + * line becomes high as the ping is transmitted and goes low when the echo is + * received. The time that the line is high determines the round trip distance * (time of flight). */ class Ultrasonic : public SensorBase, @@ -41,10 +36,10 @@ class Ultrasonic : public SensorBase, public: enum DistanceUnit { kInches = 0, kMilliMeters = 1 }; - Ultrasonic(DigitalOutput *pingChannel, DigitalInput *echoChannel, + Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel, DistanceUnit units = kInches); - Ultrasonic(DigitalOutput &pingChannel, DigitalInput &echoChannel, + Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel, DistanceUnit units = kInches); Ultrasonic(std::shared_ptr pingChannel, @@ -88,8 +83,8 @@ class Ultrasonic : public SensorBase, static constexpr double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0; static Task m_task; // task doing the round-robin automatic sensing - static std::set m_sensors; // ultrasonic sensors - static std::atomic m_automaticEnabled; // automatic round robin mode + static std::set m_sensors; // ultrasonic sensors + static std::atomic m_automaticEnabled; // automatic round robin mode std::shared_ptr m_pingChannel; std::shared_ptr m_echoChannel; diff --git a/wpilibc/Athena/include/Vision/AxisCamera.h b/wpilibc/Athena/include/Vision/AxisCamera.h index 9252442c1d..e9036c4ac8 100644 --- a/wpilibc/Athena/include/Vision/AxisCamera.h +++ b/wpilibc/Athena/include/Vision/AxisCamera.h @@ -7,8 +7,8 @@ #pragma once -#include #include +#include #include "HAL/cpp/priority_mutex.h" #include "ErrorBase.h" @@ -49,7 +49,7 @@ class AxisCamera : public ErrorBase { enum Rotation { kRotation_0, kRotation_180 }; - explicit AxisCamera(std::string const &cameraHost); + explicit AxisCamera(std::string const& cameraHost); virtual ~AxisCamera(); AxisCamera(const AxisCamera&) = delete; @@ -57,11 +57,11 @@ class AxisCamera : public ErrorBase { bool IsFreshImage() const; - int GetImage(Image *image); - int GetImage(ColorImage *image); - HSLImage *GetImage(); - int CopyJPEG(char **destImage, unsigned int &destImageSize, - unsigned int &destImageBufferSize); + int GetImage(Image* image); + int GetImage(ColorImage* image); + HSLImage* GetImage(); + int CopyJPEG(char** destImage, unsigned int& destImageSize, + unsigned int& destImageBufferSize); void WriteBrightness(int brightness); int GetBrightness(); @@ -119,5 +119,5 @@ class AxisCamera : public ErrorBase { void ReadImagesFromCamera(); bool WriteParameters(); - int CreateCameraSocket(std::string const &requestString, bool setError); + int CreateCameraSocket(std::string const& requestString, bool setError); }; diff --git a/wpilibc/Athena/include/Vision/BaeUtilities.h b/wpilibc/Athena/include/Vision/BaeUtilities.h index 5a1270b33c..5e8df42459 100644 --- a/wpilibc/Athena/include/Vision/BaeUtilities.h +++ b/wpilibc/Athena/include/Vision/BaeUtilities.h @@ -41,7 +41,7 @@ typedef enum DebugOutputType_enum { /* debug */ void SetDebugFlag(DebugOutputType flag); -void dprintf(const char *tempString, ...); /* Variable argument list */ +void dprintf(const char* tempString, ...); /* Variable argument list */ /* set FRC ranges for drive */ double RangeToNormalized(double pixel, int range); @@ -50,18 +50,18 @@ float NormalizeToRange(float normalizedValue, float minRange, float maxRange); float NormalizeToRange(float normalizedValue); /* system utilities */ -void ShowActivity(char *fmt, ...); +void ShowActivity(char* fmt, ...); double ElapsedTime(double startTime); /* servo panning utilities */ class Servo; -double SinPosition(double *period, double sinStart); +double SinPosition(double* period, double sinStart); void panInit(); void panInit(double period); -void panForTarget(Servo *panServo); -void panForTarget(Servo *panServo, double sinStart); +void panForTarget(Servo* panServo); +void panForTarget(Servo* panServo, double sinStart); /* config file read utilities */ -int processFile(char *inputFile, char *outputString, int lineNumber); -int emptyString(char *string); -void stripString(char *string); +int processFile(char* inputFile, char* outputString, int lineNumber); +int emptyString(char* string); +void stripString(char* string); diff --git a/wpilibc/Athena/include/Vision/BinaryImage.h b/wpilibc/Athena/include/Vision/BinaryImage.h index d9a24c5f6b..1c30e732fc 100644 --- a/wpilibc/Athena/include/Vision/BinaryImage.h +++ b/wpilibc/Athena/include/Vision/BinaryImage.h @@ -14,8 +14,8 @@ */ #include "Vision/VisionAPI.h" -#include #include +#include class BinaryImage : public MonoImage { public: @@ -23,20 +23,20 @@ class BinaryImage : public MonoImage { int GetNumberParticles(); ParticleAnalysisReport GetParticleAnalysisReport(int particleNumber); void GetParticleAnalysisReport(int particleNumber, - ParticleAnalysisReport *par); - std::vector *GetOrderedParticleAnalysisReports(); - BinaryImage *RemoveSmallObjects(bool connectivity8, int erosions); - BinaryImage *RemoveLargeObjects(bool connectivity8, int erosions); - BinaryImage *ConvexHull(bool connectivity8); - BinaryImage *ParticleFilter(ParticleFilterCriteria2 *criteria, + ParticleAnalysisReport* par); + std::vector* GetOrderedParticleAnalysisReports(); + BinaryImage* RemoveSmallObjects(bool connectivity8, int erosions); + BinaryImage* RemoveLargeObjects(bool connectivity8, int erosions); + BinaryImage* ConvexHull(bool connectivity8); + BinaryImage* ParticleFilter(ParticleFilterCriteria2* criteria, int criteriaCount); - virtual void Write(const char *fileName); + virtual void Write(const char* fileName); private: bool ParticleMeasurement(int particleNumber, MeasurementType whatToMeasure, - int *result); + int* result); bool ParticleMeasurement(int particleNumber, MeasurementType whatToMeasure, - double *result); + double* result); static double NormalizeFromRange(double position, int range); static bool CompareParticleSizes(ParticleAnalysisReport particle1, ParticleAnalysisReport particle2); diff --git a/wpilibc/Athena/include/Vision/ColorImage.h b/wpilibc/Athena/include/Vision/ColorImage.h index 493c5414f8..96346ec803 100644 --- a/wpilibc/Athena/include/Vision/ColorImage.h +++ b/wpilibc/Athena/include/Vision/ColorImage.h @@ -7,65 +7,65 @@ #pragma once -#include "ImageBase.h" #include "BinaryImage.h" +#include "ImageBase.h" #include "Threshold.h" class ColorImage : public ImageBase { public: ColorImage(ImageType type); virtual ~ColorImage() = default; - BinaryImage *ThresholdRGB(int redLow, int redHigh, int greenLow, + BinaryImage* ThresholdRGB(int redLow, int redHigh, int greenLow, int greenHigh, int blueLow, int blueHigh); - BinaryImage *ThresholdHSL(int hueLow, int hueHigh, int saturationLow, + BinaryImage* ThresholdHSL(int hueLow, int hueHigh, int saturationLow, int saturationHigh, int luminenceLow, int luminenceHigh); - BinaryImage *ThresholdHSV(int hueLow, int hueHigh, int saturationLow, + BinaryImage* ThresholdHSV(int hueLow, int hueHigh, int saturationLow, int saturationHigh, int valueHigh, int valueLow); - BinaryImage *ThresholdHSI(int hueLow, int hueHigh, int saturationLow, + BinaryImage* ThresholdHSI(int hueLow, int hueHigh, int saturationLow, int saturationHigh, int intensityLow, int intensityHigh); - BinaryImage *ThresholdRGB(Threshold &threshold); - BinaryImage *ThresholdHSL(Threshold &threshold); - BinaryImage *ThresholdHSV(Threshold &threshold); - BinaryImage *ThresholdHSI(Threshold &threshold); - MonoImage *GetRedPlane(); - MonoImage *GetGreenPlane(); - MonoImage *GetBluePlane(); - MonoImage *GetHSLHuePlane(); - MonoImage *GetHSVHuePlane(); - MonoImage *GetHSIHuePlane(); - MonoImage *GetHSLSaturationPlane(); - MonoImage *GetHSVSaturationPlane(); - MonoImage *GetHSISaturationPlane(); - MonoImage *GetLuminancePlane(); - MonoImage *GetValuePlane(); - MonoImage *GetIntensityPlane(); - void ReplaceRedPlane(MonoImage *plane); - void ReplaceGreenPlane(MonoImage *plane); - void ReplaceBluePlane(MonoImage *plane); - void ReplaceHSLHuePlane(MonoImage *plane); - void ReplaceHSVHuePlane(MonoImage *plane); - void ReplaceHSIHuePlane(MonoImage *plane); - void ReplaceHSLSaturationPlane(MonoImage *plane); - void ReplaceHSVSaturationPlane(MonoImage *plane); - void ReplaceHSISaturationPlane(MonoImage *plane); - void ReplaceLuminancePlane(MonoImage *plane); - void ReplaceValuePlane(MonoImage *plane); - void ReplaceIntensityPlane(MonoImage *plane); + BinaryImage* ThresholdRGB(Threshold& threshold); + BinaryImage* ThresholdHSL(Threshold& threshold); + BinaryImage* ThresholdHSV(Threshold& threshold); + BinaryImage* ThresholdHSI(Threshold& threshold); + MonoImage* GetRedPlane(); + MonoImage* GetGreenPlane(); + MonoImage* GetBluePlane(); + MonoImage* GetHSLHuePlane(); + MonoImage* GetHSVHuePlane(); + MonoImage* GetHSIHuePlane(); + MonoImage* GetHSLSaturationPlane(); + MonoImage* GetHSVSaturationPlane(); + MonoImage* GetHSISaturationPlane(); + MonoImage* GetLuminancePlane(); + MonoImage* GetValuePlane(); + MonoImage* GetIntensityPlane(); + void ReplaceRedPlane(MonoImage* plane); + void ReplaceGreenPlane(MonoImage* plane); + void ReplaceBluePlane(MonoImage* plane); + void ReplaceHSLHuePlane(MonoImage* plane); + void ReplaceHSVHuePlane(MonoImage* plane); + void ReplaceHSIHuePlane(MonoImage* plane); + void ReplaceHSLSaturationPlane(MonoImage* plane); + void ReplaceHSVSaturationPlane(MonoImage* plane); + void ReplaceHSISaturationPlane(MonoImage* plane); + void ReplaceLuminancePlane(MonoImage* plane); + void ReplaceValuePlane(MonoImage* plane); + void ReplaceIntensityPlane(MonoImage* plane); void ColorEqualize(); void LuminanceEqualize(); private: - BinaryImage *ComputeThreshold(ColorMode colorMode, int low1, int high1, + BinaryImage* ComputeThreshold(ColorMode colorMode, int low1, int high1, int low2, int high2, int low3, int high3); void Equalize(bool allPlanes); - MonoImage *ExtractColorPlane(ColorMode mode, int planeNumber); - MonoImage *ExtractFirstColorPlane(ColorMode mode); - MonoImage *ExtractSecondColorPlane(ColorMode mode); - MonoImage *ExtractThirdColorPlane(ColorMode mode); - void ReplacePlane(ColorMode mode, MonoImage *plane, int planeNumber); - void ReplaceFirstColorPlane(ColorMode mode, MonoImage *plane); - void ReplaceSecondColorPlane(ColorMode mode, MonoImage *plane); - void ReplaceThirdColorPlane(ColorMode mode, MonoImage *plane); + MonoImage* ExtractColorPlane(ColorMode mode, int planeNumber); + MonoImage* ExtractFirstColorPlane(ColorMode mode); + MonoImage* ExtractSecondColorPlane(ColorMode mode); + MonoImage* ExtractThirdColorPlane(ColorMode mode); + void ReplacePlane(ColorMode mode, MonoImage* plane, int planeNumber); + void ReplaceFirstColorPlane(ColorMode mode, MonoImage* plane); + void ReplaceSecondColorPlane(ColorMode mode, MonoImage* plane); + void ReplaceThirdColorPlane(ColorMode mode, MonoImage* plane); }; diff --git a/wpilibc/Athena/include/Vision/HSLImage.h b/wpilibc/Athena/include/Vision/HSLImage.h index 057a8c71a6..f94972e40e 100644 --- a/wpilibc/Athena/include/Vision/HSLImage.h +++ b/wpilibc/Athena/include/Vision/HSLImage.h @@ -15,6 +15,6 @@ class HSLImage : public ColorImage { public: HSLImage(); - HSLImage(const char *fileName); + HSLImage(const char* fileName); virtual ~HSLImage() = default; }; diff --git a/wpilibc/Athena/include/Vision/ImageBase.h b/wpilibc/Athena/include/Vision/ImageBase.h index 5fc04705d7..03e34b74dc 100644 --- a/wpilibc/Athena/include/Vision/ImageBase.h +++ b/wpilibc/Athena/include/Vision/ImageBase.h @@ -8,8 +8,8 @@ #pragma once #include -#include "nivision.h" #include "ErrorBase.h" +#include "nivision.h" #define DEFAULT_BORDER_SIZE 3 @@ -17,11 +17,11 @@ class ImageBase : public ErrorBase { public: ImageBase(ImageType type); virtual ~ImageBase(); - virtual void Write(const char *fileName); + virtual void Write(const char* fileName); int GetHeight(); int GetWidth(); - Image *GetImaqImage(); + Image* GetImaqImage(); protected: - Image *m_imaqImage; + Image* m_imaqImage; }; diff --git a/wpilibc/Athena/include/Vision/MonoImage.h b/wpilibc/Athena/include/Vision/MonoImage.h index 856d891288..57c3108068 100644 --- a/wpilibc/Athena/include/Vision/MonoImage.h +++ b/wpilibc/Athena/include/Vision/MonoImage.h @@ -16,9 +16,9 @@ class MonoImage : public ImageBase { MonoImage(); virtual ~MonoImage() = default; - std::vector *DetectEllipses( - EllipseDescriptor *ellipseDescriptor, CurveOptions *curveOptions, - ShapeDetectionOptions *shapeDetectionOptions, ROI *roi); - std::vector *DetectEllipses( - EllipseDescriptor *ellipseDescriptor); + std::vector* DetectEllipses( + EllipseDescriptor* ellipseDescriptor, CurveOptions* curveOptions, + ShapeDetectionOptions* shapeDetectionOptions, ROI* roi); + std::vector* DetectEllipses( + EllipseDescriptor* ellipseDescriptor); }; diff --git a/wpilibc/Athena/include/Vision/RGBImage.h b/wpilibc/Athena/include/Vision/RGBImage.h index eeed545b86..37da9bd4cc 100644 --- a/wpilibc/Athena/include/Vision/RGBImage.h +++ b/wpilibc/Athena/include/Vision/RGBImage.h @@ -15,6 +15,6 @@ class RGBImage : public ColorImage { public: RGBImage(); - RGBImage(const char *fileName); + RGBImage(const char* fileName); virtual ~RGBImage() = default; }; diff --git a/wpilibc/Athena/include/Vision/VisionAPI.h b/wpilibc/Athena/include/Vision/VisionAPI.h index 7c7b5c1069..c761f27c77 100644 --- a/wpilibc/Athena/include/Vision/VisionAPI.h +++ b/wpilibc/Athena/include/Vision/VisionAPI.h @@ -9,18 +9,18 @@ #include "nivision.h" -/* Constants */ +/* Constants */ -#define DEFAULT_BORDER_SIZE 3 // VisionAPI.frcCreateImage +#define DEFAULT_BORDER_SIZE 3 // VisionAPI.frcCreateImage #define DEFAULT_SATURATION_THRESHOLD 40 // TrackAPI.FindColor -/* Forward Declare Data Structures */ +/* Forward Declare Data Structures */ typedef struct FindEdgeOptions_struct FindEdgeOptions; typedef struct CircularEdgeReport_struct CircularEdgeReport; -/* Data Structures */ +/* Data Structures */ -/** frcParticleAnalysis returns this structure */ +/** frcParticleAnalysis returns this structure */ typedef struct ParticleAnalysisReport_struct { int imageHeight; int imageWidth; @@ -28,12 +28,14 @@ typedef struct ParticleAnalysisReport_struct { int particleIndex; // the particle index analyzed /* X-coordinate of the point representing the average position of the * total particle mass, assuming every point in the particle has a constant - * density */ + * density + */ int center_mass_x; // MeasurementType: IMAQ_MT_CENTER_OF_MASS_X /* Y-coordinate of the point representing the average position of the * total particle mass, assuming every point in the particle has a constant - * density */ - int center_mass_y; // MeasurementType: IMAQ_MT_CENTER_OF_MASS_Y + * density + */ + int center_mass_y; // MeasurementType: IMAQ_MT_CENTER_OF_MASS_Y double center_mass_x_normalized; // Center of mass x value normalized to -1.0 // to +1.0 range double center_mass_y_normalized; // Center of mass y value normalized to -1.0 @@ -66,7 +68,7 @@ typedef struct ColorReport_struct { float particleLumMean; // HistogramReport: luminance mean } ColorReport; -/* Image Management functions */ +/* Image Management functions */ /* Create: calls imaqCreateImage. Border size is set to some default value */ Image* frcCreateImage(ImageType type); @@ -90,7 +92,7 @@ int frcReadImage(Image* image, const char* fileName); /* Write Image : calls imaqWriteFile */ int frcWriteImage(const Image* image, const char* fileName); -/* Measure Intensity functions */ +/* Measure Intensity functions */ /* Histogram: calls imaqHistogram */ HistogramReport* frcHistogram(const Image* image, int numClasses, float min, @@ -102,7 +104,7 @@ ColorHistogramReport* frcColorHistogram(const Image* image, int numClasses, /* Get Pixel Value: calls imaqGetPixel */ int frcGetPixelValue(const Image* image, Point pixel, PixelValue* value); -/* Particle Analysis functions */ +/* Particle Analysis functions */ /* Particle Filter: calls imaqParticleFilter3 */ int frcParticleFilter(Image* dest, Image* source, @@ -126,7 +128,7 @@ int frcCountParticles(Image* image, int* numParticles); int frcParticleAnalysis(Image* image, int particleNumber, ParticleAnalysisReport* par); -/* Image Enhancement functions */ +/* Image Enhancement functions */ /* Equalize: calls imaqEqualize */ int frcEqualize(Image* dest, const Image* source, float min, float max); @@ -137,7 +139,7 @@ int frcEqualize(Image* dest, const Image* source, float min, float max, int frcColorEqualize(Image* dest, const Image* source); int frcColorEqualize(Image* dest, const Image* source, int colorEqualization); -/* Image Thresholding & Conversion functions */ +/* Image Thresholding & Conversion functions */ /* Smart Threshold: calls imaqLocalThreshold */ int frcSmartThreshold(Image* dest, const Image* source, diff --git a/wpilibc/Athena/include/WPILib.h b/wpilibc/Athena/include/WPILib.h index ad7c26ef32..6f92c549c9 100644 --- a/wpilibc/Athena/include/WPILib.h +++ b/wpilibc/Athena/include/WPILib.h @@ -9,8 +9,8 @@ #define REAL -#include "string.h" #include +#include "string.h" #include "ADXL345_I2C.h" #include "ADXL345_SPI.h" @@ -27,9 +27,9 @@ #include "Buttons/InternalButton.h" #include "Buttons/JoystickButton.h" #include "Buttons/NetworkButton.h" -#include "CameraServer.h" #include "CANJaguar.h" #include "CANTalon.h" +#include "CameraServer.h" #include "Commands/Command.h" #include "Commands/CommandGroup.h" #include "Commands/PIDCommand.h" @@ -54,37 +54,34 @@ #include "Filters/LinearDigitalFilter.h" #include "GearTooth.h" #include "GenericHID.h" -#include "interfaces/Accelerometer.h" -#include "interfaces/Gyro.h" -#include "interfaces/Potentiometer.h" #include "I2C.h" -#include "IterativeRobot.h" #include "InterruptableSensorBase.h" +#include "IterativeRobot.h" #include "Jaguar.h" #include "Joystick.h" #include "Notifier.h" #include "PIDController.h" #include "PIDOutput.h" #include "PIDSource.h" -#include "Preferences.h" -#include "PowerDistributionPanel.h" #include "PWM.h" #include "PWMSpeedController.h" +#include "PowerDistributionPanel.h" +#include "Preferences.h" #include "Relay.h" #include "Resource.h" #include "RobotBase.h" #include "RobotDrive.h" #include "SD540.h" +#include "SPI.h" +#include "SampleRobot.h" #include "SensorBase.h" #include "SerialPort.h" #include "Servo.h" -#include "SampleRobot.h" #include "SmartDashboard/SendableChooser.h" #include "SmartDashboard/SmartDashboard.h" #include "Solenoid.h" #include "Spark.h" #include "SpeedController.h" -#include "SPI.h" #include "Talon.h" #include "TalonSRX.h" #include "Task.h" @@ -101,5 +98,8 @@ #include "Vision/MonoImage.h" #include "Vision/RGBImage.h" #include "Vision/Threshold.h" +#include "interfaces/Accelerometer.h" +#include "interfaces/Gyro.h" +#include "interfaces/Potentiometer.h" // XXX: #include "Vision/AxisCamera.h" #include "WPIErrors.h" diff --git a/wpilibc/Athena/src/ADXL345_I2C.cpp b/wpilibc/Athena/src/ADXL345_I2C.cpp index 8de9e3b051..8714bd7923 100644 --- a/wpilibc/Athena/src/ADXL345_I2C.cpp +++ b/wpilibc/Athena/src/ADXL345_I2C.cpp @@ -6,8 +6,8 @@ /*----------------------------------------------------------------------------*/ #include "ADXL345_I2C.h" -#include "I2C.h" #include "HAL/HAL.hpp" +#include "I2C.h" #include "LiveWindow/LiveWindow.h" const uint8_t ADXL345_I2C::kAddress; @@ -19,11 +19,12 @@ constexpr double ADXL345_I2C::kGsPerLSB; /** * Constructs the ADXL345 Accelerometer over I2C. * - * @param port The I2C port the accelerometer is attached to - * @param range The range (+ or -) that the accelerometer will measure. - * @param deviceAddress the I2C address of the accelerometer (0x1D or 0x53) + * @param port The I2C port the accelerometer is attached to + * @param range The range (+ or -) that the accelerometer will measure + * @param deviceAddress The I2C address of the accelerometer (0x1D or 0x53) */ -ADXL345_I2C::ADXL345_I2C(Port port, Range range, int deviceAddress) : I2C(port, deviceAddress) { +ADXL345_I2C::ADXL345_I2C(Port port, Range range, int deviceAddress) + : I2C(port, deviceAddress) { // Turn on the measurements Write(kPowerCtlRegister, kPowerCtl_Measure); // Specify the data format to read @@ -52,7 +53,7 @@ double ADXL345_I2C::GetZ() { return GetAcceleration(kAxis_Z); } */ double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis) { int16_t rawAccel = 0; - Read(kDataRegister + (uint8_t)axis, sizeof(rawAccel), (uint8_t *)&rawAccel); + Read(kDataRegister + (uint8_t)axis, sizeof(rawAccel), (uint8_t*)&rawAccel); return rawAccel * kGsPerLSB; } @@ -60,12 +61,12 @@ double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis) { * Get the acceleration of all axes in Gs. * * @return An object containing the acceleration measured on each axis of the - * ADXL345 in Gs. + * ADXL345 in Gs. */ ADXL345_I2C::AllAxes ADXL345_I2C::GetAccelerations() { AllAxes data = AllAxes(); int16_t rawData[3]; - Read(kDataRegister, sizeof(rawData), (uint8_t *)rawData); + Read(kDataRegister, sizeof(rawData), (uint8_t*)rawData); data.XAxis = rawData[0] * kGsPerLSB; data.YAxis = rawData[1] * kGsPerLSB; diff --git a/wpilibc/Athena/src/ADXL345_SPI.cpp b/wpilibc/Athena/src/ADXL345_SPI.cpp index 0f602bdc50..b4899c7d3a 100644 --- a/wpilibc/Athena/src/ADXL345_SPI.cpp +++ b/wpilibc/Athena/src/ADXL345_SPI.cpp @@ -18,8 +18,8 @@ constexpr double ADXL345_SPI::kGsPerLSB; /** * Constructor. * - * @param port The SPI port the accelerometer is attached to - * @param range The range (+ or -) that the accelerometer will measure. + * @param port The SPI port the accelerometer is attached to + * @param range The range (+ or -) that the accelerometer will measure */ ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range) : SPI(port) { SetClockRate(500000); @@ -79,7 +79,7 @@ double ADXL345_SPI::GetAcceleration(ADXL345_SPI::Axes axis) { * Get the acceleration of all axes in Gs. * * @return An object containing the acceleration measured on each axis of the - * ADXL345 in Gs. + * ADXL345 in Gs. */ ADXL345_SPI::AllAxes ADXL345_SPI::GetAccelerations() { AllAxes data = AllAxes(); diff --git a/wpilibc/Athena/src/ADXL362.cpp b/wpilibc/Athena/src/ADXL362.cpp index 61e72cc63d..c6e364d257 100644 --- a/wpilibc/Athena/src/ADXL362.cpp +++ b/wpilibc/Athena/src/ADXL362.cpp @@ -19,13 +19,13 @@ static uint8_t kDataRegister = 0x0E; static uint8_t kFilterCtlRegister = 0x2C; static uint8_t kPowerCtlRegister = 0x2D; -//static uint8_t kFilterCtl_Range2G = 0x00; -//static uint8_t kFilterCtl_Range4G = 0x40; -//static uint8_t kFilterCtl_Range8G = 0x80; +// static uint8_t kFilterCtl_Range2G = 0x00; +// static uint8_t kFilterCtl_Range4G = 0x40; +// static uint8_t kFilterCtl_Range8G = 0x80; static uint8_t kFilterCtl_ODR_100Hz = 0x03; static uint8_t kPowerCtl_UltraLowNoise = 0x20; -//static uint8_t kPowerCtl_AutoSleep = 0x04; +// static uint8_t kPowerCtl_AutoSleep = 0x04; static uint8_t kPowerCtl_Measure = 0x02; /** @@ -38,7 +38,7 @@ ADXL362::ADXL362(Range range) : ADXL362(SPI::Port::kOnboardCS1, range) {} /** * Constructor. * - * @param port The SPI port the accelerometer is attached to + * @param port The SPI port the accelerometer is attached to * @param range The range (+ or -) that the accelerometer will measure. */ ADXL362::ADXL362(SPI::Port port, Range range) : m_spi(port) { @@ -128,7 +128,7 @@ double ADXL362::GetAcceleration(ADXL362::Axes axis) { * Get the acceleration of all axes in Gs. * * @return An object containing the acceleration measured on each axis of the - * ADXL362 in Gs. + * ADXL362 in Gs. */ ADXL362::AllAxes ADXL362::GetAccelerations() { AllAxes data = AllAxes(); diff --git a/wpilibc/Athena/src/ADXRS450_Gyro.cpp b/wpilibc/Athena/src/ADXRS450_Gyro.cpp index 00090bba27..b481f75b13 100644 --- a/wpilibc/Athena/src/ADXRS450_Gyro.cpp +++ b/wpilibc/Athena/src/ADXRS450_Gyro.cpp @@ -26,10 +26,11 @@ static constexpr uint8_t kSNLowRegister = 0x10; /** * Initialize the gyro. + * * Calibrate the gyro by running for a number of samples and computing the - * center value. - * Then use the center value as the Accumulator center value for subsequent - * measurements. + * center value. Then use the center value as the Accumulator center value for + * subsequent measurements. + * * It's important to make sure that the robot is not moving while the centering * calculations are in progress, this is typically done when the robot is first * turned on while it's sitting at rest before the competition starts. @@ -101,8 +102,7 @@ uint16_t ADXRS450_Gyro::ReadRegister(uint8_t reg) { // big endian uint8_t buf[4] = {(uint8_t)((cmd >> 24) & 0xff), - (uint8_t)((cmd >> 16) & 0xff), - (uint8_t)((cmd >> 8) & 0xff), + (uint8_t)((cmd >> 16) & 0xff), (uint8_t)((cmd >> 8) & 0xff), (uint8_t)(cmd & 0xff)}; m_spi.Write(buf, 4); @@ -113,28 +113,24 @@ uint16_t ADXRS450_Gyro::ReadRegister(uint8_t reg) { /** * Reset the gyro. + * * Resets the gyro to a heading of zero. This can be used if there is - * significant - * drift in the gyro and it needs to be recalibrated after it has been running. + * significant drift in the gyro and it needs to be recalibrated after it has + * been running. */ -void ADXRS450_Gyro::Reset() { - m_spi.ResetAccumulator(); -} +void ADXRS450_Gyro::Reset() { m_spi.ResetAccumulator(); } /** * Return the actual angle in degrees that the robot is currently facing. * * The angle is based on the current accumulator value corrected by the - * oversampling rate, the - * gyro type and the A/D calibration values. + * oversampling rate, the gyro type and the A/D calibration values. * The angle is continuous, that is it will continue from 360->361 degrees. This - * allows algorithms that wouldn't - * want to see a discontinuity in the gyro output as it sweeps from 360 to 0 on - * the second time around. + * allows algorithms that wouldn't want to see a discontinuity in the gyro + * output as it sweeps from 360 to 0 on the second time around. * * @return the current heading of the robot in degrees. This heading is based on - * integration - * of the returned rate from the gyro. + * integration of the returned rate from the gyro. */ float ADXRS450_Gyro::GetAngle() const { return (float)(m_spi.GetAccumulatorValue() * kDegreePerSecondPerLSB * diff --git a/wpilibc/Athena/src/AnalogAccelerometer.cpp b/wpilibc/Athena/src/AnalogAccelerometer.cpp index 824e751974..842b952281 100644 --- a/wpilibc/Athena/src/AnalogAccelerometer.cpp +++ b/wpilibc/Athena/src/AnalogAccelerometer.cpp @@ -6,8 +6,8 @@ /*----------------------------------------------------------------------------*/ #include "AnalogAccelerometer.h" -#include "WPIErrors.h" #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" /** * Common function for initializing the accelerometer. @@ -21,9 +21,11 @@ void AnalogAccelerometer::InitAccelerometer() { /** * Create a new instance of an accelerometer. + * * The constructor allocates desired analog input. + * * @param channel The channel number for the analog input the accelerometer is - * connected to + * connected to */ AnalogAccelerometer::AnalogAccelerometer(int32_t channel) { m_analogInput = std::make_shared(channel); @@ -32,13 +34,15 @@ AnalogAccelerometer::AnalogAccelerometer(int32_t channel) { /** * Create a new instance of Accelerometer from an existing AnalogInput. + * * Make a new instance of accelerometer given an AnalogInput. This is * particularly useful if the port is going to be read as an analog channel as * well as through the Accelerometer class. + * * @param channel The existing AnalogInput object for the analog input the - * accelerometer is connected to + * accelerometer is connected to */ -AnalogAccelerometer::AnalogAccelerometer(AnalogInput *channel) +AnalogAccelerometer::AnalogAccelerometer(AnalogInput* channel) : m_analogInput(channel, NullDeleter()) { if (channel == nullptr) { wpi_setWPIError(NullParameter); @@ -49,11 +53,13 @@ AnalogAccelerometer::AnalogAccelerometer(AnalogInput *channel) /** * Create a new instance of Accelerometer from an existing AnalogInput. + * * Make a new instance of accelerometer given an AnalogInput. This is * particularly useful if the port is going to be read as an analog channel as * well as through the Accelerometer class. + * * @param channel The existing AnalogInput object for the analog input the - * accelerometer is connected to + * accelerometer is connected to */ AnalogAccelerometer::AnalogAccelerometer(std::shared_ptr channel) : m_analogInput(channel) { @@ -79,9 +85,8 @@ float AnalogAccelerometer::GetAcceleration() const { * Set the accelerometer sensitivity. * * This sets the sensitivity of the accelerometer used for calculating the - * acceleration. - * The sensitivity varies by accelerometer model. There are constants defined - * for various models. + * acceleration. The sensitivity varies by accelerometer model. There are + * constants defined for various models. * * @param sensitivity The sensitivity of accelerometer in Volts per G. */ @@ -125,4 +130,6 @@ void AnalogAccelerometer::InitTable(std::shared_ptr subTable) { UpdateTable(); } -std::shared_ptr AnalogAccelerometer::GetTable() const { return m_table; } +std::shared_ptr AnalogAccelerometer::GetTable() const { + return m_table; +} diff --git a/wpilibc/Athena/src/AnalogGyro.cpp b/wpilibc/Athena/src/AnalogGyro.cpp index ba60d3aeeb..5b085283c5 100644 --- a/wpilibc/Athena/src/AnalogGyro.cpp +++ b/wpilibc/Athena/src/AnalogGyro.cpp @@ -6,11 +6,11 @@ /*----------------------------------------------------------------------------*/ #include "AnalogGyro.h" +#include #include "AnalogInput.h" +#include "LiveWindow/LiveWindow.h" #include "Timer.h" #include "WPIErrors.h" -#include "LiveWindow/LiveWindow.h" -#include const uint32_t AnalogGyro::kOversampleBits; const uint32_t AnalogGyro::kAverageBits; constexpr float AnalogGyro::kSamplesPerSecond; @@ -20,32 +20,37 @@ constexpr float AnalogGyro::kDefaultVoltsPerDegreePerSecond; /** * Gyro constructor using the Analog Input channel number. * - * @param channel The analog channel the gyro is connected to. Gyros - can only be used on on-board Analog Inputs 0-1. + * @param channel The analog channel the gyro is connected to. Gyros can only + * be used on on-board Analog Inputs 0-1. */ -AnalogGyro::AnalogGyro(int32_t channel) : - AnalogGyro(std::make_shared(channel)) {} +AnalogGyro::AnalogGyro(int32_t channel) + : AnalogGyro(std::make_shared(channel)) {} /** * Gyro constructor with a precreated AnalogInput object. + * * Use this constructor when the analog channel needs to be shared. * This object will not clean up the AnalogInput object when using this * constructor. + * * Gyros can only be used on on-board channels 0-1. - * @param channel A pointer to the AnalogInput object that the gyro is connected - * to. + * + * @param channel A pointer to the AnalogInput object that the gyro is + * connected to. */ -AnalogGyro::AnalogGyro(AnalogInput *channel) +AnalogGyro::AnalogGyro(AnalogInput* channel) : AnalogGyro( std::shared_ptr(channel, NullDeleter())) {} /** * Gyro constructor with a precreated AnalogInput object. + * * Use this constructor when the analog channel needs to be shared. * This object will not clean up the AnalogInput object when using this - * constructor + * constructor. + * * @param channel A pointer to the AnalogInput object that the gyro is - * connected to. + * connected to. */ AnalogGyro::AnalogGyro(std::shared_ptr channel) : m_analog(channel) { @@ -61,10 +66,11 @@ AnalogGyro::AnalogGyro(std::shared_ptr channel) * Gyro constructor using the Analog Input channel number with parameters for * presetting the center and offset values. Bypasses calibration. * - * @param channel The analog channel the gyro is connected to. Gyros - * can only be used on on-board Analog Inputs 0-1. - * @param center Preset uncalibrated value to use as the accumulator center value. - * @param offset Preset uncalibrated value to use as the gyro offset. + * @param channel The analog channel the gyro is connected to. Gyros can only + * be used on on-board Analog Inputs 0-1. + * @param center Preset uncalibrated value to use as the accumulator center + * value. + * @param offset Preset uncalibrated value to use as the gyro offset. */ AnalogGyro::AnalogGyro(int32_t channel, uint32_t center, float offset) { m_analog = std::make_shared(channel); @@ -76,14 +82,19 @@ AnalogGyro::AnalogGyro(int32_t channel, uint32_t center, float offset) { } /** - * Gyro constructor with a precreated AnalogInput object and calibrated parameters. + * Gyro constructor with a precreated AnalogInput object and calibrated + * parameters. + * * Use this constructor when the analog channel needs to be shared. * This object will not clean up the AnalogInput object when using this - * constructor + * constructor. + * * @param channel A pointer to the AnalogInput object that the gyro is - * connected to. + * connected to. */ -AnalogGyro::AnalogGyro(std::shared_ptr channel, uint32_t center, float offset) : m_analog(channel) { +AnalogGyro::AnalogGyro(std::shared_ptr channel, uint32_t center, + float offset) + : m_analog(channel) { if (channel == nullptr) { wpi_setWPIError(NullParameter); } else { @@ -97,9 +108,10 @@ AnalogGyro::AnalogGyro(std::shared_ptr channel, uint32_t center, fl /** * Reset the gyro. + * * Resets the gyro to a heading of zero. This can be used if there is - * significant - * drift in the gyro and it needs to be recalibrated after it has been running. + * significant drift in the gyro and it needs to be recalibrated after it has + * been running. */ void AnalogGyro::Reset() { if (StatusIsFatal()) return; @@ -132,7 +144,8 @@ void AnalogGyro::InitGyro() { SetPIDSourceType(PIDSourceType::kDisplacement); HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel()); - LiveWindow::GetInstance()->AddSensor("AnalogGyro", m_analog->GetChannel(), this); + LiveWindow::GetInstance()->AddSensor("AnalogGyro", m_analog->GetChannel(), + this); } void AnalogGyro::Calibrate() { @@ -157,16 +170,13 @@ void AnalogGyro::Calibrate() { * Return the actual angle in degrees that the robot is currently facing. * * The angle is based on the current accumulator value corrected by the - * oversampling rate, the - * gyro type and the A/D calibration values. + * oversampling rate, the gyro type and the A/D calibration values. * The angle is continuous, that is it will continue from 360->361 degrees. This - * allows algorithms that wouldn't - * want to see a discontinuity in the gyro output as it sweeps from 360 to 0 on - * the second time around. + * allows algorithms that wouldn't want to see a discontinuity in the gyro + * output as it sweeps from 360 to 0 on the second time around. * * @return the current heading of the robot in degrees. This heading is based on - * integration - * of the returned rate from the gyro. + * integration of the returned rate from the gyro. */ float AnalogGyro::GetAngle() const { if (StatusIsFatal()) return 0.f; @@ -205,9 +215,7 @@ double AnalogGyro::GetRate() const { * * @return the current offset value */ -float AnalogGyro::GetOffset() const { - return m_offset; -} +float AnalogGyro::GetOffset() const { return m_offset; } /** * Return the gyro center value. If run after calibration, @@ -215,17 +223,14 @@ float AnalogGyro::GetOffset() const { * * @return the current center value */ -uint32_t AnalogGyro::GetCenter() const { - return m_center; -} +uint32_t AnalogGyro::GetCenter() const { return m_center; } /** * Set the gyro sensitivity. + * * This takes the number of volts/degree/second sensitivity of the gyro and uses - * it in subsequent - * calculations to allow the code to work with multiple gyros. This value is - * typically found in - * the gyro datasheet. + * it in subsequent calculations to allow the code to work with multiple gyros. + * This value is typically found in the gyro datasheet. * * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second */ @@ -234,10 +239,11 @@ void AnalogGyro::SetSensitivity(float voltsPerDegreePerSecond) { } /** - * Set the size of the neutral zone. Any voltage from the gyro less than this - * amount from the center is considered stationary. Setting a deadband will - * decrease the amount of drift when the gyro isn't rotating, but will make it - * less accurate. + * Set the size of the neutral zone. + * + * Any voltage from the gyro less than this amount from the center is + * considered stationary. Setting a deadband will decrease the amount of drift + * when the gyro isn't rotating, but will make it less accurate. * * @param volts The size of the deadband in volts */ diff --git a/wpilibc/Athena/src/AnalogInput.cpp b/wpilibc/Athena/src/AnalogInput.cpp index 7852aead77..9ea9f0a223 100644 --- a/wpilibc/Athena/src/AnalogInput.cpp +++ b/wpilibc/Athena/src/AnalogInput.cpp @@ -6,11 +6,11 @@ /*----------------------------------------------------------------------------*/ #include "AnalogInput.h" +#include "HAL/Port.h" +#include "LiveWindow/LiveWindow.h" #include "Resource.h" #include "Timer.h" #include "WPIErrors.h" -#include "LiveWindow/LiveWindow.h" -#include "HAL/Port.h" #include @@ -24,7 +24,7 @@ const uint32_t AnalogInput::kAccumulatorChannels[] = {0, 1}; * Construct an analog input. * * @param channel The channel number on the roboRIO to represent. 0-3 are - * on-board 4-7 are on the MXP port. + * on-board 4-7 are on the MXP port. */ AnalogInput::AnalogInput(uint32_t channel) { std::stringstream buf; @@ -64,10 +64,11 @@ AnalogInput::~AnalogInput() { /** * Get a sample straight from this channel. + * * The sample is a 12-bit value representing the 0V to 5V range of the A/D - * converter in the module. - * The units are in A/D converter codes. Use GetVoltage() to get the analog - * value in calibrated units. + * converter in the module. The units are in A/D converter codes. Use + * GetVoltage() to get the analog value in calibrated units. + * * @return A sample straight from this channel. */ int16_t AnalogInput::GetValue() const { @@ -81,6 +82,7 @@ int16_t AnalogInput::GetValue() const { /** * Get a sample from the output of the oversample and average engine for this * channel. + * * The sample is 12-bit + the bits configured in SetOversampleBits(). * The value configured in SetAverageBits() will cause this value to be averaged * 2**bits number of samples. @@ -88,6 +90,7 @@ int16_t AnalogInput::GetValue() const { * 2**(OversampleBits + AverageBits) samples * have been acquired from the module on this channel. * Use GetAverageVoltage() to get the analog value in calibrated units. + * * @return A sample from the oversample and average engine for this channel. */ int32_t AnalogInput::GetAverageValue() const { @@ -100,8 +103,10 @@ int32_t AnalogInput::GetAverageValue() const { /** * Get a scaled sample straight from this channel. + * * The value is scaled to units of Volts using the calibrated scaling data from * GetLSBWeight() and GetOffset(). + * * @return A scaled sample straight from this channel. */ float AnalogInput::GetVoltage() const { @@ -115,12 +120,14 @@ float AnalogInput::GetVoltage() const { /** * Get a scaled sample from the output of the oversample and average engine for * this channel. + * * The value is scaled to units of Volts using the calibrated scaling data from * GetLSBWeight() and GetOffset(). * Using oversampling will cause this value to be higher resolution, but it will * update more slowly. * Using averaging will cause this value to be more stable, but it will update * more slowly. + * * @return A scaled sample from the output of the oversample and average engine * for this channel. */ @@ -164,6 +171,7 @@ int32_t AnalogInput::GetOffset() const { /** * Get the channel number. + * * @return The channel number. */ uint32_t AnalogInput::GetChannel() const { @@ -173,6 +181,7 @@ uint32_t AnalogInput::GetChannel() const { /** * Set the number of averaging bits. + * * This sets the number of averaging bits. The actual number of averaged samples * is 2^bits. * Use averaging to improve the stability of your measurement at the expense of @@ -190,9 +199,9 @@ void AnalogInput::SetAverageBits(uint32_t bits) { /** * Get the number of averaging bits previously configured. + * * This gets the number of averaging bits from the FPGA. The actual number of - * averaged samples is 2^bits. - * The averaging is done automatically in the FPGA. + * averaged samples is 2^bits. The averaging is done automatically in the FPGA. * * @return Number of bits of averaging previously configured. */ @@ -205,11 +214,11 @@ uint32_t AnalogInput::GetAverageBits() const { /** * Set the number of oversample bits. + * * This sets the number of oversample bits. The actual number of oversampled - * values is 2^bits. - * Use oversampling to improve the resolution of your measurements at the - * expense of sampling rate. - * The oversampling is done automatically in the FPGA. + * values is 2^bits. Use oversampling to improve the resolution of your + * measurements at the expense of sampling rate. The oversampling is done + * automatically in the FPGA. * * @param bits Number of bits of oversampling. */ @@ -222,9 +231,10 @@ void AnalogInput::SetOversampleBits(uint32_t bits) { /** * Get the number of oversample bits previously configured. + * * This gets the number of oversample bits from the FPGA. The actual number of - * oversampled values is - * 2^bits. The oversampling is done automatically in the FPGA. + * oversampled values is 2^bits. The oversampling is done automatically in the + * FPGA. * * @return Number of bits of oversampling previously configured. */ @@ -264,8 +274,9 @@ void AnalogInput::InitAccumulator() { * Set an initial value for the accumulator. * * This will be added to all values returned to the user. + * * @param initialValue The value that the accumulator should start from when - * reset. + * reset. */ void AnalogInput::SetAccumulatorInitialValue(int64_t initialValue) { if (StatusIsFatal()) return; @@ -295,14 +306,12 @@ void AnalogInput::ResetAccumulator() { * Set the center value of the accumulator. * * The center value is subtracted from each A/D value before it is added to the - * accumulator. This - * is used for the center value of devices like gyros and accelerometers to - * take the device offset into account when integrating. + * accumulator. This is used for the center value of devices like gyros and + * accelerometers to take the device offset into account when integrating. * * This center value is based on the output of the oversampled and averaged - * source from the accumulator - * channel. Because of this, any non-zero oversample bits will affect the size - * of the value for this field. + * source from the accumulator channel. Because of this, any non-zero + * oversample bits will affect the size of the value for this field. */ void AnalogInput::SetAccumulatorCenter(int32_t center) { if (StatusIsFatal()) return; @@ -313,7 +322,6 @@ void AnalogInput::SetAccumulatorCenter(int32_t center) { /** * Set the accumulator's deadband. - * @param */ void AnalogInput::SetAccumulatorDeadband(int32_t deadband) { if (StatusIsFatal()) return; @@ -363,7 +371,7 @@ uint32_t AnalogInput::GetAccumulatorCount() const { * @param value Reference to the 64-bit accumulated output. * @param count Reference to the number of accumulation cycles. */ -void AnalogInput::GetAccumulatorOutput(int64_t &value, uint32_t &count) const { +void AnalogInput::GetAccumulatorOutput(int64_t& value, uint32_t& count) const { if (StatusIsFatal()) return; int32_t status = 0; getAccumulatorOutput(m_port, &value, &count, &status); @@ -373,8 +381,10 @@ void AnalogInput::GetAccumulatorOutput(int64_t &value, uint32_t &count) const { /** * Set the sample rate per channel for all analog channels. + * * The maximum rate is 500kS/s divided by the number of channels in use. * This is 62500 samples/s per channel. + * * @param samplesPerSecond The number of samples per second. */ void AnalogInput::SetSampleRate(float samplesPerSecond) { diff --git a/wpilibc/Athena/src/AnalogOutput.cpp b/wpilibc/Athena/src/AnalogOutput.cpp index 2092936371..a2b7a5ed38 100644 --- a/wpilibc/Athena/src/AnalogOutput.cpp +++ b/wpilibc/Athena/src/AnalogOutput.cpp @@ -6,10 +6,10 @@ /*----------------------------------------------------------------------------*/ #include "AnalogOutput.h" +#include "HAL/Port.h" +#include "LiveWindow/LiveWindow.h" #include "Resource.h" #include "WPIErrors.h" -#include "LiveWindow/LiveWindow.h" -#include "HAL/Port.h" #include #include @@ -18,8 +18,10 @@ static std::unique_ptr outputs; /** * Construct an analog output on the given channel. + * * All analog outputs are located on the MXP port. - * @param The channel number on the roboRIO to represent. + * + * @param channel The channel number on the roboRIO to represent. */ AnalogOutput::AnalogOutput(uint32_t channel) { Resource::CreateResourceObject(outputs, kAnalogOutputs); @@ -55,7 +57,9 @@ AnalogOutput::AnalogOutput(uint32_t channel) { } /** - * Destructor. Frees analog output resource + * Destructor. + * + * Frees analog output resource. */ AnalogOutput::~AnalogOutput() { freeAnalogOutputPort(m_port); @@ -63,7 +67,7 @@ AnalogOutput::~AnalogOutput() { } /** - * Set the value of the analog output + * Set the value of the analog output. * * @param voltage The output value in Volts, from 0.0 to +5.0 */ diff --git a/wpilibc/Athena/src/AnalogPotentiometer.cpp b/wpilibc/Athena/src/AnalogPotentiometer.cpp index 8c8aaaca2c..43cde3047d 100644 --- a/wpilibc/Athena/src/AnalogPotentiometer.cpp +++ b/wpilibc/Athena/src/AnalogPotentiometer.cpp @@ -10,12 +10,13 @@ /** * Construct an Analog Potentiometer object from a channel number. - * @param channel The channel number on the roboRIO to represent. 0-3 are - * on-board 4-7 are on the MXP port. + * + * @param channel The channel number on the roboRIO to represent. 0-3 are + * on-board 4-7 are on the MXP port. * @param fullRange The angular value (in desired units) representing the full - * 0-5V range of the input. - * @param offset The angular value (in desired units) representing the angular - * output at 0V. + * 0-5V range of the input. + * @param offset The angular value (in desired units) representing the + * angular output at 0V. */ AnalogPotentiometer::AnalogPotentiometer(int channel, double fullRange, double offset) @@ -26,13 +27,14 @@ AnalogPotentiometer::AnalogPotentiometer(int channel, double fullRange, /** * Construct an Analog Potentiometer object from an existing Analog Input * pointer. - * @param channel The existing Analog Input pointer + * + * @param channel The existing Analog Input pointer * @param fullRange The angular value (in desired units) representing the full - * 0-5V range of the input. - * @param offset The angular value (in desired units) representing the angular - * output at 0V. + * 0-5V range of the input. + * @param offset The angular value (in desired units) representing the + * angular output at 0V. */ -AnalogPotentiometer::AnalogPotentiometer(AnalogInput *input, double fullRange, +AnalogPotentiometer::AnalogPotentiometer(AnalogInput* input, double fullRange, double offset) : m_analog_input(input, NullDeleter()), m_fullRange(fullRange), @@ -41,11 +43,12 @@ AnalogPotentiometer::AnalogPotentiometer(AnalogInput *input, double fullRange, /** * Construct an Analog Potentiometer object from an existing Analog Input * pointer. - * @param channel The existing Analog Input pointer + * + * @param channel The existing Analog Input pointer * @param fullRange The angular value (in desired units) representing the full - * 0-5V range of the input. - * @param offset The angular value (in desired units) representing the angular - * output at 0V. + * 0-5V range of the input. + * @param offset The angular value (in desired units) representing the + * angular output at 0V. */ AnalogPotentiometer::AnalogPotentiometer(std::shared_ptr input, double fullRange, double offset) @@ -55,7 +58,7 @@ AnalogPotentiometer::AnalogPotentiometer(std::shared_ptr input, * Get the current reading of the potentiometer. * * @return The current position of the potentiometer (in the units used for - * fullRaneg and offset). + * fullRange and offset). */ double AnalogPotentiometer::Get() const { return (m_analog_input->GetVoltage() / ControllerPower::GetVoltage5V()) * @@ -91,4 +94,6 @@ void AnalogPotentiometer::UpdateTable() { } } -std::shared_ptr AnalogPotentiometer::GetTable() const { return m_table; } +std::shared_ptr AnalogPotentiometer::GetTable() const { + return m_table; +} diff --git a/wpilibc/Athena/src/AnalogTrigger.cpp b/wpilibc/Athena/src/AnalogTrigger.cpp index 36a1459843..56818c2f98 100644 --- a/wpilibc/Athena/src/AnalogTrigger.cpp +++ b/wpilibc/Athena/src/AnalogTrigger.cpp @@ -8,9 +8,9 @@ #include "AnalogTrigger.h" #include "AnalogInput.h" +#include "HAL/Port.h" #include "Resource.h" #include "WPIErrors.h" -#include "HAL/Port.h" #include @@ -18,7 +18,7 @@ * Constructor for an analog trigger given a channel number. * * @param channel The channel number on the roboRIO to represent. 0-3 are - * on-board 4-7 are on the MXP port. + * on-board 4-7 are on the MXP port. */ AnalogTrigger::AnalogTrigger(int32_t channel) { void* port = getPort(channel); @@ -34,13 +34,14 @@ AnalogTrigger::AnalogTrigger(int32_t channel) { /** * Construct an analog trigger given an analog input. + * * This should be used in the case of sharing an analog channel between the * trigger and an analog input object. + * * @param channel The pointer to the existing AnalogInput object */ -AnalogTrigger::AnalogTrigger(AnalogInput *input) : - AnalogTrigger(input->GetChannel()) { -} +AnalogTrigger::AnalogTrigger(AnalogInput* input) + : AnalogTrigger(input->GetChannel()) {} AnalogTrigger::~AnalogTrigger() { int32_t status = 0; @@ -50,9 +51,10 @@ AnalogTrigger::~AnalogTrigger() { /** * Set the upper and lower limits of the analog trigger. + * * The limits are given in ADC codes. If oversampling is used, the units must - * be scaled - * appropriately. + * be scaled appropriately. + * * @param lower The lower limit of the trigger in ADC codes (12-bit values). * @param upper The upper limit of the trigger in ADC codes (12-bit values). */ @@ -65,7 +67,9 @@ void AnalogTrigger::SetLimitsRaw(int32_t lower, int32_t upper) { /** * Set the upper and lower limits of the analog trigger. + * * The limits are given as floating point voltage values. + * * @param lower The lower limit of the trigger in Volts. * @param upper The upper limit of the trigger in Volts. */ @@ -78,11 +82,12 @@ void AnalogTrigger::SetLimitsVoltage(float lower, float upper) { /** * Configure the analog trigger to use the averaged vs. raw values. + * * If the value is true, then the averaged value is selected for the analog - * trigger, otherwise - * the immediate value is used. + * trigger, otherwise the immediate value is used. + * * @param useAveragedValue If true, use the Averaged value, otherwise use the - * instantaneous reading + * instantaneous reading */ void AnalogTrigger::SetAveraged(bool useAveragedValue) { if (StatusIsFatal()) return; @@ -93,12 +98,13 @@ void AnalogTrigger::SetAveraged(bool useAveragedValue) { /** * Configure the analog trigger to use a filtered value. + * * The analog trigger will operate with a 3 point average rejection filter. This - * is designed to - * help with 360 degree pot applications for the period where the pot crosses - * through zero. + * is designed to help with 360 degree pot applications for the period where + * the pot crosses through zero. + * * @param useFilteredValue If true, use the 3 point rejection filter, otherwise - * use the unfiltered value + * use the unfiltered value */ void AnalogTrigger::SetFiltered(bool useFilteredValue) { if (StatusIsFatal()) return; @@ -109,7 +115,9 @@ void AnalogTrigger::SetFiltered(bool useFilteredValue) { /** * Return the index of the analog trigger. + * * This is the FPGA index of this analog trigger instance. + * * @return The index of the analog trigger. */ uint32_t AnalogTrigger::GetIndex() const { @@ -119,7 +127,9 @@ uint32_t AnalogTrigger::GetIndex() const { /** * Return the InWindow output of the analog trigger. + * * True if the analog input is between the upper and lower limits. + * * @return True if the analog input is between the upper and lower limits. */ bool AnalogTrigger::GetInWindow() { @@ -132,11 +142,13 @@ bool AnalogTrigger::GetInWindow() { /** * Return the TriggerState output of the analog trigger. + * * True if above upper limit. * False if below lower limit. * If in Hysteresis, maintain previous state. + * * @return True if above upper limit. False if below lower limit. If in - * Hysteresis, maintain previous state. + * Hysteresis, maintain previous state. */ bool AnalogTrigger::GetTriggerState() { if (StatusIsFatal()) return false; @@ -148,8 +160,10 @@ bool AnalogTrigger::GetTriggerState() { /** * Creates an AnalogTriggerOutput object. + * * Gets an output object that can be used for routing. * Caller is responsible for deleting the AnalogTriggerOutput object. + * * @param type An enum of the type of output object to create. * @return A pointer to a new AnalogTriggerOutput object. */ diff --git a/wpilibc/Athena/src/AnalogTriggerOutput.cpp b/wpilibc/Athena/src/AnalogTriggerOutput.cpp index eb67d3612e..077c15a034 100644 --- a/wpilibc/Athena/src/AnalogTriggerOutput.cpp +++ b/wpilibc/Athena/src/AnalogTriggerOutput.cpp @@ -14,14 +14,13 @@ * trigger. * * Because this class derives from DigitalSource, it can be passed into routing - * functions - * for Counter, Encoder, etc. + * functions for Counter, Encoder, etc. * - * @param trigger A pointer to the trigger for which this is an output. + * @param trigger A pointer to the trigger for which this is an output. * @param outputType An enum that specifies the output on the trigger to - * represent. + * represent. */ -AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger &trigger, +AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger& trigger, AnalogTriggerType outputType) : m_trigger(trigger), m_outputType(outputType) { HALReport(HALUsageReporting::kResourceType_AnalogTriggerOutput, @@ -40,6 +39,7 @@ AnalogTriggerOutput::~AnalogTriggerOutput() { /** * Get the state of the analog trigger output. + * * @return The state of the analog trigger output. */ bool AnalogTriggerOutput::Get() const { diff --git a/wpilibc/Athena/src/BuiltInAccelerometer.cpp b/wpilibc/Athena/src/BuiltInAccelerometer.cpp index 86dd5e90c8..ec1f590818 100644 --- a/wpilibc/Athena/src/BuiltInAccelerometer.cpp +++ b/wpilibc/Athena/src/BuiltInAccelerometer.cpp @@ -7,11 +7,12 @@ #include "BuiltInAccelerometer.h" #include "HAL/HAL.hpp" -#include "WPIErrors.h" #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" /** * Constructor. + * * @param range The range the accelerometer will measure */ BuiltInAccelerometer::BuiltInAccelerometer(Range range) { @@ -65,4 +66,6 @@ void BuiltInAccelerometer::UpdateTable() { } } -std::shared_ptr BuiltInAccelerometer::GetTable() const { return m_table; } +std::shared_ptr BuiltInAccelerometer::GetTable() const { + return m_table; +} diff --git a/wpilibc/Athena/src/CANJaguar.cpp b/wpilibc/Athena/src/CANJaguar.cpp index 7dcca19031..5f76ccddd4 100644 --- a/wpilibc/Athena/src/CANJaguar.cpp +++ b/wpilibc/Athena/src/CANJaguar.cpp @@ -8,11 +8,11 @@ #include "CANJaguar.h" #include "Timer.h" #define tNIRIO_i32 int +#include +#include +#include "LiveWindow/LiveWindow.h" #include "NetworkCommunication/CANSessionMux.h" #include "WPIErrors.h" -#include -#include -#include "LiveWindow/LiveWindow.h" /* we are on ARM-LE now, not Freescale so no need to swap */ #define swap16(x) (x) @@ -33,7 +33,7 @@ static const int32_t kReceiveStatusAttempts = 50; static std::unique_ptr allocated; -static int32_t sendMessageHelper(uint32_t messageID, const uint8_t *data, +static int32_t sendMessageHelper(uint32_t messageID, const uint8_t* data, uint8_t dataSize, int32_t period) { static const uint32_t kTrustedMessages[] = { LM_API_VOLT_T_EN, LM_API_VOLT_T_SET, LM_API_SPD_T_EN, LM_API_SPD_T_SET, @@ -151,8 +151,9 @@ void CANJaguar::InitCANJaguar() { } /** - * Constructor for the CANJaguar device.
- * By default the device is configured in Percent mode. + * Constructor for the CANJaguar device. + * + *

By default the device is configured in Percent mode. * The control mode can be changed by calling one of the control modes listed * below. * @@ -174,8 +175,7 @@ void CANJaguar::InitCANJaguar() { * @see CANJaguar#SetVoltageMode(EncoderTag, int) * @see CANJaguar#SetVoltageMode(QuadEncoderTag, int) */ -CANJaguar::CANJaguar(uint8_t deviceNumber) - : m_deviceNumber(deviceNumber) { +CANJaguar::CANJaguar(uint8_t deviceNumber) : m_deviceNumber(deviceNumber) { std::stringstream buf; buf << "CANJaguar device number " << m_deviceNumber; Resource::CreateResourceObject(allocated, 63); @@ -229,17 +229,17 @@ uint8_t CANJaguar::getDeviceNumber() const { return m_deviceNumber; } /** * Sets the output set-point value. * - * The scale and the units depend on the mode the Jaguar is in.
- * In percentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM - * Jaguar).
- * In voltage Mode, the outputValue is in volts.
- * In current Mode, the outputValue is in amps.
- * In speed Mode, the outputValue is in rotations/minute.
- * In position Mode, the outputValue is in rotations. + * The scale and the units depend on the mode the Jaguar is in. + *

In percentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM + * Jaguar). + *

In voltage Mode, the outputValue is in volts. + *

In current Mode, the outputValue is in amps. + *

In speed Mode, the outputValue is in rotations/minute. + *

In position Mode, the outputValue is in rotations. * * @param outputValue The set-point to sent to the motor controller. - * @param syncGroup The update group to add this Set() to, pending - * UpdateSyncGroup(). If 0, update immediately. + * @param syncGroup The update group to add this Set() to, pending + * UpdateSyncGroup(). If 0, update immediately. */ void CANJaguar::Set(float outputValue, uint8_t syncGroup) { uint32_t messageID; @@ -303,13 +303,13 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) { /** * Get the recently set outputValue setpoint. * - * The scale and the units depend on the mode the Jaguar is in.
- * In percentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM - * Jaguar).
- * In voltage Mode, the outputValue is in volts.
- * In current Mode, the outputValue is in amps.
- * In speed Mode, the outputValue is in rotations/minute.
- * In position Mode, the outputValue is in rotations.
+ * The scale and the units depend on the mode the Jaguar is in. + *

In percentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM + * Jaguar). + *

In voltage Mode, the outputValue is in volts. + *

In current Mode, the outputValue is in amps. + *

In speed Mode, the outputValue is in rotations/minute. + *

In position Mode, the outputValue is in rotations. * * @return The most recently set outputValue setpoint. */ @@ -328,7 +328,7 @@ void CANJaguar::Disable() { DisableControl(); } * @deprecated Call Set instead. * * @param output Write out the PercentVbus value as was computed by the - * PIDController + * PIDController */ void CANJaguar::PIDWrite(float output) { if (m_controlMode == kPercentVbus) { @@ -339,59 +339,59 @@ void CANJaguar::PIDWrite(float output) { } } -uint8_t CANJaguar::packPercentage(uint8_t *buffer, double value) { +uint8_t CANJaguar::packPercentage(uint8_t* buffer, double value) { int16_t intValue = (int16_t)(value * 32767.0); - *((int16_t *)buffer) = swap16(intValue); + *((int16_t*)buffer) = swap16(intValue); return sizeof(int16_t); } -uint8_t CANJaguar::packFXP8_8(uint8_t *buffer, double value) { +uint8_t CANJaguar::packFXP8_8(uint8_t* buffer, double value) { int16_t intValue = (int16_t)(value * 256.0); - *((int16_t *)buffer) = swap16(intValue); + *((int16_t*)buffer) = swap16(intValue); return sizeof(int16_t); } -uint8_t CANJaguar::packFXP16_16(uint8_t *buffer, double value) { +uint8_t CANJaguar::packFXP16_16(uint8_t* buffer, double value) { int32_t intValue = (int32_t)(value * 65536.0); - *((int32_t *)buffer) = swap32(intValue); + *((int32_t*)buffer) = swap32(intValue); return sizeof(int32_t); } -uint8_t CANJaguar::packint16_t(uint8_t *buffer, int16_t value) { - *((int16_t *)buffer) = swap16(value); +uint8_t CANJaguar::packint16_t(uint8_t* buffer, int16_t value) { + *((int16_t*)buffer) = swap16(value); return sizeof(int16_t); } -uint8_t CANJaguar::packint32_t(uint8_t *buffer, int32_t value) { - *((int32_t *)buffer) = swap32(value); +uint8_t CANJaguar::packint32_t(uint8_t* buffer, int32_t value) { + *((int32_t*)buffer) = swap32(value); return sizeof(int32_t); } -double CANJaguar::unpackPercentage(uint8_t *buffer) const { - int16_t value = *((int16_t *)buffer); +double CANJaguar::unpackPercentage(uint8_t* buffer) const { + int16_t value = *((int16_t*)buffer); value = swap16(value); return value / 32767.0; } -double CANJaguar::unpackFXP8_8(uint8_t *buffer) const { - int16_t value = *((int16_t *)buffer); +double CANJaguar::unpackFXP8_8(uint8_t* buffer) const { + int16_t value = *((int16_t*)buffer); value = swap16(value); return value / 256.0; } -double CANJaguar::unpackFXP16_16(uint8_t *buffer) const { - int32_t value = *((int32_t *)buffer); +double CANJaguar::unpackFXP16_16(uint8_t* buffer) const { + int32_t value = *((int32_t*)buffer); value = swap32(value); return value / 65536.0; } -int16_t CANJaguar::unpackint16_t(uint8_t *buffer) const { - int16_t value = *((int16_t *)buffer); +int16_t CANJaguar::unpackint16_t(uint8_t* buffer) const { + int16_t value = *((int16_t*)buffer); return swap16(value); } -int32_t CANJaguar::unpackint32_t(uint8_t *buffer) const { - int32_t value = *((int32_t *)buffer); +int32_t CANJaguar::unpackint32_t(uint8_t* buffer) const { + int32_t value = *((int32_t*)buffer); return swap32(value); } @@ -399,13 +399,13 @@ int32_t CANJaguar::unpackint32_t(uint8_t *buffer) const { * Send a message to the Jaguar. * * @param messageID The messageID to be used on the CAN bus (device number is - * added internally) - * @param data The up to 8 bytes of data to be sent with the message - * @param dataSize Specify how much of the data in "data" to send - * @param periodic If positive, tell Network Communications to send the message - * every "period" milliseconds. + * added internally) + * @param data The up to 8 bytes of data to be sent with the message + * @param dataSize Specify how much of the data in "data" to send + * @param periodic If positive, tell Network Communications to send the + * message every "period" milliseconds. */ -void CANJaguar::sendMessage(uint32_t messageID, const uint8_t *data, +void CANJaguar::sendMessage(uint32_t messageID, const uint8_t* data, uint8_t dataSize, int32_t period) { int32_t localStatus = sendMessageHelper(messageID | m_deviceNumber, data, dataSize, period); @@ -419,8 +419,8 @@ void CANJaguar::sendMessage(uint32_t messageID, const uint8_t *data, * Request a message from the Jaguar, but don't wait for it to arrive. * * @param messageID The message to request - * @param periodic If positive, tell Network Communications to send the message - * every "period" milliseconds. + * @param periodic If positive, tell Network Communications to send the + * message every "period" milliseconds. */ void CANJaguar::requestMessage(uint32_t messageID, int32_t period) { sendMessageHelper(messageID | m_deviceNumber, nullptr, 0, period); @@ -432,15 +432,15 @@ void CANJaguar::requestMessage(uint32_t messageID, int32_t period) { * Jaguar always generates a message with the same message ID when replying. * * @param messageID The messageID to read from the CAN bus (device number is - * added internally) - * @param data The up to 8 bytes of data that was received with the message - * @param dataSize Indicates how much data was received + * added internally) + * @param data The up to 8 bytes of data that was received with the message + * @param dataSize Indicates how much data was received * * @return true if the message was found. Otherwise, no new message is - * available. + * available. */ bool CANJaguar::getMessage(uint32_t messageID, uint32_t messageMask, - uint8_t *data, uint8_t *dataSize) const { + uint8_t* data, uint8_t* dataSize) const { uint32_t targetedMessageID = messageID | m_deviceNumber; int32_t status = 0; uint32_t timeStamp; @@ -955,7 +955,7 @@ void CANJaguar::SetSpeedReference(uint8_t reference) { * Get the reference source device for speed controller mode. * * @return A speed reference indicating the currently selected reference device - * for speed controller mode. + * for speed controller mode. */ uint8_t CANJaguar::GetSpeedReference() const { return m_speedReference; } @@ -1164,7 +1164,7 @@ double CANJaguar::GetD() const { * encoder state. * * @param encoderInitialPosition Encoder position to set if position with - * encoder reference. Ignored otherwise. + * encoder reference. Ignored otherwise. */ void CANJaguar::EnableControl(double encoderInitialPosition) { uint8_t dataBuffer[8]; @@ -1232,9 +1232,10 @@ void CANJaguar::DisableControl() { /** * Enable controlling the motor voltage as a percentage of the bus voltage - * without any position or speed feedback.
- * After calling this you must call {@link CANJaguar#EnableControl()} or {@link - * CANJaguar#EnableControl(double)} to enable the device. + * without any position or speed feedback. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. */ void CANJaguar::SetPercentMode() { SetControlMode(kPercentVbus); @@ -1244,11 +1245,12 @@ void CANJaguar::SetPercentMode() { /** * Enable controlling the motor voltage as a percentage of the bus voltage, - * and enable speed sensing from a non-quadrature encoder.
- * After calling this you must call {@link CANJaguar#EnableControl()} or {@link - * CANJaguar#EnableControl(double)} to enable the device. + * and enable speed sensing from a non-quadrature encoder. * - * @param tag The constant CANJaguar::Encoder + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param tag The constant CANJaguar::Encoder * @param codesPerRev The counts per revolution on the encoder */ void CANJaguar::SetPercentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) { @@ -1260,11 +1262,12 @@ void CANJaguar::SetPercentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) { /** * Enable controlling the motor voltage as a percentage of the bus voltage, - * and enable speed sensing from a non-quadrature encoder.
- * After calling this you must call {@link CANJaguar#EnableControl()} or {@link - * CANJaguar#EnableControl(double)} to enable the device. + * and enable speed sensing from a non-quadrature encoder. * - * @param tag The constant CANJaguar::QuadEncoder + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param tag The constant CANJaguar::QuadEncoder * @param codesPerRev The counts per revolution on the encoder */ void CANJaguar::SetPercentMode(CANJaguar::QuadEncoderStruct, @@ -1276,13 +1279,14 @@ void CANJaguar::SetPercentMode(CANJaguar::QuadEncoderStruct, } /** -* Enable controlling the motor voltage as a percentage of the bus voltage, -* and enable position sensing from a potentiometer and no speed feedback.
-* After calling this you must call {@link CANJaguar#EnableControl()} or {@link -* CANJaguar#EnableControl(double)} to enable the device. -* -* @param potentiometer The constant CANJaguar::Potentiometer -*/ + * Enable controlling the motor voltage as a percentage of the bus voltage, + * and enable position sensing from a potentiometer and no speed feedback. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param potentiometer The constant CANJaguar::Potentiometer + */ void CANJaguar::SetPercentMode(CANJaguar::PotentiometerStruct) { SetControlMode(kPercentVbus); SetPositionReference(LM_REF_POT); @@ -1291,9 +1295,10 @@ void CANJaguar::SetPercentMode(CANJaguar::PotentiometerStruct) { } /** - * Enable controlling the motor current with a PID loop.
- * After calling this you must call {@link CANJaguar#EnableControl()} or {@link - * CANJaguar#EnableControl(double)} to enable the device. + * Enable controlling the motor current with a PID loop. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. * * @param p The proportional gain of the Jaguar's PID controller. * @param i The integral gain of the Jaguar's PID controller. @@ -1307,16 +1312,17 @@ void CANJaguar::SetCurrentMode(double p, double i, double d) { } /** -* Enable controlling the motor current with a PID loop, and enable speed -* sensing from a non-quadrature encoder.
-* After calling this you must call {@link CANJaguar#EnableControl()} or {@link -* CANJaguar#EnableControl(double)} to enable the device. -* -* @param encoder The constant CANJaguar::Encoder -* @param p The proportional gain of the Jaguar's PID controller. -* @param i The integral gain of the Jaguar's PID controller. -* @param d The differential gain of the Jaguar's PID controller. -*/ + * Enable controlling the motor current with a PID loop, and enable speed + * sensing from a non-quadrature encoder. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param encoder The constant CANJaguar::Encoder + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ void CANJaguar::SetCurrentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev, double p, double i, double d) { SetControlMode(kCurrent); @@ -1327,16 +1333,17 @@ void CANJaguar::SetCurrentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev, } /** -* Enable controlling the motor current with a PID loop, and enable speed and -* position sensing from a quadrature encoder.
-* After calling this you must call {@link CANJaguar#EnableControl()} or {@link -* CANJaguar#EnableControl(double)} to enable the device. -* -* @param endoer The constant CANJaguar::QuadEncoder -* @param p The proportional gain of the Jaguar's PID controller. -* @param i The integral gain of the Jaguar's PID controller. -* @param d The differential gain of the Jaguar's PID controller. -*/ + * Enable controlling the motor current with a PID loop, and enable speed and + * position sensing from a quadrature encoder. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param encoder The constant CANJaguar::QuadEncoder + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ void CANJaguar::SetCurrentMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev, double p, double i, double d) { @@ -1348,16 +1355,17 @@ void CANJaguar::SetCurrentMode(CANJaguar::QuadEncoderStruct, } /** -* Enable controlling the motor current with a PID loop, and enable position -* sensing from a potentiometer.
-* After calling this you must call {@link CANJaguar#EnableControl()} or {@link -* CANJaguar#EnableControl(double)} to enable the device. -* -* @param potentiometer The constant CANJaguar::Potentiometer -* @param p The proportional gain of the Jaguar's PID controller. -* @param i The integral gain of the Jaguar's PID controller. -* @param d The differential gain of the Jaguar's PID controller. -*/ + * Enable controlling the motor current with a PID loop, and enable position + * sensing from a potentiometer. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param potentiometer The constant CANJaguar::Potentiometer + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ void CANJaguar::SetCurrentMode(CANJaguar::PotentiometerStruct, double p, double i, double d) { SetControlMode(kCurrent); @@ -1369,15 +1377,16 @@ void CANJaguar::SetCurrentMode(CANJaguar::PotentiometerStruct, double p, /** * Enable controlling the speed with a feedback loop from a non-quadrature - * encoder.
- * After calling this you must call {@link CANJaguar#EnableControl()} or {@link - * CANJaguar#EnableControl(double)} to enable the device. + * encoder. * - * @param encoder The constant CANJaguar::Encoder + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param encoder The constant CANJaguar::Encoder * @param codesPerRev The counts per revolution on the encoder. - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. */ void CANJaguar::SetSpeedMode(CANJaguar::EncoderStruct, uint16_t codesPerRev, double p, double i, double d) { @@ -1389,17 +1398,18 @@ void CANJaguar::SetSpeedMode(CANJaguar::EncoderStruct, uint16_t codesPerRev, } /** -* Enable controlling the speed with a feedback loop from a quadrature -* encoder.
-* After calling this you must call {@link CANJaguar#EnableControl()} or {@link -* CANJaguar#EnableControl(double)} to enable the device. -* -* @param encoder The constant CANJaguar::QuadEncoder -* @param codesPerRev The counts per revolution on the encoder. -* @param p The proportional gain of the Jaguar's PID controller. -* @param i The integral gain of the Jaguar's PID controller. -* @param d The differential gain of the Jaguar's PID controller. -*/ + * Enable controlling the speed with a feedback loop from a quadrature + * encoder. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param encoder The constant CANJaguar::QuadEncoder + * @param codesPerRev The counts per revolution on the encoder. + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ void CANJaguar::SetSpeedMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev, double p, double i, double d) { SetControlMode(kSpeed); @@ -1410,16 +1420,16 @@ void CANJaguar::SetSpeedMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev, } /** - * Enable controlling the position with a feedback loop using an encoder.
- * After calling this you must call {@link CANJaguar#EnableControl()} or {@link - * CANJaguar#EnableControl(double)} to enable the device. + * Enable controlling the position with a feedback loop using an encoder. * - * @param encoder The constant CANJaguar::QuadEncoder + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param encoder The constant CANJaguar::QuadEncoder * @param codesPerRev The counts per revolution on the encoder. - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - * + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. */ void CANJaguar::SetPositionMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev, double p, double i, @@ -1431,14 +1441,16 @@ void CANJaguar::SetPositionMode(CANJaguar::QuadEncoderStruct, } /** -* Enable controlling the position with a feedback loop using a -* potentiometer.
-* After calling this you must call {@link CANJaguar#EnableControl()} or {@link -* CANJaguar#EnableControl(double)} to enable the device. -* @param p The proportional gain of the Jaguar's PID controller. -* @param i The integral gain of the Jaguar's PID controller. -* @param d The differential gain of the Jaguar's PID controller. -*/ + * Enable controlling the position with a feedback loop using a + * potentiometer. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ void CANJaguar::SetPositionMode(CANJaguar::PotentiometerStruct, double p, double i, double d) { SetControlMode(kPosition); @@ -1448,11 +1460,12 @@ void CANJaguar::SetPositionMode(CANJaguar::PotentiometerStruct, double p, } /** -* Enable controlling the motor voltage without any position or speed -* feedback.
-* After calling this you must call {@link CANJaguar#EnableControl()} or {@link -* CANJaguar#EnableControl(double)} to enable the device. -*/ + * Enable controlling the motor voltage without any position or speed + * feedback. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + */ void CANJaguar::SetVoltageMode() { SetControlMode(kVoltage); SetPositionReference(LM_REF_NONE); @@ -1460,14 +1473,15 @@ void CANJaguar::SetVoltageMode() { } /** -* Enable controlling the motor voltage with speed feedback from a -* non-quadrature encoder and no position feedback.
-* After calling this you must call {@link CANJaguar#EnableControl()} or {@link -* CANJaguar#EnableControl(double)} to enable the device. -* -* @param encoder The constant CANJaguar::Encoder -* @param codesPerRev The counts per revolution on the encoder -*/ + * Enable controlling the motor voltage with speed feedback from a + * non-quadrature encoder and no position feedback. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param encoder The constant CANJaguar::Encoder + * @param codesPerRev The counts per revolution on the encoder + */ void CANJaguar::SetVoltageMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) { SetControlMode(kVoltage); SetPositionReference(LM_REF_NONE); @@ -1476,14 +1490,15 @@ void CANJaguar::SetVoltageMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) { } /** -* Enable controlling the motor voltage with position and speed feedback from a -* quadrature encoder.
-* After calling this you must call {@link CANJaguar#EnableControl()} or {@link -* CANJaguar#EnableControl(double)} to enable the device. -* -* @param encoder The constant CANJaguar::QuadEncoder -* @param codesPerRev The counts per revolution on the encoder -*/ + * Enable controlling the motor voltage with position and speed feedback from a + * quadrature encoder. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param encoder The constant CANJaguar::QuadEncoder + * @param codesPerRev The counts per revolution on the encoder + */ void CANJaguar::SetVoltageMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev) { SetControlMode(kVoltage); @@ -1493,13 +1508,14 @@ void CANJaguar::SetVoltageMode(CANJaguar::QuadEncoderStruct, } /** -* Enable controlling the motor voltage with position feedback from a -* potentiometer and no speed feedback.
-* After calling this you must call {@link CANJaguar#EnableControl()} or {@link -* CANJaguar#EnableControl(double)} to enable the device. -* -* @param potentiometer The constant CANJaguar::Potentiometer -*/ + * Enable controlling the motor voltage with position feedback from a + * potentiometer and no speed feedback. + * + *

After calling this you must call {@link CANJaguar#EnableControl()} or + * {@link CANJaguar#EnableControl(double)} to enable the device. + * + * @param potentiometer The constant CANJaguar::Potentiometer + */ void CANJaguar::SetVoltageMode(CANJaguar::PotentiometerStruct) { SetControlMode(kVoltage); SetPositionReference(LM_REF_POT); @@ -1510,6 +1526,7 @@ void CANJaguar::SetVoltageMode(CANJaguar::PotentiometerStruct) { /** * Used internally. In order to set the control mode see the methods listed * below. + * * Change the control mode of this Jaguar object. * * After changing modes, configure any PID constants or other settings needed @@ -1665,12 +1682,11 @@ uint16_t CANJaguar::GetFaults() const { * Set the maximum voltage change rate. * * When in PercentVbus or Voltage output mode, the rate at which the voltage - * changes can - * be limited to reduce current spikes. Set this to 0.0 to disable rate - * limiting. + * changes can be limited to reduce current spikes. Set this to 0.0 to disable + * rate limiting. * - * @param rampRate The maximum rate of voltage change in Percent Voltage mode in - * V/s. + * @param rampRate The maximum rate of voltage change in Percent Voltage mode + * in V/s. */ void CANJaguar::SetVoltageRampRate(double rampRate) { uint8_t dataBuffer[8]; @@ -1721,7 +1737,7 @@ uint8_t CANJaguar::GetHardwareVersion() const { return m_hardwareVersion; } * This allows you to override the jumper configuration for brake or coast. * * @param mode Select to use the jumper setting or to override it to coast or - * brake. + * brake. */ void CANJaguar::ConfigNeutralMode(NeutralMode mode) { uint8_t dataBuffer[8]; @@ -1773,16 +1789,13 @@ void CANJaguar::ConfigPotentiometerTurns(uint16_t turns) { * Configure Soft Position Limits when in Position Controller mode. * * When controlling position, you can add additional limits on top of the limit - switch inputs - * that are based on the position feedback. If the position limit is reached or - the - * switch is opened, that direction will be disabled. + * switch inputs that are based on the position feedback. If the position + * limit is reached or the switch is opened, that direction will be disabled. * - * @param forwardLimitPosition The position that if exceeded will disable the - forward direction. + * forward direction. * @param reverseLimitPosition The position that if exceeded will disable the - reverse direction. + * reverse direction. */ void CANJaguar::ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition) { @@ -1817,10 +1830,10 @@ void CANJaguar::ConfigLimitMode(LimitMode mode) { } /** -* Set the position that if exceeded will disable the forward direction. -* -* Use ConfigSoftPositionLimits to set this and the limit mode automatically. -*/ + * Set the position that if exceeded will disable the forward direction. + * + * Use ConfigSoftPositionLimits to set this and the limit mode automatically. + */ void CANJaguar::ConfigForwardLimit(double forwardLimitPosition) { uint8_t dataBuffer[8]; uint8_t dataSize; @@ -1834,10 +1847,10 @@ void CANJaguar::ConfigForwardLimit(double forwardLimitPosition) { } /** -* Set the position that if exceeded will disable the reverse direction. -* -* Use ConfigSoftPositionLimits to set this and the limit mode automatically. -*/ + * Set the position that if exceeded will disable the reverse direction. + * + * Use ConfigSoftPositionLimits to set this and the limit mode automatically. + */ void CANJaguar::ConfigReverseLimit(double reverseLimitPosition) { uint8_t dataBuffer[8]; uint8_t dataSize; @@ -1940,15 +1953,15 @@ uint8_t CANJaguar::GetDeviceID() const { return m_deviceNumber; } * * @deprecated Call DisableControl instead. */ -void CANJaguar::StopMotor() { - m_stopped = true; -} +void CANJaguar::StopMotor() { m_stopped = true; } /** -* Common interface for inverting direction of a speed controller. -* Only works in PercentVbus, speed, and Voltage modes. -* @param isInverted The state of inversion, true is inverted -*/ + * Common interface for inverting direction of a speed controller. + * + * Only works in PercentVbus, speed, and Voltage modes. + * + * @param isInverted The state of inversion, true is inverted + */ void CANJaguar::SetInverted(bool isInverted) { m_isInverted = isInverted; } /** @@ -1961,20 +1974,22 @@ bool CANJaguar::GetInverted() const { return m_isInverted; } void CANJaguar::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) { - if(key == "Mode" && value->IsDouble()) SetControlMode(static_cast(value->GetDouble())); - if(IsModePID(m_controlMode) && value->IsDouble()) { - if(key == "p") SetP(value->GetDouble()); - if(key == "i") SetI(value->GetDouble()); - if(key == "d") SetD(value->GetDouble()); + if (key == "Mode" && value->IsDouble()) + SetControlMode( + static_cast(value->GetDouble())); + if (IsModePID(m_controlMode) && value->IsDouble()) { + if (key == "p") SetP(value->GetDouble()); + if (key == "i") SetI(value->GetDouble()); + if (key == "d") SetD(value->GetDouble()); } - if(key == "Enabled" && value->IsBoolean()) { - if (value->GetBoolean()) { - EnableControl(); - } else { - DisableControl(); - } + if (key == "Enabled" && value->IsBoolean()) { + if (value->GetBoolean()) { + EnableControl(); + } else { + DisableControl(); + } } - if(key == "Value" && value->IsDouble()) Set(value->GetDouble()); + if (key == "Value" && value->IsDouble()) Set(value->GetDouble()); } bool CANJaguar::IsModePID(CANSpeedController::ControlMode mode) const { @@ -1987,9 +2002,9 @@ void CANJaguar::UpdateTable() { m_table->PutString("Type", "CANJaguar"); m_table->PutNumber("Mode", m_controlMode); if (IsModePID(m_controlMode)) { - m_table->PutNumber("p", GetP()); - m_table->PutNumber("i", GetI()); - m_table->PutNumber("d", GetD()); + m_table->PutNumber("p", GetP()); + m_table->PutNumber("i", GetI()); + m_table->PutNumber("d", GetD()); } m_table->PutBoolean("Enabled", m_controlEnabled); m_table->PutNumber("Value", Get()); diff --git a/wpilibc/Athena/src/CANTalon.cpp b/wpilibc/Athena/src/CANTalon.cpp index 9adc66c37f..e94513fddd 100644 --- a/wpilibc/Athena/src/CANTalon.cpp +++ b/wpilibc/Athena/src/CANTalon.cpp @@ -6,10 +6,10 @@ /*----------------------------------------------------------------------------*/ #include "CANTalon.h" -#include "WPIErrors.h" #include // usleep #include #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" /** * Number of adc engineering units per 0 to 3.3V sweep. @@ -25,10 +25,11 @@ const double kNativePwdUnitsPerRotation = 4096.0; * Number of minutes per 100ms unit. Useful for scaling velocities * measured by Talon's 100ms timebase to rotations per minute. */ -const double kMinutesPer100msUnit = 1.0/600.0; +const double kMinutesPer100msUnit = 1.0 / 600.0; /** * Constructor for the CANTalon device. + * * @param deviceNumber The CAN ID of the Talon SRX */ CANTalon::CANTalon(int deviceNumber) @@ -42,13 +43,14 @@ CANTalon::CANTalon(int deviceNumber) } /** * Constructor for the CANTalon device. - * @param deviceNumber The CAN ID of the Talon SRX + * + * @param deviceNumber The CAN ID of the Talon SRX * @param controlPeriodMs The period in ms to send the CAN control frame. * Period is bounded to [1ms,95ms]. */ CANTalon::CANTalon(int deviceNumber, int controlPeriodMs) : m_deviceNumber(deviceNumber), - m_impl(new CanTalonSRX(deviceNumber,controlPeriodMs)), + m_impl(new CanTalonSRX(deviceNumber, controlPeriodMs)), m_safetyHelper(new MotorSafetyHelper(this)) { ApplyControlMode(m_controlMode); m_impl->SetProfileSlotSelect(m_profile); @@ -62,13 +64,13 @@ CANTalon::~CANTalon() { } /** -* Write out the PID value as seen in the PIDOutput base object. -* -* @deprecated Call Set instead. -* -* @param output Write out the PercentVbus value as was computed by the -* PIDController -*/ + * Write out the PID value as seen in the PIDOutput base object. + * + * @deprecated Call Set instead. + * + * @param output Write out the PercentVbus value as was computed by the + * PIDController + */ void CANTalon::PIDWrite(float output) { if (GetControlMode() == kPercentVbus) { Set(output); @@ -124,7 +126,7 @@ float CANTalon::Get() const { * In Current mode, output value is in amperes. * In Speed mode, output value is in position change / 10ms. * In Position mode, output value is in encoder ticks or an analog value, - * depending on the sensor. + * depending on the sensor. * In Follower mode, the output value is the integer device ID of the talon to * duplicate. * @@ -141,7 +143,7 @@ void CANTalon::Set(float value, uint8_t syncGroup) { } if (m_controlEnabled) { - m_setPoint = value; /* cache set point for GetSetpoint() */ + m_setPoint = value; /* cache set point for GetSetpoint() */ CTR_Code status = CTR_OKAY; switch (m_controlMode) { case CANSpeedController::kPercentVbus: { @@ -158,10 +160,12 @@ void CANTalon::Set(float value, uint8_t syncGroup) { } break; case CANSpeedController::kSpeed: /* if the caller has provided scaling info, apply it */ - status = m_impl->SetDemand(ScaleVelocityToNativeUnits(m_feedbackDevice, m_isInverted ? -value : value)); + status = m_impl->SetDemand(ScaleVelocityToNativeUnits( + m_feedbackDevice, m_isInverted ? -value : value)); break; case CANSpeedController::kPosition: - status = m_impl->SetDemand(ScaleRotationsToNativeUnits(m_feedbackDevice, value)); + status = m_impl->SetDemand( + ScaleRotationsToNativeUnits(m_feedbackDevice, value)); break; case CANSpeedController::kCurrent: { double milliamperes = (m_isInverted ? -value : value) * 1000.0; /* mA*/ @@ -270,6 +274,7 @@ void CANTalon::SetD(double d) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * Set the feedforward value of the currently selected profile. * @@ -282,9 +287,10 @@ void CANTalon::SetF(double f) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * Set the Izone to a nonzero value to auto clear the integral accumulator - * when the absolute value of CloseLoopError exceeds Izone. + * when the absolute value of CloseLoopError exceeds Izone. * * @see SelectProfileSlot to choose between the two sets of gains. */ @@ -294,6 +300,7 @@ void CANTalon::SetIzone(unsigned iz) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * SRX has two available slots for PID. * @param slotIdx one or zero depending on which slot caller wants. @@ -305,21 +312,23 @@ void CANTalon::SelectProfileSlot(int slotIdx) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * Sets control values for closed loop control. * * @param p Proportional constant. * @param i Integration constant. * @param d Differential constant. + * * This function does not modify F-gain. Considerable passing a zero for f - * using - * the four-parameter function. + * using the four-parameter function. */ void CANTalon::SetPID(double p, double i, double d) { SetP(p); SetI(i); SetD(d); } + /** * Sets control values for closed loop control. * @@ -334,11 +343,14 @@ void CANTalon::SetPID(double p, double i, double d, double f) { SetD(d); SetF(f); } + /** * Select the feedback device to use in closed-loop */ void CANTalon::SetFeedbackDevice(FeedbackDevice feedbackDevice) { - /* save the selection so that future setters/getters know which scalars to apply */ + /* save the selection so that future setters/getters know which scalars to + * apply + */ m_feedbackDevice = feedbackDevice; /* pass feedback to actual CAN frame */ CTR_Code status = m_impl->SetFeedbackDeviceSelect((int)feedbackDevice); @@ -346,6 +358,7 @@ void CANTalon::SetFeedbackDevice(FeedbackDevice feedbackDevice) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * Select the feedback device to use in closed-loop */ @@ -422,6 +435,7 @@ double CANTalon::GetD() const { } return d; } + /** * * @see SelectProfileSlot to choose between the two sets of gains. @@ -443,6 +457,7 @@ double CANTalon::GetF() const { } return f; } + /** * @see SelectProfileSlot to choose between the two sets of gains. */ @@ -523,9 +538,11 @@ float CANTalon::GetTemperature() const { } return temp; } + /** * Set the position value of the selected sensor. This is useful for zero-ing * quadrature encoders. + * * Continuous sensors (like analog encoderes) can also partially be set (the * portion of the postion based on overflows). */ @@ -536,16 +553,16 @@ void CANTalon::SetPosition(double pos) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * TODO documentation (see CANJaguar.cpp) * * @return The position of the sensor currently providing feedback. - * When using analog sensors, 0 units corresponds to 0V, 1023 - * units corresponds to 3.3V - * When using an analog encoder (wrapping around 1023 => 0 is - * possible) the units are still 3.3V per 1023 units. - * When using quadrature, each unit is a quadrature edge (4X) - * mode. + * When using analog sensors, 0 units corresponds to 0V, 1023 + * units corresponds to 3.3V. + * When using an analog encoder (wrapping around 1023 => 0 is + * possible) the units are still 3.3V per 1023 units. + * When using quadrature, each unit is a quadrature edge (4X) mode. */ double CANTalon::GetPosition() const { int32_t position; @@ -555,6 +572,7 @@ double CANTalon::GetPosition() const { } return ScaleNativeUnitsToRotations(m_feedbackDevice, position); } + /** * If sensor and motor are out of phase, sensor can be inverted * (position and velocity multiplied by -1). @@ -566,6 +584,7 @@ void CANTalon::SetSensorDirection(bool reverseSensor) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * Flips the sign (multiplies by negative one) the throttle values going into * the motor on the talon in closed loop modes. Typically the application @@ -583,6 +602,7 @@ void CANTalon::SetClosedLoopOutputDirection(bool reverseOutput) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * Returns the current error in the controller. * @@ -597,14 +617,17 @@ int CANTalon::GetClosedLoopError() const { } return error; } + /** * Set the allowable closed loop error. - * @param allowableCloseLoopError allowable closed loop error for selected profile. - * mA for Curent closed loop. - * Talon Native Units for position and velocity. + * + * @param allowableCloseLoopError allowable closed loop error for selected + * profile. + * + * Units: mA for Curent closed loop. + * Talon Native Units for position and velocity. */ -void CANTalon::SetAllowableClosedLoopErr(uint32_t allowableCloseLoopError) -{ +void CANTalon::SetAllowableClosedLoopErr(uint32_t allowableCloseLoopError) { /* grab param enum */ CanTalonSRX::param_t param; if (m_profile == 1) { @@ -623,17 +646,12 @@ void CANTalon::SetAllowableClosedLoopErr(uint32_t allowableCloseLoopError) * * The speed units will be in the sensor's native ticks per 100ms. * - * For analog sensors, 3.3V corresponds to 1023 units. - * So a speed of 200 equates to ~0.645 dV per 100ms or 6.451 dV per - * second. - * If this is an analog encoder, that likely means 1.9548 rotations - * per sec. - * For quadrature encoders, each unit corresponds a quadrature edge (4X). - * So a 250 count encoder will produce 1000 edge events per - * rotation. - * An example speed of 200 would then equate to 20% of a rotation - * per 100ms, - * or 10 rotations per second. + * For analog sensors, 3.3V corresponds to 1023 units. So a speed of 200 + * equates to ~0.645 dV per 100ms or 6.451 dV per second. If this is an analog + * encoder, that likely means 1.9548 rotations per sec. For quadrature + * encoders, each unit corresponds a quadrature edge (4X). So a 250 count + * encoder will produce 1000 edge events per rotation. An example speed of 200 + * would then equate to 20% of a rotation per 100ms, or 10 rotations per second. */ double CANTalon::GetSpeed() const { int32_t speed; @@ -649,9 +667,8 @@ double CANTalon::GetSpeed() const { * whether it is actually being used for feedback. * * @returns The 24bit analog value. The bottom ten bits is the ADC (0 - 1023) - * on the analog pin of the Talon. - * The upper 14 bits tracks the overflows and - * underflows (continuous sensor). + * on the analog pin of the Talon. The upper 14 bits tracks the + * overflows and underflows (continuous sensor). */ int CANTalon::GetAnalogIn() const { int position; @@ -668,6 +685,7 @@ void CANTalon::SetAnalogPosition(int newPosition) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * Get the position of whatever is in the analog pin of the Talon, regardless of * whether it is actually being used for feedback. @@ -675,6 +693,7 @@ void CANTalon::SetAnalogPosition(int newPosition) { * @returns The ADC (0 - 1023) on analog pin of the Talon. */ int CANTalon::GetAnalogInRaw() const { return GetAnalogIn() & 0x3FF; } + /** * Get the position of whatever is in the analog pin of the Talon, regardless of * whether it is actually being used for feedback. @@ -732,43 +751,40 @@ int CANTalon::GetPulseWidthPosition() const { wpi_setErrorWithContext(status, getHALErrorMessage(status)); return param; } -void CANTalon::SetPulseWidthPosition(int newPosition) -{ +void CANTalon::SetPulseWidthPosition(int newPosition) { CTR_Code status = m_impl->SetParam(CanTalonSRX::ePwdPosition, newPosition); if (status != CTR_OKAY) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } -int CANTalon::GetPulseWidthVelocity()const -{ +int CANTalon::GetPulseWidthVelocity() const { int param; CTR_Code status = m_impl->GetPulseWidthVelocity(param); if (status != CTR_OKAY) wpi_setErrorWithContext(status, getHALErrorMessage(status)); return param; } -int CANTalon::GetPulseWidthRiseToFallUs()const -{ +int CANTalon::GetPulseWidthRiseToFallUs() const { int param; CTR_Code status = m_impl->GetPulseWidthRiseToFallUs(param); if (status != CTR_OKAY) wpi_setErrorWithContext(status, getHALErrorMessage(status)); return param; } -int CANTalon::GetPulseWidthRiseToRiseUs()const -{ +int CANTalon::GetPulseWidthRiseToRiseUs() const { int param; CTR_Code status = m_impl->GetPulseWidthRiseToRiseUs(param); if (status != CTR_OKAY) wpi_setErrorWithContext(status, getHALErrorMessage(status)); return param; } + /** * @param which feedback sensor to check it if is connected. * @return status of caller's specified sensor type. */ -CANTalon::FeedbackDeviceStatus CANTalon::IsSensorPresent(FeedbackDevice feedbackDevice)const -{ +CANTalon::FeedbackDeviceStatus CANTalon::IsSensorPresent( + FeedbackDevice feedbackDevice) const { FeedbackDeviceStatus retval = FeedbackStatusUnknown; int param; /* detecting sensor health depends on which sensor caller cares about */ @@ -803,6 +819,7 @@ CANTalon::FeedbackDeviceStatus CANTalon::IsSensorPresent(FeedbackDevice feedback } return retval; } + /** * @return IO level of QUADA pin. */ @@ -814,6 +831,7 @@ int CANTalon::GetPinStateQuadA() const { } return retval; } + /** * @return IO level of QUADB pin. */ @@ -825,6 +843,7 @@ int CANTalon::GetPinStateQuadB() const { } return retval; } + /** * @return IO level of QUAD Index pin. */ @@ -836,6 +855,7 @@ int CANTalon::GetPinStateQuadIdx() const { } return retval; } + /** * @return '1' iff forward limit switch is closed, 0 iff switch is open. * This function works regardless if limit switch feature is enabled. @@ -849,6 +869,7 @@ int CANTalon::IsFwdLimitSwitchClosed() const { } return retval ? 0 : 1; } + /** * @return '1' iff reverse limit switch is closed, 0 iff switch is open. * This function works regardless if limit switch feature is enabled. @@ -862,6 +883,7 @@ int CANTalon::IsRevLimitSwitchClosed() const { } return retval ? 0 : 1; } + /* * Simple accessor for tracked rise eventso index pin. * @return number of rising edges on idx pin. @@ -875,9 +897,10 @@ int CANTalon::GetNumberOfQuadIdxRises() const { } return rises; } + /* * @param rises integral value to set into index-rises register. Great way to - * zero the index count. + * zero the index count. */ void CANTalon::SetNumberOfQuadIdxRises(int rises) { CTR_Code status = m_impl->SetParam( @@ -887,6 +910,7 @@ void CANTalon::SetNumberOfQuadIdxRises(int rises) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * TODO documentation (see CANJaguar.cpp) */ @@ -977,6 +1001,7 @@ uint16_t CANTalon::GetFaults() const { return retval; } + uint16_t CANTalon::GetStickyFaults() const { uint16_t retval = 0; int val; @@ -1026,6 +1051,7 @@ uint16_t CANTalon::GetStickyFaults() const { return retval; } + void CANTalon::ClearStickyFaults() { CTR_Code status = m_impl->ClearStickyFaults(); wpi_setErrorWithContext(status, getHALErrorMessage(status)); @@ -1033,16 +1059,14 @@ void CANTalon::ClearStickyFaults() { /** * Set the maximum voltage change rate. This ramp rate is in affect regardless - * of which control mode - * the TALON is in. + * of which control mode the TALON is in. * * When in PercentVbus or Voltage output mode, the rate at which the voltage - * changes can - * be limited to reduce current spikes. Set this to 0.0 to disable rate - * limiting. + * changes can be limited to reduce current spikes. Set this to 0.0 to disable + * rate limiting. * - * @param rampRate The maximum rate of voltage change in Percent Voltage mode in - * V/s. + * @param rampRate The maximum rate of voltage change in Percent Voltage mode + * in V/s. */ void CANTalon::SetVoltageRampRate(double rampRate) { /* Caller is expressing ramp as Voltage per sec, assuming 12V is full. @@ -1054,6 +1078,7 @@ void CANTalon::SetVoltageRampRate(double rampRate) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + void CANTalon::SetVoltageCompensationRampRate(double rampRate) { /* when in voltage compensation mode, the voltage compensation rate directly caps the change in target voltage */ @@ -1063,13 +1088,14 @@ void CANTalon::SetVoltageCompensationRampRate(double rampRate) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * Sets a voltage change rate that applies only when a close loop contorl mode * is enabled. * This allows close loop specific ramp behavior. * - * @param rampRate The maximum rate of voltage change in Percent Voltage mode in - * V/s. + * @param rampRate The maximum rate of voltage change in Percent Voltage mode + * in V/s. */ void CANTalon::SetCloseLoopRampRate(double rampRate) { CTR_Code status = m_impl->SetCloseLoopRampRate( @@ -1103,6 +1129,7 @@ uint32_t CANTalon::GetFirmwareVersion() const { return firmwareVersion; } + /** * @return The accumulator for I gain. */ @@ -1119,6 +1146,7 @@ int CANTalon::GetIaccum() const { } return iaccum; } + /** * Clear the accumulator for I gain. */ @@ -1154,6 +1182,7 @@ void CANTalon::ConfigNeutralMode(NeutralMode mode) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * @return nonzero if brake is enabled during neutral. Zero if coast is enabled * during neutral. @@ -1166,17 +1195,21 @@ int CANTalon::GetBrakeEnableDuringNeutral() const { } return brakeEn; } + /** * Configure how many codes per revolution are generated by your encoder. * * @param codesPerRev The number of counts per revolution. */ void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev) { - /* first save the scalar so that all getters/setter work as the user expects */ + /* first save the scalar so that all getters/setter work as the user expects + */ m_codesPerRev = codesPerRev; - /* next send the scalar to the Talon over CAN. This is so that the Talon can report - it to whoever needs it, like the webdash. Don't bother checking the return, - this is only for instrumentation and is not necessary for Talon functionality. */ + /* next send the scalar to the Talon over CAN. This is so that the Talon can + * report it to whoever needs it, like the webdash. Don't bother checking + * the return, this is only for instrumentation and is not necessary for + * Talon functionality. + */ (void)m_impl->SetParam(CanTalonSRX::eNumberEncoderCPR, m_codesPerRev); } @@ -1186,11 +1219,14 @@ void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev) { * @param turns The number of turns of the potentiometer. */ void CANTalon::ConfigPotentiometerTurns(uint16_t turns) { - /* first save the scalar so that all getters/setter work as the user expects */ + /* first save the scalar so that all getters/setter work as the user expects + */ m_numPotTurns = turns; - /* next send the scalar to the Talon over CAN. This is so that the Talon can report - it to whoever needs it, like the webdash. Don't bother checking the return, - this is only for instrumentation and is not necessary for Talon functionality. */ + /* next send the scalar to the Talon over CAN. This is so that the Talon can + * report it to whoever needs it, like the webdash. Don't bother checking + * the return, this is only for instrumentation and is not necessary for + * Talon functionality. + */ (void)m_impl->SetParam(CanTalonSRX::eNumberPotTurns, m_numPotTurns); } @@ -1213,18 +1249,21 @@ void CANTalon::DisableSoftPositionLimits() { /** * Overrides the forward and reverse limit switch enables. - * Unlike ConfigLimitMode, this function allows individual control of forward and - * reverse limit switch enables. - * Unlike ConfigLimitMode, this function does not affect the soft-limit features of Talon SRX. + * + * Unlike ConfigLimitMode, this function allows individual control of forward + * and reverse limit switch enables. + * Unlike ConfigLimitMode, this function does not affect the soft-limit features + * of Talon SRX. * @see ConfigLimitMode() */ -void CANTalon::ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, bool bReverseLimitSwitchEn) { +void CANTalon::ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, + bool bReverseLimitSwitchEn) { CTR_Code status = CTR_OKAY; int fwdRevEnable; /* chose correct signal value based on caller's requests enables */ - if(!bForwardLimitSwitchEn) { + if (!bForwardLimitSwitchEn) { /* caller wants Forward Limit Switch OFF */ - if(!bReverseLimitSwitchEn) { + if (!bReverseLimitSwitchEn) { /* caller wants both OFF */ fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_DisableFwd_DisableRev; } else { @@ -1233,7 +1272,7 @@ void CANTalon::ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, bool bReve } } else { /* caller wants Forward Limit Switch ON */ - if(!bReverseLimitSwitchEn) { + if (!bReverseLimitSwitchEn) { /* caller wants Forward ON and Reverse OFF */ fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_EnableFwd_DisableRev; } else { @@ -1319,7 +1358,8 @@ void CANTalon::ConfigLimitMode(LimitMode mode) { */ void CANTalon::ConfigForwardLimit(double forwardLimitPosition) { CTR_Code status = CTR_OKAY; - int32_t nativeLimitPos = ScaleRotationsToNativeUnits(m_feedbackDevice, forwardLimitPosition); + int32_t nativeLimitPos = + ScaleRotationsToNativeUnits(m_feedbackDevice, forwardLimitPosition); status = m_impl->SetForwardSoftLimit(nativeLimitPos); if (status != CTR_OKAY) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); @@ -1328,7 +1368,9 @@ void CANTalon::ConfigForwardLimit(double forwardLimitPosition) { /** * Set the Forward Soft Limit Enable. + * * This is the same setting that is in the Web-Based Configuration. + * * @param bForwardSoftLimitEn true to enable Soft limit, false to disable. */ void CANTalon::ConfigForwardSoftLimitEnable(bool bForwardSoftLimitEn) { @@ -1341,7 +1383,9 @@ void CANTalon::ConfigForwardSoftLimitEnable(bool bForwardSoftLimitEn) { /** * Set the Reverse Soft Limit Enable. + * * This is the same setting that is in the Web-Based Configuration. + * * @param bReverseSoftLimitEn true to enable Soft limit, false to disable. */ void CANTalon::ConfigReverseSoftLimitEnable(bool bReverseSoftLimitEn) { @@ -1351,6 +1395,7 @@ void CANTalon::ConfigReverseSoftLimitEnable(bool bReverseSoftLimitEn) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * Change the fwd limit switch setting to normally open or closed. * Talon will disable momentarilly if the Talon's current setting @@ -1369,6 +1414,7 @@ void CANTalon::ConfigFwdLimitSwitchNormallyOpen(bool normallyOpen) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * Change the rev limit switch setting to normally open or closed. * Talon will disable momentarilly if the Talon's current setting @@ -1387,25 +1433,31 @@ void CANTalon::ConfigRevLimitSwitchNormallyOpen(bool normallyOpen) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * TODO documentation (see CANJaguar.cpp) */ void CANTalon::ConfigReverseLimit(double reverseLimitPosition) { CTR_Code status = CTR_OKAY; - int32_t nativeLimitPos = ScaleRotationsToNativeUnits(m_feedbackDevice, reverseLimitPosition); + int32_t nativeLimitPos = + ScaleRotationsToNativeUnits(m_feedbackDevice, reverseLimitPosition); status = m_impl->SetReverseSoftLimit(nativeLimitPos); if (status != CTR_OKAY) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * TODO documentation (see CANJaguar.cpp) */ void CANTalon::ConfigMaxOutputVoltage(double voltage) { - /* config peak throttle when in closed-loop mode in the fwd and rev direction. */ + /* config peak throttle when in closed-loop mode in the fwd and rev direction. + */ ConfigPeakOutputVoltage(voltage, -voltage); } -void CANTalon::ConfigPeakOutputVoltage(double forwardVoltage,double reverseVoltage) { + +void CANTalon::ConfigPeakOutputVoltage(double forwardVoltage, + double reverseVoltage) { /* bounds checking */ if (forwardVoltage > 12) forwardVoltage = 12; @@ -1419,7 +1471,9 @@ void CANTalon::ConfigPeakOutputVoltage(double forwardVoltage,double reverseVolta ConfigSetParameter(CanTalonSRX::ePeakPosOutput, 1023 * forwardVoltage / 12.0); ConfigSetParameter(CanTalonSRX::ePeakNegOutput, 1023 * reverseVoltage / 12.0); } -void CANTalon::ConfigNominalOutputVoltage(double forwardVoltage,double reverseVoltage) { + +void CANTalon::ConfigNominalOutputVoltage(double forwardVoltage, + double reverseVoltage) { /* bounds checking */ if (forwardVoltage > 12) forwardVoltage = 12; @@ -1430,9 +1484,12 @@ void CANTalon::ConfigNominalOutputVoltage(double forwardVoltage,double reverseVo else if (reverseVoltage < -12) reverseVoltage = -12; /* config calls */ - ConfigSetParameter(CanTalonSRX::eNominalPosOutput,1023*forwardVoltage/12.0); - ConfigSetParameter(CanTalonSRX::eNominalNegOutput,1023*reverseVoltage/12.0); + ConfigSetParameter(CanTalonSRX::eNominalPosOutput, + 1023 * forwardVoltage / 12.0); + ConfigSetParameter(CanTalonSRX::eNominalNegOutput, + 1023 * reverseVoltage / 12.0); } + /** * General set frame. Since the parameter is a general integral type, this can * be used for testing future features. @@ -1440,16 +1497,17 @@ void CANTalon::ConfigNominalOutputVoltage(double forwardVoltage,double reverseVo void CANTalon::ConfigSetParameter(uint32_t paramEnum, double value) { CTR_Code status; /* config peak throttle when in closed-loop mode in the positive direction. */ - status = m_impl->SetParam((CanTalonSRX::param_t)paramEnum,value); + status = m_impl->SetParam((CanTalonSRX::param_t)paramEnum, value); if (status != CTR_OKAY) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * General get frame. Since the parameter is a general integral type, this can * be used for testing future features. */ -bool CANTalon::GetParameter(uint32_t paramEnum, double & dvalue) const { +bool CANTalon::GetParameter(uint32_t paramEnum, double& dvalue) const { bool retval = true; /* send the request frame */ CTR_Code status = m_impl->RequestParam((CanTalonSRX::param_t)paramEnum); @@ -1516,6 +1574,7 @@ void CANTalon::ApplyControlMode(CANSpeedController::ControlMode mode) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** * TODO documentation (see CANJaguar.cpp) */ @@ -1553,29 +1612,32 @@ void CANTalon::SetSafetyEnabled(bool enabled) { } void CANTalon::GetDescription(std::ostringstream& desc) const { - desc << "CANTalon ID " << m_deviceNumber; + desc << "CANTalon ID " << m_deviceNumber; } + /** * @param devToLookup FeedbackDevice to lookup the scalar for. Because Talon - * allows multiple sensors to be attached simultaneously, caller must - * specify which sensor to lookup. - * @return The number of native Talon units per rotation of the selected sensor. - * Zero if the necessary sensor information is not available. + * allows multiple sensors to be attached simultaneously, + * caller must specify which sensor to lookup. + * @return The number of native Talon units per rotation of the selected + * sensor. Zero if the necessary sensor information is not available. * @see ConfigEncoderCodesPerRev * @see ConfigPotentiometerTurns */ -double CANTalon::GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup)const -{ +double CANTalon::GetNativeUnitsPerRotationScalar( + FeedbackDevice devToLookup) const { bool scalingAvail = false; CTR_Code status = CTR_OKAY; double retval = 0; switch (devToLookup) { - case QuadEncoder: - { /* When caller wants to lookup Quadrature, the QEI may be in 1x if the selected feedback is edge counter. - * Additionally if the quadrature source is the CTRE Mag encoder, then the CPR is known. - * This is nice in that the calling app does not require knowing the CPR at all. - * So do both checks here. - */ + case QuadEncoder: { /* When caller wants to lookup Quadrature, the QEI may + * be in 1x if the selected feedback is edge counter. + * Additionally if the quadrature source is the CTRE Mag + * encoder, then the CPR is known. + * This is nice in that the calling app does not require + * knowing the CPR at all. + * So do both checks here. + */ int32_t qeiPulsePerCount = 4; /* default to 4x */ switch (m_feedbackDevice) { case CtreMagEncoder_Relative: @@ -1590,7 +1652,8 @@ double CANTalon::GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup)cons qeiPulsePerCount = 1; break; case QuadEncoder: /* Talon's QEI is 4x */ - default: /* pulse width and everything else, assume its regular quad use. */ + default: /* pulse width and everything else, assume its regular quad + use. */ break; } if (scalingAvail) { @@ -1599,21 +1662,23 @@ double CANTalon::GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup)cons /* we couldn't deduce the scalar just based on the selection */ if (0 == m_codesPerRev) { /* caller has never set the CPR. Most likely caller - is just using engineering units so fall to the - bottom of this func.*/ + * is just using engineering units so fall to the + * bottom of this func. + */ } else { /* Talon expects PPR units */ retval = qeiPulsePerCount * m_codesPerRev; scalingAvail = true; } } - } break; + } break; case EncRising: case EncFalling: if (0 == m_codesPerRev) { /* caller has never set the CPR. Most likely caller - is just using engineering units so fall to the - bottom of this func.*/ + * is just using engineering units so fall to the + * bottom of this func. + */ } else { /* Talon expects PPR units */ retval = 1 * m_codesPerRev; @@ -1624,8 +1689,9 @@ double CANTalon::GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup)cons case AnalogEncoder: if (0 == m_numPotTurns) { /* caller has never set the CPR. Most likely caller - is just using engineering units so fall to the - bottom of this func.*/ + * is just using engineering units so fall to the + * bottom of this func. + */ } else { retval = (double)kNativeAdcUnitsPerRotation / m_numPotTurns; scalingAvail = true; @@ -1643,210 +1709,243 @@ double CANTalon::GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup)cons wpi_setErrorWithContext(status, getHALErrorMessage(status)); } /* if scaling information is not possible, signal caller - by returning zero */ - if (false == scalingAvail) - retval = 0; + * by returning zero + */ + if (false == scalingAvail) retval = 0; return retval; } + /** - * @param fullRotations double precision value representing number of rotations of selected feedback sensor. - * If user has never called the config routine for the selected sensor, then the caller - * is likely passing rotations in engineering units already, in which case it is returned - * as is. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev + * @param fullRotations double precision value representing number of + * rotations of selected feedback sensor. If user has + * never called the config routine for the selected + * sensor, then the caller is likely passing rotations + * in engineering units already, in which case it is + * returned as is. + * @see ConfigPotentiometerTurns + * @see ConfigEncoderCodesPerRev * @return fullRotations in native engineering units of the Talon SRX firmware. */ -int32_t CANTalon::ScaleRotationsToNativeUnits(FeedbackDevice devToLookup,double fullRotations)const -{ +int32_t CANTalon::ScaleRotationsToNativeUnits(FeedbackDevice devToLookup, + double fullRotations) const { /* first assume we don't have config info, prep the default return */ int32_t retval = (int32_t)fullRotations; /* retrieve scaling info */ double scalar = GetNativeUnitsPerRotationScalar(devToLookup); /* apply scalar if its available */ - if (scalar > 0) - retval = (int32_t)(fullRotations*scalar); + if (scalar > 0) retval = (int32_t)(fullRotations * scalar); return retval; } + /** - * @param rpm double precision value representing number of rotations per minute of selected feedback sensor. - * If user has never called the config routine for the selected sensor, then the caller - * is likely passing rotations in engineering units already, in which case it is returned - * as is. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return sensor velocity in native engineering units of the Talon SRX firmware. + * @param rpm double precision value representing number of rotations per + * minute of selected feedback sensor. If user has never called + * the config routine for the selected sensor, then the caller is + * likely passing rotations in engineering units already, in which + * case it is returned as is. + * @see ConfigPotentiometerTurns + * @see ConfigEncoderCodesPerRev + * @return sensor velocity in native engineering units of the Talon SRX + * firmware. */ -int32_t CANTalon::ScaleVelocityToNativeUnits(FeedbackDevice devToLookup,double rpm)const -{ +int32_t CANTalon::ScaleVelocityToNativeUnits(FeedbackDevice devToLookup, + double rpm) const { /* first assume we don't have config info, prep the default return */ int32_t retval = (int32_t)rpm; /* retrieve scaling info */ double scalar = GetNativeUnitsPerRotationScalar(devToLookup); /* apply scalar if its available */ - if (scalar > 0) - retval = (int32_t)(rpm * kMinutesPer100msUnit * scalar); + if (scalar > 0) retval = (int32_t)(rpm * kMinutesPer100msUnit * scalar); return retval; } + /** - * @param nativePos integral position of the feedback sensor in native Talon SRX units. - * If user has never called the config routine for the selected sensor, then the return - * will be in TALON SRX units as well to match the behavior in the 2015 season. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return double precision number of rotations, unless config was never performed. + * @param nativePos integral position of the feedback sensor in native Talon + * SRX units. If user has never called the config routine for + * the selected sensor, then the return will be in TALON SRX + * units as well to match the behavior in the 2015 season. + * @see ConfigPotentiometerTurns + * @see ConfigEncoderCodesPerRev + * @return double precision number of rotations, unless config was never + * performed. */ -double CANTalon::ScaleNativeUnitsToRotations(FeedbackDevice devToLookup,int32_t nativePos)const -{ +double CANTalon::ScaleNativeUnitsToRotations(FeedbackDevice devToLookup, + int32_t nativePos) const { /* first assume we don't have config info, prep the default return */ double retval = (double)nativePos; /* retrieve scaling info */ double scalar = GetNativeUnitsPerRotationScalar(devToLookup); /* apply scalar if its available */ - if (scalar > 0) - retval = ((double)nativePos) / scalar; + if (scalar > 0) retval = ((double)nativePos) / scalar; return retval; } + /** - * @param nativeVel integral velocity of the feedback sensor in native Talon SRX units. - * If user has never called the config routine for the selected sensor, then the return - * will be in TALON SRX units as well to match the behavior in the 2015 season. - * @see ConfigPotentiometerTurns - * @see ConfigEncoderCodesPerRev - * @return double precision of sensor velocity in RPM, unless config was never performed. + * @param nativeVel integral velocity of the feedback sensor in native Talon + * SRX units. If user has never called the config routine for + * the selected sensor, then the return will be in TALON SRX + * units as well to match the behavior in the 2015 season. + * @see ConfigPotentiometerTurns + * @see ConfigEncoderCodesPerRev + * @return double precision of sensor velocity in RPM, unless config was never + * performed. */ -double CANTalon::ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, int32_t nativeVel)const -{ +double CANTalon::ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, + int32_t nativeVel) const { /* first assume we don't have config info, prep the default return */ double retval = (double)nativeVel; /* retrieve scaling info */ double scalar = GetNativeUnitsPerRotationScalar(devToLookup); /* apply scalar if its available */ if (scalar > 0) - retval = (double)(nativeVel) / (scalar*kMinutesPer100msUnit); + retval = (double)(nativeVel) / (scalar * kMinutesPer100msUnit); return retval; } /** * Enables Talon SRX to automatically zero the Sensor Position whenever an * edge is detected on the index signal. - * @param enable boolean input, pass true to enable feature or false to disable. - * @param risingEdge boolean input, pass true to clear the position on rising edge, - * pass false to clear the position on falling edge. + * + * @param enable boolean input, pass true to enable feature or false to + * disable. + * @param risingEdge boolean input, pass true to clear the position on rising + * edge, pass false to clear the position on falling edge. */ -void CANTalon::EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge) -{ +void CANTalon::EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge) { if (enable) { /* enable the feature, update the edge polarity first to ensure it is correct before the feature is enabled. */ - ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity,risingEdge ? 1 : 0); - ConfigSetParameter(CanTalonSRX::eClearPositionOnIdx,1); + ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity, risingEdge ? 1 : 0); + ConfigSetParameter(CanTalonSRX::eClearPositionOnIdx, 1); } else { /* disable the feature first, then update the edge polarity. */ - ConfigSetParameter(CanTalonSRX::eClearPositionOnIdx,0); - ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity,risingEdge ? 1 : 0); + ConfigSetParameter(CanTalonSRX::eClearPositionOnIdx, 0); + ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity, risingEdge ? 1 : 0); } } /** - * Calling application can opt to speed up the handshaking between the robot API and the Talon to increase the - * download rate of the Talon's Motion Profile. Ideally the period should be no more than half the period - * of a trajectory point. + * Calling application can opt to speed up the handshaking between the robot API + * and the Talon to increase the download rate of the Talon's Motion Profile. + * Ideally the period should be no more than half the period of a trajectory + * point. */ -void CANTalon::ChangeMotionControlFramePeriod(int periodMs) -{ +void CANTalon::ChangeMotionControlFramePeriod(int periodMs) { m_impl->ChangeMotionControlFramePeriod(periodMs); } /** - * Clear the buffered motion profile in both Talon RAM (bottom), and in the API (top). - * Be sure to check GetMotionProfileStatus() to know when the buffer is actually cleared. + * Clear the buffered motion profile in both Talon RAM (bottom), and in the API + * (top). + * + * Be sure to check GetMotionProfileStatus() to know when the buffer is actually + * cleared. */ -void CANTalon::ClearMotionProfileTrajectories() -{ +void CANTalon::ClearMotionProfileTrajectories() { m_impl->ClearMotionProfileTrajectories(); } /** * Retrieve just the buffer count for the api-level (top) buffer. + * * This routine performs no CAN or data structure lookups, so its fast and ideal - * if caller needs to quickly poll the progress of trajectory points being emptied - * into Talon's RAM. Otherwise just use GetMotionProfileStatus. + * if caller needs to quickly poll the progress of trajectory points being + * emptied into Talon's RAM. Otherwise just use GetMotionProfileStatus. + * * @return number of trajectory points in the top buffer. */ -int CANTalon::GetMotionProfileTopLevelBufferCount() -{ +int CANTalon::GetMotionProfileTopLevelBufferCount() { return m_impl->GetMotionProfileTopLevelBufferCount(); } /** - * Push another trajectory point into the top level buffer (which is emptied into - * the Talon's bottom buffer as room allows). + * Push another trajectory point into the top level buffer (which is emptied + * into the Talon's bottom buffer as room allows). + * * @param trajPt the trajectory point to insert into buffer. - * @return true if trajectory point push ok. CTR_BufferFull if buffer is full - * due to kMotionProfileTopBufferCapacity. + * @return true if trajectory point push ok. CTR_BufferFull if buffer is full + * due to kMotionProfileTopBufferCapacity. */ -bool CANTalon::PushMotionProfileTrajectory(const TrajectoryPoint & trajPt) -{ +bool CANTalon::PushMotionProfileTrajectory(const TrajectoryPoint& trajPt) { /* convert positiona and velocity to native units */ - int32_t targPos = ScaleRotationsToNativeUnits(m_feedbackDevice, trajPt.position); - int32_t targVel = ScaleVelocityToNativeUnits(m_feedbackDevice, trajPt.velocity); + int32_t targPos = + ScaleRotationsToNativeUnits(m_feedbackDevice, trajPt.position); + int32_t targVel = + ScaleVelocityToNativeUnits(m_feedbackDevice, trajPt.velocity); /* bounds check signals that require it */ uint32_t profileSlotSelect = (trajPt.profileSlotSelect) ? 1 : 0; - uint8_t timeDurMs = (trajPt.timeDurMs >= 255) ? 255 : trajPt.timeDurMs; /* cap time to 255ms */ + uint8_t timeDurMs = (trajPt.timeDurMs >= 255) + ? 255 + : trajPt.timeDurMs; /* cap time to 255ms */ /* send it to the top level buffer */ - CTR_Code status = m_impl->PushMotionProfileTrajectory(targPos, targVel, profileSlotSelect, timeDurMs, trajPt.velocityOnly, trajPt.isLastPoint, trajPt.zeroPos); + CTR_Code status = m_impl->PushMotionProfileTrajectory( + targPos, targVel, profileSlotSelect, timeDurMs, trajPt.velocityOnly, + trajPt.isLastPoint, trajPt.zeroPos); return (status == CTR_OKAY) ? true : false; } + /** * @return true if api-level (top) buffer is full. */ -bool CANTalon::IsMotionProfileTopLevelBufferFull() -{ +bool CANTalon::IsMotionProfileTopLevelBufferFull() { return m_impl->IsMotionProfileTopLevelBufferFull(); } /** - * This must be called periodically to funnel the trajectory points from the API's top level buffer to - * the Talon's bottom level buffer. Recommendation is to call this twice as fast as the executation rate of the motion profile. - * So if MP is running with 20ms trajectory points, try calling this routine every 10ms. All motion profile functions are thread-safe - * through the use of a mutex, so there is no harm in having the caller utilize threading. + * This must be called periodically to funnel the trajectory points from the + * API's top level buffer to the Talon's bottom level buffer. Recommendation + * is to call this twice as fast as the executation rate of the motion profile. + * So if MP is running with 20ms trajectory points, try calling this routine + * every 10ms. All motion profile functions are thread-safe through the use of + * a mutex, so there is no harm in having the caller utilize threading. */ -void CANTalon::ProcessMotionProfileBuffer() -{ +void CANTalon::ProcessMotionProfileBuffer() { m_impl->ProcessMotionProfileBuffer(); } /** * Retrieve all status information. - * Since this all comes from one CAN frame, its ideal to have one routine to retrieve the frame once and decode everything. - * @param [out] motionProfileStatus contains all progress information on the currently running MP. + * + * Since this all comes from one CAN frame, its ideal to have one routine to + * retrieve the frame once and decode everything. + * + * @param [out] motionProfileStatus contains all progress information on the + * currently running MP. */ -void CANTalon::GetMotionProfileStatus(MotionProfileStatus & motionProfileStatus) -{ +void CANTalon::GetMotionProfileStatus( + MotionProfileStatus& motionProfileStatus) { uint32_t flags; uint32_t profileSlotSelect; int32_t targPos, targVel; uint32_t topBufferRem, topBufferCnt, btmBufferCnt; uint32_t outputEnable; /* retrieve all motion profile signals from status frame */ - CTR_Code status = m_impl->GetMotionProfileStatus(flags, profileSlotSelect, targPos, targVel, topBufferRem, topBufferCnt, btmBufferCnt, outputEnable); + CTR_Code status = m_impl->GetMotionProfileStatus( + flags, profileSlotSelect, targPos, targVel, topBufferRem, topBufferCnt, + btmBufferCnt, outputEnable); /* completely update the caller's structure */ motionProfileStatus.topBufferRem = topBufferRem; motionProfileStatus.topBufferCnt = topBufferCnt; motionProfileStatus.btmBufferCnt = btmBufferCnt; - motionProfileStatus.hasUnderrun = (flags & CanTalonSRX::kMotionProfileFlag_HasUnderrun) ? true :false; - motionProfileStatus.isUnderrun = (flags & CanTalonSRX::kMotionProfileFlag_IsUnderrun) ? true :false; - motionProfileStatus.activePointValid = (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_IsValid) ? true :false; - motionProfileStatus.activePoint.isLastPoint = (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_IsLast) ? true :false; - motionProfileStatus.activePoint.velocityOnly = (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_VelOnly) ? true :false; - motionProfileStatus.activePoint.position = ScaleNativeUnitsToRotations(m_feedbackDevice, targPos); - motionProfileStatus.activePoint.velocity = ScaleNativeUnitsToRpm(m_feedbackDevice, targVel); + motionProfileStatus.hasUnderrun = + (flags & CanTalonSRX::kMotionProfileFlag_HasUnderrun) ? true : false; + motionProfileStatus.isUnderrun = + (flags & CanTalonSRX::kMotionProfileFlag_IsUnderrun) ? true : false; + motionProfileStatus.activePointValid = + (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_IsValid) ? true : false; + motionProfileStatus.activePoint.isLastPoint = + (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_IsLast) ? true : false; + motionProfileStatus.activePoint.velocityOnly = + (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_VelOnly) ? true : false; + motionProfileStatus.activePoint.position = + ScaleNativeUnitsToRotations(m_feedbackDevice, targPos); + motionProfileStatus.activePoint.velocity = + ScaleNativeUnitsToRpm(m_feedbackDevice, targVel); motionProfileStatus.activePoint.profileSlotSelect = profileSlotSelect; - switch(outputEnable){ + switch (outputEnable) { case CanTalonSRX::kMotionProf_Disabled: motionProfileStatus.outputEnable = SetValueMotionProfileDisable; - break; + break; case CanTalonSRX::kMotionProf_Enable: motionProfileStatus.outputEnable = SetValueMotionProfileEnable; break; @@ -1857,67 +1956,75 @@ void CANTalon::GetMotionProfileStatus(MotionProfileStatus & motionProfileStatus) motionProfileStatus.outputEnable = SetValueMotionProfileDisable; break; } - motionProfileStatus.activePoint.zeroPos = false; /* this signal is only used sending pts to Talon */ - motionProfileStatus.activePoint.timeDurMs = 0; /* this signal is only used sending pts to Talon */ + motionProfileStatus.activePoint.zeroPos = + false; /* this signal is only used sending pts to Talon */ + motionProfileStatus.activePoint.timeDurMs = + 0; /* this signal is only used sending pts to Talon */ if (status != CTR_OKAY) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } + /** - * Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is ready for another point, - * but the low level buffer is empty. + * Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is + * ready for another point, but the low level buffer is empty. * - * Once the Motion Profile Executer sets the hasUnderrun flag, it stays set until - * Robot Application clears it with this routine, which ensures Robot Application - * gets a chance to instrument or react. Caller could also check the isUnderrun flag - * which automatically clears when fault condition is removed. + * Once the Motion Profile Executer sets the hasUnderrun flag, it stays set + * until Robot Application clears it with this routine, which ensures Robot + * Application gets a chance to instrument or react. Caller could also check + * the isUnderrun flag which automatically clears when fault condition is + * removed. */ -void CANTalon::ClearMotionProfileHasUnderrun() -{ +void CANTalon::ClearMotionProfileHasUnderrun() { ConfigSetParameter(CanTalonSRX::eMotionProfileHasUnderrunErr, 0); } + /** -* Common interface for inverting direction of a speed controller. -* Only works in PercentVbus, speed, and Voltage modes. -* @param isInverted The state of inversion, true is inverted. -*/ + * Common interface for inverting direction of a speed controller. + * + * Only works in PercentVbus, speed, and Voltage modes. + * + * @param isInverted The state of inversion, true is inverted. + */ void CANTalon::SetInverted(bool isInverted) { m_isInverted = isInverted; } /** * Common interface for the inverting direction of a speed controller. * * @return isInverted The state of inversion, true is inverted. - * */ bool CANTalon::GetInverted() const { return m_isInverted; } /** - * Common interface for stopping the motor until the next Set() call - * Part of the MotorSafety interface + * Common interface for stopping the motor until the next Set() call. + * + * Part of the MotorSafety interface. * * @deprecated Call Disable instead. -*/ + */ void CANTalon::StopMotor() { - Disable(); - m_stopped = true; + Disable(); + m_stopped = true; } void CANTalon::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) { - if(key == "Mode" && value->IsDouble()) SetControlMode(static_cast(value->GetDouble())); - if(key == "p" && value->IsDouble()) SetP(value->GetDouble()); - if(key == "i" && value->IsDouble()) SetI(value->GetDouble()); - if(key == "d" && value->IsDouble()) SetD(value->GetDouble()); - if(key == "f" && value->IsDouble()) SetF(value->GetDouble()); - if(key == "Enabled" && value->IsBoolean()) { - if (value->GetBoolean()) { - Enable(); - } else { - Disable(); - } + if (key == "Mode" && value->IsDouble()) + SetControlMode( + static_cast(value->GetDouble())); + if (key == "p" && value->IsDouble()) SetP(value->GetDouble()); + if (key == "i" && value->IsDouble()) SetI(value->GetDouble()); + if (key == "d" && value->IsDouble()) SetD(value->GetDouble()); + if (key == "f" && value->IsDouble()) SetF(value->GetDouble()); + if (key == "Enabled" && value->IsBoolean()) { + if (value->GetBoolean()) { + Enable(); + } else { + Disable(); + } } - if(key == "Value" && value->IsDouble()) Set(value->GetDouble()); + if (key == "Value" && value->IsDouble()) Set(value->GetDouble()); } bool CANTalon::IsModePID(CANSpeedController::ControlMode mode) const { diff --git a/wpilibc/Athena/src/CameraServer.cpp b/wpilibc/Athena/src/CameraServer.cpp index 5906cac397..d66774ab38 100644 --- a/wpilibc/Athena/src/CameraServer.cpp +++ b/wpilibc/Athena/src/CameraServer.cpp @@ -6,15 +6,15 @@ /*----------------------------------------------------------------------------*/ #include "CameraServer.h" -#include "WPIErrors.h" #include "Utility.h" +#include "WPIErrors.h" -#include -#include -#include +#include #include #include -#include +#include +#include +#include constexpr uint8_t CameraServer::kMagicNumber[]; @@ -210,8 +210,7 @@ void CameraServer::Serve() { req.size = ntohl(req.size); // TODO: Support the SW Compression. The rest of the code below will work as - // though this - // check isn't here + // though this check isn't here if (req.compression != kHardwareCompression) { wpi_setWPIErrorWithContext(IncompatibleState, "Choose \"USB Camera HW\" on the dashboard"); diff --git a/wpilibc/Athena/src/Compressor.cpp b/wpilibc/Athena/src/Compressor.cpp index 9ab89e0584..369cf6eda5 100644 --- a/wpilibc/Athena/src/Compressor.cpp +++ b/wpilibc/Athena/src/Compressor.cpp @@ -6,7 +6,7 @@ #include "WPIErrors.h" /** - * Constructor + * Constructor. * * @param module The PCM ID to use (0-62) */ @@ -16,19 +16,20 @@ Compressor::Compressor(uint8_t pcmID) { } /** - * Starts closed-loop control. Note that closed loop control is enabled by + * Starts closed-loop control. Note that closed loop control is enabled by * default. */ void Compressor::Start() { SetClosedLoopControl(true); } /** - * Stops closed-loop control. Note that closed loop control is enabled by + * Stops closed-loop control. Note that closed loop control is enabled by * default. */ void Compressor::Stop() { SetClosedLoopControl(false); } /** - * Check if compressor output is active + * Check if compressor output is active. + * * @return true if the compressor is on */ bool Compressor::Enabled() const { @@ -45,7 +46,8 @@ bool Compressor::Enabled() const { } /** - * Check if the pressure switch is triggered + * Check if the pressure switch is triggered. + * * @return true if pressure is low */ bool Compressor::GetPressureSwitchValue() const { @@ -62,7 +64,8 @@ bool Compressor::GetPressureSwitchValue() const { } /** - * Query how much current the compressor is drawing + * Query how much current the compressor is drawing. + * * @return The current through the compressor, in amps */ float Compressor::GetCompressorCurrent() const { @@ -81,8 +84,9 @@ float Compressor::GetCompressorCurrent() const { /** * Enables or disables automatically turning the compressor on when the * pressure is low. + * * @param on Set to true to enable closed loop control of the compressor. False - * to disable. + * to disable. */ void Compressor::SetClosedLoopControl(bool on) { int32_t status = 0; @@ -97,8 +101,9 @@ void Compressor::SetClosedLoopControl(bool on) { /** * Returns true if the compressor will automatically turn on when the * pressure is low. + * * @return True if closed loop control of the compressor is enabled. False if - * disabled. + * disabled. */ bool Compressor::GetClosedLoopControl() const { int32_t status = 0; @@ -115,8 +120,9 @@ bool Compressor::GetClosedLoopControl() const { /** * Query if the compressor output has been disabled due to high current draw. + * * @return true if PCM is in fault state : Compressor Drive is - * disabled due to compressor current being too high. + * disabled due to compressor current being too high. */ bool Compressor::GetCompressorCurrentTooHighFault() const { int32_t status = 0; @@ -130,13 +136,16 @@ bool Compressor::GetCompressorCurrentTooHighFault() const { return value; } + /** * Query if the compressor output has been disabled due to high current draw * (sticky). + * * A sticky fault will not clear on device reboot, it must be cleared through * code or the webdash. + * * @return true if PCM sticky fault is set : Compressor Drive is - * disabled due to compressor current being too high. + * disabled due to compressor current being too high. */ bool Compressor::GetCompressorCurrentTooHighStickyFault() const { int32_t status = 0; @@ -150,13 +159,16 @@ bool Compressor::GetCompressorCurrentTooHighStickyFault() const { return value; } + /** * Query if the compressor output has been disabled due to a short circuit * (sticky). + * * A sticky fault will not clear on device reboot, it must be cleared through * code or the webdash. + * * @return true if PCM sticky fault is set : Compressor output - * appears to be shorted. + * appears to be shorted. */ bool Compressor::GetCompressorShortedStickyFault() const { int32_t status = 0; @@ -170,10 +182,12 @@ bool Compressor::GetCompressorShortedStickyFault() const { return value; } + /** * Query if the compressor output has been disabled due to a short circuit. + * * @return true if PCM is in fault state : Compressor output - * appears to be shorted. + * appears to be shorted. */ bool Compressor::GetCompressorShortedFault() const { int32_t status = 0; @@ -187,13 +201,15 @@ bool Compressor::GetCompressorShortedFault() const { return value; } + /** * Query if the compressor output does not appear to be wired (sticky). + * * A sticky fault will not clear on device reboot, it must be cleared through * code or the webdash. + * * @return true if PCM sticky fault is set : Compressor does not - * appear to be wired, i.e. compressor is - * not drawing enough current. + * appear to be wired, i.e. compressor is not drawing enough current. */ bool Compressor::GetCompressorNotConnectedStickyFault() const { int32_t status = 0; @@ -207,11 +223,12 @@ bool Compressor::GetCompressorNotConnectedStickyFault() const { return value; } + /** * Query if the compressor output does not appear to be wired. + * * @return true if PCM is in fault state : Compressor does not - * appear to be wired, i.e. compressor is - * not drawing enough current. + * appear to be wired, i.e. compressor is not drawing enough current. */ bool Compressor::GetCompressorNotConnectedFault() const { int32_t status = 0; @@ -225,16 +242,14 @@ bool Compressor::GetCompressorNotConnectedFault() const { return value; } + /** * Clear ALL sticky faults inside PCM that Compressor is wired to. * * If a sticky fault is set, then it will be persistently cleared. Compressor - * drive - * maybe momentarily disable while flags are being cleared. Care - * should be - * taken to not call this too frequently, otherwise normal - * compressor - * functionality may be prevented. + * drive maybe momentarily disable while flags are being cleared. Care should + * be taken to not call this too frequently, otherwise normal compressor + * functionality may be prevented. * * If no sticky faults are set then this call will have no effect. */ @@ -247,6 +262,7 @@ void Compressor::ClearAllPCMStickyFaults() { wpi_setWPIError(Timeout); } } + void Compressor::UpdateTable() { if (m_table) { m_table->PutBoolean("Enabled", Enabled()); diff --git a/wpilibc/Athena/src/ControllerPower.cpp b/wpilibc/Athena/src/ControllerPower.cpp index a62539fbd7..1c50f275bf 100644 --- a/wpilibc/Athena/src/ControllerPower.cpp +++ b/wpilibc/Athena/src/ControllerPower.cpp @@ -8,12 +8,13 @@ #include "ControllerPower.h" #include -#include #include +#include #include "ErrorBase.h" /** - * Get the input voltage to the robot controller + * Get the input voltage to the robot controller. + * * @return The controller input voltage value in Volts */ double ControllerPower::GetInputVoltage() { @@ -24,7 +25,8 @@ double ControllerPower::GetInputVoltage() { } /** - * Get the input current to the robot controller + * Get the input current to the robot controller. + * * @return The controller input current value in Amps */ double ControllerPower::GetInputCurrent() { @@ -35,7 +37,8 @@ double ControllerPower::GetInputCurrent() { } /** - * Get the voltage of the 6V rail + * Get the voltage of the 6V rail. + * * @return The controller 6V rail voltage value in Volts */ double ControllerPower::GetVoltage6V() { @@ -46,7 +49,8 @@ double ControllerPower::GetVoltage6V() { } /** - * Get the current output of the 6V rail + * Get the current output of the 6V rail. + * * @return The controller 6V rail output current value in Amps */ double ControllerPower::GetCurrent6V() { @@ -58,8 +62,8 @@ double ControllerPower::GetCurrent6V() { /** * Get the enabled state of the 6V rail. The rail may be disabled due to a - * controller - * brownout, a short circuit on the rail, or controller over-voltage + * controller brownout, a short circuit on the rail, or controller over-voltage. + * * @return The controller 6V rail enabled value. True for enabled. */ bool ControllerPower::GetEnabled6V() { @@ -71,7 +75,8 @@ bool ControllerPower::GetEnabled6V() { /** * Get the count of the total current faults on the 6V rail since the controller - * has booted + * has booted. + * * @return The number of faults. */ int ControllerPower::GetFaultCount6V() { @@ -82,7 +87,8 @@ int ControllerPower::GetFaultCount6V() { } /** - * Get the voltage of the 5V rail + * Get the voltage of the 5V rail. + * * @return The controller 5V rail voltage value in Volts */ double ControllerPower::GetVoltage5V() { @@ -93,7 +99,8 @@ double ControllerPower::GetVoltage5V() { } /** - * Get the current output of the 5V rail + * Get the current output of the 5V rail. + * * @return The controller 5V rail output current value in Amps */ double ControllerPower::GetCurrent5V() { @@ -105,8 +112,8 @@ double ControllerPower::GetCurrent5V() { /** * Get the enabled state of the 5V rail. The rail may be disabled due to a - * controller - * brownout, a short circuit on the rail, or controller over-voltage + * controller brownout, a short circuit on the rail, or controller over-voltage. + * * @return The controller 5V rail enabled value. True for enabled. */ bool ControllerPower::GetEnabled5V() { @@ -118,7 +125,8 @@ bool ControllerPower::GetEnabled5V() { /** * Get the count of the total current faults on the 5V rail since the controller - * has booted + * has booted. + * * @return The number of faults */ int ControllerPower::GetFaultCount5V() { @@ -129,7 +137,8 @@ int ControllerPower::GetFaultCount5V() { } /** - * Get the voltage of the 3.3V rail + * Get the voltage of the 3.3V rail. + * * @return The controller 3.3V rail voltage value in Volts */ double ControllerPower::GetVoltage3V3() { @@ -140,7 +149,8 @@ double ControllerPower::GetVoltage3V3() { } /** - * Get the current output of the 3.3V rail + * Get the current output of the 3.3V rail. + * * @return The controller 3.3V rail output current value in Amps */ double ControllerPower::GetCurrent3V3() { @@ -152,8 +162,8 @@ double ControllerPower::GetCurrent3V3() { /** * Get the enabled state of the 3.3V rail. The rail may be disabled due to a - * controller - * brownout, a short circuit on the rail, or controller over-voltage + * controller brownout, a short circuit on the rail, or controller over-voltage. + * * @return The controller 3.3V rail enabled value. True for enabled. */ bool ControllerPower::GetEnabled3V3() { @@ -165,7 +175,8 @@ bool ControllerPower::GetEnabled3V3() { /** * Get the count of the total current faults on the 3.3V rail since the - * controller has booted + * controller has booted. + * * @return The number of faults */ int ControllerPower::GetFaultCount3V3() { @@ -173,4 +184,4 @@ int ControllerPower::GetFaultCount3V3() { int retVal = getUserCurrentFaults3V3(&status); wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status)); return retVal; -} \ No newline at end of file +} diff --git a/wpilibc/Athena/src/Counter.cpp b/wpilibc/Athena/src/Counter.cpp index a138867b06..59ea902879 100644 --- a/wpilibc/Athena/src/Counter.cpp +++ b/wpilibc/Athena/src/Counter.cpp @@ -13,14 +13,15 @@ /** * Create an instance of a counter where no sources are selected. + * * They all must be selected by calling functions to specify the upsource and - * the downsource - * independently. + * the downsource independently. * * This creates a ChipObject counter and initializes status variables * appropriately. * * The counter will start counting immediately. + * * @param mode The counter mode */ Counter::Counter(Mode mode) { @@ -36,16 +37,16 @@ Counter::Counter(Mode mode) { /** * Create an instance of a counter from a Digital Source (such as a Digital * Input). + * * This is used if an existing digital input is to be shared by multiple other - * objects such - * as encoders or if the Digital Source is not a Digital Input channel (such as - * an Analog Trigger). + * objects such as encoders or if the Digital Source is not a Digital Input + * channel (such as an Analog Trigger). * * The counter will start counting immediately. * @param source A pointer to the existing DigitalSource object. It will be set - * as the Up Source. + * as the Up Source. */ -Counter::Counter(DigitalSource *source) : Counter(kTwoPulse) { +Counter::Counter(DigitalSource* source) : Counter(kTwoPulse) { SetUpSource(source); ClearDownSource(); } @@ -53,14 +54,15 @@ Counter::Counter(DigitalSource *source) : Counter(kTwoPulse) { /** * Create an instance of a counter from a Digital Source (such as a Digital * Input). + * * This is used if an existing digital input is to be shared by multiple other - * objects such - * as encoders or if the Digital Source is not a Digital Input channel (such as - * an Analog Trigger). + * objects such as encoders or if the Digital Source is not a Digital Input + * channel (such as an Analog Trigger). * * The counter will start counting immediately. + * * @param source A pointer to the existing DigitalSource object. It will be - * set as the Up Source. + * set as the Up Source. */ Counter::Counter(std::shared_ptr source) : Counter(kTwoPulse) { SetUpSource(source); @@ -69,11 +71,13 @@ Counter::Counter(std::shared_ptr source) : Counter(kTwoPulse) { /** * Create an instance of a Counter object. + * * Create an up-Counter instance given a channel. * * The counter will start counting immediately. + * * @param channel The DIO channel to use as the up source. 0-9 are on-board, - * 10-25 are on the MXP + * 10-25 are on the MXP */ Counter::Counter(int32_t channel) : Counter(kTwoPulse) { SetUpSource(channel); @@ -82,55 +86,64 @@ Counter::Counter(int32_t channel) : Counter(kTwoPulse) { /** * Create an instance of a Counter object. + * * Create an instance of a simple up-Counter given an analog trigger. * Use the trigger state output from the analog trigger. * * The counter will start counting immediately. + * * @param trigger The pointer to the existing AnalogTrigger object. */ DEPRECATED("Use pass-by-reference instead.") -Counter::Counter(AnalogTrigger *trigger) : Counter(kTwoPulse) { +Counter::Counter(AnalogTrigger* trigger) : Counter(kTwoPulse) { SetUpSource(trigger->CreateOutput(kState)); ClearDownSource(); } /** * Create an instance of a Counter object. + * * Create an instance of a simple up-Counter given an analog trigger. * Use the trigger state output from the analog trigger. * * The counter will start counting immediately. + * * @param trigger The reference to the existing AnalogTrigger object. */ -Counter::Counter(const AnalogTrigger &trigger) : Counter(kTwoPulse) { +Counter::Counter(const AnalogTrigger& trigger) : Counter(kTwoPulse) { SetUpSource(trigger.CreateOutput(kState)); ClearDownSource(); } /** * Create an instance of a Counter object. - * Creates a full up-down counter given two Digital Sources + * + * Creates a full up-down counter given two Digital Sources. + * * @param encodingType The quadrature decoding mode (1x or 2x) - * @param upSource The pointer to the DigitalSource to set as the up source - * @param downSource The pointer to the DigitalSource to set as the down source - * @param inverted True to invert the output (reverse the direction) + * @param upSource The pointer to the DigitalSource to set as the up source + * @param downSource The pointer to the DigitalSource to set as the down + * source + * @param inverted True to invert the output (reverse the direction) */ -Counter::Counter(EncodingType encodingType, DigitalSource *upSource, - DigitalSource *downSource, bool inverted) - : Counter(encodingType, - std::shared_ptr(upSource, - NullDeleter()), +Counter::Counter(EncodingType encodingType, DigitalSource* upSource, + DigitalSource* downSource, bool inverted) + : Counter(encodingType, std::shared_ptr( + upSource, NullDeleter()), std::shared_ptr(downSource, - NullDeleter()), + NullDeleter()), inverted) {} /** * Create an instance of a Counter object. - * Creates a full up-down counter given two Digital Sources + * + * Creates a full up-down counter given two Digital Sources. + * * @param encodingType The quadrature decoding mode (1x or 2x) - * @param upSource The pointer to the DigitalSource to set as the up source - * @param downSource The pointer to the DigitalSource to set as the down source - * @param inverted True to invert the output (reverse the direction) + * @param upSource The pointer to the DigitalSource to set as the up source + * @param downSource The pointer to the DigitalSource to set as the down + * source + * @param inverted True to invert the output (reverse the direction) */ Counter::Counter(EncodingType encodingType, std::shared_ptr upSource, @@ -172,8 +185,9 @@ Counter::~Counter() { /** * Set the upsource for the counter as a digital input channel. + * * @param channel The DIO channel to use as the up source. 0-9 are on-board, - * 10-25 are on the MXP + * 10-25 are on the MXP */ void Counter::SetUpSource(int32_t channel) { if (StatusIsFatal()) return; @@ -182,20 +196,22 @@ void Counter::SetUpSource(int32_t channel) { /** * Set the up counting source to be an analog trigger. + * * @param analogTrigger The analog trigger object that is used for the Up Source - * @param triggerType The analog trigger output that will trigger the counter. + * @param triggerType The analog trigger output that will trigger the counter. */ -void Counter::SetUpSource(AnalogTrigger *analogTrigger, +void Counter::SetUpSource(AnalogTrigger* analogTrigger, AnalogTriggerType triggerType) { SetUpSource(std::shared_ptr(analogTrigger, - NullDeleter()), + NullDeleter()), triggerType); } /** * Set the up counting source to be an analog trigger. + * * @param analogTrigger The analog trigger object that is used for the Up Source - * @param triggerType The analog trigger output that will trigger the counter. + * @param triggerType The analog trigger output that will trigger the counter. */ void Counter::SetUpSource(std::shared_ptr analogTrigger, AnalogTriggerType triggerType) { @@ -205,7 +221,9 @@ void Counter::SetUpSource(std::shared_ptr analogTrigger, /** * Set the source object that causes the counter to count up. + * * Set the up counting DigitalSource. + * * @param source Pointer to the DigitalSource object to set as the up source */ void Counter::SetUpSource(std::shared_ptr source) { @@ -221,25 +239,29 @@ void Counter::SetUpSource(std::shared_ptr source) { } } -void Counter::SetUpSource(DigitalSource *source) { +void Counter::SetUpSource(DigitalSource* source) { SetUpSource( std::shared_ptr(source, NullDeleter())); } /** * Set the source object that causes the counter to count up. + * * Set the up counting DigitalSource. + * * @param source Reference to the DigitalSource object to set as the up source */ -void Counter::SetUpSource(DigitalSource &source) { +void Counter::SetUpSource(DigitalSource& source) { SetUpSource( std::shared_ptr(&source, NullDeleter())); } /** * Set the edge sensitivity on an up counting source. + * * Set the up source to either detect rising edges or falling edges or both. - * @param risingEdge True to trigger on rising edges + * + * @param risingEdge True to trigger on rising edges * @param fallingEdge True to trigger on falling edges */ void Counter::SetUpSourceEdge(bool risingEdge, bool fallingEdge) { @@ -267,8 +289,9 @@ void Counter::ClearUpSource() { /** * Set the down counting source to be a digital input channel. + * * @param channel The DIO channel to use as the up source. 0-9 are on-board, - * 10-25 are on the MXP + * 10-25 are on the MXP */ void Counter::SetDownSource(int32_t channel) { if (StatusIsFatal()) return; @@ -277,20 +300,24 @@ void Counter::SetDownSource(int32_t channel) { /** * Set the down counting source to be an analog trigger. + * * @param analogTrigger The analog trigger object that is used for the Down - * Source - * @param triggerType The analog trigger output that will trigger the counter. + * Source + * @param triggerType The analog trigger output that will trigger the counter. */ -void Counter::SetDownSource(AnalogTrigger *analogTrigger, +void Counter::SetDownSource(AnalogTrigger* analogTrigger, AnalogTriggerType triggerType) { - SetDownSource(std::shared_ptr(analogTrigger, NullDeleter()), triggerType); + SetDownSource(std::shared_ptr(analogTrigger, + NullDeleter()), + triggerType); } /** * Set the down counting source to be an analog trigger. + * * @param analogTrigger The analog trigger object that is used for the Down - * Source - * @param triggerType The analog trigger output that will trigger the counter. + * Source + * @param triggerType The analog trigger output that will trigger the counter. */ void Counter::SetDownSource(std::shared_ptr analogTrigger, AnalogTriggerType triggerType) { @@ -300,7 +327,9 @@ void Counter::SetDownSource(std::shared_ptr analogTrigger, /** * Set the source object that causes the counter to count down. + * * Set the down counting DigitalSource. + * * @param source Pointer to the DigitalSource object to set as the down source */ void Counter::SetDownSource(std::shared_ptr source) { @@ -316,23 +345,29 @@ void Counter::SetDownSource(std::shared_ptr source) { } } -void Counter::SetDownSource(DigitalSource *source) { - SetDownSource(std::shared_ptr(source, NullDeleter())); +void Counter::SetDownSource(DigitalSource* source) { + SetDownSource( + std::shared_ptr(source, NullDeleter())); } /** * Set the source object that causes the counter to count down. + * * Set the down counting DigitalSource. + * * @param source Reference to the DigitalSource object to set as the down source */ -void Counter::SetDownSource(DigitalSource &source) { - SetDownSource(std::shared_ptr(&source, NullDeleter())); +void Counter::SetDownSource(DigitalSource& source) { + SetDownSource( + std::shared_ptr(&source, NullDeleter())); } /** * Set the edge sensitivity on a down counting source. + * * Set the down source to either detect rising edges or falling edges. - * @param risingEdge True to trigger on rising edges + * + * @param risingEdge True to trigger on rising edges * @param fallingEdge True to trigger on falling edges */ void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge) { @@ -360,6 +395,7 @@ void Counter::ClearDownSource() { /** * Set standard up / down counting mode on this counter. + * * Up and down counts are sourced independently from two inputs. */ void Counter::SetUpDownCounterMode() { @@ -371,6 +407,7 @@ void Counter::SetUpDownCounterMode() { /** * Set external direction mode on this counter. + * * Counts are sourced on the Up counter input. * The Down counter input represents the direction to count. */ @@ -383,6 +420,7 @@ void Counter::SetExternalDirectionMode() { /** * Set Semi-period mode on this counter. + * * Counts up on both rising and falling edges. */ void Counter::SetSemiPeriodMode(bool highSemiPeriod) { @@ -395,9 +433,11 @@ void Counter::SetSemiPeriodMode(bool highSemiPeriod) { /** * Configure the counter to count in up or down based on the length of the input * pulse. + * * This mode is most useful for direction sensitive gear tooth sensors. + * * @param threshold The pulse length beyond which the counter counts the - * opposite direction. Units are seconds. + * opposite direction. Units are seconds. */ void Counter::SetPulseLengthMode(float threshold) { if (StatusIsFatal()) return; @@ -408,10 +448,12 @@ void Counter::SetPulseLengthMode(float threshold) { /** * Get the Samples to Average which specifies the number of samples of the timer - * to - * average when calculating the period. Perform averaging to account for - * mechanical imperfections or as oversampling to increase resolution. - * @return SamplesToAverage The number of samples being averaged (from 1 to 127) + * to average when calculating the period. + * + * Perform averaging to account for mechanical imperfections or as oversampling + * to increase resolution. + * + * @return The number of samples being averaged (from 1 to 127) */ int Counter::GetSamplesToAverage() const { int32_t status = 0; @@ -422,9 +464,9 @@ int Counter::GetSamplesToAverage() const { /** * Set the Samples to Average which specifies the number of samples of the timer - * to - * average when calculating the period. Perform averaging to account for + * to average when calculating the period. Perform averaging to account for * mechanical imperfections or as oversampling to increase resolution. + * * @param samplesToAverage The number of samples to average from 1 to 127. */ void Counter::SetSamplesToAverage(int samplesToAverage) { @@ -440,9 +482,9 @@ void Counter::SetSamplesToAverage(int samplesToAverage) { /** * Read the current counter value. + * * Read the value at this instant. It may still be running, so it reflects the - * current value. Next - * time it is read, it might have a different value. + * current value. Next time it is read, it might have a different value. */ int32_t Counter::Get() const { if (StatusIsFatal()) return 0; @@ -454,9 +496,9 @@ int32_t Counter::Get() const { /** * Reset the Counter to zero. + * * Set the counter value to zero. This doesn't effect the running state of the - * counter, just sets - * the current value to zero. + * counter, just sets the current value to zero. */ void Counter::Reset() { if (StatusIsFatal()) return; @@ -467,9 +509,10 @@ void Counter::Reset() { /** * Get the Period of the most recent count. + * * Returns the time interval of the most recent count. This can be used for - * velocity calculations - * to determine shaft speed. + * velocity calculations to determine shaft speed. + * * @returns The period between the last two pulses in units of seconds. */ double Counter::GetPeriod() const { @@ -482,12 +525,13 @@ double Counter::GetPeriod() const { /** * Set the maximum period where the device is still considered "moving". + * * Sets the maximum period where the device is considered moving. This value is - * used to determine - * the "stopped" state of the counter using the GetStopped method. + * used to determine the "stopped" state of the counter using the GetStopped + * method. + * * @param maxPeriod The maximum period where the counted device is considered - * moving in - * seconds. + * moving in seconds. */ void Counter::SetMaxPeriod(double maxPeriod) { if (StatusIsFatal()) return; @@ -499,23 +543,18 @@ void Counter::SetMaxPeriod(double maxPeriod) { /** * Select whether you want to continue updating the event timer output when * there are no samples captured. + * * The output of the event timer has a buffer of periods that are averaged and - * posted to - * a register on the FPGA. When the timer detects that the event source has - * stopped - * (based on the MaxPeriod) the buffer of samples to be averaged is emptied. If - * you - * enable the update when empty, you will be notified of the stopped source and - * the event - * time will report 0 samples. If you disable update when empty, the most - * recent average - * will remain on the output until a new sample is acquired. You will never see - * 0 samples + * posted to a register on the FPGA. When the timer detects that the event + * source has stopped (based on the MaxPeriod) the buffer of samples to be + * averaged is emptied. If you enable the update when empty, you will be + * notified of the stopped source and the event time will report 0 samples. + * If you disable update when empty, the most recent average will remain on + * the output until a new sample is acquired. You will never see 0 samples * output (except when there have been no events since an FPGA reset) and you - * will likely not - * see the stopped bit become true (since it is updated at the end of an average - * and there are - * no samples to average). + * will likely not see the stopped bit become true (since it is updated at the + * end of an average and there are no samples to average). + * * @param enabled True to enable update when empty */ void Counter::SetUpdateWhenEmpty(bool enabled) { @@ -527,14 +566,13 @@ void Counter::SetUpdateWhenEmpty(bool enabled) { /** * Determine if the clock is stopped. + * * Determine if the clocked input is stopped based on the MaxPeriod value set - * using the - * SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the device (and - * counter) are - * assumed to be stopped and it returns true. + * using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the + * device (and counter) are assumed to be stopped and it returns true. + * * @return Returns true if the most recent counter period exceeds the MaxPeriod - * value set by - * SetMaxPeriod. + * value set by SetMaxPeriod. */ bool Counter::GetStopped() const { if (StatusIsFatal()) return false; @@ -546,6 +584,7 @@ bool Counter::GetStopped() const { /** * The last direction the counter value changed. + * * @return The last direction the counter value changed. */ bool Counter::GetDirection() const { @@ -558,9 +597,10 @@ bool Counter::GetDirection() const { /** * Set the Counter to return reversed sensing on the direction. + * * This allows counters to change the direction they are counting in the case of - * 1X and 2X - * quadrature encoding only. Any other counter mode isn't supported. + * 1X and 2X quadrature encoding only. Any other counter mode isn't supported. + * * @param reverseDirection true if the value counted should be negated. */ void Counter::SetReverseDirection(bool reverseDirection) { diff --git a/wpilibc/Athena/src/DigitalGlitchFilter.cpp b/wpilibc/Athena/src/DigitalGlitchFilter.cpp index 4cde562c5c..bcc75cbcaf 100644 --- a/wpilibc/Athena/src/DigitalGlitchFilter.cpp +++ b/wpilibc/Athena/src/DigitalGlitchFilter.cpp @@ -8,15 +8,15 @@ #include #include -#include "DigitalGlitchFilter.h" -#include "Resource.h" -#include "WPIErrors.h" -#include "Encoder.h" #include "Counter.h" +#include "DigitalGlitchFilter.h" +#include "Encoder.h" +#include "Resource.h" #include "Utility.h" +#include "WPIErrors.h" -std::array DigitalGlitchFilter::m_filterAllocated = {{false, false, - false}}; +std::array DigitalGlitchFilter::m_filterAllocated = { + {false, false, false}}; priority_mutex DigitalGlitchFilter::m_mutex; DigitalGlitchFilter::DigitalGlitchFilter() { @@ -43,11 +43,11 @@ DigitalGlitchFilter::~DigitalGlitchFilter() { * * @param input The DigitalSource to add. */ -void DigitalGlitchFilter::Add(DigitalSource *input) { +void DigitalGlitchFilter::Add(DigitalSource* input) { DoAdd(input, m_channelIndex + 1); } -void DigitalGlitchFilter::DoAdd(DigitalSource *input, int requested_index) { +void DigitalGlitchFilter::DoAdd(DigitalSource* input, int requested_index) { // Some sources from Counters and Encoders are null. By pushing the check // here, we catch the issue more generally. if (input) { @@ -71,7 +71,7 @@ void DigitalGlitchFilter::DoAdd(DigitalSource *input, int requested_index) { * * @param input The Encoder to add. */ -void DigitalGlitchFilter::Add(Encoder *input) { +void DigitalGlitchFilter::Add(Encoder* input) { Add(input->m_aSource.get()); if (StatusIsFatal()) { return; @@ -84,7 +84,7 @@ void DigitalGlitchFilter::Add(Encoder *input) { * * @param input The Counter to add. */ -void DigitalGlitchFilter::Add(Counter *input) { +void DigitalGlitchFilter::Add(Counter* input) { Add(input->m_upSource.get()); if (StatusIsFatal()) { return; @@ -100,9 +100,7 @@ void DigitalGlitchFilter::Add(Counter *input) { * * @param input The DigitalSource to remove. */ -void DigitalGlitchFilter::Remove(DigitalSource *input) { - DoAdd(input, 0); -} +void DigitalGlitchFilter::Remove(DigitalSource* input) { DoAdd(input, 0); } /** * Removes an encoder from this filter. @@ -112,7 +110,7 @@ void DigitalGlitchFilter::Remove(DigitalSource *input) { * * @param input The Encoder to remove. */ -void DigitalGlitchFilter::Remove(Encoder *input) { +void DigitalGlitchFilter::Remove(Encoder* input) { Remove(input->m_aSource.get()); if (StatusIsFatal()) { return; @@ -128,7 +126,7 @@ void DigitalGlitchFilter::Remove(Encoder *input) { * * @param input The Counter to remove. */ -void DigitalGlitchFilter::Remove(Counter *input) { +void DigitalGlitchFilter::Remove(Counter* input) { Remove(input->m_upSource.get()); if (StatusIsFatal()) { return; diff --git a/wpilibc/Athena/src/DigitalInput.cpp b/wpilibc/Athena/src/DigitalInput.cpp index 19538a4fe0..3d004cbee5 100644 --- a/wpilibc/Athena/src/DigitalInput.cpp +++ b/wpilibc/Athena/src/DigitalInput.cpp @@ -6,15 +6,16 @@ /*----------------------------------------------------------------------------*/ #include "DigitalInput.h" +#include "LiveWindow/LiveWindow.h" #include "Resource.h" #include "WPIErrors.h" -#include "LiveWindow/LiveWindow.h" #include #include /** * Create an instance of a Digital Input class. + * * Creates a digital input given a channel. * * @param channel The DIO channel 0-9 are on-board, 10-25 are on the MXP port @@ -58,6 +59,7 @@ DigitalInput::~DigitalInput() { /** * Get the value from a digital input channel. + * * Retrieve the value of a single digital input channel from the FPGA. */ bool DigitalInput::Get() const { diff --git a/wpilibc/Athena/src/DigitalOutput.cpp b/wpilibc/Athena/src/DigitalOutput.cpp index 99b16e83fa..eadd44adc5 100644 --- a/wpilibc/Athena/src/DigitalOutput.cpp +++ b/wpilibc/Athena/src/DigitalOutput.cpp @@ -14,15 +14,16 @@ /** * Create an instance of a digital output. + * * Create a digital output given a channel. * * @param channel The digital channel 0-9 are on-board, 10-25 are on the MXP - * port + * port */ DigitalOutput::DigitalOutput(uint32_t channel) { std::stringstream buf; - m_pwmGenerator = (void *)std::numeric_limits::max(); + m_pwmGenerator = (void*)std::numeric_limits::max(); if (!CheckDigitalChannel(channel)) { buf << "Digital Channel " << channel; wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str()); @@ -54,7 +55,9 @@ DigitalOutput::~DigitalOutput() { /** * Set the value of a digital output. + * * Set the value of a digital output to either one (true) or zero (false). + * * @param value 1 (true) for high, 0 (false) for disabled */ void DigitalOutput::Set(uint32_t value) { @@ -72,9 +75,10 @@ uint32_t DigitalOutput::GetChannel() const { return m_channel; } /** * Output a single pulse on the digital output line. + * * Send a single pulse on the digital output line where the pulse duration is - * specified in seconds. - * Maximum pulse length is 0.0016 seconds. + * specified in seconds. Maximum pulse length is 0.0016 seconds. + * * @param length The pulselength in seconds */ void DigitalOutput::Pulse(float length) { @@ -87,6 +91,7 @@ void DigitalOutput::Pulse(float length) { /** * Determine if the pulse is still going. + * * Determine if a previously started pulse is still going. */ bool DigitalOutput::IsPulsing() const { @@ -130,7 +135,7 @@ void DigitalOutput::SetPWMRate(float rate) { * @param initialDutyCycle The duty-cycle to start generating. [0..1] */ void DigitalOutput::EnablePWM(float initialDutyCycle) { - if (m_pwmGenerator != (void *)std::numeric_limits::max()) return; + if (m_pwmGenerator != (void*)std::numeric_limits::max()) return; int32_t status = 0; @@ -154,7 +159,7 @@ void DigitalOutput::EnablePWM(float initialDutyCycle) { */ void DigitalOutput::DisablePWM() { if (StatusIsFatal()) return; - if (m_pwmGenerator == (void *)std::numeric_limits::max()) return; + if (m_pwmGenerator == (void*)std::numeric_limits::max()) return; int32_t status = 0; @@ -165,7 +170,7 @@ void DigitalOutput::DisablePWM() { freePWM(m_pwmGenerator, &status); wpi_setErrorWithContext(status, getHALErrorMessage(status)); - m_pwmGenerator = (void *)std::numeric_limits::max(); + m_pwmGenerator = (void*)std::numeric_limits::max(); } /** diff --git a/wpilibc/Athena/src/DoubleSolenoid.cpp b/wpilibc/Athena/src/DoubleSolenoid.cpp index f8ed2677ed..8b7cb854af 100644 --- a/wpilibc/Athena/src/DoubleSolenoid.cpp +++ b/wpilibc/Athena/src/DoubleSolenoid.cpp @@ -6,24 +6,27 @@ /*----------------------------------------------------------------------------*/ #include "DoubleSolenoid.h" -#include "WPIErrors.h" #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" #include /** * Constructor. - * Uses the default PCM ID of 0 + * + * Uses the default PCM ID of 0. + * * @param forwardChannel The forward channel number on the PCM (0..7). * @param reverseChannel The reverse channel number on the PCM (0..7). */ DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel) - : DoubleSolenoid(GetDefaultSolenoidModule(), forwardChannel, reverseChannel) {} + : DoubleSolenoid(GetDefaultSolenoidModule(), forwardChannel, + reverseChannel) {} /** * Constructor. * - * @param moduleNumber The CAN ID of the PCM. + * @param moduleNumber The CAN ID of the PCM. * @param forwardChannel The forward channel on the PCM to control (0..7). * @param reverseChannel The reverse channel on the PCM to control (0..7). */ @@ -48,8 +51,7 @@ DoubleSolenoid::DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str()); return; } - Resource::CreateResourceObject( - m_allocated, m_maxModules * m_maxPorts); + Resource::CreateResourceObject(m_allocated, m_maxModules * m_maxPorts); buf << "Solenoid " << m_forwardChannel << " (Module: " << m_moduleNumber << ")"; @@ -129,9 +131,10 @@ DoubleSolenoid::Value DoubleSolenoid::Get() const { } /** * Check if the forward solenoid is blacklisted. - * If a solenoid is shorted, it is added to the blacklist and - * disabled until power cycle, or until faults are cleared. - * @see ClearAllPCMStickyFaults() + * + * If a solenoid is shorted, it is added to the blacklist and + * disabled until power cycle, or until faults are cleared. + * @see ClearAllPCMStickyFaults() * * @return If solenoid is disabled due to short. */ @@ -141,9 +144,10 @@ bool DoubleSolenoid::IsFwdSolenoidBlackListed() const { } /** * Check if the reverse solenoid is blacklisted. - * If a solenoid is shorted, it is added to the blacklist and - * disabled until power cycle, or until faults are cleared. - * @see ClearAllPCMStickyFaults() + * + * If a solenoid is shorted, it is added to the blacklist and + * disabled until power cycle, or until faults are cleared. + * @see ClearAllPCMStickyFaults() * * @return If solenoid is disabled due to short. */ diff --git a/wpilibc/Athena/src/DriverStation.cpp b/wpilibc/Athena/src/DriverStation.cpp index 32262236a8..1d7e003b2c 100644 --- a/wpilibc/Athena/src/DriverStation.cpp +++ b/wpilibc/Athena/src/DriverStation.cpp @@ -6,14 +6,14 @@ /*----------------------------------------------------------------------------*/ #include "DriverStation.h" +#include #include "AnalogInput.h" -#include "Timer.h" -#include "NetworkCommunication/FRCComm.h" +#include "Log.hpp" #include "MotorSafetyHelper.h" +#include "NetworkCommunication/FRCComm.h" +#include "Timer.h" #include "Utility.h" #include "WPIErrors.h" -#include -#include "Log.hpp" // set the logging level TLogLevel dsLogLevel = logDEBUG; @@ -85,15 +85,17 @@ void DriverStation::Run() { /** * Return a pointer to the singleton DriverStation. + * * @return Pointer to the DS instance */ -DriverStation &DriverStation::GetInstance() { - static DriverStation *instance = new DriverStation(); +DriverStation& DriverStation::GetInstance() { + static DriverStation* instance = new DriverStation(); return *instance; } /** * Copy data from the DS task for the user. + * * If no new data exists, it will just be returned, otherwise * the data will be copied from the DS polling loop. */ @@ -134,8 +136,9 @@ void DriverStation::ReportJoystickUnpluggedError(std::string message) { } /** - * Reports errors related to unplugged joysticks - * Throttles the errors so that they don't overwhelm the DS + * Reports errors related to unplugged joysticks. + * + * Throttles the errors so that they don't overwhelm the DS. */ void DriverStation::ReportJoystickUnpluggedWarning(std::string message) { double currentTime = Timer::GetFPGATimestamp(); @@ -146,7 +149,7 @@ void DriverStation::ReportJoystickUnpluggedWarning(std::string message) { } /** - * Returns the number of axes on a given joystick port + * Returns the number of axes on a given joystick port. * * @param stick The joystick port number * @return The number of axes on the indicated joystick @@ -162,7 +165,7 @@ int DriverStation::GetStickAxisCount(uint32_t stick) const { } /** - * Returns the name of the joystick at the given port + * Returns the name of the joystick at the given port. * * @param stick The joystick port number * @return The name of the joystick at the given port @@ -176,7 +179,7 @@ std::string DriverStation::GetJoystickName(uint32_t stick) const { } /** - * Returns the type of joystick at a given port + * Returns the type of joystick at a given port. * * @param stick The joystick port number * @return The HID type of joystick at the given port @@ -204,7 +207,7 @@ bool DriverStation::GetJoystickIsXbox(uint32_t stick) const { } /** - * Returns the types of Axes on a given joystick port + * Returns the types of Axes on a given joystick port. * * @param stick The joystick port number and the target axis * @return What type of axis the axis is reporting to be @@ -218,7 +221,7 @@ int DriverStation::GetJoystickAxisType(uint32_t stick, uint8_t axis) const { } /** - * Returns the number of POVs on a given joystick port + * Returns the number of POVs on a given joystick port. * * @param stick The joystick port number * @return The number of POVs on the indicated joystick @@ -234,7 +237,7 @@ int DriverStation::GetStickPOVCount(uint32_t stick) const { } /** - * Returns the number of buttons on a given joystick port + * Returns the number of buttons on a given joystick port. * * @param stick The joystick port number * @return The number of buttons on the indicated joystick @@ -251,10 +254,11 @@ int DriverStation::GetStickButtonCount(uint32_t stick) const { /** * Get the value of the axis on a joystick. + * * This depends on the mapping of the joystick connected to the specified port. * * @param stick The joystick to read. - * @param axis The analog axis value to read from the joystick. + * @param axis The analog axis value to read from the joystick. * @return The value of the axis on the joystick. */ float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) { @@ -322,7 +326,7 @@ uint32_t DriverStation::GetStickButtons(uint32_t stick) const { /** * The state of one joystick button. Button indexes begin at 1. * - * @param stick The joystick to read. + * @param stick The joystick to read. * @param button The button index, beginning at 1. * @return The state of the joystick button. */ @@ -346,7 +350,8 @@ bool DriverStation::GetStickButton(uint32_t stick, uint8_t button) { } /** - * Check if the DS has enabled the robot + * Check if the DS has enabled the robot. + * * @return True if the robot is enabled and the DS is connected */ bool DriverStation::IsEnabled() const { @@ -357,7 +362,8 @@ bool DriverStation::IsEnabled() const { } /** - * Check if the robot is disabled + * Check if the robot is disabled. + * * @return True if the robot is explicitly disabled or the DS is not connected */ bool DriverStation::IsDisabled() const { @@ -368,7 +374,8 @@ bool DriverStation::IsDisabled() const { } /** - * Check if the DS is commanding autonomous mode + * Check if the DS is commanding autonomous mode. + * * @return True if the robot is being commanded to be in autonomous mode */ bool DriverStation::IsAutonomous() const { @@ -379,7 +386,8 @@ bool DriverStation::IsAutonomous() const { } /** - * Check if the DS is commanding teleop mode + * Check if the DS is commanding teleop mode. + * * @return True if the robot is being commanded to be in teleop mode */ bool DriverStation::IsOperatorControl() const { @@ -390,7 +398,8 @@ bool DriverStation::IsOperatorControl() const { } /** - * Check if the DS is commanding test mode + * Check if the DS is commanding test mode. + * * @return True if the robot is being commanded to be in test mode */ bool DriverStation::IsTest() const { @@ -400,7 +409,8 @@ bool DriverStation::IsTest() const { } /** - * Check if the DS is attached + * Check if the DS is attached. + * * @return True if the DS is connected to the robot */ bool DriverStation::IsDSAttached() const { @@ -411,9 +421,11 @@ bool DriverStation::IsDSAttached() const { } /** - * Check if the FPGA outputs are enabled. The outputs may be disabled if the - * robot is disabled - * or e-stopped, the watchdog has expired, or if the roboRIO browns out. + * Check if the FPGA outputs are enabled. + * + * The outputs may be disabled if the robot is disabled or e-stopped, the + * watchdog has expired, or if the roboRIO browns out. + * * @return True if the FPGA outputs are enabled. */ bool DriverStation::IsSysActive() const { @@ -425,6 +437,7 @@ bool DriverStation::IsSysActive() const { /** * Check if the system is browned out. + * * @return True if the system is browned out */ bool DriverStation::IsSysBrownedOut() const { @@ -437,8 +450,10 @@ bool DriverStation::IsSysBrownedOut() const { /** * Has a new control packet from the driver station arrived since the last time * this function was called? + * * Warning: If you call this function from more than one place at the same time, * you will not get the get the intended behaviour. + * * @return True if the control data has been updated since the last call. */ bool DriverStation::IsNewControlData() const { @@ -447,8 +462,9 @@ bool DriverStation::IsNewControlData() const { /** * Is the driver station attached to a Field Management System? + * * @return True if the robot is competing on a field being controlled by a Field - * Management System + * Management System */ bool DriverStation::IsFMSAttached() const { HALControlWord controlWord; @@ -458,7 +474,9 @@ bool DriverStation::IsFMSAttached() const { /** * Return the alliance that the driver station says it is on. - * This could return kRed or kBlue + * + * This could return kRed or kBlue. + * * @return The Alliance enum (kRed, kBlue or kInvalid) */ DriverStation::Alliance DriverStation::GetAlliance() const { @@ -479,8 +497,10 @@ DriverStation::Alliance DriverStation::GetAlliance() const { } /** - * Return the driver station location on the field - * This could return 1, 2, or 3 + * Return the driver station location on the field. + * + * This could return 1, 2, or 3. + * * @return The location of the driver station (1-3, 0 for invalid) */ uint32_t DriverStation::GetLocation() const { @@ -502,10 +522,12 @@ uint32_t DriverStation::GetLocation() const { } /** - * Wait until a new packet comes from the driver station + * Wait until a new packet comes from the driver station. + * * This blocks on a semaphore, so the waiting is efficient. + * * This is a good way to delay processing until there is new driver station data - * to act on + * to act on. */ void DriverStation::WaitForData() { std::unique_lock lock(m_waitForDataMutex); @@ -513,16 +535,18 @@ void DriverStation::WaitForData() { } /** - * Return the approximate match time + * Return the approximate match time. + * * The FMS does not send an official match time to the robots, but does send an - * approximate match time. - * The value will count down the time remaining in the current period (auto or - * teleop). + * approximate match time. The value will count down the time remaining in the + * current period (auto or teleop). + * * Warning: This is not an official time (so it cannot be used to dispute ref - * calls or guarantee that a function - * will trigger before the match ends) + * calls or guarantee that a function will trigger before the match ends). + * * The Practice Match function of the DS approximates the behaviour seen on the * field. + * * @return Time remaining in current match period (auto or teleop) */ double DriverStation::GetMatchTime() const { @@ -533,6 +557,7 @@ double DriverStation::GetMatchTime() const { /** * Report an error to the DriverStation messages window. + * * The error is also printed to the program console. */ void DriverStation::ReportError(std::string error) { @@ -541,6 +566,7 @@ void DriverStation::ReportError(std::string error) { /** * Report a warning to the DriverStation messages window. + * * The warning is also printed to the program console. */ void DriverStation::ReportWarning(std::string error) { @@ -549,12 +575,13 @@ void DriverStation::ReportWarning(std::string error) { /** * Report an error to the DriverStation messages window. + * * The error is also printed to the program console. */ void DriverStation::ReportError(bool is_error, int32_t code, - const std::string &error, - const std::string &location, - const std::string &stack) { + const std::string& error, + const std::string& location, + const std::string& stack) { HALSendError(is_error, code, 0, error.c_str(), location.c_str(), stack.c_str(), 1); } diff --git a/wpilibc/Athena/src/Encoder.cpp b/wpilibc/Athena/src/Encoder.cpp index 7c5a499c1b..0dc461cf1c 100644 --- a/wpilibc/Athena/src/Encoder.cpp +++ b/wpilibc/Athena/src/Encoder.cpp @@ -7,27 +7,27 @@ #include "Encoder.h" #include "DigitalInput.h" +#include "LiveWindow/LiveWindow.h" #include "Resource.h" #include "WPIErrors.h" -#include "LiveWindow/LiveWindow.h" /** * Common initialization code for Encoders. + * * This code allocates resources for Encoders and is common to all constructors. * * The counter will start counting immediately. * * @param reverseDirection If true, counts down instead of up (this is all - * relative) - * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X - * decoding. If 4X is - * selected, then an encoder FPGA object is used and the returned counts will be - * 4x the encoder - * spec'd value since all rising and falling edges are counted. If 1X or 2X are - * selected then - * a counter object will be used and the returned value will either exactly - * match the spec'd count - * or be double (2x) the spec'd count. + * relative) + * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X + * decoding. If 4X is selected, then an encoder FPGA + * object is used and the returned counts will be 4x + * the encoder spec'd value since all rising and + * falling edges are counted. If 1X or 2X are selected + * then a counter object will be used and the returned + * value will either exactly match the spec'd count or + * be double (2x) the spec'd count. */ void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) { m_encodingType = encodingType; @@ -58,7 +58,7 @@ void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) { case k2X: { m_encodingScale = encodingType == k1X ? 1 : 2; m_counter = std::make_unique(m_encodingType, m_aSource, - m_bSource, reverseDirection); + m_bSource, reverseDirection); m_index = m_counter->GetFPGAIndex(); break; } @@ -74,26 +74,26 @@ void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) { /** * Encoder constructor. + * * Construct a Encoder given a and b channels. * * The counter will start counting immediately. * - * @param aChannel The a channel DIO channel. 0-9 are on-board, 10-25 are on the - * MXP port - * @param bChannel The b channel DIO channel. 0-9 are on-board, 10-25 are on the - * MXP port - * @param reverseDirection represents the orientation of the encoder and inverts - * the output values - * if necessary so forward represents positive values. - * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X - * decoding. If 4X is - * selected, then an encoder FPGA object is used and the returned counts will be - * 4x the encoder - * spec'd value since all rising and falling edges are counted. If 1X or 2X are - * selected then - * a counter object will be used and the returned value will either exactly - * match the spec'd count - * or be double (2x) the spec'd count. + * @param aChannel The a channel DIO channel. 0-9 are on-board, 10-25 + * are on the MXP port + * @param bChannel The b channel DIO channel. 0-9 are on-board, 10-25 + * are on the MXP port + * @param reverseDirection represents the orientation of the encoder and + * inverts the output values if necessary so forward + * represents positive values. + * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X + * decoding. If 4X is selected, then an encoder FPGA + * object is used and the returned counts will be 4x + * the encoder spec'd value since all rising and + * falling edges are counted. If 1X or 2X are selected + * then a counter object will be used and the returned + * value will either exactly match the spec'd count or + * be double (2x) the spec'd count. */ Encoder::Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection, EncodingType encodingType) { @@ -104,27 +104,28 @@ Encoder::Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection, /** * Encoder constructor. + * * Construct a Encoder given a and b channels as digital inputs. This is used in * the case where the digital inputs are shared. The Encoder class will not * allocate the digital inputs and assume that they already are counted. + * * The counter will start counting immediately. * - * @param aSource The source that should be used for the a channel. - * @param bSource the source that should be used for the b channel. - * @param reverseDirection represents the orientation of the encoder and inverts - * the output values - * if necessary so forward represents positive values. - * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X - * decoding. If 4X is - * selected, then an encoder FPGA object is used and the returned counts will be - * 4x the encoder - * spec'd value since all rising and falling edges are counted. If 1X or 2X are - * selected then - * a counter object will be used and the returned value will either exactly - * match the spec'd count - * or be double (2x) the spec'd count. + * @param aSource The source that should be used for the a channel. + * @param bSource the source that should be used for the b channel. + * @param reverseDirection represents the orientation of the encoder and + * inverts the output values if necessary so forward + * represents positive values. + * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X + * decoding. If 4X is selected, then an encoder FPGA + * object is used and the returned counts will be 4x + * the encoder spec'd value since all rising and + * falling edges are counted. If 1X or 2X are selected + * then a counter object will be used and the returned + * value will either exactly match the spec'd count or + * be double (2x) the spec'd count. */ -Encoder::Encoder(DigitalSource *aSource, DigitalSource *bSource, +Encoder::Encoder(DigitalSource* aSource, DigitalSource* bSource, bool reverseDirection, EncodingType encodingType) : m_aSource(aSource, NullDeleter()), m_bSource(bSource, NullDeleter()) { @@ -135,8 +136,8 @@ Encoder::Encoder(DigitalSource *aSource, DigitalSource *bSource, } Encoder::Encoder(std::shared_ptr aSource, - std::shared_ptr bSource, - bool reverseDirection, EncodingType encodingType) + std::shared_ptr bSource, bool reverseDirection, + EncodingType encodingType) : m_aSource(aSource), m_bSource(bSource) { if (m_aSource == nullptr || m_bSource == nullptr) wpi_setWPIError(NullParameter); @@ -146,39 +147,37 @@ Encoder::Encoder(std::shared_ptr aSource, /** * Encoder constructor. + * * Construct a Encoder given a and b channels as digital inputs. This is used in - * the case - * where the digital inputs are shared. The Encoder class will not allocate the - * digital inputs - * and assume that they already are counted. + * the case where the digital inputs are shared. The Encoder class will not + * allocate the digital inputs and assume that they already are counted. * * The counter will start counting immediately. * - * @param aSource The source that should be used for the a channel. - * @param bSource the source that should be used for the b channel. - * @param reverseDirection represents the orientation of the encoder and inverts - * the output values - * if necessary so forward represents positive values. - * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X - * decoding. If 4X is - * selected, then an encoder FPGA object is used and the returned counts will be - * 4x the encoder - * spec'd value since all rising and falling edges are counted. If 1X or 2X are - * selected then - * a counter object will be used and the returned value will either exactly - * match the spec'd count - * or be double (2x) the spec'd count. + * @param aSource The source that should be used for the a channel. + * @param bSource the source that should be used for the b channel. + * @param reverseDirection represents the orientation of the encoder and + * inverts the output values if necessary so forward + * represents positive values. + * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X + * decoding. If 4X is selected, then an encoder FPGA + * object is used and the returned counts will be 4x + * the encoder spec'd value since all rising and + * falling edges are counted. If 1X or 2X are selected + * then a counter object will be used and the returned + * value will either exactly match the spec'd count or + * be double (2x) the spec'd count. */ -Encoder::Encoder(DigitalSource &aSource, DigitalSource &bSource, +Encoder::Encoder(DigitalSource& aSource, DigitalSource& bSource, bool reverseDirection, EncodingType encodingType) : m_aSource(&aSource, NullDeleter()), - m_bSource(&bSource, NullDeleter()) -{ + m_bSource(&bSource, NullDeleter()) { InitEncoder(reverseDirection, encodingType); } /** * Free the resources for an Encoder. + * * Frees the FPGA resources associated with an Encoder. */ Encoder::~Encoder() { @@ -191,14 +190,17 @@ Encoder::~Encoder() { /** * The encoding scale factor 1x, 2x, or 4x, per the requested encodingType. + * * Used to divide raw edge counts down to spec'd counts. */ int32_t Encoder::GetEncodingScale() const { return m_encodingScale; } /** * Gets the raw value from the encoder. + * * The raw value is the actual count unscaled by the 1x, 2x, or 4x scale * factor. + * * @return Current raw count from the encoder */ int32_t Encoder::GetRaw() const { @@ -216,11 +218,12 @@ int32_t Encoder::GetRaw() const { /** * Gets the current count. - * Returns the current count on the Encoder. - * This method compensates for the decoding type. + * + * Returns the current count on the Encoder. This method compensates for the + * decoding type. * * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale - * factor. + * factor. */ int32_t Encoder::Get() const { if (StatusIsFatal()) return 0; @@ -229,6 +232,7 @@ int32_t Encoder::Get() const { /** * Reset the Encoder distance to zero. + * * Resets the current count to zero on the encoder. */ void Encoder::Reset() { @@ -244,11 +248,13 @@ void Encoder::Reset() { /** * Returns the period of the most recent pulse. + * * Returns the period of the most recent Encoder pulse in seconds. * This method compensates for the decoding type. * * @deprecated Use GetRate() in favor of this method. This returns unscaled - * periods and GetRate() scales using value from SetDistancePerPulse(). + * periods and GetRate() scales using value from + * SetDistancePerPulse(). * * @return Period in seconds of the most recent pulse. */ @@ -266,19 +272,19 @@ double Encoder::GetPeriod() const { /** * Sets the maximum period for stopped detection. + * * Sets the value that represents the maximum period of the Encoder before it - * will assume - * that the attached device is stopped. This timeout allows users to determine - * if the wheels or - * other shaft has stopped rotating. + * will assume that the attached device is stopped. This timeout allows users + * to determine if the wheels or other shaft has stopped rotating. * This method compensates for the decoding type. * * @deprecated Use SetMinRate() in favor of this method. This takes unscaled - * periods and SetMinRate() scales using value from SetDistancePerPulse(). + * periods and SetMinRate() scales using value from + * SetDistancePerPulse(). * - * @param maxPeriod The maximum time between rising and falling edges before the - * FPGA will - * report the device stopped. This is expressed in seconds. + * @param maxPeriod The maximum time between rising and falling edges before + * the FPGA will report the device stopped. This is expressed + * in seconds. */ void Encoder::SetMaxPeriod(double maxPeriod) { if (StatusIsFatal()) return; @@ -293,11 +299,11 @@ void Encoder::SetMaxPeriod(double maxPeriod) { /** * Determine if the encoder is stopped. + * * Using the MaxPeriod value, a boolean is returned that is true if the encoder - * is considered - * stopped and false if it is still moving. A stopped encoder is one where the - * most recent pulse - * width exceeds the MaxPeriod. + * is considered stopped and false if it is still moving. A stopped encoder is + * one where the most recent pulse width exceeds the MaxPeriod. + * * @return True if the encoder is considered stopped. */ bool Encoder::GetStopped() const { @@ -314,6 +320,7 @@ bool Encoder::GetStopped() const { /** * The last direction the encoder value changed. + * * @return The last direction the encoder value changed. */ bool Encoder::GetDirection() const { @@ -350,7 +357,7 @@ double Encoder::DecodingScaleFactor() const { * Get the distance the robot has driven since the last reset. * * @return The distance driven since the last reset as scaled by the value from - * SetDistancePerPulse(). + * SetDistancePerPulse(). */ double Encoder::GetDistance() const { if (StatusIsFatal()) return 0.0; @@ -359,6 +366,7 @@ double Encoder::GetDistance() const { /** * Get the current rate of the encoder. + * * Units are distance per second as scaled by the value from * SetDistancePerPulse(). * @@ -373,7 +381,7 @@ double Encoder::GetRate() const { * Set the minimum rate of the device before the hardware reports it stopped. * * @param minRate The minimum rate. The units are in distance per second as - * scaled by the value from SetDistancePerPulse(). + * scaled by the value from SetDistancePerPulse(). */ void Encoder::SetMinRate(double minRate) { if (StatusIsFatal()) return; @@ -382,17 +390,20 @@ void Encoder::SetMinRate(double minRate) { /** * Set the distance per pulse for this encoder. + * * This sets the multiplier used to determine the distance driven based on the - * count value - * from the encoder. + * count value from the encoder. + * * Do not include the decoding type in this scale. The library already * compensates for the decoding type. + * * Set this value based on the encoder's rated Pulses per Revolution and * factor in gearing reductions following the encoder shaft. + * * This distance can be in any units you like, linear or angular. * * @param distancePerPulse The scale factor that will be used to convert pulses - * to useful units. + * to useful units. */ void Encoder::SetDistancePerPulse(double distancePerPulse) { if (StatusIsFatal()) return; @@ -401,9 +412,10 @@ void Encoder::SetDistancePerPulse(double distancePerPulse) { /** * Set the direction sensing for this encoder. + * * This sets the direction sensing on the encoder so that it could count in the - * correct - * software direction regardless of the mounting. + * correct software direction regardless of the mounting. + * * @param reverseDirection true if the encoder direction should be reversed */ void Encoder::SetReverseDirection(bool reverseDirection) { @@ -419,9 +431,11 @@ void Encoder::SetReverseDirection(bool reverseDirection) { /** * Set the Samples to Average which specifies the number of samples of the timer - * to - * average when calculating the period. Perform averaging to account for - * mechanical imperfections or as oversampling to increase resolution. + * to average when calculating the period. + * + * Perform averaging to account for mechanical imperfections or as oversampling + * to increase resolution. + * * @param samplesToAverage The number of samples to average from 1 to 127. */ void Encoder::SetSamplesToAverage(int samplesToAverage) { @@ -445,10 +459,12 @@ void Encoder::SetSamplesToAverage(int samplesToAverage) { /** * Get the Samples to Average which specifies the number of samples of the timer - * to - * average when calculating the period. Perform averaging to account for - * mechanical imperfections or as oversampling to increase resolution. - * @return SamplesToAverage The number of samples being averaged (from 1 to 127) + * to average when calculating the period. + * + * Perform averaging to account for mechanical imperfections or as oversampling + * to increase resolution. + * + * @return The number of samples being averaged (from 1 to 127) */ int Encoder::GetSamplesToAverage() const { int result = 1; @@ -484,11 +500,12 @@ double Encoder::PIDGet() { } /** - * Set the index source for the encoder. When this source is activated, the - * encoder count automatically resets. + * Set the index source for the encoder. + * + * When this source is activated, the encoder count automatically resets. * * @param channel A DIO channel to set as the encoder index - * @param type The state that will cause the encoder to reset + * @param type The state that will cause the encoder to reset */ void Encoder::SetIndexSource(uint32_t channel, Encoder::IndexingType type) { int32_t status = 0; @@ -502,26 +519,28 @@ void Encoder::SetIndexSource(uint32_t channel, Encoder::IndexingType type) { } /** - * Set the index source for the encoder. When this source is activated, the - * encoder count automatically resets. + * Set the index source for the encoder. + * + * When this source is activated, the encoder count automatically resets. * * @param channel A digital source to set as the encoder index - * @param type The state that will cause the encoder to reset + * @param type The state that will cause the encoder to reset */ DEPRECATED("Use pass-by-reference instead.") -void Encoder::SetIndexSource(DigitalSource *source, +void Encoder::SetIndexSource(DigitalSource* source, Encoder::IndexingType type) { SetIndexSource(*source, type); } /** - * Set the index source for the encoder. When this source is activated, the - * encoder count automatically resets. + * Set the index source for the encoder. + * + * When this source is activated, the encoder count automatically resets. * * @param channel A digital source to set as the encoder index - * @param type The state that will cause the encoder to reset + * @param type The state that will cause the encoder to reset */ -void Encoder::SetIndexSource(const DigitalSource &source, +void Encoder::SetIndexSource(const DigitalSource& source, Encoder::IndexingType type) { int32_t status = 0; bool activeHigh = (type == kResetWhileHigh) || (type == kResetOnRisingEdge); diff --git a/wpilibc/Athena/src/GearTooth.cpp b/wpilibc/Athena/src/GearTooth.cpp index 4eed6b6ac3..c2c1cae7af 100644 --- a/wpilibc/Athena/src/GearTooth.cpp +++ b/wpilibc/Athena/src/GearTooth.cpp @@ -22,10 +22,10 @@ void GearTooth::EnableDirectionSensing(bool directionSensitive) { /** * Construct a GearTooth sensor given a channel. * - * @param channel The DIO channel that the sensor is connected to. 0-9 are - * on-board, 10-25 are on the MXP. + * @param channel The DIO channel that the sensor is connected to. + * 0-9 are on-board, 10-25 are on the MXP. * @param directionSensitive True to enable the pulse length decoding in - * hardware to specify count direction. + * hardware to specify count direction. */ GearTooth::GearTooth(uint32_t channel, bool directionSensitive) : Counter(channel) { @@ -35,28 +35,31 @@ GearTooth::GearTooth(uint32_t channel, bool directionSensitive) /** * Construct a GearTooth sensor given a digital input. + * * This should be used when sharing digital inputs. * - * @param source A pointer to the existing DigitalSource object (such as a - * DigitalInput) + * @param source A pointer to the existing DigitalSource object + * (such as a DigitalInput) * @param directionSensitive True to enable the pulse length decoding in - * hardware to specify count direction. + * hardware to specify count direction. */ -GearTooth::GearTooth(DigitalSource *source, bool directionSensitive) +GearTooth::GearTooth(DigitalSource* source, bool directionSensitive) : Counter(source) { EnableDirectionSensing(directionSensitive); } /** * Construct a GearTooth sensor given a digital input. + * * This should be used when sharing digital inputs. * - * @param source A reference to the existing DigitalSource object (such as a - * DigitalInput) + * @param source A reference to the existing DigitalSource object + * (such as a DigitalInput) * @param directionSensitive True to enable the pulse length decoding in - * hardware to specify count direction. + * hardware to specify count direction. */ -GearTooth::GearTooth(std::shared_ptr source, bool directionSensitive) +GearTooth::GearTooth(std::shared_ptr source, + bool directionSensitive) : Counter(source) { EnableDirectionSensing(directionSensitive); } diff --git a/wpilibc/Athena/src/I2C.cpp b/wpilibc/Athena/src/I2C.cpp index da7828cb12..a8f8b27e69 100644 --- a/wpilibc/Athena/src/I2C.cpp +++ b/wpilibc/Athena/src/I2C.cpp @@ -6,14 +6,14 @@ /*----------------------------------------------------------------------------*/ #include "I2C.h" -#include "HAL/HAL.hpp" #include "HAL/Digital.hpp" +#include "HAL/HAL.hpp" #include "WPIErrors.h" /** * Constructor. * - * @param port The I2C port to which the device is connected. + * @param port The I2C port to which the device is connected. * @param deviceAddress The address of the device on the I2C bus. */ I2C::I2C(Port port, uint8_t deviceAddress) @@ -36,14 +36,14 @@ I2C::~I2C() { i2CClose(m_port); } * This is a lower-level interface to the I2C hardware giving you more control * over each transaction. * - * @param dataToSend Buffer of data to send as part of the transaction. - * @param sendSize Number of bytes to send as part of the transaction. + * @param dataToSend Buffer of data to send as part of the transaction. + * @param sendSize Number of bytes to send as part of the transaction. * @param dataReceived Buffer to read data into. - * @param receiveSize Number of bytes to read from the device. + * @param receiveSize Number of bytes to read from the device. * @return Transfer Aborted... false for success, true for aborted. */ -bool I2C::Transaction(uint8_t *dataToSend, uint8_t sendSize, - uint8_t *dataReceived, uint8_t receiveSize) { +bool I2C::Transaction(uint8_t* dataToSend, uint8_t sendSize, + uint8_t* dataReceived, uint8_t receiveSize) { int32_t status = 0; status = i2CTransaction(m_port, m_deviceAddress, dataToSend, sendSize, dataReceived, receiveSize); @@ -59,9 +59,7 @@ bool I2C::Transaction(uint8_t *dataToSend, uint8_t sendSize, * * @return Transfer Aborted... false for success, true for aborted. */ -bool I2C::AddressOnly() { - return Transaction(nullptr, 0, nullptr, 0); -} +bool I2C::AddressOnly() { return Transaction(nullptr, 0, nullptr, 0); } /** * Execute a write transaction with the device. @@ -70,8 +68,8 @@ bool I2C::AddressOnly() { * transaction is complete. * * @param registerAddress The address of the register on the device to be - * written. - * @param data The byte to write to the register on the device. + * written. + * @param data The byte to write to the register on the device. * @return Transfer Aborted... false for success, true for aborted. */ bool I2C::Write(uint8_t registerAddress, uint8_t data) { @@ -89,11 +87,11 @@ bool I2C::Write(uint8_t registerAddress, uint8_t data) { * Write multiple bytes to a device and wait until the * transaction is complete. * - * @param data The data to write to the register on the device. + * @param data The data to write to the register on the device. * @param count The number of bytes to be written. * @return Transfer Aborted... false for success, true for aborted. */ -bool I2C::WriteBulk(uint8_t *data, uint8_t count) { +bool I2C::WriteBulk(uint8_t* data, uint8_t count) { int32_t status = 0; status = i2CWrite(m_port, m_deviceAddress, data, count); return status < count; @@ -104,15 +102,15 @@ bool I2C::WriteBulk(uint8_t *data, uint8_t count) { * * Read bytes from a device. * Most I2C devices will auto-increment the register pointer internally allowing - * you to read consecutive registers on a device in a single transaction. + * you to read consecutive registers on a device in a single transaction. * * @param registerAddress The register to read first in the transaction. - * @param count The number of bytes to read in the transaction. - * @param buffer A pointer to the array of bytes to store the data read from the - * device. + * @param count The number of bytes to read in the transaction. + * @param buffer A pointer to the array of bytes to store the data + * read from the device. * @return Transfer Aborted... false for success, true for aborted. */ -bool I2C::Read(uint8_t registerAddress, uint8_t count, uint8_t *buffer) { +bool I2C::Read(uint8_t registerAddress, uint8_t count, uint8_t* buffer) { if (count < 1) { wpi_setWPIErrorWithContext(ParameterOutOfRange, "count"); return true; @@ -130,14 +128,12 @@ bool I2C::Read(uint8_t registerAddress, uint8_t count, uint8_t *buffer) { * Read bytes from a device. This method does not write any data to prompt the * device. * - * @param buffer - * A pointer to the array of bytes to store the data read from - * the device. - * @param count - The number of bytes to read in the transaction. + * @param buffer A pointer to the array of bytes to store the data read from + * the device. + * @param count The number of bytes to read in the transaction. * @return Transfer Aborted... false for success, true for aborted. */ -bool I2C::ReadOnly(uint8_t count, uint8_t *buffer) { +bool I2C::ReadOnly(uint8_t count, uint8_t* buffer) { if (count < 1) { wpi_setWPIErrorWithContext(ParameterOutOfRange, "count"); return true; @@ -157,27 +153,28 @@ bool I2C::ReadOnly(uint8_t count, uint8_t *buffer) { * This is not currently implemented! * * @param registerAddress The register to write on all devices on the bus. - * @param data The value to write to the devices. + * @param data The value to write to the devices. */ -[[gnu::warning("I2C::Broadcast() is not implemented.")]] -void I2C::Broadcast(uint8_t registerAddress, uint8_t data) {} +[[gnu::warning("I2C::Broadcast() is not implemented.")]] void I2C::Broadcast( + uint8_t registerAddress, uint8_t data) {} /** * Verify that a device's registers contain expected values. * * Most devices will have a set of registers that contain a known value that - * can be used to identify them. This allows an I2C device driver to easily - * verify that the device contains the expected value. + * can be used to identify them. This allows an I2C device driver to easily + * verify that the device contains the expected value. * * @pre The device must support and be configured to use register * auto-increment. * * @param registerAddress The base register to start reading from the device. - * @param count The size of the field to be verified. - * @param expected A buffer containing the values expected from the device. + * @param count The size of the field to be verified. + * @param expected A buffer containing the values expected from the + * device. */ bool I2C::VerifySensor(uint8_t registerAddress, uint8_t count, - const uint8_t *expected) { + const uint8_t* expected) { // TODO: Make use of all 7 read bytes uint8_t deviceData[4]; for (uint8_t i = 0, curRegisterAddress = registerAddress; i < count; diff --git a/wpilibc/Athena/src/InterruptableSensorBase.cpp b/wpilibc/Athena/src/InterruptableSensorBase.cpp index 7c9a80f4d9..4ecfc239ea 100644 --- a/wpilibc/Athena/src/InterruptableSensorBase.cpp +++ b/wpilibc/Athena/src/InterruptableSensorBase.cpp @@ -12,20 +12,18 @@ std::unique_ptr InterruptableSensorBase::m_interrupts = std::make_unique(interrupt_kNumSystems); -InterruptableSensorBase::InterruptableSensorBase() { -} +InterruptableSensorBase::InterruptableSensorBase() {} /** -* Request one of the 8 interrupts asynchronously on this digital input. -* Request interrupts in asynchronous mode where the user's interrupt handler -* will be -* called when the interrupt fires. Users that want control over the thread -* priority -* should use the synchronous method with their own spawned thread. -* The default is interrupt on rising edges only. -*/ + * Request one of the 8 interrupts asynchronously on this digital input. + * + * Request interrupts in asynchronous mode where the user's interrupt handler + * will be called when the interrupt fires. Users that want control over the + * thread priority should use the synchronous method with their own spawned + * thread. The default is interrupt on rising edges only. + */ void InterruptableSensorBase::RequestInterrupts( - InterruptHandlerFunction handler, void *param) { + InterruptHandlerFunction handler, void* param) { if (StatusIsFatal()) return; uint32_t index = m_interrupts->Allocate("Async Interrupt"); if (index == std::numeric_limits::max()) { @@ -46,12 +44,12 @@ void InterruptableSensorBase::RequestInterrupts( } /** -* Request one of the 8 interrupts synchronously on this digital input. -* Request interrupts in synchronous mode where the user program will have to -* explicitly -* wait for the interrupt to occur using WaitForInterrupt. -* The default is interrupt on rising edges only. -*/ + * Request one of the 8 interrupts synchronously on this digital input. + * + * Request interrupts in synchronous mode where the user program will have to + * explicitly wait for the interrupt to occur using WaitForInterrupt. + * The default is interrupt on rising edges only. + */ void InterruptableSensorBase::RequestInterrupts() { if (StatusIsFatal()) return; uint32_t index = m_interrupts->Allocate("Sync Interrupt"); @@ -80,6 +78,7 @@ void InterruptableSensorBase::AllocateInterrupts(bool watcher) { /** * Cancel interrupts on this device. + * * This deallocates all the chipobject structures and disables any interrupts. */ void InterruptableSensorBase::CancelInterrupts() { @@ -93,14 +92,15 @@ void InterruptableSensorBase::CancelInterrupts() { } /** - * In synchronous mode, wait for the defined interrupt to occur. You should - * NOT attempt to read the - * sensor from another thread while waiting for an interrupt. This is not - * threadsafe, and can cause - * memory corruption - * @param timeout Timeout in seconds + * In synchronous mode, wait for the defined interrupt to occur. + * + * You should NOT attempt to read the sensor from another thread while + * waiting for an interrupt. This is not threadsafe, and can cause memory + * corruption + * + * @param timeout Timeout in seconds * @param ignorePrevious If true, ignore interrupts that happened before - * WaitForInterrupt was called. + * WaitForInterrupt was called. * @return What interrupts fired */ InterruptableSensorBase::WaitResult InterruptableSensorBase::WaitForInterrupt( @@ -118,9 +118,10 @@ InterruptableSensorBase::WaitResult InterruptableSensorBase::WaitForInterrupt( /** * Enable interrupts to occur on this input. + * * Interrupts are disabled when the RequestInterrupt call is made. This gives - * time to do the - * setup of the other options before starting to field interrupts. + * time to do the setup of the other options before starting to field + * interrupts. */ void InterruptableSensorBase::EnableInterrupts() { if (StatusIsFatal()) return; @@ -143,9 +144,11 @@ void InterruptableSensorBase::DisableInterrupts() { /** * Return the timestamp for the rising interrupt that occurred most recently. + * * This is in the same time domain as GetClock(). * The rising-edge interrupt should be enabled with * {@link #DigitalInput.SetUpSourceEdge} + * * @return Timestamp in seconds since boot. */ double InterruptableSensorBase::ReadRisingTimestamp() { @@ -159,9 +162,11 @@ double InterruptableSensorBase::ReadRisingTimestamp() { /** * Return the timestamp for the falling interrupt that occurred most recently. + * * This is in the same time domain as GetClock(). * The falling-edge interrupt should be enabled with * {@link #DigitalInput.SetUpSourceEdge} + * * @return Timestamp in seconds since boot. */ double InterruptableSensorBase::ReadFallingTimestamp() { @@ -176,10 +181,8 @@ double InterruptableSensorBase::ReadFallingTimestamp() { /** * Set which edge to trigger interrupts on * - * @param risingEdge - * true to interrupt on rising edge - * @param fallingEdge - * true to interrupt on falling edge + * @param risingEdge true to interrupt on rising edge + * @param fallingEdge true to interrupt on falling edge */ void InterruptableSensorBase::SetUpSourceEdge(bool risingEdge, bool fallingEdge) { diff --git a/wpilibc/Athena/src/IterativeRobot.cpp b/wpilibc/Athena/src/IterativeRobot.cpp index 3d6946a055..7bb61f082a 100644 --- a/wpilibc/Athena/src/IterativeRobot.cpp +++ b/wpilibc/Athena/src/IterativeRobot.cpp @@ -9,8 +9,8 @@ #include "DriverStation.h" #include "HAL/HAL.hpp" -#include "SmartDashboard/SmartDashboard.h" #include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SmartDashboard.h" #include "networktables/NetworkTable.h" constexpr double IterativeRobot::kDefaultPeriod; @@ -25,7 +25,7 @@ void IterativeRobot::StartCompetition() { HALReport(HALUsageReporting::kResourceType_Framework, HALUsageReporting::kFramework_Iterative); - LiveWindow *lw = LiveWindow::GetInstance(); + LiveWindow* lw = LiveWindow::GetInstance(); // first and one-time initialization SmartDashboard::init(); NetworkTable::GetTable("LiveWindow") @@ -134,8 +134,7 @@ void IterativeRobot::DisabledInit() { * Initialization code for autonomous mode should go here. * * Users should override this method for initialization code which will be - * called each time - * the robot enters autonomous mode. + * called each time the robot enters autonomous mode. */ void IterativeRobot::AutonomousInit() { printf("Default %s() method... Overload me!\n", __FUNCTION__); @@ -145,8 +144,7 @@ void IterativeRobot::AutonomousInit() { * Initialization code for teleop mode should go here. * * Users should override this method for initialization code which will be - * called each time - * the robot enters teleop mode. + * called each time the robot enters teleop mode. */ void IterativeRobot::TeleopInit() { printf("Default %s() method... Overload me!\n", __FUNCTION__); @@ -156,8 +154,7 @@ void IterativeRobot::TeleopInit() { * Initialization code for test mode should go here. * * Users should override this method for initialization code which will be - * called each time - * the robot enters test mode. + * called each time the robot enters test mode. */ void IterativeRobot::TestInit() { printf("Default %s() method... Overload me!\n", __FUNCTION__); @@ -167,8 +164,7 @@ void IterativeRobot::TestInit() { * Periodic code for disabled mode should go here. * * Users should override this method for code which will be called periodically - * at a regular - * rate while the robot is in disabled mode. + * at a regular rate while the robot is in disabled mode. */ void IterativeRobot::DisabledPeriodic() { static bool firstRun = true; @@ -183,8 +179,7 @@ void IterativeRobot::DisabledPeriodic() { * Periodic code for autonomous mode should go here. * * Users should override this method for code which will be called periodically - * at a regular - * rate while the robot is in autonomous mode. + * at a regular rate while the robot is in autonomous mode. */ void IterativeRobot::AutonomousPeriodic() { static bool firstRun = true; @@ -199,8 +194,7 @@ void IterativeRobot::AutonomousPeriodic() { * Periodic code for teleop mode should go here. * * Users should override this method for code which will be called periodically - * at a regular - * rate while the robot is in teleop mode. + * at a regular rate while the robot is in teleop mode. */ void IterativeRobot::TeleopPeriodic() { static bool firstRun = true; @@ -215,8 +209,7 @@ void IterativeRobot::TeleopPeriodic() { * Periodic code for test mode should go here. * * Users should override this method for code which will be called periodically - * at a regular - * rate while the robot is in test mode. + * at a regular rate while the robot is in test mode. */ void IterativeRobot::TestPeriodic() { static bool firstRun = true; diff --git a/wpilibc/Athena/src/Jaguar.cpp b/wpilibc/Athena/src/Jaguar.cpp index 29d603c34d..9379c5a19e 100644 --- a/wpilibc/Athena/src/Jaguar.cpp +++ b/wpilibc/Athena/src/Jaguar.cpp @@ -9,9 +9,10 @@ #include "LiveWindow/LiveWindow.h" /** - * Constructor for a Jaguar connected via PWM + * Constructor for a Jaguar connected via PWM. + * * @param channel The PWM channel that the Jaguar is attached to. 0-9 are - * on-board, 10-19 are on the MXP port + * on-board, 10-19 are on the MXP port */ Jaguar::Jaguar(uint32_t channel) : PWMSpeedController(channel) { /** @@ -31,4 +32,3 @@ Jaguar::Jaguar(uint32_t channel) : PWMSpeedController(channel) { HALReport(HALUsageReporting::kResourceType_Jaguar, GetChannel()); LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this); } - diff --git a/wpilibc/Athena/src/Joystick.cpp b/wpilibc/Athena/src/Joystick.cpp index 0e928cd4d9..1d3538579e 100644 --- a/wpilibc/Athena/src/Joystick.cpp +++ b/wpilibc/Athena/src/Joystick.cpp @@ -6,10 +6,10 @@ /*----------------------------------------------------------------------------*/ #include "Joystick.h" -#include "DriverStation.h" -#include "WPIErrors.h" #include #include +#include "DriverStation.h" +#include "WPIErrors.h" const uint32_t Joystick::kDefaultXAxis; const uint32_t Joystick::kDefaultYAxis; @@ -18,15 +18,16 @@ const uint32_t Joystick::kDefaultTwistAxis; const uint32_t Joystick::kDefaultThrottleAxis; const uint32_t Joystick::kDefaultTriggerButton; const uint32_t Joystick::kDefaultTopButton; -static Joystick *joysticks[DriverStation::kJoystickPorts]; +static Joystick* joysticks[DriverStation::kJoystickPorts]; static bool joySticksInitialized = false; /** * Construct an instance of a joystick. + * * The joystick index is the usb port on the drivers station. * * @param port The port on the driver station that the joystick is plugged into - * (0-5). + * (0-5). */ Joystick::Joystick(uint32_t port) : Joystick(port, kNumAxisTypes, kNumButtonTypes) { @@ -48,8 +49,9 @@ Joystick::Joystick(uint32_t port) * This constructor allows the subclass to configure the number of constants * for axes and buttons. * - * @param port The port on the driver station that the joystick is plugged into. - * @param numAxisTypes The number of axis types in the enum. + * @param port The port on the driver station that the joystick is + * plugged into. + * @param numAxisTypes The number of axis types in the enum. * @param numButtonTypes The number of button types in the enum. */ Joystick::Joystick(uint32_t port, uint32_t numAxisTypes, @@ -69,8 +71,8 @@ Joystick::Joystick(uint32_t port, uint32_t numAxisTypes, } } -Joystick *Joystick::GetStickForPort(uint32_t port) { - Joystick *stick = joysticks[port]; +Joystick* Joystick::GetStickForPort(uint32_t port) { + Joystick* stick = joysticks[port]; if (stick == nullptr) { stick = new Joystick(port); joysticks[port] = stick; @@ -80,9 +82,11 @@ Joystick *Joystick::GetStickForPort(uint32_t port) { /** * Get the X value of the joystick. + * * This depends on the mapping of the joystick connected to the current port. - * @param hand This parameter is ignored for the Joystick class and is only here - * to complete the GenericHID interface. + * + * @param hand This parameter is ignored for the Joystick class and is only + * here to complete the GenericHID interface. */ float Joystick::GetX(JoystickHand hand) const { return GetRawAxis(m_axes[kXAxis]); @@ -90,9 +94,11 @@ float Joystick::GetX(JoystickHand hand) const { /** * Get the Y value of the joystick. + * * This depends on the mapping of the joystick connected to the current port. - * @param hand This parameter is ignored for the Joystick class and is only here - * to complete the GenericHID interface. + * + * @param hand This parameter is ignored for the Joystick class and is only + * here to complete the GenericHID interface. */ float Joystick::GetY(JoystickHand hand) const { return GetRawAxis(m_axes[kYAxis]); @@ -100,18 +106,21 @@ float Joystick::GetY(JoystickHand hand) const { /** * Get the Z value of the current joystick. + * * This depends on the mapping of the joystick connected to the current port. */ float Joystick::GetZ() const { return GetRawAxis(m_axes[kZAxis]); } /** * Get the twist value of the current joystick. + * * This depends on the mapping of the joystick connected to the current port. */ float Joystick::GetTwist() const { return GetRawAxis(m_axes[kTwistAxis]); } /** * Get the throttle value of the current joystick. + * * This depends on the mapping of the joystick connected to the current port. */ float Joystick::GetThrottle() const { @@ -132,8 +141,8 @@ float Joystick::GetRawAxis(uint32_t axis) const { * For the current joystick, return the axis determined by the argument. * * This is for cases where the joystick axis is returned programatically, - * otherwise one of the - * previous functions would be preferable (for example GetX()). + * otherwise one of the previous functions would be preferable (for example + * GetX()). * * @param axis The axis to read. * @return The value of the axis. @@ -161,8 +170,8 @@ float Joystick::GetAxis(AxisType axis) const { * * Look up which button has been assigned to the trigger and read its state. * - * @param hand This parameter is ignored for the Joystick class and is only here - * to complete the GenericHID interface. + * @param hand This parameter is ignored for the Joystick class and is only + * here to complete the GenericHID interface. * @return The state of the trigger. */ bool Joystick::GetTrigger(JoystickHand hand) const { @@ -174,8 +183,8 @@ bool Joystick::GetTrigger(JoystickHand hand) const { * * Look up which button has been assigned to the top and read its state. * - * @param hand This parameter is ignored for the Joystick class and is only here - * to complete the GenericHID interface. + * @param hand This parameter is ignored for the Joystick class and is only + * here to complete the GenericHID interface. * @return The state of the top button. */ bool Joystick::GetTop(JoystickHand hand) const { @@ -184,6 +193,7 @@ bool Joystick::GetTop(JoystickHand hand) const { /** * This is not supported for the Joystick. + * * This method is only here to complete the GenericHID interface. */ bool Joystick::GetBumper(JoystickHand hand) const { @@ -195,8 +205,8 @@ bool Joystick::GetBumper(JoystickHand hand) const { * Get the button value (starting at button 1) * * The buttons are returned in a single 16 bit value with one bit representing - * the state - * of each button. The appropriate button is returned as a boolean value. + * the state of each button. The appropriate button is returned as a boolean + * value. * * @param button The button number to be read (starting at 1) * @return The state of the button. @@ -208,8 +218,8 @@ bool Joystick::GetRawButton(uint32_t button) const { /** * Get the angle in degrees of a POV on the joystick. * - * The POV angles start at 0 in the up direction, and increase - * clockwise (eg right is 90, upper-left is 315). + * The POV angles start at 0 in the up direction, and increase clockwise + * (e.g. right is 90, upper-left is 315). * * @param pov The index of the POV to read (starting at 0) * @return the angle of the POV in degrees, or -1 if the POV is not pressed. @@ -269,22 +279,22 @@ std::string Joystick::GetName() const { return m_ds.GetJoystickName(m_port); } // int Joystick::GetAxisType(uint8_t axis) const //{ -// return m_ds.GetJoystickAxisType(m_port, axis); +// return m_ds.GetJoystickAxisType(m_port, axis); //} /** - * Get the number of axis for a joystick - * -* @return the number of buttons on the current joystick + * Get the number of axis for a joystick. + * + * @return the number of buttons on the current joystick */ int Joystick::GetButtonCount() const { return m_ds.GetStickButtonCount(m_port); } /** - * Get the number of axis for a joystick + * Get the number of axis for a joystick. * - * @return then umber of POVs for the current joystick + * @return the number of POVs for the current joystick */ int Joystick::GetPOVCount() const { return m_ds.GetStickPOVCount(m_port); } @@ -299,7 +309,7 @@ uint32_t Joystick::GetAxisChannel(AxisType axis) const { return m_axes[axis]; } /** * Set the channel associated with a specified axis. * - * @param axis The axis to set the channel for. + * @param axis The axis to set the channel for. * @param channel The channel to set the axis to. */ void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) { @@ -308,7 +318,7 @@ void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) { /** * Get the magnitude of the direction vector formed by the joystick's - * current position relative to its origin + * current position relative to its origin. * * @return The magnitude of the direction vector */ @@ -318,7 +328,7 @@ float Joystick::GetMagnitude() const { /** * Get the direction of the vector formed by the joystick and its origin - * in radians + * in radians. * * @return The direction of the vector in radians */ @@ -326,7 +336,7 @@ float Joystick::GetDirectionRadians() const { return atan2(GetX(), -GetY()); } /** * Get the direction of the vector formed by the joystick and its origin - * in degrees + * in degrees. * * uses acos(-1) to represent Pi due to absence of readily accessible Pi * constant in C++ @@ -338,10 +348,11 @@ float Joystick::GetDirectionDegrees() const { } /** - * Set the rumble output for the joystick. The DS currently supports 2 rumble - * values, - * left rumble and right rumble - * @param type Which rumble value to set + * Set the rumble output for the joystick. + * + * The DS currently supports 2 rumble values, left rumble and right rumble. + * + * @param type Which rumble value to set * @param value The normalized value (0 to 1) to set the rumble to */ void Joystick::SetRumble(RumbleType type, float value) { @@ -358,8 +369,9 @@ void Joystick::SetRumble(RumbleType type, float value) { /** * Set a single HID output value for the joystick. + * * @param outputNumber The index of the output to set (1-32) - * @param value The value to set the output to + * @param value The value to set the output to */ void Joystick::SetOutput(uint8_t outputNumber, bool value) { @@ -371,6 +383,7 @@ void Joystick::SetOutput(uint8_t outputNumber, bool value) { /** * Set all HID output values for the joystick. + * * @param value The 32 bit output value (1 bit for each output) */ void Joystick::SetOutputs(uint32_t value) { diff --git a/wpilibc/Athena/src/MotorSafetyHelper.cpp b/wpilibc/Athena/src/MotorSafetyHelper.cpp index a78c3489ba..24908c3500 100644 --- a/wpilibc/Athena/src/MotorSafetyHelper.cpp +++ b/wpilibc/Athena/src/MotorSafetyHelper.cpp @@ -20,18 +20,17 @@ priority_recursive_mutex MotorSafetyHelper::m_listMutex; /** * The constructor for a MotorSafetyHelper object. + * * The helper object is constructed for every object that wants to implement the - * Motor - * Safety protocol. The helper object has the code to actually do the timing and - * call the - * motors Stop() method when the timeout expires. The motor object is expected - * to call the - * Feed() method whenever the motors value is updated. + * Motor Safety protocol. The helper object has the code to actually do the + * timing and call the motors Stop() method when the timeout expires. The motor + * object is expected to call the Feed() method whenever the motors value is + * updated. + * * @param safeObject a pointer to the motor object implementing MotorSafety. - * This is used - * to call the Stop() method on the motor. + * This is used to call the Stop() method on the motor. */ -MotorSafetyHelper::MotorSafetyHelper(MotorSafety *safeObject) +MotorSafetyHelper::MotorSafetyHelper(MotorSafety* safeObject) : m_safeObject(safeObject) { m_enabled = false; m_expiration = DEFAULT_SAFETY_EXPIRATION; @@ -86,20 +85,18 @@ bool MotorSafetyHelper::IsAlive() const { /** * Check if this motor has exceeded its timeout. * This method is called periodically to determine if this motor has exceeded - * its timeout - * value. If it has, the stop method is called, and the motor is shut down until - * its value is - * updated again. + * its timeout value. If it has, the stop method is called, and the motor is + * shut down until its value is updated again. */ void MotorSafetyHelper::Check() { - DriverStation &ds = DriverStation::GetInstance(); + DriverStation& ds = DriverStation::GetInstance(); if (!m_enabled || ds.IsDisabled() || ds.IsTest()) return; std::lock_guard sync(m_syncMutex); if (m_stopTime < Timer::GetFPGATimestamp()) { std::ostringstream desc; m_safeObject->GetDescription(desc); - desc << "... Output not updated often enough."; + desc << "... Output not updated often enough."; wpi_setWPIErrorWithContext(Timeout, desc.str().c_str()); m_safeObject->StopMotor(); } @@ -128,8 +125,7 @@ bool MotorSafetyHelper::IsSafetyEnabled() const { /** * Check the motors to see if any have timed out. * This static method is called periodically to poll all the motors and stop - * any that have - * timed out. + * any that have timed out. */ void MotorSafetyHelper::CheckMotors() { std::lock_guard sync(m_listMutex); diff --git a/wpilibc/Athena/src/Notifier.cpp b/wpilibc/Athena/src/Notifier.cpp index 0023100d8e..08b0ace0d4 100644 --- a/wpilibc/Athena/src/Notifier.cpp +++ b/wpilibc/Athena/src/Notifier.cpp @@ -6,15 +6,16 @@ /*----------------------------------------------------------------------------*/ #include "Notifier.h" +#include "HAL/HAL.hpp" #include "Timer.h" #include "Utility.h" #include "WPIErrors.h" -#include "HAL/HAL.hpp" /** * Create a Notifier for timer event notification. + * * @param handler The handler is called at the notification time which is set - * using StartSingle or StartPeriodic. + * using StartSingle or StartPeriodic. */ Notifier::Notifier(TimerEventHandler handler) { if (handler == nullptr) @@ -52,7 +53,7 @@ void Notifier::UpdateAlarm() { * Notify is called by the HAL layer. We simply need to pass it through to * the user handler. */ -void Notifier::Notify(uint64_t currentTimeInt, void *param) { +void Notifier::Notify(uint64_t currentTimeInt, void* param) { Notifier* notifier = static_cast(param); notifier->m_processMutex.lock(); @@ -72,7 +73,9 @@ void Notifier::Notify(uint64_t currentTimeInt, void *param) { /** * Register for single event notification. + * * A timer event is queued for a single event after the specified delay. + * * @param delay Seconds to wait before the handler is called. */ void Notifier::StartSingle(double delay) { @@ -85,11 +88,13 @@ void Notifier::StartSingle(double delay) { /** * Register for periodic event notification. + * * A timer event is queued for periodic event notification. Each time the * interrupt occurs, the event will be immediately requeued for the same time * interval. - * @param period Period in seconds to call the handler starting one period after - * the call to this method. + * + * @param period Period in seconds to call the handler starting one period + * after the call to this method. */ void Notifier::StartPeriodic(double period) { std::lock_guard sync(m_processMutex); @@ -101,8 +106,10 @@ void Notifier::StartPeriodic(double period) { /** * Stop timer events from occuring. + * * Stop any repeating timer events from occuring. This will also remove any * single notification events from the queue. + * * If a timer-based call to the registered handler is in progress, this function * will block until the handler call is complete. */ diff --git a/wpilibc/Athena/src/PIDController.cpp b/wpilibc/Athena/src/PIDController.cpp index b21cb9be8e..65c1d6d898 100644 --- a/wpilibc/Athena/src/PIDController.cpp +++ b/wpilibc/Athena/src/PIDController.cpp @@ -6,12 +6,12 @@ /*----------------------------------------------------------------------------*/ #include "PIDController.h" -#include "Notifier.h" -#include "PIDSource.h" -#include "PIDOutput.h" #include #include #include "HAL/HAL.hpp" +#include "Notifier.h" +#include "PIDOutput.h" +#include "PIDSource.h" static const std::string kP = "p"; static const std::string kI = "i"; @@ -21,39 +21,42 @@ static const std::string kSetpoint = "setpoint"; static const std::string kEnabled = "enabled"; /** - * Allocate a PID object with the given constants for P, I, D - * @param Kp the proportional coefficient - * @param Ki the integral coefficient - * @param Kd the derivative coefficient + * Allocate a PID object with the given constants for P, I, D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient * @param source The PIDSource object that is used to get values * @param output The PIDOutput object that is set to the output value - * @param period the loop time for doing calculations. This particularly effects - * calculations of the - * integral and differental terms. The default is 50ms. + * @param period the loop time for doing calculations. This particularly + * effects calculations of the integral and differental terms. + * The default is 50ms. */ -PIDController::PIDController(float Kp, float Ki, float Kd, PIDSource *source, - PIDOutput *output, float period) { +PIDController::PIDController(float Kp, float Ki, float Kd, PIDSource* source, + PIDOutput* output, float period) { Initialize(Kp, Ki, Kd, 0.0f, source, output, period); } /** - * Allocate a PID object with the given constants for P, I, D - * @param Kp the proportional coefficient - * @param Ki the integral coefficient - * @param Kd the derivative coefficient + * Allocate a PID object with the given constants for P, I, D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient * @param source The PIDSource object that is used to get values * @param output The PIDOutput object that is set to the output value - * @param period the loop time for doing calculations. This particularly effects - * calculations of the - * integral and differental terms. The default is 50ms. + * @param period the loop time for doing calculations. This particularly + * effects calculations of the integral and differental terms. + * The default is 50ms. */ PIDController::PIDController(float Kp, float Ki, float Kd, float Kf, - PIDSource *source, PIDOutput *output, float period) { + PIDSource* source, PIDOutput* output, + float period) { Initialize(Kp, Ki, Kd, Kf, source, output, period); } void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf, - PIDSource *source, PIDOutput *output, + PIDSource* source, PIDOutput* output, float period) { m_controlLoop = std::make_unique(&PIDController::Calculate, this); @@ -84,8 +87,8 @@ PIDController::~PIDController() { */ void PIDController::Calculate() { bool enabled; - PIDSource *pidInput; - PIDOutput *pidOutput; + PIDSource* pidInput; + PIDOutput* pidOutput; { std::lock_guard sync(m_mutex); @@ -101,7 +104,7 @@ void PIDController::Calculate() { std::lock_guard sync(m_mutex); float input = pidInput->PIDGet(); float result; - PIDOutput *pidOutput; + PIDOutput* pidOutput; m_error = m_setpoint - input; if (m_continuous) { @@ -128,8 +131,7 @@ void PIDController::Calculate() { } m_result = m_D * m_error + m_P * m_totalError + CalculateFeedForward(); - } - else { + } else { if (m_I != 0) { double potentialIGain = (m_totalError + m_error) * m_I; if (potentialIGain < m_maximumOutput) { @@ -169,11 +171,11 @@ void PIDController::Calculate() { } /** - * Calculate the feed forward term + * Calculate the feed forward term. * * Both of the provided feed forward calculations are velocity feed forwards. * If a different feed forward calculation is desired, the user can override - * this function and provide his or her own. This function does no + * this function and provide his or her own. This function does no * synchronization because the PIDController class only calls it in synchronized * code, so be careful if calling it oneself. * @@ -186,8 +188,7 @@ void PIDController::Calculate() { double PIDController::CalculateFeedForward() { if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) { return m_F * GetSetpoint(); - } - else { + } else { double temp = m_F * GetDeltaSetpoint(); m_prevSetpoint = m_setpoint; m_setpointTimer.Reset(); @@ -197,7 +198,9 @@ double PIDController::CalculateFeedForward() { /** * Set the PID Controller gain parameters. + * * Set the proportional, integral, and differential coefficients. + * * @param p Proportional coefficient * @param i Integral coefficient * @param d Differential coefficient @@ -219,7 +222,9 @@ void PIDController::SetPID(double p, double i, double d) { /** * Set the PID Controller gain parameters. + * * Set the proportional, integral, and differential coefficients. + * * @param p Proportional coefficient * @param i Integral coefficient * @param d Differential coefficient @@ -243,7 +248,8 @@ void PIDController::SetPID(double p, double i, double d, double f) { } /** - * Get the Proportional coefficient + * Get the Proportional coefficient. + * * @return proportional coefficient */ double PIDController::GetP() const { @@ -252,7 +258,8 @@ double PIDController::GetP() const { } /** - * Get the Integral coefficient + * Get the Integral coefficient. + * * @return integral coefficient */ double PIDController::GetI() const { @@ -261,7 +268,8 @@ double PIDController::GetI() const { } /** - * Get the Differential coefficient + * Get the Differential coefficient. + * * @return differential coefficient */ double PIDController::GetD() const { @@ -270,7 +278,8 @@ double PIDController::GetD() const { } /** - * Get the Feed forward coefficient + * Get the Feed forward coefficient. + * * @return Feed forward coefficient */ double PIDController::GetF() const { @@ -279,8 +288,10 @@ double PIDController::GetF() const { } /** - * Return the current PID result - * This is always centered on zero and constrained the the max and min outs + * Return the current PID result. + * + * This is always centered on zero and constrained the the max and min outs. + * * @return the latest calculated output */ float PIDController::Get() const { @@ -289,11 +300,13 @@ float PIDController::Get() const { } /** - * Set the PID controller to consider the input to be continuous, - * Rather then using the max and min in as constraints, it considers them to - * be the same point and automatically calculates the shortest route to - * the setpoint. - * @param continuous Set to true turns on continuous, false turns off continuous + * Set the PID controller to consider the input to be continuous, + * + * Rather then using the max and min in as constraints, it considers them to + * be the same point and automatically calculates the shortest route to + * the setpoint. + * + * @param continuous true turns on continuous, false turns off continuous */ void PIDController::SetContinuous(bool continuous) { std::lock_guard sync(m_mutex); @@ -331,8 +344,10 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) { } /** - * Set the setpoint for the PIDController + * Set the setpoint for the PIDController. + * * Clears the queue for GetAvgError(). + * * @param setpoint the desired setpoint */ void PIDController::SetSetpoint(float setpoint) { @@ -361,7 +376,8 @@ void PIDController::SetSetpoint(float setpoint) { } /** - * Returns the current setpoint of the PIDController + * Returns the current setpoint of the PIDController. + * * @return the current setpoint */ double PIDController::GetSetpoint() const { @@ -370,7 +386,8 @@ double PIDController::GetSetpoint() const { } /** - * Returns the change in setpoint over time of the PIDController + * Returns the change in setpoint over time of the PIDController. + * * @return the change in setpoint over time */ double PIDController::GetDeltaSetpoint() const { @@ -379,7 +396,8 @@ double PIDController::GetDeltaSetpoint() const { } /** - * Returns the current difference of the input from the setpoint + * Returns the current difference of the input from the setpoint. + * * @return the current error */ float PIDController::GetError() const { @@ -392,13 +410,14 @@ float PIDController::GetError() const { } /** - * Sets what type of input the PID controller will use + * Sets what type of input the PID controller will use. */ void PIDController::SetPIDSourceType(PIDSourceType pidSource) { m_pidInput->SetPIDSourceType(pidSource); } /** - * Returns the type of input the PID controller is using + * Returns the type of input the PID controller is using. + * * @return the PID controller input type */ PIDSourceType PIDController::GetPIDSourceType() const { @@ -407,8 +426,10 @@ PIDSourceType PIDController::GetPIDSourceType() const { /** * Returns the current average of the error over the past few iterations. + * * You can specify the number of iterations to average with SetToleranceBuffer() * (defaults to 1). This is the same value that is used for OnTarget(). + * * @return the average error */ float PIDController::GetAvgError() const { @@ -424,6 +445,7 @@ float PIDController::GetAvgError() const { /* * Set the percentage error which is considered tolerable for use with * OnTarget. + * * @param percentage error which is tolerable */ void PIDController::SetTolerance(float percent) { @@ -437,6 +459,7 @@ void PIDController::SetTolerance(float percent) { /* * Set the percentage error which is considered tolerable for use with * OnTarget. + * * @param percentage error which is tolerable */ void PIDController::SetPercentTolerance(float percent) { @@ -450,6 +473,7 @@ void PIDController::SetPercentTolerance(float percent) { /* * Set the absolute error which is considered tolerable for use with * OnTarget. + * * @param percentage error which is tolerable */ void PIDController::SetAbsoluteTolerance(float absTolerance) { @@ -467,6 +491,7 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) { * velocity. This is useful for noisy sensors which return a few erroneous * measurements when the mechanism is on target. However, the mechanism will * not register as on target for at least the specified bufLength cycles. + * * @param bufLength Number of previous cycles to average. Defaults to 1. */ void PIDController::SetToleranceBuffer(unsigned bufLength) { @@ -484,10 +509,11 @@ void PIDController::SetToleranceBuffer(unsigned bufLength) { * Return true if the error is within the percentage of the total input range, * determined by SetTolerance. This asssumes that the maximum and minimum input * were set using SetInput. + * * Currently this just reports on target as the actual value passes through the - * setpoint. - * Ideally it should be based on being within the tolerance for some period of - * time. + * setpoint. Ideally it should be based on being within the tolerance for some + * period of time. + * * This will return false until at least one input value has been computed. */ bool PIDController::OnTarget() const { @@ -496,7 +522,8 @@ bool PIDController::OnTarget() const { double error = GetAvgError(); switch (m_toleranceType) { case kPercentTolerance: - return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput); + return fabs(error) < + m_tolerance / 100 * (m_maximumInput - m_minimumInput); break; case kAbsoluteTolerance: return fabs(error) < m_tolerance; @@ -509,7 +536,7 @@ bool PIDController::OnTarget() const { } /** - * Begin running the PIDController + * Begin running the PIDController. */ void PIDController::Enable() { { @@ -546,7 +573,7 @@ bool PIDController::IsEnabled() const { } /** - * Reset the previous error,, the integral term, and disable the controller. + * Reset the previous error, the integral term, and disable the controller. */ void PIDController::Reset() { Disable(); diff --git a/wpilibc/Athena/src/PWM.cpp b/wpilibc/Athena/src/PWM.cpp index 76d10cc899..10afd48214 100644 --- a/wpilibc/Athena/src/PWM.cpp +++ b/wpilibc/Athena/src/PWM.cpp @@ -7,10 +7,10 @@ #include "PWM.h" +#include "HAL/HAL.hpp" #include "Resource.h" #include "Utility.h" #include "WPIErrors.h" -#include "HAL/HAL.hpp" #include @@ -25,8 +25,9 @@ const int32_t PWM::kPwmDisabled; * Checks channel value range and allocates the appropriate channel. * The allocation is only done to help users ensure that they don't double * assign channels. - * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP - * port + * + * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the + * MXP port */ PWM::PWM(uint32_t channel) { std::stringstream buf; @@ -70,11 +71,11 @@ PWM::~PWM() { /** * Optionally eliminate the deadband from a speed controller. + * * @param eliminateDeadband If true, set the motor curve on the Jaguar to - * eliminate - * the deadband in the middle of the range. Otherwise, keep the full range - * without - * modifying any values. + * eliminate the deadband in the middle of the range. + * Otherwise, keep the full range without modifying + * any values. */ void PWM::EnableDeadbandElimination(bool eliminateDeadband) { if (StatusIsFatal()) return; @@ -83,14 +84,16 @@ void PWM::EnableDeadbandElimination(bool eliminateDeadband) { /** * Set the bounds on the PWM values. + * * This sets the bounds on the PWM values for a particular each type of - * controller. The values - * determine the upper and lower speeds as well as the deadband bracket. - * @param max The Minimum pwm value + * controller. The values determine the upper and lower speeds as well as the + * deadband bracket. + * + * @param max The Minimum pwm value * @param deadbandMax The high end of the deadband range - * @param center The center speed (off) + * @param center The center speed (off) * @param deadbandMin The low end of the deadband range - * @param min The minimum pwm value + * @param min The minimum pwm value */ void PWM::SetBounds(int32_t max, int32_t deadbandMax, int32_t center, int32_t deadbandMin, int32_t min) { @@ -104,14 +107,16 @@ void PWM::SetBounds(int32_t max, int32_t deadbandMax, int32_t center, /** * Set the bounds on the PWM pulse widths. + * * This sets the bounds on the PWM values for a particular type of controller. - * The values - * determine the upper and lower speeds as well as the deadband bracket. - * @param max The max PWM pulse width in ms + * The values determine the upper and lower speeds as well as the deadband + * bracket. + * + * @param max The max PWM pulse width in ms * @param deadbandMax The high end of the deadband range pulse width in ms - * @param center The center (off) pulse width in ms + * @param center The center (off) pulse width in ms * @param deadbandMin The low end of the deadband pulse width in ms - * @param min The minimum pulse width in ms + * @param min The minimum pulse width in ms */ void PWM::SetBounds(double max, double deadbandMax, double center, double deadbandMin, double min) { @@ -157,12 +162,12 @@ void PWM::SetPosition(float pos) { // converting to int unsigned short rawValue = (int32_t)((pos * (float)GetFullRangeScaleFactor()) + GetMinNegativePwm()); - // printf("MinNegPWM: %d FullRangeScaleFactor: %d Raw value: %5d Input - //value: %4.4f\n", GetMinNegativePwm(), GetFullRangeScaleFactor(), rawValue, - //pos); + // printf("MinNegPWM: %d FullRangeScaleFactor: %d Raw value: %5d Input " + // "value: %4.4f\n", GetMinNegativePwm(), GetFullRangeScaleFactor(), rawValue, + // pos); - // wpi_assert((rawValue >= GetMinNegativePwm()) && (rawValue <= - //GetMaxPositivePwm())); + // wpi_assert((rawValue >= GetMinNegativePwm()) && (rawValue <= + // GetMaxPositivePwm())); wpi_assert(rawValue != kPwmDisabled); // send the computed pwm value to the FPGA diff --git a/wpilibc/Athena/src/PWMSpeedController.cpp b/wpilibc/Athena/src/PWMSpeedController.cpp index ee93a170d0..e117889849 100644 --- a/wpilibc/Athena/src/PWMSpeedController.cpp +++ b/wpilibc/Athena/src/PWMSpeedController.cpp @@ -8,12 +8,12 @@ #include "PWMSpeedController.h" /** - * Constructor for a PWM Speed Controller connected via PWM + * Constructor for a PWM Speed Controller connected via PWM. + * * @param channel The PWM channel that the controller is attached to. 0-9 are - * on-board, 10-19 are on the MXP port + * on-board, 10-19 are on the MXP port */ -PWMSpeedController::PWMSpeedController(uint32_t channel) : SafePWM(channel) { -} +PWMSpeedController::PWMSpeedController(uint32_t channel) : SafePWM(channel) {} /** * Set the PWM value. @@ -21,7 +21,7 @@ PWMSpeedController::PWMSpeedController(uint32_t channel) : SafePWM(channel) { * The PWM value is set using a range of -1.0 to 1.0, appropriately * scaling the value for the FPGA. * - * @param speed The speed value between -1.0 and 1.0 to set. + * @param speed The speed value between -1.0 and 1.0 to set. * @param syncGroup Unused interface. */ void PWMSpeedController::Set(float speed, uint8_t syncGroup) { @@ -41,10 +41,13 @@ float PWMSpeedController::Get() const { return GetSpeed(); } void PWMSpeedController::Disable() { SetRaw(kPwmDisabled); } /** -* Common interface for inverting direction of a speed controller. -* @param isInverted The state of inversion, true is inverted. -*/ -void PWMSpeedController::SetInverted(bool isInverted) { m_isInverted = isInverted; } + * Common interface for inverting direction of a speed controller. + * + * @param isInverted The state of inversion, true is inverted. + */ +void PWMSpeedController::SetInverted(bool isInverted) { + m_isInverted = isInverted; +} /** * Common interface for the inverting direction of a speed controller. @@ -64,4 +67,4 @@ void PWMSpeedController::PIDWrite(float output) { Set(output); } /** * Common interface to stop the motor until Set is called again. */ -void PWMSpeedController::StopMotor() { this->SafePWM::StopMotor(); } \ No newline at end of file +void PWMSpeedController::StopMotor() { this->SafePWM::StopMotor(); } diff --git a/wpilibc/Athena/src/PowerDistributionPanel.cpp b/wpilibc/Athena/src/PowerDistributionPanel.cpp index 8378e2f0e3..7049ee126b 100644 --- a/wpilibc/Athena/src/PowerDistributionPanel.cpp +++ b/wpilibc/Athena/src/PowerDistributionPanel.cpp @@ -6,9 +6,9 @@ /*----------------------------------------------------------------------------*/ #include "PowerDistributionPanel.h" -#include "WPIErrors.h" #include "HAL/PDP.hpp" #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" #include @@ -23,7 +23,8 @@ PowerDistributionPanel::PowerDistributionPanel(uint8_t module) } /** - * Query the input voltage of the PDP + * Query the input voltage of the PDP. + * * @return The voltage of the PDP in volts */ double PowerDistributionPanel::GetVoltage() const { @@ -39,7 +40,8 @@ double PowerDistributionPanel::GetVoltage() const { } /** - * Query the temperature of the PDP + * Query the temperature of the PDP. + * * @return The temperature of the PDP in degrees Celsius */ double PowerDistributionPanel::GetTemperature() const { @@ -55,7 +57,8 @@ double PowerDistributionPanel::GetTemperature() const { } /** - * Query the current of a single channel of the PDP + * Query the current of a single channel of the PDP. + * * @return The current of one of the PDP channels (channels 0-15) in Amperes */ double PowerDistributionPanel::GetCurrent(uint8_t channel) const { @@ -77,7 +80,8 @@ double PowerDistributionPanel::GetCurrent(uint8_t channel) const { } /** - * Query the total current of all monitored PDP channels (0-15) + * Query the total current of all monitored PDP channels (0-15). + * * @return The the total current drawn from the PDP channels in Amperes */ double PowerDistributionPanel::GetTotalCurrent() const { @@ -93,7 +97,8 @@ double PowerDistributionPanel::GetTotalCurrent() const { } /** - * Query the total power drawn from the monitored PDP channels + * Query the total power drawn from the monitored PDP channels. + * * @return The the total power drawn from the PDP channels in Watts */ double PowerDistributionPanel::GetTotalPower() const { @@ -109,7 +114,8 @@ double PowerDistributionPanel::GetTotalPower() const { } /** - * Query the total energy drawn from the monitored PDP channels + * Query the total energy drawn from the monitored PDP channels. + * * @return The the total energy drawn from the PDP channels in Joules */ double PowerDistributionPanel::GetTotalEnergy() const { @@ -125,7 +131,8 @@ double PowerDistributionPanel::GetTotalEnergy() const { } /** - * Reset the total energy drawn from the PDP + * Reset the total energy drawn from the PDP. + * * @see PowerDistributionPanel#GetTotalEnergy */ void PowerDistributionPanel::ResetTotalEnergy() { @@ -139,7 +146,7 @@ void PowerDistributionPanel::ResetTotalEnergy() { } /** - * Remove all of the fault flags on the PDP + * Remove all of the fault flags on the PDP. */ void PowerDistributionPanel::ClearStickyFaults() { int32_t status = 0; @@ -187,4 +194,6 @@ void PowerDistributionPanel::InitTable(std::shared_ptr subTable) { UpdateTable(); } -std::shared_ptr PowerDistributionPanel::GetTable() const { return m_table; } +std::shared_ptr PowerDistributionPanel::GetTable() const { + return m_table; +} diff --git a/wpilibc/Athena/src/Preferences.cpp b/wpilibc/Athena/src/Preferences.cpp index 60a5520e3b..731d5d3ecf 100644 --- a/wpilibc/Athena/src/Preferences.cpp +++ b/wpilibc/Athena/src/Preferences.cpp @@ -7,14 +7,14 @@ #include "Preferences.h" -#include "WPIErrors.h" #include "HAL/HAL.hpp" +#include "WPIErrors.h" #include #include /** The Preferences table name */ -static const char *kTableName = "Preferences"; +static const char* kTableName = "Preferences"; void Preferences::Listener::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, @@ -31,16 +31,18 @@ Preferences::Preferences() : m_table(NetworkTable::GetTable(kTableName)) { } /** - * Get the one and only {@link Preferences} object + * Get the one and only {@link Preferences} object. + * * @return pointer to the {@link Preferences} */ -Preferences *Preferences::GetInstance() { +Preferences* Preferences::GetInstance() { static Preferences instance; return &instance; } /** - * Returns a vector of all the keys + * Returns a vector of all the keys. + * * @return a vector of the keys */ std::vector Preferences::GetKeys() { return m_table->GetKeys(); } @@ -48,7 +50,8 @@ std::vector Preferences::GetKeys() { return m_table->GetKeys(); } /** * Returns the string at the given key. If this table does not have a value * for that position, then the given defaultValue will be returned. - * @param key the key + * + * @param key the key * @param defaultValue the value to return if none exists in the table * @return either the value in the table, or the defaultValue */ @@ -60,7 +63,8 @@ std::string Preferences::GetString(llvm::StringRef key, /** * Returns the int at the given key. If this table does not have a value * for that position, then the given defaultValue value will be returned. - * @param key the key + * + * @param key the key * @param defaultValue the value to return if none exists in the table * @return either the value in the table, or the defaultValue */ @@ -71,7 +75,8 @@ int Preferences::GetInt(llvm::StringRef key, int defaultValue) { /** * Returns the double at the given key. If this table does not have a value * for that position, then the given defaultValue value will be returned. - * @param key the key + * + * @param key the key * @param defaultValue the value to return if none exists in the table * @return either the value in the table, or the defaultValue */ @@ -82,7 +87,8 @@ double Preferences::GetDouble(llvm::StringRef key, double defaultValue) { /** * Returns the float at the given key. If this table does not have a value * for that position, then the given defaultValue value will be returned. - * @param key the key + * + * @param key the key * @param defaultValue the value to return if none exists in the table * @return either the value in the table, or the defaultValue */ @@ -93,7 +99,8 @@ float Preferences::GetFloat(llvm::StringRef key, float defaultValue) { /** * Returns the boolean at the given key. If this table does not have a value * for that position, then the given defaultValue value will be returned. - * @param key the key + * + * @param key the key * @param defaultValue the value to return if none exists in the table * @return either the value in the table, or the defaultValue */ @@ -103,9 +110,9 @@ bool Preferences::GetBoolean(llvm::StringRef key, bool defaultValue) { /** * Returns the long (int64_t) at the given key. If this table does not have a - * value - * for that position, then the given defaultValue value will be returned. - * @param key the key + * value for that position, then the given defaultValue value will be returned. + * + * @param key the key * @param defaultValue the value to return if none exists in the table * @return either the value in the table, or the defaultValue */ @@ -119,7 +126,7 @@ int64_t Preferences::GetLong(llvm::StringRef key, int64_t defaultValue) { *

The value may not have quotation marks, nor may the key * have any whitespace nor an equals sign

* - * @param key the key + * @param key the key * @param value the value */ void Preferences::PutString(llvm::StringRef key, llvm::StringRef value) { @@ -132,7 +139,7 @@ void Preferences::PutString(llvm::StringRef key, llvm::StringRef value) { * *

The key may not have any whitespace nor an equals sign

* - * @param key the key + * @param key the key * @param value the value */ void Preferences::PutInt(llvm::StringRef key, int value) { @@ -145,7 +152,7 @@ void Preferences::PutInt(llvm::StringRef key, int value) { * *

The key may not have any whitespace nor an equals sign

* - * @param key the key + * @param key the key * @param value the value */ void Preferences::PutDouble(llvm::StringRef key, double value) { @@ -158,7 +165,7 @@ void Preferences::PutDouble(llvm::StringRef key, double value) { * *

The key may not have any whitespace nor an equals sign

* - * @param key the key + * @param key the key * @param value the value */ void Preferences::PutFloat(llvm::StringRef key, float value) { @@ -171,7 +178,7 @@ void Preferences::PutFloat(llvm::StringRef key, float value) { * *

The key may not have any whitespace nor an equals sign

* - * @param key the key + * @param key the key * @param value the value */ void Preferences::PutBoolean(llvm::StringRef key, bool value) { @@ -184,7 +191,7 @@ void Preferences::PutBoolean(llvm::StringRef key, bool value) { * *

The key may not have any whitespace nor an equals sign

* - * @param key the key + * @param key the key * @param value the value */ void Preferences::PutLong(llvm::StringRef key, int64_t value) { @@ -202,6 +209,7 @@ void Preferences::Save() {} /** * Returns whether or not there is a key with the given name. + * * @param key the key * @return if there is a value at the given key */ @@ -210,9 +218,8 @@ bool Preferences::ContainsKey(llvm::StringRef key) { } /** - * Remove a preference + * Remove a preference. + * * @param key the key */ -void Preferences::Remove(llvm::StringRef key) { - m_table->Delete(key); -} +void Preferences::Remove(llvm::StringRef key) { m_table->Delete(key); } diff --git a/wpilibc/Athena/src/Relay.cpp b/wpilibc/Athena/src/Relay.cpp index 5290d8b826..f7bf3ccb14 100644 --- a/wpilibc/Athena/src/Relay.cpp +++ b/wpilibc/Athena/src/Relay.cpp @@ -7,11 +7,11 @@ #include "Relay.h" +#include "HAL/HAL.hpp" +#include "LiveWindow/LiveWindow.h" #include "MotorSafetyHelper.h" #include "Resource.h" #include "WPIErrors.h" -#include "LiveWindow/LiveWindow.h" -#include "HAL/HAL.hpp" #include @@ -23,7 +23,8 @@ static std::unique_ptr relayChannels; * * This code initializes the relay and reserves all resources that need to be * locked. Initially the relay is set to both lines at 0v. - * @param channel The channel number (0-3). + * + * @param channel The channel number (0-3). * @param direction The direction that the Relay object will control. */ Relay::Relay(uint32_t channel, Relay::Direction direction) @@ -71,6 +72,7 @@ Relay::Relay(uint32_t channel, Relay::Direction direction) /** * Free the resource associated with a relay. + * * The relay channels are set to free and the relay output is turned off. */ Relay::~Relay() { @@ -95,13 +97,11 @@ Relay::~Relay() { * object. * * When set to kBothDirections, the relay can be any of the four states: - * 0v-0v, 0v-12v, 12v-0v, 12v-12v + * 0v-0v, 0v-12v, 12v-0v, 12v-12v * * When set to kForwardOnly or kReverseOnly, you can specify the constant for - * the - * direction or you can simply specify kOff and kOn. Using only kOff and - * kOn is - * recommended. + * the direction or you can simply specify kOff and kOn. Using only kOff and + * kOn is recommended. * * @param value The state to set the relay. */ @@ -194,9 +194,7 @@ Relay::Value Relay::Get() const { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } -uint32_t Relay::GetChannel() const { - return m_channel; -} +uint32_t Relay::GetChannel() const { return m_channel; } /** * Set the expiration time for the Relay object @@ -214,21 +212,25 @@ float Relay::GetExpiration() const { return m_safetyHelper->GetExpiration(); } /** * Check if the relay object is currently alive or stopped due to a timeout. - * @returns a bool value that is true if the motor has NOT timed out and should - * still be running. + * + * @return a bool value that is true if the motor has NOT timed out and should + * still be running. */ bool Relay::IsAlive() const { return m_safetyHelper->IsAlive(); } /** * Stop the motor associated with this PWM object. + * * This is called by the MotorSafetyHelper object when it has a timeout for this * relay and needs to stop it from running. */ void Relay::StopMotor() { Set(kOff); } /** - * Enable/disable motor safety for this device + * Enable/disable motor safety for this device. + * * Turn on and off the motor safety option for this relay object. + * * @param enabled True if motor safety is enforced for this object */ void Relay::SetSafetyEnabled(bool enabled) { @@ -236,7 +238,8 @@ void Relay::SetSafetyEnabled(bool enabled) { } /** - * Check if motor safety is enabled for this object + * Check if motor safety is enabled for this object. + * * @returns True if motor safety is enforced for this object */ bool Relay::IsSafetyEnabled() const { @@ -250,10 +253,14 @@ void Relay::GetDescription(std::ostringstream& desc) const { void Relay::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) { if (!value->IsString()) return; - if (value->GetString() == "Off") Set(kOff); - else if (value->GetString() == "Forward") Set(kForward); - else if (value->GetString() == "Reverse") Set(kReverse); - else if (value->GetString() == "On") Set(kOn); + if (value->GetString() == "Off") + Set(kOff); + else if (value->GetString() == "Forward") + Set(kForward); + else if (value->GetString() == "Reverse") + Set(kReverse); + else if (value->GetString() == "On") + Set(kOn); } void Relay::UpdateTable() { diff --git a/wpilibc/Athena/src/RobotBase.cpp b/wpilibc/Athena/src/RobotBase.cpp index 8d7b96f49b..df8c7e10db 100644 --- a/wpilibc/Athena/src/RobotBase.cpp +++ b/wpilibc/Athena/src/RobotBase.cpp @@ -7,41 +7,40 @@ #include "RobotBase.h" +#include +#include #include "DriverStation.h" -#include "RobotState.h" +#include "HAL/HAL.hpp" #include "HLUsageReporting.h" #include "Internal/HardwareHLReporting.h" +#include "RobotState.h" #include "Utility.h" #include "networktables/NetworkTable.h" -#include -#include "HAL/HAL.hpp" -#include -RobotBase *RobotBase::m_instance = nullptr; +RobotBase* RobotBase::m_instance = nullptr; -void RobotBase::setInstance(RobotBase *robot) { +void RobotBase::setInstance(RobotBase* robot) { wpi_assert(m_instance == nullptr); m_instance = robot; } -RobotBase &RobotBase::getInstance() { return *m_instance; } +RobotBase& RobotBase::getInstance() { return *m_instance; } -void RobotBase::robotSetup(RobotBase *robot) { +void RobotBase::robotSetup(RobotBase* robot) { printf("\n********** Robot program starting **********\n"); robot->StartCompetition(); } /** * Constructor for a generic robot program. + * * User code should be placed in the constructor that runs before the Autonomous - * or Operator - * Control period starts. The constructor will run to completion before - * Autonomous is entered. + * or Operator Control period starts. The constructor will run to completion + * before Autonomous is entered. * * This must be used to ensure that the communications code starts. In the - * future it would be - * nice to put this code into it's own task that loads on boot so ensure that it - * runs. + * future it would be nice to put this code into it's own task that loads on + * boot so ensure that it runs. */ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) { RobotState::SetImplementation(DriverStation::GetInstance()); @@ -52,7 +51,7 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) { NetworkTable::SetNetworkIdentity("Robot"); NetworkTable::SetPersistentFilename("/home/lvuser/networktables.ini"); - FILE *file = nullptr; + FILE* file = nullptr; file = fopen("/tmp/frc_versions/FRC_Lib_Version.ini", "w"); if (file != nullptr) { @@ -64,8 +63,7 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) { /** * Free the resources for a RobotBase class. * This includes deleting all classes that might have been allocated as - * Singletons to they - * would never be deleted except here. + * Singletons to they would never be deleted except here. */ RobotBase::~RobotBase() { SensorBase::DeleteSingletons(); @@ -118,10 +116,8 @@ bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); } * This class exists for the sole purpose of getting its destructor called when * the module unloads. * Before the module is done unloading, we need to delete the RobotBase derived - * singleton. This should delete - * the other remaining singletons that were registered. This should also stop - * all tasks that are using - * the Task class. + * singleton. This should delete the other remaining singletons that were + * registered. This should also stop all tasks that are using the Task class. */ class RobotDeleter { public: diff --git a/wpilibc/Athena/src/RobotDrive.cpp b/wpilibc/Athena/src/RobotDrive.cpp index 69184e3463..8617b4229b 100644 --- a/wpilibc/Athena/src/RobotDrive.cpp +++ b/wpilibc/Athena/src/RobotDrive.cpp @@ -7,34 +7,34 @@ #include "RobotDrive.h" +#include #include "CANJaguar.h" #include "GenericHID.h" #include "Joystick.h" #include "Talon.h" #include "Utility.h" #include "WPIErrors.h" -#include #undef max #include const int32_t RobotDrive::kMaxNumberOfMotors; -static auto make_shared_nodelete(SpeedController *ptr) { +static auto make_shared_nodelete(SpeedController* ptr) { return std::shared_ptr(ptr, NullDeleter()); } /* * Driving functions * These functions provide an interface to multiple motors that is used for C - * programming + * programming. * The Drive(speed, direction) function is the main part of the set that makes - * it easy - * to set speeds and direction independently in one call. + * it easy to set speeds and direction independently in one call. */ /** * Common function to initialize all the robot drive constructors. + * * Create a motor safety object (the real reason for the common code) and * initialize all the motor assignments. The default timeout is set for the * robot drive. @@ -46,13 +46,15 @@ void RobotDrive::InitRobotDrive() { /** * Constructor for RobotDrive with 2 motors specified with channel numbers. + * * Set up parameters for a two wheel drive system where the * left and right motor pwm channels are specified in the call. * This call assumes Talons for controlling the motors. - * @param leftMotorChannel The PWM channel number that drives the left motor. - * 0-9 are on-board, 10-19 are on the MXP port + * + * @param leftMotorChannel The PWM channel number that drives the left motor. + * 0-9 are on-board, 10-19 are on the MXP port * @param rightMotorChannel The PWM channel number that drives the right motor. - * 0-9 are on-board, 10-19 are on the MXP port + * 0-9 are on-board, 10-19 are on the MXP port */ RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel) { InitRobotDrive(); @@ -63,17 +65,19 @@ RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel) { /** * Constructor for RobotDrive with 4 motors specified with channel numbers. + * * Set up parameters for a four wheel drive system where all four motor * pwm channels are specified in the call. * This call assumes Talons for controlling the motors. - * @param frontLeftMotor Front left motor channel number. 0-9 are on-board, - * 10-19 are on the MXP port - * @param rearLeftMotor Rear Left motor channel number. 0-9 are on-board, 10-19 - * are on the MXP port + * + * @param frontLeftMotor Front left motor channel number. 0-9 are on-board, + * 10-19 are on the MXP port + * @param rearLeftMotor Rear Left motor channel number. 0-9 are on-board, + * 10-19 are on the MXP port * @param frontRightMotor Front right motor channel number. 0-9 are on-board, - * 10-19 are on the MXP port - * @param rearRightMotor Rear Right motor channel number. 0-9 are on-board, - * 10-19 are on the MXP port + * 10-19 are on the MXP port + * @param rearRightMotor Rear Right motor channel number. 0-9 are on-board, + * 10-19 are on the MXP port */ RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor, uint32_t frontRightMotor, uint32_t rearRightMotor) { @@ -88,16 +92,17 @@ RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor, /** * Constructor for RobotDrive with 2 motors specified as SpeedController * objects. + * * The SpeedController version of the constructor enables programs to use the - * RobotDrive classes with - * subclasses of the SpeedController objects, for example, versions with ramping - * or reshaping of - * the curve to suit motor bias or deadband elimination. - * @param leftMotor The left SpeedController object used to drive the robot. - * @param rightMotor the right SpeedController object used to drive the robot. + * RobotDrive classes with subclasses of the SpeedController objects, for + * example, versions with ramping or reshaping of the curve to suit motor bias + * or deadband elimination. + * + * @param leftMotor The left SpeedController object used to drive the robot. + * @param rightMotor The right SpeedController object used to drive the robot. */ -RobotDrive::RobotDrive(SpeedController *leftMotor, - SpeedController *rightMotor) { +RobotDrive::RobotDrive(SpeedController* leftMotor, + SpeedController* rightMotor) { InitRobotDrive(); if (leftMotor == nullptr || rightMotor == nullptr) { wpi_setWPIError(NullParameter); @@ -108,9 +113,9 @@ RobotDrive::RobotDrive(SpeedController *leftMotor, m_rearRightMotor = make_shared_nodelete(rightMotor); } -//TODO: Change to rvalue references & move syntax. -RobotDrive::RobotDrive(SpeedController &leftMotor, - SpeedController &rightMotor) { +// TODO: Change to rvalue references & move syntax. +RobotDrive::RobotDrive(SpeedController& leftMotor, + SpeedController& rightMotor) { InitRobotDrive(); m_rearLeftMotor = make_shared_nodelete(&leftMotor); m_rearRightMotor = make_shared_nodelete(&rightMotor); @@ -131,20 +136,22 @@ RobotDrive::RobotDrive(std::shared_ptr leftMotor, /** * Constructor for RobotDrive with 4 motors specified as SpeedController * objects. + * * Speed controller input version of RobotDrive (see previous comments). - * @param rearLeftMotor The back left SpeedController object used to drive the - * robot. - * @param frontLeftMotor The front left SpeedController object used to drive the - * robot - * @param rearRightMotor The back right SpeedController object used to drive the - * robot. + * + * @param rearLeftMotor The back left SpeedController object used to drive + * the robot. + * @param frontLeftMotor The front left SpeedController object used to drive + * the robot. + * @param rearRightMotor The back right SpeedController object used to drive + * the robot. * @param frontRightMotor The front right SpeedController object used to drive - * the robot. + * the robot. */ -RobotDrive::RobotDrive(SpeedController *frontLeftMotor, - SpeedController *rearLeftMotor, - SpeedController *frontRightMotor, - SpeedController *rearRightMotor) { +RobotDrive::RobotDrive(SpeedController* frontLeftMotor, + SpeedController* rearLeftMotor, + SpeedController* frontRightMotor, + SpeedController* rearRightMotor) { InitRobotDrive(); if (frontLeftMotor == nullptr || rearLeftMotor == nullptr || frontRightMotor == nullptr || rearRightMotor == nullptr) { @@ -157,10 +164,10 @@ RobotDrive::RobotDrive(SpeedController *frontLeftMotor, m_rearRightMotor = make_shared_nodelete(rearRightMotor); } -RobotDrive::RobotDrive(SpeedController &frontLeftMotor, - SpeedController &rearLeftMotor, - SpeedController &frontRightMotor, - SpeedController &rearRightMotor) { +RobotDrive::RobotDrive(SpeedController& frontLeftMotor, + SpeedController& rearLeftMotor, + SpeedController& frontRightMotor, + SpeedController& rearRightMotor) { InitRobotDrive(); m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor); m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor); @@ -197,9 +204,11 @@ RobotDrive::RobotDrive(std::shared_ptr frontLeftMotor, * This function will most likely be used in an autonomous routine. * * @param outputMagnitude The speed setting for the outside wheel in a turn, - * forward or backwards, +1 to -1. - * @param curve The rate of turn, constant for different forward speeds. Set - * curve < 0 for left turn or curve > 0 for right turn. + * forward or backwards, +1 to -1. + * @param curve The rate of turn, constant for different forward + * speeds. Set curve < 0 for left turn or curve > 0 for + * right turn. + * * Set curve = e^(-r/w) to get a turn radius r for wheelbase w of your robot. * Conversely, turn radius r = -ln(curve)*w for a given value of curve and * wheelbase w. @@ -234,12 +243,14 @@ void RobotDrive::Drive(float outputMagnitude, float curve) { /** * Provide tank steering using the stored robot configuration. + * * Drive the robot using two joystick inputs. The Y-axis will be selected from * each Joystick object. - * @param leftStick The joystick to control the left side of the robot. + * + * @param leftStick The joystick to control the left side of the robot. * @param rightStick The joystick to control the right side of the robot. */ -void RobotDrive::TankDrive(GenericHID *leftStick, GenericHID *rightStick, +void RobotDrive::TankDrive(GenericHID* leftStick, GenericHID* rightStick, bool squaredInputs) { if (leftStick == nullptr || rightStick == nullptr) { wpi_setWPIError(NullParameter); @@ -248,23 +259,25 @@ void RobotDrive::TankDrive(GenericHID *leftStick, GenericHID *rightStick, TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs); } -void RobotDrive::TankDrive(GenericHID &leftStick, GenericHID &rightStick, +void RobotDrive::TankDrive(GenericHID& leftStick, GenericHID& rightStick, bool squaredInputs) { TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs); } /** * Provide tank steering using the stored robot configuration. + * * This function lets you pick the axis to be used on each Joystick object for - * the left - * and right sides of the robot. - * @param leftStick The Joystick object to use for the left side of the robot. - * @param leftAxis The axis to select on the left side Joystick object. - * @param rightStick The Joystick object to use for the right side of the robot. - * @param rightAxis The axis to select on the right side Joystick object. + * the left and right sides of the robot. + * + * @param leftStick The Joystick object to use for the left side of the robot. + * @param leftAxis The axis to select on the left side Joystick object. + * @param rightStick The Joystick object to use for the right side of the + * robot. + * @param rightAxis The axis to select on the right side Joystick object. */ -void RobotDrive::TankDrive(GenericHID *leftStick, uint32_t leftAxis, - GenericHID *rightStick, uint32_t rightAxis, +void RobotDrive::TankDrive(GenericHID* leftStick, uint32_t leftAxis, + GenericHID* rightStick, uint32_t rightAxis, bool squaredInputs) { if (leftStick == nullptr || rightStick == nullptr) { wpi_setWPIError(NullParameter); @@ -274,8 +287,8 @@ void RobotDrive::TankDrive(GenericHID *leftStick, uint32_t leftAxis, squaredInputs); } -void RobotDrive::TankDrive(GenericHID &leftStick, uint32_t leftAxis, - GenericHID &rightStick, uint32_t rightAxis, +void RobotDrive::TankDrive(GenericHID& leftStick, uint32_t leftAxis, + GenericHID& rightStick, uint32_t rightAxis, bool squaredInputs) { TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis), squaredInputs); @@ -283,8 +296,10 @@ void RobotDrive::TankDrive(GenericHID &leftStick, uint32_t leftAxis, /** * Provide tank steering using the stored robot configuration. + * * This function lets you directly provide joystick values from any source. - * @param leftValue The value of the left stick. + * + * @param leftValue The value of the left stick. * @param rightValue The value of the right stick. */ void RobotDrive::TankDrive(float leftValue, float rightValue, @@ -318,55 +333,58 @@ void RobotDrive::TankDrive(float leftValue, float rightValue, /** * Arcade drive implements single stick driving. + * * Given a single Joystick, the class assumes the Y axis for the move value and - * the X axis - * for the rotate value. + * the X axis for the rotate value. * (Should add more information here regarding the way that arcade drive works.) - * @param stick The joystick to use for Arcade single-stick driving. The Y-axis - * will be selected - * for forwards/backwards and the X-axis will be selected for rotation rate. + * + * @param stick The joystick to use for Arcade single-stick driving. + * The Y-axis will be selected for forwards/backwards and + * the X-axis will be selected for rotation rate. * @param squaredInputs If true, the sensitivity will be increased for small - * values + * values */ -void RobotDrive::ArcadeDrive(GenericHID *stick, bool squaredInputs) { +void RobotDrive::ArcadeDrive(GenericHID* stick, bool squaredInputs) { // simply call the full-featured ArcadeDrive with the appropriate values ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs); } /** * Arcade drive implements single stick driving. + * * Given a single Joystick, the class assumes the Y axis for the move value and - * the X axis - * for the rotate value. + * the X axis for the rotate value. * (Should add more information here regarding the way that arcade drive works.) - * @param stick The joystick to use for Arcade single-stick driving. The Y-axis - * will be selected - * for forwards/backwards and the X-axis will be selected for rotation rate. + * + * @param stick The joystick to use for Arcade single-stick driving. + * The Y-axis will be selected for forwards/backwards and + * the X-axis will be selected for rotation rate. * @param squaredInputs If true, the sensitivity will be increased for small - * values + * values */ -void RobotDrive::ArcadeDrive(GenericHID &stick, bool squaredInputs) { +void RobotDrive::ArcadeDrive(GenericHID& stick, bool squaredInputs) { // simply call the full-featured ArcadeDrive with the appropriate values ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs); } /** * Arcade drive implements single stick driving. + * * Given two joystick instances and two axis, compute the values to send to - * either two - * or four motors. - * @param moveStick The Joystick object that represents the forward/backward - * direction - * @param moveAxis The axis on the moveStick object to use for fowards/backwards - * (typically Y_AXIS) - * @param rotateStick The Joystick object that represents the rotation value - * @param rotateAxis The axis on the rotation object to use for the rotate - * right/left (typically X_AXIS) - * @param squaredInputs Setting this parameter to true increases the sensitivity - * at lower speeds + * either two or four motors. + * + * @param moveStick The Joystick object that represents the + * forward/backward direction + * @param moveAxis The axis on the moveStick object to use for + * forwards/backwards (typically Y_AXIS) + * @param rotateStick The Joystick object that represents the rotation value + * @param rotateAxis The axis on the rotation object to use for the rotate + * right/left (typically X_AXIS) + * @param squaredInputs Setting this parameter to true increases the + * sensitivity at lower speeds */ -void RobotDrive::ArcadeDrive(GenericHID *moveStick, uint32_t moveAxis, - GenericHID *rotateStick, uint32_t rotateAxis, +void RobotDrive::ArcadeDrive(GenericHID* moveStick, uint32_t moveAxis, + GenericHID* rotateStick, uint32_t rotateAxis, bool squaredInputs) { float moveValue = moveStick->GetRawAxis(moveAxis); float rotateValue = rotateStick->GetRawAxis(rotateAxis); @@ -376,22 +394,22 @@ void RobotDrive::ArcadeDrive(GenericHID *moveStick, uint32_t moveAxis, /** * Arcade drive implements single stick driving. + * * Given two joystick instances and two axis, compute the values to send to - * either two - * or four motors. - * @param moveStick The Joystick object that represents the forward/backward - * direction - * @param moveAxis The axis on the moveStick object to use for fowards/backwards - * (typically Y_AXIS) - * @param rotateStick The Joystick object that represents the rotation value - * @param rotateAxis The axis on the rotation object to use for the rotate - * right/left (typically X_AXIS) - * @param squaredInputs Setting this parameter to true increases the sensitivity - * at lower speeds + * either two or four motors. + * + * @param moveStick The Joystick object that represents the + * forward/backward direction + * @param moveAxis The axis on the moveStick object to use for + * forwards/backwards (typically Y_AXIS) + * @param rotateStick The Joystick object that represents the rotation value + * @param rotateAxis The axis on the rotation object to use for the rotate + * right/left (typically X_AXIS) + * @param squaredInputs Setting this parameter to true increases the + * sensitivity at lower speeds */ - -void RobotDrive::ArcadeDrive(GenericHID &moveStick, uint32_t moveAxis, - GenericHID &rotateStick, uint32_t rotateAxis, +void RobotDrive::ArcadeDrive(GenericHID& moveStick, uint32_t moveAxis, + GenericHID& rotateStick, uint32_t rotateAxis, bool squaredInputs) { float moveValue = moveStick.GetRawAxis(moveAxis); float rotateValue = rotateStick.GetRawAxis(rotateAxis); @@ -401,9 +419,11 @@ void RobotDrive::ArcadeDrive(GenericHID &moveStick, uint32_t moveAxis, /** * Arcade drive implements single stick driving. + * * This function lets you directly provide joystick values from any source. - * @param moveValue The value to use for fowards/backwards - * @param rotateValue The value to use for the rotate right/left + * + * @param moveValue The value to use for fowards/backwards + * @param rotateValue The value to use for the rotate right/left * @param squaredInputs If set, increases the sensitivity at low speeds */ void RobotDrive::ArcadeDrive(float moveValue, float rotateValue, @@ -468,16 +488,15 @@ void RobotDrive::ArcadeDrive(float moveValue, float rotateValue, * * This is designed to be directly driven by joystick axes. * - * @param x The speed that the robot should drive in the X direction. - * [-1.0..1.0] - * @param y The speed that the robot should drive in the Y direction. - * This input is inverted to match the forward == -1.0 that joysticks produce. - * [-1.0..1.0] - * @param rotation The rate of rotation for the robot that is completely - * independent of - * the translation. [-1.0..1.0] + * @param x The speed that the robot should drive in the X direction. + * [-1.0..1.0] + * @param y The speed that the robot should drive in the Y direction. + * This input is inverted to match the forward == -1.0 that + * joysticks produce. [-1.0..1.0] + * @param rotation The rate of rotation for the robot that is completely + * independent of the translation. [-1.0..1.0] * @param gyroAngle The current angle reading from the gyro. Use this to - * implement field-oriented controls. + * implement field-oriented controls. */ void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float gyroAngle) { @@ -528,13 +547,12 @@ void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, * across the robot. * * @param magnitude The speed that the robot should drive in a given direction. - * [-1.0..1.0] + * [-1.0..1.0] * @param direction The direction the robot should drive in degrees. The - * direction and maginitute are - * independent of the rotation rate. - * @param rotation The rate of rotation for the robot that is completely - * independent of - * the magnitute or direction. [-1.0..1.0] + * direction and maginitute are independent of the rotation + * rate. + * @param rotation The rate of rotation for the robot that is completely + * independent of the magnitute or direction. [-1.0..1.0] */ void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rotation) { @@ -581,24 +599,25 @@ void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, * This is an alias to MecanumDrive_Polar() for backward compatability * * @param magnitude The speed that the robot should drive in a given direction. - * [-1.0..1.0] + * [-1.0..1.0] * @param direction The direction the robot should drive. The direction and - * maginitute are - * independent of the rotation rate. - * @param rotation The rate of rotation for the robot that is completely - * independent of - * the magnitute or direction. [-1.0..1.0] + * magnitude are independent of the rotation rate. + * @param rotation The rate of rotation for the robot that is completely + * independent of the magnitude or direction. [-1.0..1.0] */ void RobotDrive::HolonomicDrive(float magnitude, float direction, float rotation) { MecanumDrive_Polar(magnitude, direction, rotation); } -/** Set the speed of the right and left motors. +/** + * Set the speed of the right and left motors. + * * This is used once an appropriate drive setup function is called such as * TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput" * and includes flipping the direction of one side for opposing motors. - * @param leftOutput The speed to send to the left side of the robot. + * + * @param leftOutput The speed to send to the left side of the robot. * @param rightOutput The speed to send to the right side of the robot. */ void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput) { @@ -635,7 +654,7 @@ float RobotDrive::Limit(float num) { /** * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0. */ -void RobotDrive::Normalize(double *wheelSpeeds) { +void RobotDrive::Normalize(double* wheelSpeeds) { double maxMagnitude = fabs(wheelSpeeds[0]); int32_t i; for (i = 1; i < kMaxNumberOfMotors; i++) { @@ -652,7 +671,7 @@ void RobotDrive::Normalize(double *wheelSpeeds) { /** * Rotate a vector in Cartesian space. */ -void RobotDrive::RotateVector(double &x, double &y, double angle) { +void RobotDrive::RotateVector(double& x, double& y, double angle) { double cosA = cos(angle * (3.14159 / 180.0)); double sinA = sin(angle * (3.14159 / 180.0)); double xOut = x * cosA - y * sinA; @@ -663,11 +682,12 @@ void RobotDrive::RotateVector(double &x, double &y, double angle) { /* * Invert a motor direction. + * * This is used when a motor should run in the opposite direction as the drive * code would normally run it. Motors that are direct drive would be inverted, - * the - * Drive code assumes that the motors are geared with one reversal. - * @param motor The motor index to invert. + * the Drive code assumes that the motors are geared with one reversal. + * + * @param motor The motor index to invert. * @param isInverted True if the motor should be inverted when operated. */ void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) { @@ -695,8 +715,9 @@ void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) { * Set the turning sensitivity. * * This only impacts the Drive() entry-point. + * * @param sensitivity Effectively sets the turning sensitivity (or turn radius - * for a given value) + * for a given value) */ void RobotDrive::SetSensitivity(float sensitivity) { m_sensitivity = sensitivity; @@ -705,16 +726,16 @@ void RobotDrive::SetSensitivity(float sensitivity) { /** * Configure the scaling factor for using RobotDrive with motor controllers in a * mode other than PercentVbus. + * * @param maxOutput Multiplied with the output percentage computed by the drive - * functions. + * functions. */ void RobotDrive::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; } /** * Set the number of the sync group for the motor controllers. If the motor - * controllers are {@link CANJaguar}s, - * then they will all be added to this sync group, causing them to update their - * values at the same time. + * controllers are {@link CANJaguar}s, then they will all be added to this sync + * group, causing them to update their values at the same time. * * @param syncGroup the update group to add the motor controllers to */ diff --git a/wpilibc/Athena/src/SD540.cpp b/wpilibc/Athena/src/SD540.cpp index b74c18be5f..d069f0ec77 100644 --- a/wpilibc/Athena/src/SD540.cpp +++ b/wpilibc/Athena/src/SD540.cpp @@ -11,11 +11,9 @@ /** * Note that the SD540 uses the following bounds for PWM values. These values - * should work reasonably well for - * most controllers, but if users experience issues such as asymmetric behavior - * around - * the deadband or inability to saturate the controller in either direction, - * calibration is recommended. + * should work reasonably well for most controllers, but if users experience + * issues such as asymmetric behavior around the deadband or inability to + * saturate the controller in either direction, calibration is recommended. * The calibration procedure can be found in the SD540 User Manual available * from Mindsensors. * @@ -27,9 +25,10 @@ */ /** - * Constructor for a SD540 + * Constructor for a SD540. + * * @param channel The PWM channel that the SD540 is attached to. 0-9 are - * on-board, 10-19 are on the MXP port + * on-board, 10-19 are on the MXP port */ SD540::SD540(uint32_t channel) : PWMSpeedController(channel) { SetBounds(2.05, 1.55, 1.50, 1.44, .94); @@ -40,4 +39,3 @@ SD540::SD540(uint32_t channel) : PWMSpeedController(channel) { HALReport(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel()); LiveWindow::GetInstance()->AddActuator("SD540", GetChannel(), this); } - diff --git a/wpilibc/Athena/src/SPI.cpp b/wpilibc/Athena/src/SPI.cpp index f0af9e7f7b..ba3960e7e3 100644 --- a/wpilibc/Athena/src/SPI.cpp +++ b/wpilibc/Athena/src/SPI.cpp @@ -7,8 +7,8 @@ #include "SPI.h" -#include "WPIErrors.h" #include "HAL/Digital.hpp" +#include "WPIErrors.h" #include @@ -39,7 +39,7 @@ SPI::~SPI() { spiClose(m_port); } * The default value is 500,000Hz. * The maximum value is 4,000,000Hz. * - * @param hz The clock rate in Hertz. + * @param hz The clock rate in Hertz. */ void SPI::SetClockRate(double hz) { spiSetSpeed(m_port, hz); } @@ -142,11 +142,10 @@ int32_t SPI::Write(uint8_t* data, uint8_t size) { * If the receive FIFO is empty, there is no active transfer, and initiate * is false, errors. * - * @param initiate If true, this function pushes "0" into the - * transmit buffer and initiates a transfer. - * If false, this function assumes that data is - * already in the receive FIFO from a previous - * write. + * @param initiate If true, this function pushes "0" into the transmit buffer + * and initiates a transfer. If false, this function assumes + * that data is already in the receive FIFO from a previous + * write. */ int32_t SPI::Read(bool initiate, uint8_t* dataReceived, uint8_t size) { int32_t retVal = 0; @@ -162,9 +161,9 @@ int32_t SPI::Read(bool initiate, uint8_t* dataReceived, uint8_t size) { /** * Perform a simultaneous read/write transaction with the device * - * @param dataToSend The data to be written out to the device + * @param dataToSend The data to be written out to the device * @param dataReceived Buffer to receive data from the device - * @param size The length of the transaction, in bytes + * @param size The length of the transaction, in bytes */ int32_t SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, uint8_t size) { @@ -176,16 +175,16 @@ int32_t SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, /** * Initialize the accumulator. * - * @param period Time between reads - * @param cmd SPI command to send to request data - * @param xfer_size SPI transfer size, in bytes + * @param period Time between reads + * @param cmd SPI command to send to request data + * @param xfer_size SPI transfer size, in bytes * @param valid_mask Mask to apply to received data for validity checking * @param valid_data After valid_mask is applied, required matching value for * validity checking * @param data_shift Bit shift to apply to received data to get actual data * value - * @param data_size Size (in bits) of data field - * @param is_signed Is data field signed? + * @param data_size Size (in bits) of data field + * @param is_signed Is data field signed? * @param big_endian Is device big endian? */ void SPI::InitAccumulator(double period, uint32_t cmd, uint8_t xfer_size, @@ -220,9 +219,10 @@ void SPI::ResetAccumulator() { /** * Set the center value of the accumulator. * - * The center value is subtracted from each value before it is added to the accumulator. This - * is used for the center value of devices like gyros and accelerometers to make integration work - * and to take the device offset into account when integrating. + * The center value is subtracted from each value before it is added to the + * accumulator. This is used for the center value of devices like gyros and + * accelerometers to make integration work and to take the device offset into + * account when integrating. */ void SPI::SetAccumulatorCenter(int32_t center) { int32_t status = 0; @@ -264,7 +264,8 @@ int64_t SPI::GetAccumulatorValue() const { /** * Read the number of accumulated values. * - * Read the count of the accumulated values since the accumulator was last Reset(). + * Read the count of the accumulated values since the accumulator was last + * Reset(). * * @return The number of times samples from the channel were accumulated. */ @@ -296,7 +297,7 @@ double SPI::GetAccumulatorAverage() const { * @param value Pointer to the 64-bit accumulated output. * @param count Pointer to the number of accumulation cycles. */ -void SPI::GetAccumulatorOutput(int64_t &value, uint32_t &count) const { +void SPI::GetAccumulatorOutput(int64_t& value, uint32_t& count) const { int32_t status = 0; spiGetAccumulatorOutput(m_port, &value, &count, &status); wpi_setErrorWithContext(status, getHALErrorMessage(status)); diff --git a/wpilibc/Athena/src/SafePWM.cpp b/wpilibc/Athena/src/SafePWM.cpp index d400ce4250..e54e3ae36d 100644 --- a/wpilibc/Athena/src/SafePWM.cpp +++ b/wpilibc/Athena/src/SafePWM.cpp @@ -9,8 +9,9 @@ /** * Constructor for a SafePWM object taking a channel number. + * * @param channel The PWM channel number 0-9 are on-board, 10-19 are on the MXP - * port + * port */ SafePWM::SafePWM(uint32_t channel) : PWM(channel) { m_safetyHelper = std::make_unique(this); @@ -18,7 +19,8 @@ SafePWM::SafePWM(uint32_t channel) : PWM(channel) { } /** - * Set the expiration time for the PWM object + * Set the expiration time for the PWM object. + * * @param timeout The timeout (in seconds) for this motor object */ void SafePWM::SetExpiration(float timeout) { @@ -27,29 +29,32 @@ void SafePWM::SetExpiration(float timeout) { /** * Return the expiration time for the PWM object. + * * @returns The expiration time value. */ float SafePWM::GetExpiration() const { return m_safetyHelper->GetExpiration(); } /** * Check if the PWM object is currently alive or stopped due to a timeout. - * @returns a bool value that is true if the motor has NOT timed out and should - * still - * be running. + * + * @return a bool value that is true if the motor has NOT timed out and should + * still be running. */ bool SafePWM::IsAlive() const { return m_safetyHelper->IsAlive(); } /** * Stop the motor associated with this PWM object. + * * This is called by the MotorSafetyHelper object when it has a timeout for this - * PWM and needs to - * stop it from running. + * PWM and needs to stop it from running. */ void SafePWM::StopMotor() { SetRaw(kPwmDisabled); } /** - * Enable/disable motor safety for this device + * Enable/disable motor safety for this device. + * * Turn on and off the motor safety option for this PWM object. + * * @param enabled True if motor safety is enforced for this object */ void SafePWM::SetSafetyEnabled(bool enabled) { @@ -57,7 +62,8 @@ void SafePWM::SetSafetyEnabled(bool enabled) { } /** - * Check if motor safety is enabled for this object + * Check if motor safety is enabled for this object. + * * @returns True if motor safety is enforced for this object */ bool SafePWM::IsSafetyEnabled() const { @@ -70,9 +76,10 @@ void SafePWM::GetDescription(std::ostringstream& desc) const { /** * Feed the MotorSafety timer when setting the speed. + * * This method is called by the subclass motor whenever it updates its speed, - * thereby reseting - * the timeout value. + * thereby reseting the timeout value. + * * @param speed Value to pass to the PWM class */ void SafePWM::SetSpeed(float speed) { diff --git a/wpilibc/Athena/src/SampleRobot.cpp b/wpilibc/Athena/src/SampleRobot.cpp index 3cd3cadcea..93d40c32c2 100644 --- a/wpilibc/Athena/src/SampleRobot.cpp +++ b/wpilibc/Athena/src/SampleRobot.cpp @@ -8,9 +8,9 @@ #include "SampleRobot.h" #include "DriverStation.h" -#include "Timer.h" -#include "SmartDashboard/SmartDashboard.h" #include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SmartDashboard.h" +#include "Timer.h" #include "networktables/NetworkTable.h" SampleRobot::SampleRobot() : m_robotMainOverridden(true) {} @@ -33,9 +33,9 @@ void SampleRobot::RobotInit() { /** * Disabled should go here. + * * Programmers should override this method to run code that should run while the - * field is - * disabled. + * field is disabled. */ void SampleRobot::Disabled() { printf("Default %s() method... Override me!\n", __FUNCTION__); @@ -43,11 +43,10 @@ void SampleRobot::Disabled() { /** * Autonomous should go here. + * * Programmers should override this method to run code that should run while the - * field is - * in the autonomous period. This will be called once each time the robot enters - * the - * autonomous state. + * field is in the autonomous period. This will be called once each time the + * robot enters the autonomous state. */ void SampleRobot::Autonomous() { printf("Default %s() method... Override me!\n", __FUNCTION__); @@ -55,11 +54,10 @@ void SampleRobot::Autonomous() { /** * Operator control (tele-operated) code should go here. + * * Programmers should override this method to run code that should run while the - * field is - * in the Operator Control (tele-operated) period. This is called once each time - * the robot - * enters the teleop state. + * field is in the Operator Control (tele-operated) period. This is called once + * each time the robot enters the teleop state. */ void SampleRobot::OperatorControl() { printf("Default %s() method... Override me!\n", __FUNCTION__); @@ -67,9 +65,10 @@ void SampleRobot::OperatorControl() { /** * Test program should go here. + * * Programmers should override this method to run code that executes while the - * robot is - * in test mode. This will be called once whenever the robot enters test mode + * robot is in test mode. This will be called once whenever the robot enters + * test mode */ void SampleRobot::Test() { printf("Default %s() method... Override me!\n", __FUNCTION__); @@ -79,33 +78,28 @@ void SampleRobot::Test() { * Robot main program for free-form programs. * * This should be overridden by user subclasses if the intent is to not use the - * Autonomous() and - * OperatorControl() methods. In that case, the program is responsible for - * sensing when to run - * the autonomous and operator control functions in their program. + * Autonomous() and OperatorControl() methods. In that case, the program is + * responsible for sensing when to run the autonomous and operator control + * functions in their program. * * This method will be called immediately after the constructor is called. If it - * has not been - * overridden by a user subclass (i.e. the default version runs), then the - * Autonomous() and - * OperatorControl() methods will be called. + * has not been overridden by a user subclass (i.e. the default version runs), + * then the Autonomous() and OperatorControl() methods will be called. */ void SampleRobot::RobotMain() { m_robotMainOverridden = false; } /** * Start a competition. + * * This code needs to track the order of the field starting to ensure that - * everything happens - * in the right order. Repeatedly run the correct method, either Autonomous or - * OperatorControl - * or Test when the robot is enabled. After running the correct method, wait for - * some state to - * change, either the other mode starts or the robot is disabled. Then go back - * and wait for the + * everything happens in the right order. Repeatedly run the correct method, + * either Autonomous or OperatorControl or Test when the robot is enabled. + * After running the correct method, wait for some state to change, either the + * other mode starts or the robot is disabled. Then go back and wait for the * robot to be enabled again. */ void SampleRobot::StartCompetition() { - LiveWindow *lw = LiveWindow::GetInstance(); + LiveWindow* lw = LiveWindow::GetInstance(); HALReport(HALUsageReporting::kResourceType_Framework, HALUsageReporting::kFramework_Sample); diff --git a/wpilibc/Athena/src/SensorBase.cpp b/wpilibc/Athena/src/SensorBase.cpp index 984ad576ec..c6277ef152 100644 --- a/wpilibc/Athena/src/SensorBase.cpp +++ b/wpilibc/Athena/src/SensorBase.cpp @@ -7,10 +7,10 @@ #include "SensorBase.h" -#include "NetworkCommunication/LoadOut.h" -#include "WPIErrors.h" #include "HAL/HAL.hpp" #include "HAL/Port.h" +#include "NetworkCommunication/LoadOut.h" +#include "WPIErrors.h" const uint32_t SensorBase::kDigitalChannels; const uint32_t SensorBase::kAnalogInputs; @@ -60,6 +60,7 @@ SensorBase::SensorBase() { /** * Add sensor to the singleton list. + * * Add this sensor to the list of singletons that need to be deleted when * the robot program exits. Each of the sensors on this list are singletons, * that is they aren't allocated directly with new, but instead are allocated @@ -75,6 +76,7 @@ void SensorBase::AddToSingletonList() { /** * Delete all the singleton classes on the list. + * * All the classes that were allocated as singletons need to be deleted so * their resources can be freed. */ @@ -99,9 +101,9 @@ bool SensorBase::CheckSolenoidModule(uint8_t moduleNumber) { /** * Check that the digital channel number is valid. + * * Verify that the channel number is one of the legal channel numbers. Channel - * numbers are - * 1-based. + * numbers are 1-based. * * @return Digital channel is valid */ @@ -112,9 +114,9 @@ bool SensorBase::CheckDigitalChannel(uint32_t channel) { /** * Check that the digital channel number is valid. + * * Verify that the channel number is one of the legal channel numbers. Channel - * numbers are - * 1-based. + * numbers are 1-based. * * @return Relay channel is valid */ @@ -125,9 +127,9 @@ bool SensorBase::CheckRelayChannel(uint32_t channel) { /** * Check that the digital channel number is valid. + * * Verify that the channel number is one of the legal channel numbers. Channel - * numbers are - * 1-based. + * numbers are 1-based. * * @return PWM channel is valid */ @@ -138,9 +140,9 @@ bool SensorBase::CheckPWMChannel(uint32_t channel) { /** * Check that the analog input number is value. + * * Verify that the analog input number is one of the legal channel numbers. - * Channel numbers - * are 0-based. + * Channel numbers are 0-based. * * @return Analog channel is valid */ @@ -151,9 +153,9 @@ bool SensorBase::CheckAnalogInput(uint32_t channel) { /** * Check that the analog output number is valid. + * * Verify that the analog output number is one of the legal channel numbers. - * Channel numbers - * are 0-based. + * Channel numbers are 0-based. * * @return Analog channel is valid */ diff --git a/wpilibc/Athena/src/SerialPort.cpp b/wpilibc/Athena/src/SerialPort.cpp index 1544a62471..4c9bea1368 100644 --- a/wpilibc/Athena/src/SerialPort.cpp +++ b/wpilibc/Athena/src/SerialPort.cpp @@ -7,8 +7,8 @@ #include "SerialPort.h" -#include "HAL/HAL.hpp" #include +#include "HAL/HAL.hpp" // static ViStatus _VI_FUNCH ioCompleteHandler (ViSession vi, ViEventType // eventType, ViEvent event, ViAddr userHandle); @@ -17,16 +17,16 @@ * Create an instance of a Serial Port class. * * @param baudRate The baud rate to configure the serial port. - * @param port The physical port to use + * @param port The physical port to use * @param dataBits The number of data bits per transfer. Valid values are - * between 5 and 8 bits. - * @param parity Select the type of parity checking to use. + * between 5 and 8 bits. + * @param parity Select the type of parity checking to use. * @param stopBits The number of stop bits to use as defined by the enum - * StopBits. + * StopBits. */ SerialPort::SerialPort(uint32_t baudRate, Port port, uint8_t dataBits, - SerialPort::Parity parity, SerialPort::StopBits stopBits) -{ + SerialPort::Parity parity, + SerialPort::StopBits stopBits) { int32_t status = 0; m_port = port; @@ -82,7 +82,7 @@ void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) { * * Termination is currently only implemented for receive. * When the the terminator is recieved, the Read() or Scanf() will return - * fewer bytes than requested, stopping after the terminator. + * fewer bytes than requested, stopping after the terminator. * * @param terminator The character to use for termination. */ @@ -117,10 +117,10 @@ int32_t SerialPort::GetBytesReceived() { * Read raw bytes out of the buffer. * * @param buffer Pointer to the buffer to store the bytes in. - * @param count The maximum number of bytes to read. + * @param count The maximum number of bytes to read. * @return The number of bytes actually read into the buffer. */ -uint32_t SerialPort::Read(char *buffer, int32_t count) { +uint32_t SerialPort::Read(char* buffer, int32_t count) { int32_t status = 0; int32_t retVal = serialRead(m_port, buffer, count, &status); wpi_setErrorWithContext(status, getHALErrorMessage(status)); @@ -131,10 +131,10 @@ uint32_t SerialPort::Read(char *buffer, int32_t count) { * Write raw bytes to the buffer. * * @param buffer Pointer to the buffer to read the bytes from. - * @param count The maximum number of bytes to write. + * @param count The maximum number of bytes to write. * @return The number of bytes actually written into the port. */ -uint32_t SerialPort::Write(const std::string &buffer, int32_t count) { +uint32_t SerialPort::Write(const std::string& buffer, int32_t count) { int32_t status = 0; int32_t retVal = serialWrite(m_port, buffer.c_str(), count, &status); wpi_setErrorWithContext(status, getHALErrorMessage(status)); @@ -191,10 +191,10 @@ void SerialPort::SetWriteBufferSize(uint32_t size) { * Specify the flushing behavior of the output buffer. * * When set to kFlushOnAccess, data is synchronously written to the serial port - * after each call to either Printf() or Write(). + * after each call to either Printf() or Write(). * * When set to kFlushWhenFull, data will only be written to the serial port when - * the buffer is full or when Flush() is called. + * the buffer is full or when Flush() is called. * * @param mode The write buffer mode. */ diff --git a/wpilibc/Athena/src/Servo.cpp b/wpilibc/Athena/src/Servo.cpp index 6c8c3b0645..12ae667fb7 100644 --- a/wpilibc/Athena/src/Servo.cpp +++ b/wpilibc/Athena/src/Servo.cpp @@ -17,7 +17,7 @@ constexpr float Servo::kDefaultMinServoPWM; /** * @param channel The PWM channel to which the servo is attached. 0-9 are - * on-board, 10-19 are on the MXP port + * on-board, 10-19 are on the MXP port */ Servo::Servo(uint32_t channel) : SafePWM(channel) { // Set minimum and maximum PWM values supported by the servo @@ -26,7 +26,7 @@ Servo::Servo(uint32_t channel) : SafePWM(channel) { // Assign defaults for period multiplier for the servo PWM control signal SetPeriodMultiplier(kPeriodMultiplier_4X); - // printf("Done initializing servo %d\n", channel); + // printf("Done initializing servo %d\n", channel); } Servo::~Servo() { @@ -69,11 +69,10 @@ float Servo::Get() const { return GetPosition(); } * assumption, need to test). * * Servo angles that are out of the supported range of the servo simply - * "saturate" in that direction - * In other words, if the servo has a range of (X degrees to Y degrees) than - * angles of less than X - * result in an angle of X being set and angles of more than Y degrees result in - * an angle of Y being set. + * "saturate" in that direction. In other words, if the servo has a range of + * (X degrees to Y degrees) than angles of less than X result in an angle of + * X being set and angles of more than Y degrees result in an angle of Y being + * set. * * @param degrees The angle in degrees to set the servo. */ @@ -92,6 +91,7 @@ void Servo::SetAngle(float degrees) { * * Assume that the servo angle is linear with respect to the PWM value (big * assumption, need to test). + * * @return The angle in degrees to which the servo is set. */ float Servo::GetAngle() const { diff --git a/wpilibc/Athena/src/Solenoid.cpp b/wpilibc/Athena/src/Solenoid.cpp index 1192d26c06..9051339eac 100644 --- a/wpilibc/Athena/src/Solenoid.cpp +++ b/wpilibc/Athena/src/Solenoid.cpp @@ -6,8 +6,8 @@ /*----------------------------------------------------------------------------*/ #include "Solenoid.h" -#include "WPIErrors.h" #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" #include @@ -23,7 +23,7 @@ Solenoid::Solenoid(uint32_t channel) * Constructor. * * @param moduleNumber The CAN ID of the PCM the solenoid is attached to - * @param channel The channel on the PCM to control (0..7). + * @param channel The channel on the PCM to control (0..7). */ Solenoid::Solenoid(uint8_t moduleNumber, uint32_t channel) : SolenoidBase(moduleNumber), m_channel(channel) { @@ -86,11 +86,14 @@ bool Solenoid::Get() const { uint8_t value = GetAll(m_moduleNumber) & (1 << m_channel); return (value != 0); } + /** * Check if solenoid is blacklisted. - * If a solenoid is shorted, it is added to the blacklist and - * disabled until power cycle, or until faults are cleared. - * @see ClearAllPCMStickyFaults() + * + * If a solenoid is shorted, it is added to the blacklist and + * disabled until power cycle, or until faults are cleared. + * + * @see ClearAllPCMStickyFaults() * * @return If solenoid is disabled due to short. */ diff --git a/wpilibc/Athena/src/SolenoidBase.cpp b/wpilibc/Athena/src/SolenoidBase.cpp index 254d1447df..d7325db6a3 100644 --- a/wpilibc/Athena/src/SolenoidBase.cpp +++ b/wpilibc/Athena/src/SolenoidBase.cpp @@ -31,7 +31,7 @@ SolenoidBase::SolenoidBase(uint8_t moduleNumber) * Set the value of a solenoid. * * @param value The value you want to set on the module. - * @param mask The channels you want to be affected. + * @param mask The channels you want to be affected. */ void SolenoidBase::Set(uint8_t value, uint8_t mask, int module) { int32_t status = 0; @@ -55,12 +55,13 @@ uint8_t SolenoidBase::GetAll(int module) const { wpi_setErrorWithContext(status, getHALErrorMessage(status)); return value; } + /** * Reads complete solenoid blacklist for all 8 solenoids as a single byte. * - * If a solenoid is shorted, it is added to the blacklist and - * disabled until power cycle, or until faults are cleared. - * @see ClearAllPCMStickyFaults() + * If a solenoid is shorted, it is added to the blacklist and + * disabled until power cycle, or until faults are cleared. + * @see ClearAllPCMStickyFaults() * * @return The solenoid blacklist of all 8 solenoids on the module. */ @@ -68,34 +69,32 @@ uint8_t SolenoidBase::GetPCMSolenoidBlackList(int module) const { int32_t status = 0; return getPCMSolenoidBlackList(m_ports[module][0], &status); } + /** - * @return true if PCM sticky fault is set : The common - * highside solenoid voltage rail is too low, - * most likely a solenoid channel is shorted. + * @return true if PCM sticky fault is set : The common highside solenoid + * voltage rail is too low, most likely a solenoid channel is shorted. */ bool SolenoidBase::GetPCMSolenoidVoltageStickyFault(int module) const { int32_t status = 0; return getPCMSolenoidVoltageStickyFault(m_ports[module][0], &status); } + /** - * @return true if PCM is in fault state : The common - * highside solenoid voltage rail is too low, - * most likely a solenoid channel is shorted. + * @return true if PCM is in fault state : The common highside solenoid voltage + * rail is too low, most likely a solenoid channel is shorted. */ bool SolenoidBase::GetPCMSolenoidVoltageFault(int module) const { int32_t status = 0; return getPCMSolenoidVoltageFault(m_ports[module][0], &status); } + /** * Clear ALL sticky faults inside PCM that Compressor is wired to. * * If a sticky fault is set, then it will be persistently cleared. Compressor - * drive - * maybe momentarily disable while flags are being cleared. Care - * should be - * taken to not call this too frequently, otherwise normal - * compressor - * functionality may be prevented. + * drive maybe momentarily disable while flags are being cleared. Care should + * be taken to not call this too frequently, otherwise normal compressor + * functionality may be prevented. * * If no sticky faults are set then this call will have no effect. */ diff --git a/wpilibc/Athena/src/Spark.cpp b/wpilibc/Athena/src/Spark.cpp index 35643b542a..5a8683719a 100644 --- a/wpilibc/Athena/src/Spark.cpp +++ b/wpilibc/Athena/src/Spark.cpp @@ -11,11 +11,9 @@ /** * Note that the Spark uses the following bounds for PWM values. These values - * should work reasonably well for - * most controllers, but if users experience issues such as asymmetric behavior - * around - * the deadband or inability to saturate the controller in either direction, - * calibration is recommended. + * should work reasonably well for most controllers, but if users experience + * issues such as asymmetric behavior around the deadband or inability to + * saturate the controller in either direction, calibration is recommended. * The calibration procedure can be found in the Spark User Manual available * from REV Robotics. * @@ -27,9 +25,10 @@ */ /** - * Constructor for a Spark + * Constructor for a Spark. + * * @param channel The PWM channel that the Spark is attached to. 0-9 are - * on-board, 10-19 are on the MXP port + * on-board, 10-19 are on the MXP port */ Spark::Spark(uint32_t channel) : PWMSpeedController(channel) { SetBounds(2.003, 1.55, 1.50, 1.46, .999); diff --git a/wpilibc/Athena/src/Talon.cpp b/wpilibc/Athena/src/Talon.cpp index e3674d11cc..251f1ca9e5 100644 --- a/wpilibc/Athena/src/Talon.cpp +++ b/wpilibc/Athena/src/Talon.cpp @@ -10,9 +10,10 @@ #include "LiveWindow/LiveWindow.h" /** - * Constructor for a Talon (original or Talon SR) + * Constructor for a Talon (original or Talon SR). + * * @param channel The PWM channel number that the Talon is attached to. 0-9 are - * on-board, 10-19 are on the MXP port + * on-board, 10-19 are on the MXP port */ Talon::Talon(uint32_t channel) : PWMSpeedController(channel) { /* Note that the Talon uses the following bounds for PWM values. These values diff --git a/wpilibc/Athena/src/TalonSRX.cpp b/wpilibc/Athena/src/TalonSRX.cpp index 22cefac174..07bea8f282 100644 --- a/wpilibc/Athena/src/TalonSRX.cpp +++ b/wpilibc/Athena/src/TalonSRX.cpp @@ -10,9 +10,10 @@ #include "LiveWindow/LiveWindow.h" /** - * Construct a TalonSRX connected via PWM + * Construct a TalonSRX connected via PWM. + * * @param channel The PWM channel that the TalonSRX is attached to. 0-9 are - * on-board, 10-19 are on the MXP port + * on-board, 10-19 are on the MXP port */ TalonSRX::TalonSRX(uint32_t channel) : PWMSpeedController(channel) { /* Note that the TalonSRX uses the following bounds for PWM values. These diff --git a/wpilibc/Athena/src/Task.cpp b/wpilibc/Athena/src/Task.cpp index 5aa795bd5d..3c2217cc2a 100644 --- a/wpilibc/Athena/src/Task.cpp +++ b/wpilibc/Athena/src/Task.cpp @@ -7,10 +7,10 @@ #include "Task.h" -#include "WPIErrors.h" #include -#include #include +#include +#include "WPIErrors.h" #ifndef OK #define OK 0 @@ -34,21 +34,13 @@ Task::~Task() { } } -bool Task::joinable() const noexcept { - return m_thread.joinable(); -} +bool Task::joinable() const noexcept { return m_thread.joinable(); } -void Task::join() { - m_thread.join(); -} +void Task::join() { m_thread.join(); } -void Task::detach() { - m_thread.detach(); -} +void Task::detach() { m_thread.detach(); } -std::thread::id Task::get_id() const noexcept { - return m_thread.get_id(); -} +std::thread::id Task::get_id() const noexcept { return m_thread.get_id(); } std::thread::native_handle_type Task::native_handle() { return m_thread.native_handle(); diff --git a/wpilibc/Athena/src/Timer.cpp b/wpilibc/Athena/src/Timer.cpp index 850b6becfa..f3a5142f23 100644 --- a/wpilibc/Athena/src/Timer.cpp +++ b/wpilibc/Athena/src/Timer.cpp @@ -9,19 +9,17 @@ #include +#include #include "HAL/HAL.hpp" #include "Utility.h" -#include /** * Pause the task for a specified time. * * Pause the execution of the program for a specified period of time given in - * seconds. - * Motors will continue to run at their last assigned values, and sensors will - * continue to - * update. Only the task containing the wait will pause until the wait time is - * expired. + * seconds. Motors will continue to run at their last assigned values, and + * sensors will continue to update. Only the task containing the wait will + * pause until the wait time is expired. * * @param seconds Length of time to pause, in seconds. */ @@ -40,8 +38,8 @@ double GetClock() { return Timer::GetFPGATimestamp(); } /** * @brief Gives real-time clock system time with nanosecond resolution * @return The time, just in case you want the robot to start autonomous at 8pm - * on Saturday. -*/ + * on Saturday. + */ double GetTime() { struct timespec tp; @@ -51,7 +49,7 @@ double GetTime() { return (realTime); } -//for compatibility with msvc12--see C2864 +// for compatibility with msvc12--see C2864 const double Timer::kRolloverTime = (1ll << 32) / 1e6; /** * Create a new timer object. @@ -68,10 +66,8 @@ Timer::Timer() { /** * Get the current time from the timer. If the clock is running it is derived - * from - * the current system clock the start time stored in the timer class. If the - * clock - * is not running, then return the time when it was last stopped. + * from the current system clock the start time stored in the timer class. If + * the clock is not running, then return the time when it was last stopped. * * @return Current time value for this timer in seconds */ @@ -100,7 +96,7 @@ double Timer::Get() const { * Reset the timer by setting the time to 0. * * Make the timer startTime the current time so new requests will be relative to - * now + * now. */ void Timer::Reset() { std::lock_guard sync(m_mutex); @@ -110,6 +106,7 @@ void Timer::Reset() { /** * Start the timer running. + * * Just set the running flag to true indicating that all time requests should be * relative to the system clock. */ @@ -123,6 +120,7 @@ void Timer::Start() { /** * Stop the timer. + * * This computes the time as of now and clears the running flag, causing all * subsequent time requests to be read from the accumulated time rather than * looking at the system clock. @@ -160,8 +158,8 @@ bool Timer::HasPeriodPassed(double period) { * Return the FPGA system clock time in seconds. * * Return the time from the FPGA hardware clock in seconds since the FPGA - * started. - * Rolls over after 71 minutes. + * started. Rolls over after 71 minutes. + * * @returns Robot running time in seconds. */ double Timer::GetFPGATimestamp() { diff --git a/wpilibc/Athena/src/USBCamera.cpp b/wpilibc/Athena/src/USBCamera.cpp index 3f682a262d..738d56adaa 100644 --- a/wpilibc/Athena/src/USBCamera.cpp +++ b/wpilibc/Athena/src/USBCamera.cpp @@ -9,12 +9,12 @@ #include "Utility.h" -#include #include -#include -#include -#include #include +#include +#include +#include +#include // This macro expands the given imaq function to ensure that it is called and // properly checked for an error, calling the wpi_setImaqErrorWithContext @@ -80,8 +80,7 @@ unsigned int USBCamera::GetJpegSize(void* buffer, unsigned int buffSize) { } USBCamera::USBCamera(std::string name, bool useJpeg) - : m_name(name), - m_useJpeg(useJpeg) {} + : m_name(name), m_useJpeg(useJpeg) {} void USBCamera::OpenCamera() { std::lock_guard lock(m_mutex); @@ -137,9 +136,11 @@ void USBCamera::UpdateSettings() { uInt32 count = 0; uInt32 currentMode = 0; - SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, nullptr, &count, ¤tMode); + SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, nullptr, &count, + ¤tMode); auto modes = std::make_unique(count); - SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, modes.get(), &count, ¤tMode); + SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, modes.get(), &count, + ¤tMode); // Groups are: // 0 - width diff --git a/wpilibc/Athena/src/Ultrasonic.cpp b/wpilibc/Athena/src/Ultrasonic.cpp index 183e1ce1f9..86e71bb928 100644 --- a/wpilibc/Athena/src/Ultrasonic.cpp +++ b/wpilibc/Athena/src/Ultrasonic.cpp @@ -10,10 +10,10 @@ #include "Counter.h" #include "DigitalInput.h" #include "DigitalOutput.h" +#include "LiveWindow/LiveWindow.h" #include "Timer.h" #include "Utility.h" #include "WPIErrors.h" -#include "LiveWindow/LiveWindow.h" // Time (sec) for the ping trigger pulse. constexpr double Ultrasonic::kPingTime; @@ -29,8 +29,8 @@ std::set Ultrasonic::m_sensors; /** * Background task that goes through the list of ultrasonic sensors and pings - * each one in turn. The counter - * is configured to read the timing of the returned echo pulse. + * each one in turn. The counter is configured to read the timing of the + * returned echo pulse. * * DANGER WILL ROBINSON, DANGER WILL ROBINSON: * This code runs as a task and assumes that none of the ultrasonic sensors @@ -43,27 +43,25 @@ void Ultrasonic::UltrasonicChecker() { if (!m_automaticEnabled) break; if (sensor->IsEnabled()) { - sensor->m_pingChannel->Pulse(kPingTime); // do the ping + sensor->m_pingChannel->Pulse(kPingTime); // do the ping } - Wait(0.1); // wait for ping to return + Wait(0.1); // wait for ping to return } } } /** * Initialize the Ultrasonic Sensor. + * * This is the common code that initializes the ultrasonic sensor given that - * there - * are two digital I/O channels allocated. If the system was running in - * automatic mode (round robin) - * when the new sensor is added, it is stopped, the sensor is added, then - * automatic mode is - * restored. + * there are two digital I/O channels allocated. If the system was running in + * automatic mode (round robin) when the new sensor is added, it is stopped, + * the sensor is added, then automatic mode is restored. */ void Ultrasonic::Initialize() { bool originalMode = m_automaticEnabled; - SetAutomaticMode(false); // kill task when adding a new sensor + SetAutomaticMode(false); // kill task when adding a new sensor // link this instance on the list m_sensors.insert(this); @@ -81,16 +79,17 @@ void Ultrasonic::Initialize() { } /** - * Create an instance of the Ultrasonic Sensor - * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic + * Create an instance of the Ultrasonic Sensor. + * + * This is designed to support the Daventech SRF04 and Vex ultrasonic * sensors. + * * @param pingChannel The digital output channel that sends the pulse to - * initiate the sensor sending - * the ping. + * initiate the sensor sending the ping. * @param echoChannel The digital input channel that receives the echo. The - * length of time that the - * echo is high represents the round trip time of the ping, and the distance. - * @param units The units returned in either kInches or kMilliMeters + * length of time that the echo is high represents the + * round trip time of the ping, and the distance. + * @param units The units returned in either kInches or kMilliMeters */ Ultrasonic::Ultrasonic(uint32_t pingChannel, uint32_t echoChannel, DistanceUnit units) @@ -103,15 +102,15 @@ Ultrasonic::Ultrasonic(uint32_t pingChannel, uint32_t echoChannel, /** * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo - * channel and a DigitalOutput - * for the ping channel. + * channel and a DigitalOutput for the ping channel. + * * @param pingChannel The digital output object that starts the sensor doing a - * ping. Requires a 10uS pulse to start. + * ping. Requires a 10uS pulse to start. * @param echoChannel The digital input object that times the return pulse to - * determine the range. - * @param units The units returned in either kInches or kMilliMeters + * determine the range. + * @param units The units returned in either kInches or kMilliMeters */ -Ultrasonic::Ultrasonic(DigitalOutput *pingChannel, DigitalInput *echoChannel, +Ultrasonic::Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel, DistanceUnit units) : m_pingChannel(pingChannel, NullDeleter()), m_echoChannel(echoChannel, NullDeleter()), @@ -127,15 +126,15 @@ Ultrasonic::Ultrasonic(DigitalOutput *pingChannel, DigitalInput *echoChannel, /** * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo - * channel and a DigitalOutput - * for the ping channel. + * channel and a DigitalOutput for the ping channel. + * * @param pingChannel The digital output object that starts the sensor doing a - * ping. Requires a 10uS pulse to start. + * ping. Requires a 10uS pulse to start. * @param echoChannel The digital input object that times the return pulse to - * determine the range. - * @param units The units returned in either kInches or kMilliMeters + * determine the range. + * @param units The units returned in either kInches or kMilliMeters */ -Ultrasonic::Ultrasonic(DigitalOutput &pingChannel, DigitalInput &echoChannel, +Ultrasonic::Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel, DistanceUnit units) : m_pingChannel(&pingChannel, NullDeleter()), m_echoChannel(&echoChannel, NullDeleter()), @@ -146,13 +145,13 @@ Ultrasonic::Ultrasonic(DigitalOutput &pingChannel, DigitalInput &echoChannel, /** * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo - * channel and a DigitalOutput - * for the ping channel. + * channel and a DigitalOutput for the ping channel. + * * @param pingChannel The digital output object that starts the sensor doing a - * ping. Requires a 10uS pulse to start. + * ping. Requires a 10uS pulse to start. * @param echoChannel The digital input object that times the return pulse to - * determine the range. - * @param units The units returned in either kInches or kMilliMeters + * determine the range. + * @param units The units returned in either kInches or kMilliMeters */ Ultrasonic::Ultrasonic(std::shared_ptr pingChannel, std::shared_ptr echoChannel, @@ -166,11 +165,11 @@ Ultrasonic::Ultrasonic(std::shared_ptr pingChannel, /** * Destructor for the ultrasonic sensor. + * * Delete the instance of the ultrasonic sensor by freeing the allocated digital - * channels. - * If the system was in automatic mode (round robin), then it is stopped, then - * started again - * after this sensor is removed (provided this wasn't the last sensor). + * channels. If the system was in automatic mode (round robin), then it is + * stopped, then started again after this sensor is removed (provided this + * wasn't the last sensor). */ Ultrasonic::~Ultrasonic() { bool wasAutomaticMode = m_automaticEnabled; @@ -186,15 +185,16 @@ Ultrasonic::~Ultrasonic() { /** * Turn Automatic mode on/off. + * * When in Automatic mode, all sensors will fire in round robin, waiting a set * time between each sensor. + * * @param enabling Set to true if round robin scheduling should start for all - * the ultrasonic sensors. This - * scheduling method assures that the sensors are non-interfering because no two - * sensors fire at the same time. - * If another scheduling algorithm is prefered, it can be implemented by - * pinging the sensors manually and waiting - * for the results to come back. + * the ultrasonic sensors. This scheduling method assures that + * the sensors are non-interfering because no two sensors fire + * at the same time. If another scheduling algorithm is + * prefered, it can be implemented by pinging the sensors + * manually and waiting for the results to come back. */ void Ultrasonic::SetAutomaticMode(bool enabling) { if (enabling == m_automaticEnabled) return; // ignore the case of no change @@ -214,7 +214,7 @@ void Ultrasonic::SetAutomaticMode(bool enabling) { // TODO: Currently, lvuser does not have permissions to set task priorities. // Until that is the case, uncommenting this will break user code that calls // Ultrasonic::SetAutomicMode(). - //m_task.SetPriority(kPriority); + // m_task.SetPriority(kPriority); } else { // Wait for background task to stop running m_task.join(); @@ -231,6 +231,7 @@ void Ultrasonic::SetAutomaticMode(bool enabling) { /** * Single ping to ultrasonic sensor. + * * Send out a single ping to the ultrasonic sensor. This only works if automatic * (round robin) mode is disabled. A single ping is sent out, and the counter * should count the semi-period when it comes in. The counter is reset to make @@ -245,18 +246,19 @@ void Ultrasonic::Ping() { /** * Check if there is a valid range measurement. + * * The ranges are accumulated in a counter that will increment on each edge of - * the echo (return) - * signal. If the count is not at least 2, then the range has not yet been - * measured, and is invalid. + * the echo (return) signal. If the count is not at least 2, then the range has + * not yet been measured, and is invalid. */ bool Ultrasonic::IsRangeValid() const { return m_counter.Get() > 1; } /** * Get the range in inches from the ultrasonic sensor. + * * @return double Range in inches of the target returned from the ultrasonic - * sensor. If there is no valid value yet, i.e. at least one measurement hasn't - * completed, then return 0. + * sensor. If there is no valid value yet, i.e. at least one + * measurement hasn't completed, then return 0. */ double Ultrasonic::GetRangeInches() const { if (IsRangeValid()) @@ -267,10 +269,10 @@ double Ultrasonic::GetRangeInches() const { /** * Get the range in millimeters from the ultrasonic sensor. + * * @return double Range in millimeters of the target returned by the ultrasonic - * sensor. - * If there is no valid value yet, i.e. at least one measurement hasn't - * completed, then return 0. + * sensor. If there is no valid value yet, i.e. at least one + * measurement hasn't completed, then return 0. */ double Ultrasonic::GetRangeMM() const { return GetRangeInches() * 25.4; } diff --git a/wpilibc/Athena/src/Utility.cpp b/wpilibc/Athena/src/Utility.cpp index d675e60f6b..d1249debd3 100644 --- a/wpilibc/Athena/src/Utility.cpp +++ b/wpilibc/Athena/src/Utility.cpp @@ -8,15 +8,15 @@ #include "Utility.h" //#include "NetworkCommunication/FRCComm.h" -#include "HAL/HAL.hpp" -#include "Task.h" -#include -#include +#include +#include #include #include #include -#include -#include +#include +#include +#include "HAL/HAL.hpp" +#include "Task.h" #include "nivision.h" /** @@ -25,9 +25,9 @@ * The users don't call this, but instead use the wpi_assert macros in * Utility.h. */ -bool wpi_assert_impl(bool conditionValue, const char *conditionText, - const char *message, const char *fileName, - uint32_t lineNumber, const char *funcName) { +bool wpi_assert_impl(bool conditionValue, const char* conditionText, + const char* message, const char* fileName, + uint32_t lineNumber, const char* funcName) { if (!conditionValue) { std::stringstream locStream; locStream << funcName << " ["; @@ -57,15 +57,12 @@ bool wpi_assert_impl(bool conditionValue, const char *conditionText, /** * Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl * This should not be called directly; it should only be used by - * wpi_assertEqual_impl - * and wpi_assertNotEqual_impl. + * wpi_assertEqual_impl and wpi_assertNotEqual_impl. */ -void wpi_assertEqual_common_impl(const char *valueA, const char *valueB, - const char *equalityType, - const char *message, - const char *fileName, - uint32_t lineNumber, - const char *funcName) { +void wpi_assertEqual_common_impl(const char* valueA, const char* valueB, + const char* equalityType, const char* message, + const char* fileName, uint32_t lineNumber, + const char* funcName) { std::stringstream locStream; locStream << funcName << " ["; locStream << basename(fileName) << ":" << lineNumber << "]"; @@ -96,10 +93,10 @@ void wpi_assertEqual_common_impl(const char *valueA, const char *valueB, * The users don't call this, but instead use the wpi_assertEqual macros in * Utility.h. */ -bool wpi_assertEqual_impl(int valueA, int valueB, const char *valueAString, - const char *valueBString, const char *message, - const char *fileName, uint32_t lineNumber, - const char *funcName) { +bool wpi_assertEqual_impl(int valueA, int valueB, const char* valueAString, + const char* valueBString, const char* message, + const char* fileName, uint32_t lineNumber, + const char* funcName) { if (!(valueA == valueB)) { wpi_assertEqual_common_impl(valueAString, valueBString, "==", message, fileName, lineNumber, funcName); @@ -114,10 +111,10 @@ bool wpi_assertEqual_impl(int valueA, int valueB, const char *valueAString, * The users don't call this, but instead use the wpi_assertNotEqual macros in * Utility.h. */ -bool wpi_assertNotEqual_impl(int valueA, int valueB, const char *valueAString, - const char *valueBString, const char *message, - const char *fileName, uint32_t lineNumber, - const char *funcName) { +bool wpi_assertNotEqual_impl(int valueA, int valueB, const char* valueAString, + const char* valueBString, const char* message, + const char* fileName, uint32_t lineNumber, + const char* funcName) { if (!(valueA != valueB)) { wpi_assertEqual_common_impl(valueAString, valueBString, "!=", message, fileName, lineNumber, funcName); @@ -127,6 +124,7 @@ bool wpi_assertNotEqual_impl(int valueA, int valueB, const char *valueAString, /** * Return the FPGA Version number. + * * For now, expect this to be competition year. * @return FPGA Version number. */ @@ -156,7 +154,7 @@ uint32_t GetFPGARevision() { * Read the microsecond-resolution timer on the FPGA. * * @return The current time in microseconds according to the FPGA (since FPGA - * reset). + * reset). */ uint64_t GetFPGATime() { int32_t status = 0; @@ -166,7 +164,8 @@ uint64_t GetFPGATime() { } /** - * Get the state of the "USER" button on the RoboRIO + * Get the state of the "USER" button on the RoboRIO. + * * @return True if the button is currently pressed down */ bool GetUserButton() { @@ -181,13 +180,13 @@ bool GetUserButton() { /** * Demangle a C++ symbol, used for printing stack traces. */ -static std::string demangle(char const *mangledSymbol) { +static std::string demangle(char const* mangledSymbol) { char buffer[256]; size_t length; int status; if (sscanf(mangledSymbol, "%*[^(]%*[(]%255[^)+]", buffer)) { - char *symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status); + char* symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status); if (status == 0) { return symbol; } else { @@ -206,9 +205,9 @@ static std::string demangle(char const *mangledSymbol) { * @param offset The number of symbols at the top of the stack to ignore */ std::string GetStackTrace(uint32_t offset) { - void *stackTrace[128]; + void* stackTrace[128]; int stackSize = backtrace(stackTrace, 128); - char **mangledSymbols = backtrace_symbols(stackTrace, stackSize); + char** mangledSymbols = backtrace_symbols(stackTrace, stackSize); std::stringstream trace; for (int i = offset; i < stackSize; i++) { diff --git a/wpilibc/Athena/src/Victor.cpp b/wpilibc/Athena/src/Victor.cpp index 1a3f0c463b..0595a55b74 100644 --- a/wpilibc/Athena/src/Victor.cpp +++ b/wpilibc/Athena/src/Victor.cpp @@ -10,9 +10,10 @@ #include "LiveWindow/LiveWindow.h" /** - * Constructor for a Victor - * @param channel The PWM channel number that the Victor is attached to. 0-9 are - * on-board, 10-19 are on the MXP port + * Constructor for a Victor. + * + * @param channel The PWM channel number that the Victor is attached to. 0-9 + * are on-board, 10-19 are on the MXP port */ Victor::Victor(uint32_t channel) : PWMSpeedController(channel) { /* Note that the Victor uses the following bounds for PWM values. These diff --git a/wpilibc/Athena/src/VictorSP.cpp b/wpilibc/Athena/src/VictorSP.cpp index e59a050591..d39dcf19cd 100644 --- a/wpilibc/Athena/src/VictorSP.cpp +++ b/wpilibc/Athena/src/VictorSP.cpp @@ -10,28 +10,26 @@ #include "LiveWindow/LiveWindow.h" /** - * Note that the VictorSP uses the following bounds for PWM values. These values - * should work reasonably well for - * most controllers, but if users experience issues such as asymmetric behavior - * around - * the deadband or inability to saturate the controller in either direction, - * calibration is recommended. - * The calibration procedure can be found in the VictorSP User Manual available - * from Vex. + * Constructor for a VictorSP. * - * 2.004ms = full "forward" - * 1.52ms = the "high end" of the deadband range - * 1.50ms = center of the deadband range (off) - * 1.48ms = the "low end" of the deadband range - * 0.997ms = full "reverse" - */ - -/** - * Constructor for a VictorSP * @param channel The PWM channel that the VictorSP is attached to. 0-9 are - * on-board, 10-19 are on the MXP port + * on-board, 10-19 are on the MXP port */ VictorSP::VictorSP(uint32_t channel) : PWMSpeedController(channel) { + /** + * Note that the VictorSP uses the following bounds for PWM values. These + * values should work reasonably well for most controllers, but if users + * experience issues such as asymmetric behavior around the deadband or + * inability to saturate the controller in either direction, calibration is + * recommended. The calibration procedure can be found in the VictorSP User + * Manual available from Vex. + * + * 2.004ms = full "forward" + * 1.52ms = the "high end" of the deadband range + * 1.50ms = center of the deadband range (off) + * 1.48ms = the "low end" of the deadband range + * 0.997ms = full "reverse" + */ SetBounds(2.004, 1.52, 1.50, 1.48, .997); SetPeriodMultiplier(kPeriodMultiplier_1X); SetRaw(m_centerPwm); diff --git a/wpilibc/Athena/src/Vision/AxisCamera.cpp b/wpilibc/Athena/src/Vision/AxisCamera.cpp index 39dcf5b4f1..499337354e 100644 --- a/wpilibc/Athena/src/Vision/AxisCamera.cpp +++ b/wpilibc/Athena/src/Vision/AxisCamera.cpp @@ -9,13 +9,13 @@ #include "WPIErrors.h" -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include #include #include @@ -40,10 +40,11 @@ static const std::string kRotationStrings[] = { }; /** - * AxisCamera constructor + * AxisCamera constructor. + * * @param cameraHost The host to find the camera at, typically an IP address */ -AxisCamera::AxisCamera(std::string const &cameraHost) +AxisCamera::AxisCamera(std::string const& cameraHost) : m_cameraHost(cameraHost) { m_captureThread = std::thread(&AxisCamera::Capture, this); } @@ -56,17 +57,19 @@ AxisCamera::~AxisCamera() { /* * Return true if the latest image from the camera has not been retrieved by * calling GetImage() yet. + * * @return true if the image has not been retrieved yet. */ bool AxisCamera::IsFreshImage() const { return m_freshImage; } /** * Get an image from the camera and store it in the provided image. + * * @param image The imaq image to store the result in. This must be an HSL or - * RGB image. + * RGB image. * @return 1 upon success, zero on a failure */ -int AxisCamera::GetImage(Image *image) { +int AxisCamera::GetImage(Image* image) { if (m_imageData.size() == 0) { return 0; } @@ -82,11 +85,12 @@ int AxisCamera::GetImage(Image *image) { /** * Get an image from the camera and store it in the provided image. + * * @param image The image to store the result in. This must be an HSL or RGB - * image + * image * @return 1 upon success, zero on a failure */ -int AxisCamera::GetImage(ColorImage *image) { +int AxisCamera::GetImage(ColorImage* image) { return GetImage(image->GetImaqImage()); } @@ -96,9 +100,10 @@ int AxisCamera::GetImage(ColorImage *image) { * * The returned pointer is owned by the caller and is their responsibility to * delete. + * * @return a pointer to an HSLImage object */ -HSLImage *AxisCamera::GetImage() { +HSLImage* AxisCamera::GetImage() { auto image = new HSLImage(); GetImage(image); return image; @@ -106,17 +111,19 @@ HSLImage *AxisCamera::GetImage() { /** * Copy an image into an existing buffer. + * * This copies an image into an existing buffer rather than creating a new image * in memory. That way a new image is only allocated when the image being copied - * is - * larger than the destination. + * is larger than the destination. + * * This method is called by the PCVideoServer class. + * * @param imageData The destination image. - * @param numBytes The size of the destination image. + * @param numBytes The size of the destination image. * @return 0 if failed (no source image or no memory), 1 if success. */ -int AxisCamera::CopyJPEG(char **destImage, unsigned int &destImageSize, - unsigned int &destImageBufferSize) { +int AxisCamera::CopyJPEG(char** destImage, unsigned int& destImageSize, + unsigned int& destImageBufferSize) { std::lock_guard lock(m_imageDataMutex); if (destImage == nullptr) { wpi_setWPIErrorWithContext(NullParameter, "destImage must not be nullptr"); @@ -146,6 +153,7 @@ int AxisCamera::CopyJPEG(char **destImage, unsigned int &destImageSize, /** * Request a change in the brightness of the camera images. + * * @param brightness valid values 0 .. 100 */ void AxisCamera::WriteBrightness(int brightness) { @@ -173,6 +181,7 @@ int AxisCamera::GetBrightness() { /** * Request a change in the white balance on the camera. + * * @param whiteBalance Valid values from the WhiteBalance enum. */ void AxisCamera::WriteWhiteBalance(AxisCamera::WhiteBalance whiteBalance) { @@ -194,6 +203,7 @@ AxisCamera::WhiteBalance AxisCamera::GetWhiteBalance() { /** * Request a change in the color level of the camera images. + * * @param colorLevel valid values are 0 .. 100 */ void AxisCamera::WriteColorLevel(int colorLevel) { @@ -221,6 +231,7 @@ int AxisCamera::GetColorLevel() { /** * Request a change in the camera's exposure mode. + * * @param exposureControl A mode to write in the Exposure enum. */ void AxisCamera::WriteExposureControl( @@ -243,10 +254,11 @@ AxisCamera::ExposureControl AxisCamera::GetExposureControl() { /** * Request a change in the exposure priority of the camera. + * * @param exposurePriority Valid values are 0, 50, 100. - * 0 = Prioritize image quality - * 50 = None - * 100 = Prioritize frame rate + * 0 = Prioritize image quality + * 50 = None + * 100 = Prioritize frame rate */ void AxisCamera::WriteExposurePriority(int exposurePriority) { if (exposurePriority != 0 && exposurePriority != 50 && @@ -275,8 +287,9 @@ int AxisCamera::GetExposurePriority() { /** * Write the maximum frames per second that the camera should send * Write 0 to send as many as possible. + * * @param maxFPS The number of frames the camera should send in a second, - * exposure permitting. + * exposure permitting. */ void AxisCamera::WriteMaxFPS(int maxFPS) { std::lock_guard lock(m_parametersMutex); @@ -298,6 +311,7 @@ int AxisCamera::GetMaxFPS() { /** * Write resolution value to camera. + * * @param resolution The camera resolution value to write to the camera. */ void AxisCamera::WriteResolution(AxisCamera::Resolution resolution) { @@ -312,7 +326,7 @@ void AxisCamera::WriteResolution(AxisCamera::Resolution resolution) { /** * @return The configured resolution of the camera (not necessarily the same - * resolution as the most recent image, if it was changed recently.) + * resolution as the most recent image, if it was changed recently.) */ AxisCamera::Resolution AxisCamera::GetResolution() { std::lock_guard lock(m_parametersMutex); @@ -321,10 +335,12 @@ AxisCamera::Resolution AxisCamera::GetResolution() { /** * Write the rotation value to the camera. + * * If you mount your camera upside down, use this to adjust the image for you. + * * @param rotation The angle to rotate the camera - * (AxisCamera::Rotation::k0 - * or AxisCamera::Rotation::k180) + * (AxisCamera::Rotation::k0 + * or AxisCamera::Rotation::k180) */ void AxisCamera::WriteRotation(AxisCamera::Rotation rotation) { std::lock_guard lock(m_parametersMutex); @@ -346,6 +362,7 @@ AxisCamera::Rotation AxisCamera::GetRotation() { /** * Write the compression value to the camera. + * * @param compression Values between 0 and 100. */ void AxisCamera::WriteCompression(int compression) { @@ -404,7 +421,7 @@ void AxisCamera::Capture() { * This function actually reads the images from the camera. */ void AxisCamera::ReadImagesFromCamera() { - char *imgBuffer = nullptr; + char* imgBuffer = nullptr; int imgBufferLength = 0; // TODO: these recv calls must be non-blocking. Otherwise if the camera @@ -415,7 +432,7 @@ void AxisCamera::ReadImagesFromCamera() { while (!m_done) { char initialReadBuffer[kMaxPacketSize] = ""; char intermediateBuffer[1]; - char *trailingPtr = initialReadBuffer; + char* trailingPtr = initialReadBuffer; int trailingCounter = 0; while (counter) { // TODO: fix me... this cannot be the most efficient way to approach this, @@ -438,7 +455,7 @@ void AxisCamera::ReadImagesFromCamera() { } } counter = 1; - char *contentLength = strstr(initialReadBuffer, "Content-Length: "); + char* contentLength = strstr(initialReadBuffer, "Content-Length: "); if (contentLength == nullptr) { wpi_setWPIErrorWithContext(IncompatibleMode, "No content-length token found in packet"); @@ -496,7 +513,7 @@ void AxisCamera::ReadImagesFromCamera() { * completely successfully. * * @return true if the stream should be restarted due to a - * parameter changing. + * parameter changing. */ bool AxisCamera::WriteParameters() { if (m_parametersDirty) { @@ -543,21 +560,23 @@ bool AxisCamera::WriteParameters() { } /** - * Create a socket connected to camera + * Create a socket connected to camera. + * * Used to create a connection to the camera for both capturing images and * setting parameters. + * * @param requestString The initial request string to send upon successful - * connection. - * @param setError If true, rais an error if there's a problem creating the - * connection. - * This is only enabled after several unsucessful connections, so a single one - * doesn't - * cause an error message to be printed if it immediately recovers. + * connection. + * @param setError If true, rais an error if there's a problem creating the + * connection. This is only enabled after several + * unsucessful connections, so a single one doesn't cause + * an error message to be printed if it immediately + * recovers. * @return -1 if failed, socket handle if successful. */ -int AxisCamera::CreateCameraSocket(std::string const &requestString, +int AxisCamera::CreateCameraSocket(std::string const& requestString, bool setError) { - struct addrinfo *address = nullptr; + struct addrinfo* address = nullptr; int camSocket; /* create socket */ diff --git a/wpilibc/Athena/src/Vision/BaeUtilities.cpp b/wpilibc/Athena/src/Vision/BaeUtilities.cpp index f1b13eec1c..3d625f1329 100644 --- a/wpilibc/Athena/src/Vision/BaeUtilities.cpp +++ b/wpilibc/Athena/src/Vision/BaeUtilities.cpp @@ -5,18 +5,18 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include -#include -#include -#include -#include #include -#include #include +#include +#include +#include +#include +#include +#include -#include "Vision/BaeUtilities.h" #include "Servo.h" #include "Timer.h" +#include "Vision/BaeUtilities.h" /** @file * Utility functions @@ -30,32 +30,35 @@ static DebugOutputType dprintfFlag = DEBUG_OFF; /** - * Set the debug flag to print to screen, file on cRIO, both or neither + * Set the debug flag to print to screen, file on cRIO, both or neither. + * * @param tempString The format string. */ void SetDebugFlag(DebugOutputType flag) { dprintfFlag = flag; } /** * Debug print to a file and/or a terminal window. + * * Call like you would call printf. * Set functionName in the function if you want the correct function name to * print out. * The file line number will also be printed. + * * @param tempString The format string. */ -void dprintf(const char *tempString, ...) /* Variable argument list */ +void dprintf(const char* tempString, ...) /* Variable argument list */ { va_list args; /* Input argument list */ int line_number; /* Line number passed in argument */ int type; - const char *functionName; /* Format passed in argument */ - const char *fmt; /* Format passed in argument */ + const char* functionName; /* Format passed in argument */ + const char* fmt; /* Format passed in argument */ char text[512]; /* Text string */ char outtext[512]; /* Text string */ - FILE *outfile_fd; /* Output file pointer */ + FILE* outfile_fd; /* Output file pointer */ char filepath[128]; /* Text string */ int fatalFlag = 0; - const char *filename; + const char* filename; int index; int tempStringLen; @@ -78,7 +81,7 @@ void dprintf(const char *tempString, ...) /* Variable argument list */ } /* Extract function name */ - functionName = va_arg(args, const char *); + functionName = va_arg(args, const char*); /* Extract line number from argument list */ line_number = va_arg(args, int); @@ -87,7 +90,7 @@ void dprintf(const char *tempString, ...) /* Variable argument list */ type = va_arg(args, int); /* Extract format from argument list */ - fmt = va_arg(args, const char *); + fmt = va_arg(args, const char*); vsprintf(text, fmt, args); @@ -157,9 +160,9 @@ void dprintf(const char *tempString, ...) /* Variable argument list */ } /** - * @brief Normalizes a value in a range, used for drive input + * @brief Normalizes a value in a range, used for drive input. * @param position The position in the range, starting at 0 - * @param range The size of the range that position is in + * @param range The size of the range that position is in * @return The normalized position from -1 to +1 */ double RangeToNormalized(double position, int range) { @@ -170,8 +173,8 @@ double RangeToNormalized(double position, int range) { * @brief Convert a normalized value to the corresponding value in a range. * This is used to convert normalized values to the servo command range. * @param normalizedValue The normalized value (in the -1 to +1 range) - * @param minRange The minimum of the range (0 is default) - * @param maxRange The maximum of the range (1 is default) + * @param minRange The minimum of the range (0 is default) + * @param maxRange The maximum of the range (1 is default) * @return The value in the range corresponding to the input normalized value */ float NormalizeToRange(float normalizedValue, float minRange, float maxRange) { @@ -188,7 +191,7 @@ float NormalizeToRange(float normalizedValue) { * Call this function like you would call printf. * @param fmt The format string */ -void ShowActivity(char *fmt, ...) { +void ShowActivity(char* fmt, ...) { static char activity_indication_string[] = "|/-\\"; static int ai = 3; va_list args; @@ -210,15 +213,14 @@ void ShowActivity(char *fmt, ...) { /** * @brief Calculate sine wave increments (-1.0 to 1.0). * The first time this is called, it sets up the time increment. Subsequent - * calls - * will give values along the sine wave depending on current time. If the wave - * is - * stopped and restarted, it must be reinitialized with a new "first call". + * calls will give values along the sine wave depending on current time. If + * the wave is stopped and restarted, it must be reinitialized with a new + * "first call". * - * @param period length of time to complete a complete wave + * @param period length of time to complete a complete wave * @param sinStart Where to start the sine wave (0.0 = 2 pi, pi/2 = 1.0, etc.) */ -double SinPosition(double *period, double sinStart) { +double SinPosition(double* period, double sinStart) { double rtnVal; static double sinePeriod = 0.0; static double timestamp; @@ -269,29 +271,30 @@ void panInit(double period) { * @param panServo The servo object to move * @param sinStart The position on the sine wave to begin the pan */ -void panForTarget(Servo *panServo) { panForTarget(panServo, 0.0); } +void panForTarget(Servo* panServo) { panForTarget(panServo, 0.0); } -void panForTarget(Servo *panServo, double sinStart) { +void panForTarget(Servo* panServo, double sinStart) { float normalizedSinPosition = (float)SinPosition(nullptr, sinStart); float newServoPosition = NormalizeToRange(normalizedSinPosition); panServo->Set(newServoPosition); // ShowActivity ("pan x: normalized %f newServoPosition = %f" , - // normalizedSinPosition, newServoPosition ); + // normalizedSinPosition, newServoPosition ); } /** @brief Read a file and return non-comment output string - -Call the first time with 0 lineNumber to get the number of lines to read -Then call with each lineNumber to get one camera parameter. There should -be one property=value entry on each line, i.e. "exposure=auto" - - * @param inputFile filename to read + * + * Call the first time with 0 lineNumber to get the number of lines to read + * Then call with each lineNumber to get one camera parameter. There should + * be one property=value entry on each line, i.e. "exposure=auto" + * + * @param inputFile filename to read * @param outputString one string - * @param lineNumber if 0, return number of lines; else return that line number + * @param lineNumber if 0, return number of lines; else return that line + * number * @return int number of lines or -1 if error **/ -int processFile(char *inputFile, char *outputString, int lineNumber) { - FILE *infile; +int processFile(char* inputFile, char* outputString, int lineNumber) { + FILE* infile; const int stringSize = 80; // max size of one line in file char inputStr[stringSize]; inputStr[0] = '\0'; @@ -332,10 +335,11 @@ int processFile(char *inputFile, char *outputString, int lineNumber) { return (lineCount); } -/** Ignore empty string +/** + * Ignore empty string. * @param string to check if empty **/ -int emptyString(char *string) { +int emptyString(char* string) { int i, len; if (string == nullptr) return (1); @@ -351,10 +355,11 @@ int emptyString(char *string) { return (1); } -/** Remove special characters from string +/** + * Remove special characters from string. * @param string to process **/ -void stripString(char *string) { +void stripString(char* string) { int i, j, len; if (string == nullptr) return; diff --git a/wpilibc/Athena/src/Vision/BinaryImage.cpp b/wpilibc/Athena/src/Vision/BinaryImage.cpp index 4a30a79d96..76c2e39a6b 100644 --- a/wpilibc/Athena/src/Vision/BinaryImage.cpp +++ b/wpilibc/Athena/src/Vision/BinaryImage.cpp @@ -6,14 +6,15 @@ /*----------------------------------------------------------------------------*/ #include "Vision/BinaryImage.h" -#include "WPIErrors.h" #include +#include "WPIErrors.h" using namespace std; /** * Get then number of particles for the image. - * @returns the number of particles found for the image. + * + * @return the number of particles found for the image. */ int BinaryImage::GetNumberParticles() { int numParticles = 0; @@ -24,9 +25,11 @@ int BinaryImage::GetNumberParticles() { /** * Get a single particle analysis report. + * * Get one (of possibly many) particle analysis reports for an image. + * * @param particleNumber Which particle analysis report to return. - * @returns the selected particle analysis report + * @return the selected particle analysis report */ ParticleAnalysisReport BinaryImage::GetParticleAnalysisReport( int particleNumber) { @@ -37,13 +40,15 @@ ParticleAnalysisReport BinaryImage::GetParticleAnalysisReport( /** * Get a single particle analysis report. + * * Get one (of possibly many) particle analysis reports for an image. * This version could be more efficient when copying many reports. + * * @param particleNumber Which particle analysis report to return. - * @param par the selected particle analysis report + * @param par the selected particle analysis report */ void BinaryImage::GetParticleAnalysisReport(int particleNumber, - ParticleAnalysisReport *par) { + ParticleAnalysisReport* par) { int success; int numParticles = 0; @@ -95,13 +100,14 @@ void BinaryImage::GetParticleAnalysisReport(int particleNumber, /** * Get an ordered vector of particles for the image. + * * Create a vector of particle analysis reports sorted by size for an image. * The vector contains the actual report structures. - * @returns a pointer to the vector of particle analysis reports. The caller - * must delete the - * vector when finished using it. + * + * @return a pointer to the vector of particle analysis reports. The caller + * must delete the vector when finished using it. */ -vector * +vector* BinaryImage::GetOrderedParticleAnalysisReports() { auto particles = new vector; int particleCount = GetNumberParticles(); @@ -118,10 +124,12 @@ BinaryImage::GetOrderedParticleAnalysisReports() { /** * Write a binary image to flash. + * * Writes the binary image to flash on the cRIO for later inspection. + * * @param fileName the name of the image file written to the flash. */ -void BinaryImage::Write(const char *fileName) { +void BinaryImage::Write(const char* fileName) { RGBValue colorTable[256]; memset(colorTable, 0, sizeof(colorTable)); colorTable[0].R = 0; @@ -134,17 +142,18 @@ void BinaryImage::Write(const char *fileName) { /** * Measure a single parameter for an image. + * * Get the measurement for a single parameter about an image by calling the - * imaqMeasureParticle - * function for the selected parameter. + * imaqMeasureParticle function for the selected parameter. + * * @param particleNumber which particle in the set of particles - * @param whatToMeasure the imaq MeasurementType (what to measure) - * @param result the value of the measurement - * @returns false on failure, true on success + * @param whatToMeasure the imaq MeasurementType (what to measure) + * @param result the value of the measurement + * @return false on failure, true on success */ bool BinaryImage::ParticleMeasurement(int particleNumber, MeasurementType whatToMeasure, - int *result) { + int* result) { double resultDouble; bool success = ParticleMeasurement(particleNumber, whatToMeasure, &resultDouble); @@ -154,17 +163,18 @@ bool BinaryImage::ParticleMeasurement(int particleNumber, /** * Measure a single parameter for an image. + * * Get the measurement for a single parameter about an image by calling the - * imaqMeasureParticle - * function for the selected parameter. + * imaqMeasureParticle function for the selected parameter. + * * @param particleNumber which particle in the set of particles - * @param whatToMeasure the imaq MeasurementType (what to measure) - * @param result the value of the measurement + * @param whatToMeasure the imaq MeasurementType (what to measure) + * @param result the value of the measurement * @returns true on failure, false on success */ bool BinaryImage::ParticleMeasurement(int particleNumber, MeasurementType whatToMeasure, - double *result) { + double* result) { int success; success = imaqMeasureParticle(m_imaqImage, particleNumber, 0, whatToMeasure, result); @@ -179,8 +189,10 @@ double BinaryImage::NormalizeFromRange(double position, int range) { /** * The compare helper function for sort. + * * This function compares two particle analysis reports as a helper for the sort * function. + * * @param particle1 The first particle to compare * @param particle2 the second particle to compare * @returns true if particle1 is greater than particle2 @@ -191,23 +203,25 @@ bool BinaryImage::CompareParticleSizes(ParticleAnalysisReport particle1, return particle1.particleToImagePercent > particle2.particleToImagePercent; } -BinaryImage *BinaryImage::RemoveSmallObjects(bool connectivity8, int erosions) { +BinaryImage* BinaryImage::RemoveSmallObjects(bool connectivity8, int erosions) { auto result = new BinaryImage(); - int success = imaqSizeFilter(result->GetImaqImage(), m_imaqImage, - connectivity8, erosions, IMAQ_KEEP_LARGE, nullptr); + int success = + imaqSizeFilter(result->GetImaqImage(), m_imaqImage, connectivity8, + erosions, IMAQ_KEEP_LARGE, nullptr); wpi_setImaqErrorWithContext(success, "Error in RemoveSmallObjects"); return result; } -BinaryImage *BinaryImage::RemoveLargeObjects(bool connectivity8, int erosions) { +BinaryImage* BinaryImage::RemoveLargeObjects(bool connectivity8, int erosions) { auto result = new BinaryImage(); - int success = imaqSizeFilter(result->GetImaqImage(), m_imaqImage, - connectivity8, erosions, IMAQ_KEEP_SMALL, nullptr); + int success = + imaqSizeFilter(result->GetImaqImage(), m_imaqImage, connectivity8, + erosions, IMAQ_KEEP_SMALL, nullptr); wpi_setImaqErrorWithContext(success, "Error in RemoveLargeObjects"); return result; } -BinaryImage *BinaryImage::ConvexHull(bool connectivity8) { +BinaryImage* BinaryImage::ConvexHull(bool connectivity8) { auto result = new BinaryImage(); int success = imaqConvexHull(result->GetImaqImage(), m_imaqImage, connectivity8); @@ -215,14 +229,14 @@ BinaryImage *BinaryImage::ConvexHull(bool connectivity8) { return result; } -BinaryImage *BinaryImage::ParticleFilter(ParticleFilterCriteria2 *criteria, +BinaryImage* BinaryImage::ParticleFilter(ParticleFilterCriteria2* criteria, int criteriaCount) { auto result = new BinaryImage(); int numParticles; ParticleFilterOptions2 filterOptions = {0, 0, 0, 1}; - int success = - imaqParticleFilter4(result->GetImaqImage(), m_imaqImage, criteria, - criteriaCount, &filterOptions, nullptr, &numParticles); + int success = imaqParticleFilter4(result->GetImaqImage(), m_imaqImage, + criteria, criteriaCount, &filterOptions, + nullptr, &numParticles); wpi_setImaqErrorWithContext(success, "Error in particle filter operation"); return result; } diff --git a/wpilibc/Athena/src/Vision/ColorImage.cpp b/wpilibc/Athena/src/Vision/ColorImage.cpp index ab37c0fd6b..5b93dc8c7d 100644 --- a/wpilibc/Athena/src/Vision/ColorImage.cpp +++ b/wpilibc/Athena/src/Vision/ColorImage.cpp @@ -13,12 +13,14 @@ ColorImage::ColorImage(ImageType type) : ImageBase(type) {} /** * Perform a threshold operation on a ColorImage. + * * Perform a threshold operation on a ColorImage using the ColorMode supplied * as a parameter. + * * @param colorMode The type of colorspace this operation should be performed in - * @returns a pointer to a binary image + * @return a pointer to a binary image */ -BinaryImage *ColorImage::ComputeThreshold(ColorMode colorMode, int low1, +BinaryImage* ColorImage::ComputeThreshold(ColorMode colorMode, int low1, int high1, int low2, int high2, int low3, int high3) { auto result = new BinaryImage(); @@ -32,16 +34,17 @@ BinaryImage *ColorImage::ComputeThreshold(ColorMode colorMode, int low1, /** * Perform a threshold in RGB space. - * @param redLow Red low value - * @param redHigh Red high value - * @param greenLow Green low value + * + * @param redLow Red low value + * @param redHigh Red high value + * @param greenLow Green low value * @param greenHigh Green high value - * @param blueLow Blue low value - * @param blueHigh Blue high value - * @returns A pointer to a BinaryImage that represents the result of the - * threshold operation. + * @param blueLow Blue low value + * @param blueHigh Blue high value + * @return A pointer to a BinaryImage that represents the result of the + * threshold operation. */ -BinaryImage *ColorImage::ThresholdRGB(int redLow, int redHigh, int greenLow, +BinaryImage* ColorImage::ThresholdRGB(int redLow, int redHigh, int greenLow, int greenHigh, int blueLow, int blueHigh) { return ComputeThreshold(IMAQ_RGB, redLow, redHigh, greenLow, greenHigh, @@ -50,27 +53,29 @@ BinaryImage *ColorImage::ThresholdRGB(int redLow, int redHigh, int greenLow, /** * Perform a threshold in RGB space. + * * @param threshold a reference to the Threshold object to use. - * @returns A pointer to a BinaryImage that represents the result of the - * threshold operation. + * @return A pointer to a BinaryImage that represents the result of the + * threshold operation. */ -BinaryImage *ColorImage::ThresholdRGB(Threshold &t) { +BinaryImage* ColorImage::ThresholdRGB(Threshold& t) { return ComputeThreshold(IMAQ_RGB, t.plane1Low, t.plane1High, t.plane2Low, t.plane2High, t.plane3Low, t.plane3High); } /** * Perform a threshold in HSL space. - * @param hueLow Low value for hue - * @param hueHigh High value for hue - * @param saturationLow Low value for saturation + * + * @param hueLow Low value for hue + * @param hueHigh High value for hue + * @param saturationLow Low value for saturation * @param saturationHigh High value for saturation - * @param luminenceLow Low value for luminence - * @param luminenceHigh High value for luminence - * @returns a pointer to a BinaryImage that represents the result of the - * threshold operation. + * @param luminenceLow Low value for luminence + * @param luminenceHigh High value for luminence + * @return a pointer to a BinaryImage that represents the result of the + * threshold operation. */ -BinaryImage *ColorImage::ThresholdHSL(int hueLow, int hueHigh, +BinaryImage* ColorImage::ThresholdHSL(int hueLow, int hueHigh, int saturationLow, int saturationHigh, int luminenceLow, int luminenceHigh) { return ComputeThreshold(IMAQ_HSL, hueLow, hueHigh, saturationLow, @@ -79,27 +84,29 @@ BinaryImage *ColorImage::ThresholdHSL(int hueLow, int hueHigh, /** * Perform a threshold in HSL space. + * * @param threshold a reference to the Threshold object to use. - * @returns A pointer to a BinaryImage that represents the result of the - * threshold operation. + * @return A pointer to a BinaryImage that represents the result of the + * threshold operation. */ -BinaryImage *ColorImage::ThresholdHSL(Threshold &t) { +BinaryImage* ColorImage::ThresholdHSL(Threshold& t) { return ComputeThreshold(IMAQ_HSL, t.plane1Low, t.plane1High, t.plane2Low, t.plane2High, t.plane3Low, t.plane3High); } /** * Perform a threshold in HSV space. - * @param hueLow Low value for hue - * @param hueHigh High value for hue - * @param saturationLow Low value for saturation + * + * @param hueLow Low value for hue + * @param hueHigh High value for hue + * @param saturationLow Low value for saturation * @param saturationHigh High value for saturation - * @param valueLow Low value - * @param valueHigh High value - * @returns a pointer to a BinaryImage that represents the result of the - * threshold operation. + * @param valueLow Low value + * @param valueHigh High value + * @return a pointer to a BinaryImage that represents the result of the + * threshold operation. */ -BinaryImage *ColorImage::ThresholdHSV(int hueLow, int hueHigh, +BinaryImage* ColorImage::ThresholdHSV(int hueLow, int hueHigh, int saturationLow, int saturationHigh, int valueLow, int valueHigh) { return ComputeThreshold(IMAQ_HSV, hueLow, hueHigh, saturationLow, @@ -108,27 +115,29 @@ BinaryImage *ColorImage::ThresholdHSV(int hueLow, int hueHigh, /** * Perform a threshold in HSV space. + * * @param threshold a reference to the Threshold object to use. - * @returns A pointer to a BinaryImage that represents the result of the - * threshold operation. + * @return A pointer to a BinaryImage that represents the result of the + * threshold operation. */ -BinaryImage *ColorImage::ThresholdHSV(Threshold &t) { +BinaryImage* ColorImage::ThresholdHSV(Threshold& t) { return ComputeThreshold(IMAQ_HSV, t.plane1Low, t.plane1High, t.plane2Low, t.plane2High, t.plane3Low, t.plane3High); } /** * Perform a threshold in HSI space. - * @param hueLow Low value for hue - * @param hueHigh High value for hue - * @param saturationLow Low value for saturation + * + * @param hueLow Low value for hue + * @param hueHigh High value for hue + * @param saturationLow Low value for saturation * @param saturationHigh High value for saturation - * @param valueLow Low intensity - * @param valueHigh High intensity - * @returns a pointer to a BinaryImage that represents the result of the - * threshold operation. + * @param valueLow Low intensity + * @param valueHigh High intensity + * @return a pointer to a BinaryImage that represents the result of the + * threshold operation. */ -BinaryImage *ColorImage::ThresholdHSI(int hueLow, int hueHigh, +BinaryImage* ColorImage::ThresholdHSI(int hueLow, int hueHigh, int saturationLow, int saturationHigh, int intensityLow, int intensityHigh) { return ComputeThreshold(IMAQ_HSI, hueLow, hueHigh, saturationLow, @@ -137,22 +146,24 @@ BinaryImage *ColorImage::ThresholdHSI(int hueLow, int hueHigh, /** * Perform a threshold in HSI space. + * * @param threshold a reference to the Threshold object to use. - * @returns A pointer to a BinaryImage that represents the result of the - * threshold operation. + * @return A pointer to a BinaryImage that represents the result of the + * threshold operation. */ -BinaryImage *ColorImage::ThresholdHSI(Threshold &t) { +BinaryImage* ColorImage::ThresholdHSI(Threshold& t) { return ComputeThreshold(IMAQ_HSI, t.plane1Low, t.plane1High, t.plane2Low, t.plane2High, t.plane3Low, t.plane3High); } /** - * Extract a color plane from the image - * @param mode The ColorMode to use for the plane extraction + * Extract a color plane from the image. + * + * @param mode The ColorMode to use for the plane extraction * @param planeNumber Which plane is to be extracted - * @returns A pointer to a MonoImage that represents the extracted plane. + * @return A pointer to a MonoImage that represents the extracted plane. */ -MonoImage *ColorImage::ExtractColorPlane(ColorMode mode, int planeNumber) { +MonoImage* ColorImage::ExtractColorPlane(ColorMode mode, int planeNumber) { auto result = new MonoImage(); if (m_imaqImage == nullptr) wpi_setWPIError(NullParameter); int success = imaqExtractColorPlanes( @@ -163,269 +174,298 @@ MonoImage *ColorImage::ExtractColorPlane(ColorMode mode, int planeNumber) { return result; } -/* +/** * Extract the first color plane for an image. + * * @param mode The color mode in which to operate - * @returns a pointer to a MonoImage that is the extracted plane. + * @return a pointer to a MonoImage that is the extracted plane. */ -MonoImage *ColorImage::ExtractFirstColorPlane(ColorMode mode) { +MonoImage* ColorImage::ExtractFirstColorPlane(ColorMode mode) { return ExtractColorPlane(mode, 1); } -/* +/** * Extract the second color plane for an image. + * * @param mode The color mode in which to operate - * @returns a pointer to a MonoImage that is the extracted plane. + * @return a pointer to a MonoImage that is the extracted plane. */ -MonoImage *ColorImage::ExtractSecondColorPlane(ColorMode mode) { +MonoImage* ColorImage::ExtractSecondColorPlane(ColorMode mode) { return ExtractColorPlane(mode, 2); } -/* +/** * Extract the third color plane for an image. + * * @param mode The color mode in which to operate - * @returns a pointer to a MonoImage that is the extracted plane. + * @return a pointer to a MonoImage that is the extracted plane. */ -MonoImage *ColorImage::ExtractThirdColorPlane(ColorMode mode) { +MonoImage* ColorImage::ExtractThirdColorPlane(ColorMode mode) { return ExtractColorPlane(mode, 3); } -/* +/** * Extract the red plane from an RGB image. - * @returns a pointer to a MonoImage that is the extraced plane. + * + * @return a pointer to a MonoImage that is the extraced plane. */ -MonoImage *ColorImage::GetRedPlane() { +MonoImage* ColorImage::GetRedPlane() { return ExtractFirstColorPlane(IMAQ_RGB); } -/* +/** * Extract the green plane from an RGB image. - * @returns a pointer to a MonoImage that is the extraced plane. + * + * @return a pointer to a MonoImage that is the extraced plane. */ -MonoImage *ColorImage::GetGreenPlane() { +MonoImage* ColorImage::GetGreenPlane() { return ExtractSecondColorPlane(IMAQ_RGB); } -/* +/** * Extract the blue plane from an RGB image. - * @returns a pointer to a MonoImage that is the extraced plane. + * + * @return a pointer to a MonoImage that is the extraced plane. */ -MonoImage *ColorImage::GetBluePlane() { +MonoImage* ColorImage::GetBluePlane() { return ExtractThirdColorPlane(IMAQ_RGB); } -/* +/** * Extract the Hue plane from an HSL image. - * @returns a pointer to a MonoImage that is the extraced plane. + * + * @return a pointer to a MonoImage that is the extraced plane. */ -MonoImage *ColorImage::GetHSLHuePlane() { +MonoImage* ColorImage::GetHSLHuePlane() { return ExtractFirstColorPlane(IMAQ_HSL); } -/* +/** * Extract the Hue plane from an HSV image. - * @returns a pointer to a MonoImage that is the extraced plane. + * + * @return a pointer to a MonoImage that is the extraced plane. */ -MonoImage *ColorImage::GetHSVHuePlane() { +MonoImage* ColorImage::GetHSVHuePlane() { return ExtractFirstColorPlane(IMAQ_HSV); } -/* +/** * Extract the Hue plane from an HSI image. - * @returns a pointer to a MonoImage that is the extraced plane. + * + * @return a pointer to a MonoImage that is the extraced plane. */ -MonoImage *ColorImage::GetHSIHuePlane() { +MonoImage* ColorImage::GetHSIHuePlane() { return ExtractFirstColorPlane(IMAQ_HSI); } -/* +/** * Extract the Luminance plane from an HSL image. - * @returns a pointer to a MonoImage that is the extraced plane. + * + * @return a pointer to a MonoImage that is the extraced plane. */ -MonoImage *ColorImage::GetLuminancePlane() { +MonoImage* ColorImage::GetLuminancePlane() { return ExtractThirdColorPlane(IMAQ_HSL); } -/* +/** * Extract the Value plane from an HSV image. - * @returns a pointer to a MonoImage that is the extraced plane. + * + * @return a pointer to a MonoImage that is the extraced plane. */ -MonoImage *ColorImage::GetValuePlane() { +MonoImage* ColorImage::GetValuePlane() { return ExtractThirdColorPlane(IMAQ_HSV); } -/* +/** * Extract the Intensity plane from an HSI image. - * @returns a pointer to a MonoImage that is the extraced plane. + * + * @return a pointer to a MonoImage that is the extraced plane. */ -MonoImage *ColorImage::GetIntensityPlane() { +MonoImage* ColorImage::GetIntensityPlane() { return ExtractThirdColorPlane(IMAQ_HSI); } /** - * Replace a plane in the ColorImage with a MonoImage - * Replaces a single plane in the image with a MonoImage - * @param mode The ColorMode in which to operate - * @param plane The pointer to the replacement plane as a MonoImage + * Replace a plane in the ColorImage with a MonoImage. + * + * Replaces a single plane in the image with a MonoImage. + * + * @param mode The ColorMode in which to operate + * @param plane The pointer to the replacement plane as a MonoImage * @param planeNumber The plane number (1, 2, 3) to replace */ -void ColorImage::ReplacePlane(ColorMode mode, MonoImage *plane, +void ColorImage::ReplacePlane(ColorMode mode, MonoImage* plane, int planeNumber) { - int success = - imaqReplaceColorPlanes(m_imaqImage, (const Image *)m_imaqImage, mode, - (planeNumber == 1) ? plane->GetImaqImage() : nullptr, - (planeNumber == 2) ? plane->GetImaqImage() : nullptr, - (planeNumber == 3) ? plane->GetImaqImage() : nullptr); + int success = imaqReplaceColorPlanes( + m_imaqImage, (const Image*)m_imaqImage, mode, + (planeNumber == 1) ? plane->GetImaqImage() : nullptr, + (planeNumber == 2) ? plane->GetImaqImage() : nullptr, + (planeNumber == 3) ? plane->GetImaqImage() : nullptr); wpi_setImaqErrorWithContext(success, "Imaq ReplaceColorPlanes failed"); } /** * Replace the first color plane with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceFirstColorPlane(ColorMode mode, MonoImage *plane) { +void ColorImage::ReplaceFirstColorPlane(ColorMode mode, MonoImage* plane) { ReplacePlane(mode, plane, 1); } /** * Replace the second color plane with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceSecondColorPlane(ColorMode mode, MonoImage *plane) { +void ColorImage::ReplaceSecondColorPlane(ColorMode mode, MonoImage* plane) { ReplacePlane(mode, plane, 2); } /** * Replace the third color plane with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceThirdColorPlane(ColorMode mode, MonoImage *plane) { +void ColorImage::ReplaceThirdColorPlane(ColorMode mode, MonoImage* plane) { ReplacePlane(mode, plane, 3); } /** * Replace the red color plane with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceRedPlane(MonoImage *plane) { +void ColorImage::ReplaceRedPlane(MonoImage* plane) { ReplaceFirstColorPlane(IMAQ_RGB, plane); } /** * Replace the green color plane with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceGreenPlane(MonoImage *plane) { +void ColorImage::ReplaceGreenPlane(MonoImage* plane) { ReplaceSecondColorPlane(IMAQ_RGB, plane); } /** * Replace the blue color plane with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceBluePlane(MonoImage *plane) { +void ColorImage::ReplaceBluePlane(MonoImage* plane) { ReplaceThirdColorPlane(IMAQ_RGB, plane); } /** * Replace the Hue color plane in a HSL image with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceHSLHuePlane(MonoImage *plane) { +void ColorImage::ReplaceHSLHuePlane(MonoImage* plane) { return ReplaceFirstColorPlane(IMAQ_HSL, plane); } /** * Replace the Hue color plane in a HSV image with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceHSVHuePlane(MonoImage *plane) { +void ColorImage::ReplaceHSVHuePlane(MonoImage* plane) { return ReplaceFirstColorPlane(IMAQ_HSV, plane); } /** * Replace the first Hue plane in a HSI image with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceHSIHuePlane(MonoImage *plane) { +void ColorImage::ReplaceHSIHuePlane(MonoImage* plane) { return ReplaceFirstColorPlane(IMAQ_HSI, plane); } /** * Replace the Saturation color plane in an HSL image with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceHSLSaturationPlane(MonoImage *plane) { +void ColorImage::ReplaceHSLSaturationPlane(MonoImage* plane) { return ReplaceSecondColorPlane(IMAQ_HSL, plane); } /** * Replace the Saturation color plane in a HSV image with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceHSVSaturationPlane(MonoImage *plane) { +void ColorImage::ReplaceHSVSaturationPlane(MonoImage* plane) { return ReplaceSecondColorPlane(IMAQ_HSV, plane); } /** * Replace the Saturation color plane in a HSI image with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceHSISaturationPlane(MonoImage *plane) { +void ColorImage::ReplaceHSISaturationPlane(MonoImage* plane) { return ReplaceSecondColorPlane(IMAQ_HSI, plane); } /** * Replace the Luminance color plane in an HSL image with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceLuminancePlane(MonoImage *plane) { +void ColorImage::ReplaceLuminancePlane(MonoImage* plane) { return ReplaceThirdColorPlane(IMAQ_HSL, plane); } /** * Replace the Value color plane in an HSV with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceValuePlane(MonoImage *plane) { +void ColorImage::ReplaceValuePlane(MonoImage* plane) { return ReplaceThirdColorPlane(IMAQ_HSV, plane); } /** * Replace the Intensity color plane in a HSI image with a MonoImage. - * @param mode The color mode in which to operate. + * + * @param mode The color mode in which to operate. * @param plane A pointer to a MonoImage that will replace the specified color - * plane. + * plane. */ -void ColorImage::ReplaceIntensityPlane(MonoImage *plane) { +void ColorImage::ReplaceIntensityPlane(MonoImage* plane) { return ReplaceThirdColorPlane(IMAQ_HSI, plane); } @@ -435,7 +475,7 @@ void ColorImage::ReplaceIntensityPlane(MonoImage *plane) { // to imaqColorEqualize. void ColorImage::Equalize(bool allPlanes) { // Note that this call uses NI-defined TRUE and FALSE - int success = imaqColorEqualize(m_imaqImage, (const Image *)m_imaqImage, + int success = imaqColorEqualize(m_imaqImage, (const Image*)m_imaqImage, (allPlanes) ? TRUE : FALSE); wpi_setImaqErrorWithContext(success, "Imaq ColorEqualize error"); } diff --git a/wpilibc/Athena/src/Vision/FrcError.cpp b/wpilibc/Athena/src/Vision/FrcError.cpp index 31b76be8d8..79645d609b 100644 --- a/wpilibc/Athena/src/Vision/FrcError.cpp +++ b/wpilibc/Athena/src/Vision/FrcError.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include "nivision.h" #include "Vision/FrcError.h" +#include "nivision.h" /** * Get the error code returned from the NI Vision library @@ -20,13 +20,15 @@ int GetLastVisionError() { } /** -* Get the error text for an NI Vision error code. -* Note: imaqGetErrorText() is not supported on real time system, so -* so relevant strings are hardcoded here - the maintained version is -* in the LabWindows/CVI help file. -* @param errorCode The error code to find the text for. -* @return The error text -*/ + * Get the error text for an NI Vision error code. + * + * Note: imaqGetErrorText() is not supported on real time system, so + * so relevant strings are hardcoded here - the maintained version is + * in the LabWindows/CVI help file. + * + * @param errorCode The error code to find the text for. + * @return The error text + */ const char* GetVisionErrorText(int errorCode) { const char* errorText; diff --git a/wpilibc/Athena/src/Vision/HSLImage.cpp b/wpilibc/Athena/src/Vision/HSLImage.cpp index c9cdd116ff..3bc30d3c21 100644 --- a/wpilibc/Athena/src/Vision/HSLImage.cpp +++ b/wpilibc/Athena/src/Vision/HSLImage.cpp @@ -14,9 +14,10 @@ HSLImage::HSLImage() : ColorImage(IMAQ_IMAGE_HSL) {} /** * Create a new image by loading a file. + * * @param fileName The path of the file to load. */ -HSLImage::HSLImage(const char *fileName) : ColorImage(IMAQ_IMAGE_HSL) { +HSLImage::HSLImage(const char* fileName) : ColorImage(IMAQ_IMAGE_HSL) { int success = imaqReadFile(m_imaqImage, fileName, nullptr, nullptr); wpi_setImaqErrorWithContext(success, "Imaq ReadFile error"); } diff --git a/wpilibc/Athena/src/Vision/ImageBase.cpp b/wpilibc/Athena/src/Vision/ImageBase.cpp index 9ef0a61ee3..979c7551c9 100644 --- a/wpilibc/Athena/src/Vision/ImageBase.cpp +++ b/wpilibc/Athena/src/Vision/ImageBase.cpp @@ -10,8 +10,10 @@ /** * Create a new instance of an ImageBase. - * Imagebase is the base of all the other image classes. The constructor + * + * ImageBase is the base of all the other image classes. The constructor * creates any type of image and stores the pointer to it in the class. + * * @param type The type of image to create */ ImageBase::ImageBase(ImageType type) { @@ -20,6 +22,7 @@ ImageBase::ImageBase(ImageType type) { /** * Frees memory associated with an ImageBase. + * * Destructor frees the imaq image allocated with the class. */ ImageBase::~ImageBase() { @@ -28,16 +31,17 @@ ImageBase::~ImageBase() { /** * Writes an image to a file with the given filename. - * Write the image to a file in the flash on the cRIO. + * * @param fileName The name of the file to write */ -void ImageBase::Write(const char *fileName) { +void ImageBase::Write(const char* fileName) { int success = imaqWriteFile(m_imaqImage, fileName, nullptr); wpi_setImaqErrorWithContext(success, "Imaq Image writeFile error"); } /** * Gets the height of an image. + * * @return The height of the image in pixels. */ int ImageBase::GetHeight() { @@ -48,6 +52,7 @@ int ImageBase::GetHeight() { /** * Gets the width of an image. + * * @return The width of the image in pixels. */ int ImageBase::GetWidth() { @@ -61,4 +66,4 @@ int ImageBase::GetWidth() { * * @return A pointer to the internal IMAQ Image data structure. */ -Image *ImageBase::GetImaqImage() { return m_imaqImage; } +Image* ImageBase::GetImaqImage() { return m_imaqImage; } diff --git a/wpilibc/Athena/src/Vision/MonoImage.cpp b/wpilibc/Athena/src/Vision/MonoImage.cpp index 469182db09..7f0e872031 100644 --- a/wpilibc/Athena/src/Vision/MonoImage.cpp +++ b/wpilibc/Athena/src/Vision/MonoImage.cpp @@ -14,18 +14,20 @@ MonoImage::MonoImage() : ImageBase(IMAQ_IMAGE_U8) {} /** * Look for ellipses in an image. + * * Given some input parameters, look for any number of ellipses in an image. - * @param ellipseDescriptor Ellipse descriptor - * @param curveOptions Curve options + * + * @param ellipseDescriptor Ellipse descriptor + * @param curveOptions Curve options * @param shapeDetectionOptions Shape detection options - * @param roi Region of Interest + * @param roi Region of Interest * @returns a vector of EllipseMatch structures (0 length vector on no match) */ -vector *MonoImage::DetectEllipses( - EllipseDescriptor *ellipseDescriptor, CurveOptions *curveOptions, - ShapeDetectionOptions *shapeDetectionOptions, ROI *roi) { +vector* MonoImage::DetectEllipses( + EllipseDescriptor* ellipseDescriptor, CurveOptions* curveOptions, + ShapeDetectionOptions* shapeDetectionOptions, ROI* roi) { int numberOfMatches; - EllipseMatch *e = + EllipseMatch* e = imaqDetectEllipses(m_imaqImage, ellipseDescriptor, curveOptions, shapeDetectionOptions, roi, &numberOfMatches); auto ellipses = new vector; @@ -39,9 +41,9 @@ vector *MonoImage::DetectEllipses( return ellipses; } -vector *MonoImage::DetectEllipses( - EllipseDescriptor *ellipseDescriptor) { - vector *ellipses = +vector* MonoImage::DetectEllipses( + EllipseDescriptor* ellipseDescriptor) { + vector* ellipses = DetectEllipses(ellipseDescriptor, nullptr, nullptr, nullptr); return ellipses; } diff --git a/wpilibc/Athena/src/Vision/RGBImage.cpp b/wpilibc/Athena/src/Vision/RGBImage.cpp index 92a305d968..f4ac34a468 100644 --- a/wpilibc/Athena/src/Vision/RGBImage.cpp +++ b/wpilibc/Athena/src/Vision/RGBImage.cpp @@ -14,9 +14,10 @@ RGBImage::RGBImage() : ColorImage(IMAQ_IMAGE_RGB) {} /** * Create a new image by loading a file. + * * @param fileName The path of the file to load. */ -RGBImage::RGBImage(const char *fileName) : ColorImage(IMAQ_IMAGE_RGB) { +RGBImage::RGBImage(const char* fileName) : ColorImage(IMAQ_IMAGE_RGB) { int success = imaqReadFile(m_imaqImage, fileName, nullptr, nullptr); wpi_setImaqErrorWithContext(success, "Imaq ReadFile error"); } diff --git a/wpilibc/Athena/src/Vision/VisionAPI.cpp b/wpilibc/Athena/src/Vision/VisionAPI.cpp index 4d9c7c1ce7..14f8e7fd99 100644 --- a/wpilibc/Athena/src/Vision/VisionAPI.cpp +++ b/wpilibc/Athena/src/Vision/VisionAPI.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Vision/BaeUtilities.h" #include "Vision/FrcError.h" @@ -21,41 +21,43 @@ int VisionAPI_debugFlag = 1; */ /** -* @brief Create an image object -* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_COMPLEX, -* IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64 -* The border size is defaulted to 3 so that convolutional algorithms work at the -* edges. -* When you are finished with the created image, dispose of it by calling -* frcDispose(). -* To get extended error information, call GetLastError(). -* -* @param type Type of image to create -* @return Image* On success, this function returns the created image. On -* failure, it returns nullptr. -*/ + * @brief Create an image object + * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_COMPLEX, + * IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64 + * The border size is defaulted to 3 so that convolutional algorithms work at + * the edges. + * When you are finished with the created image, dispose of it by calling + * frcDispose(). + * To get extended error information, call GetLastError(). + * + * @param type Type of image to create + * @return Image* On success, this function returns the created image. On + * failure, it returns nullptr. + */ Image* frcCreateImage(ImageType type) { return imaqCreateImage(type, DEFAULT_BORDER_SIZE); } /** -* @brief Dispose of one object. Supports any object created on the heap. -* -* @param object object to dispose of -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Dispose of one object. Supports any object created on the heap. + * + * @param object object to dispose of + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcDispose(void* object) { return imaqDispose(object); } + /** -* @brief Dispose of a list of objects. Supports any object created on the heap. -* -* @param functionName The name of the function -* @param ... A list of pointers to structures that need to be disposed of. -* The last pointer in the list should always be set to nullptr. -* -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Dispose of a list of objects. Supports any object created on the heap. + * + * @param functionName The name of the function + * @param ... A list of pointers to structures that need to be + * disposed of. The last pointer in the list should always + * be set to nullptr. + * + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcDispose(const char* functionName, ...) /* Variable argument list */ { va_list disposalPtrList; /* Input argument list */ @@ -76,53 +78,52 @@ int frcDispose(const char* functionName, ...) /* Variable argument list */ } /** -* @brief Copy an image object. -* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_RGB, -* IMAQ_IMAGE_HSL. -* -* @param dest Copy of image. On failure, dest is nullptr. Must have already been -* created using frcCreateImage(). -* When you are finished with the created image, dispose of it by calling -* frcDispose(). -* @param source Image to copy -* -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Copy an image object. + * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_RGB, + * IMAQ_IMAGE_HSL. + * + * @param dest Copy of image. On failure, dest is nullptr. Must have already + * been created using frcCreateImage(). When you are finished with + * the created image, dispose of it by calling frcDispose(). + * @param source Image to copy + * + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcCopyImage(Image* dest, const Image* source) { return imaqDuplicate(dest, source); } /** -* @brief Crop image without changing the scale. -* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_RGB, -* IMAQ_IMAGE_HSL. -* -* @param dest Modified image -* @param source Image to crop -* @param rect region to process, or IMAQ_NO_RECT -* -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Crop image without changing the scale. + * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_RGB, + * IMAQ_IMAGE_HSL. + * + * @param dest Modified image + * @param source Image to crop + * @param rect region to process, or IMAQ_NO_RECT + * + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcCrop(Image* dest, const Image* source, Rect rect) { return imaqScale(dest, source, 1, 1, IMAQ_SCALE_LARGER, rect); } /** -* @brief Scales the entire image larger or smaller. -* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_RGB, -* IMAQ_IMAGE_HSL. -* -* @param dest Modified image -* @param source Image to scale -* @param xScale the horizontal reduction ratio -* @param yScale the vertical reduction ratio -* @param scaleMode IMAQ_SCALE_LARGER or IMAQ_SCALE_SMALLER -* -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Scales the entire image larger or smaller. + * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_RGB, + * IMAQ_IMAGE_HSL. + * + * @param dest Modified image + * @param source Image to scale + * @param xScale the horizontal reduction ratio + * @param yScale the vertical reduction ratio + * @param scaleMode IMAQ_SCALE_LARGER or IMAQ_SCALE_SMALLER + * + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcScale(Image* dest, const Image* source, int xScale, int yScale, ScalingMode scaleMode) { Rect rect = IMAQ_NO_RECT; @@ -136,95 +137,92 @@ int frcScale(Image* dest, const Image* source, int xScale, int yScale, * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_COMPLEX, * IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64. * - * @param image Image read in + * @param image Image read in * @param fileName File to read. Cannot be nullptr * * @return On success: 1. On failure: 0. To get extended error information, call - * GetLastError(). + * GetLastError(). */ int frcReadImage(Image* image, const char* fileName) { return imaqReadFile(image, fileName, nullptr, nullptr); } /** -* @brief Write image to a file. -* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_COMPLEX, -* IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64. -* -* The file type is determined by the extension, as follows: -* -* Extension File Type -* aipd or .apd AIPD -* .bmp BMP -* .jpg or .jpeg JPEG -* .jp2 JPEG2000 -* .png PNG -* .tif or .tiff TIFF -* -* -* The following are the supported image types for each file type: -* -* File Types Image Types -* AIPD all image types -* BMP, JPEG 8-bit, RGB -* PNG, TIFF, JPEG2000 8-bit, 16-bit, RGB, RGBU64 -* -* @param image Image to write -* @param fileName File to read. Cannot be nullptr. The extension determines the -* file format that is written. -* -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Write image to a file. + * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_COMPLEX, + * IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64. + * + * The file type is determined by the extension, as follows: + * + * Extension File Type + * aipd or .apd AIPD + * .bmp BMP + * .jpg or .jpeg JPEG + * .jp2 JPEG2000 + * .png PNG + * .tif or .tiff TIFF + * + * + * The following are the supported image types for each file type: + * + * File Types Image Types + * AIPD all image types + * BMP, JPEG 8-bit, RGB + * PNG, TIFF, JPEG2000 8-bit, 16-bit, RGB, RGBU64 + * + * @param image Image to write + * @param fileName File to read. Cannot be nullptr. The extension determines the + * file format that is written. + * + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcWriteImage(const Image* image, const char* fileName) { RGBValue* colorTable = nullptr; return imaqWriteFile(image, fileName, colorTable); } -/* Measure Intensity functions */ +/* Measure Intensity functions */ /** -* @brief Measures the pixel intensities in a rectangle of an image. -* Outputs intensity based statistics about an image such as Max, Min, Mean and -* Std Dev of pixel value. -* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL. -* -* Parameter Discussion : -* Relevant parameters of the HistogramReport include: -* min, max, mean and stdDev -* min/max —Setting both min and max to 0 causes the function to set -* min to 0 -* and the max to 255 for 8-bit images and to the actual -* minimum value and -* maximum value of the image for all other image types. -* max—Setting both min and max to 0 causes the function to set max -* to 255 -* for 8-bit images and to the actual maximum value of the -* image for -* all other image types. -* -* @param image Image whose histogram the function calculates. -* @param numClasses The number of classes into which the function separates the -* pixels. -* Determines the number of elements in the histogram array returned -* @param min The minimum pixel value to consider for the histogram. -* The function does not count pixels with values less than min. -* @param max The maximum pixel value to consider for the histogram. -* The function does not count pixels with values greater than max. -* @param rect Region of interest in the image. If not included, the entire image -* is used. -* @return On success, this function returns a report describing the pixel value -* classification. -* When you are finished with the report, dispose of it by calling frcDispose(). -* On failure, this function returns nullptr. To get extended error information, -* call GetLastError(). -* -*/ + * @brief Measures the pixel intensities in a rectangle of an image. + * Outputs intensity based statistics about an image such as Max, Min, Mean and + * Std Dev of pixel value. + * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL. + * + * Parameter Discussion: + * Relevant parameters of the HistogramReport include: + * min, max, mean and stdDev + * min/max —Setting both min and max to 0 causes the function to + * set min to 0 and the max to 255 for 8-bit images and to the actual + * minimum value and maximum value of the image for all other image + * types. + * max—Setting both min and max to 0 causes the function to set max + * to 255 for 8-bit images and to the actual maximum value of the + * image for all other image types. + * + * @param image Image whose histogram the function calculates. + * @param numClasses The number of classes into which the function separates the + * pixels. Determines the number of elements in the histogram + * array returned + * @param min The minimum pixel value to consider for the histogram. The + * function does not count pixels with values less than min. + * @param max The maximum pixel value to consider for the histogram. The + * function does not count pixels with values greater than + * max. + * @param rect Region of interest in the image. If not included, the + * entire image is used. + * @return On success, this function returns a report describing the pixel value + * classification. When you are finished with the report, dispose of it + * by calling frcDispose(). On failure, this function returns nullptr. + * To get extended error information, call GetLastError(). + */ HistogramReport* frcHistogram(const Image* image, int numClasses, float min, float max) { Rect rect = IMAQ_NO_RECT; return frcHistogram(image, numClasses, min, max, rect); } + HistogramReport* frcHistogram(const Image* image, int numClasses, float min, float max, Rect rect) { int success; @@ -258,29 +256,28 @@ HistogramReport* frcHistogram(const Image* image, int numClasses, float min, } /** -* @brief Calculates the histogram, or pixel distribution, of a color image. -* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL. -* -* @param image Image whose histogram the function calculates. -* @param numClasses The number of classes into which the function separates the -* pixels. -* Determines the number of elements in the histogram array returned -* @param mode The color space in which to perform the histogram. Possible values -* include IMAQ_RGB and IMAQ_HSL. -* @param mask An optional mask image. This image must be an IMAQ_IMAGE_U8 image. -* The function calculates the histogram using only those pixels in the image -* whose -* corresponding pixels in the mask are non-zero. Set this parameter to nullptr to -* calculate -* the histogram of the entire image, or use the simplified call. -* -* @return On success, this function returns a report describing the -* classification -* of each plane in a HistogramReport. -* When you are finished with the report, dispose of it by calling frcDispose(). -* On failure, this function returns nullptr. -* To get extended error information, call imaqGetLastError(). -*/ + * @brief Calculates the histogram, or pixel distribution, of a color image. + * Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL. + * + * @param image Image whose histogram the function calculates. + * @param numClasses The number of classes into which the function separates the + * pixels. Determines the number of elements in the histogram + * array returned + * @param mode The color space in which to perform the histogram. Possible + * values include IMAQ_RGB and IMAQ_HSL. + * @param mask An optional mask image. This image must be an IMAQ_IMAGE_U8 + * image. The function calculates the histogram using only + * those pixels in the image whose corresponding pixels in the + * mask are non-zero. Set this parameter to nullptr to + * calculate the histogram of the entire image, or use the + * simplified call. + * + * @return On success, this function returns a report describing the + * classification of each plane in a HistogramReport. When you are + * finished with the report, dispose of it by calling frcDispose(). + * On failure, this function returns nullptr. To get extended error + * information, call imaqGetLastError(). + */ ColorHistogramReport* frcColorHistogram(const Image* image, int numClasses, ColorMode mode) { return frcColorHistogram(image, numClasses, mode, nullptr); @@ -292,45 +289,47 @@ ColorHistogramReport* frcColorHistogram(const Image* image, int numClasses, } /** -* @brief Measures the pixel intensities in a rectangle of an image. -* Outputs intensity based statistics about an image such as Max, Min, Mean and -* Std Dev of pixel value. -* Supports IMAQ_IMAGE_U8 (grayscale) IMAQ_IMAGE_RGB (color) IMAQ_IMAGE_HSL -* (color-HSL). -* -* @param image The image whose pixel value the function queries -* @param pixel The coordinates of the pixel that the function queries -* @param value On return, the value of the specified image pixel. This parameter -* cannot be nullptr. -* This data structure contains either grayscale, RGB, HSL, Complex or -* RGBU64Value depending on the type of image. -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Measures the pixel intensities in a rectangle of an image. + * Outputs intensity based statistics about an image such as Max, Min, Mean and + * Std Dev of pixel value. + * Supports IMAQ_IMAGE_U8 (grayscale) IMAQ_IMAGE_RGB (color) IMAQ_IMAGE_HSL + * (color-HSL). + * + * @param image The image whose pixel value the function queries + * @param pixel The coordinates of the pixel that the function queries + * @param value On return, the value of the specified image pixel. This + * parameter cannot be nullptr. This data structure contains + * either grayscale, RGB, HSL, Complex or RGBU64Value depending on + * the type of image. + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcGetPixelValue(const Image* image, Point pixel, PixelValue* value) { return imaqGetPixel(image, pixel, value); } -/* Particle Analysis functions */ +/* Particle Analysis functions */ /** -* @brief Filters particles out of an image based on their measurements. -* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL. -* -* @param dest The destination image. If dest is used, it must be the same size -* as the Source image. It will contain only the filtered particles. -* @param source The image containing the particles to filter. -* @param criteria An array of criteria to apply to the particles in the source -* image. This array cannot be nullptr. -* See the NIVisionCVI.chm help file for definitions of criteria. -* @param criteriaCount The number of elements in the criteria array. -* @param options Binary filter options, including rejectMatches, rejectBorder, -* and connectivity8. -* @param rect Area of image to filter. If omitted, the default is entire image. -* @param numParticles On return, the number of particles left in the image -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Filters particles out of an image based on their measurements. + * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL. + * + * @param dest The destination image. If dest is used, it must be the + * same size as the Source image. It will contain only the + * filtered particles. + * @param source The image containing the particles to filter. + * @param criteria An array of criteria to apply to the particles in the + * source image. This array cannot be nullptr. See the + * NIVisionCVI.chm help file for definitions of criteria. + * @param criteriaCount The number of elements in the criteria array. + * @param options Binary filter options, including rejectMatches, + * rejectBorder, and connectivity8. + * @param rect Area of image to filter. If omitted, the default is + * entire image. + * @param numParticles On return, the number of particles left in the image + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcParticleFilter(Image* dest, Image* source, const ParticleFilterCriteria2* criteria, int criteriaCount, const ParticleFilterOptions* options, @@ -351,27 +350,29 @@ int frcParticleFilter(Image* dest, Image* source, } /** -* @brief Performs morphological transformations on binary images. -* Supports IMAQ_IMAGE_U8. -* -* @param dest The destination image. The border size of the destination image is -* not important. -* @param source The image on which the function performs the morphological -* operations. The calculation -* modifies the border of the source image. The border must be at least half as -* large as the larger -* dimension of the structuring element. The connected source image for a -* morphological transformation -* must have been created with a border capable of supporting the size of the -* structuring element. -* A 3 by 3 structuring element requires a minimal border of 1, a 5 by 5 -* structuring element requires a minimal border of 2, and so on. -* @param method The morphological transform to apply. -* @param structuringElement The structuring element used in the operation. Omit -* this parameter if you do not want a custom structuring element. -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Performs morphological transformations on binary images. + * Supports IMAQ_IMAGE_U8. + * + * @param dest The destination image. The border size of the + * destination image is not important. + * @param source The image on which the function performs the + * morphological operations. The calculation modifies + * the border of the source image. The border must be + * at least half as large as the larger dimension of + * the structuring element. The connected source + * image for a morphological transformation must have + * been created with a border capable of supporting + * the size of the structuring element. A 3 by 3 + * structuring element requires a minimal border of 1, + * a 5 by 5 structuring element requires a minimal + * border of 2, and so on. + * @param method The morphological transform to apply. + * @param structuringElement The structuring element used in the operation. Omit + * this parameter if you do not want a custom + * structuring element. + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcMorphology(Image* dest, Image* source, MorphologyMethod method) { return imaqMorphology(dest, source, method, nullptr); } @@ -382,24 +383,23 @@ int frcMorphology(Image* dest, Image* source, MorphologyMethod method, } /** -* @brief Eliminates particles that touch the border of the image. -* Supports IMAQ_IMAGE_U8. -* -* @param dest The destination image. -* @param source The source image. If the image has a border, the function sets -* all border pixel values to 0. -* @param connectivity8 specifies the type of connectivity used by the algorithm -* for particle detection. -* The connectivity mode directly determines whether an adjacent pixel belongs to -* the same particle or a -* different particle. Set to TRUE to use connectivity-8 to determine whether -* particles are touching -* Set to FALSE to use connectivity-4 to determine whether particles are -* touching. -* The default setting for the simplified call is TRUE -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Eliminates particles that touch the border of the image. + * Supports IMAQ_IMAGE_U8. + * + * @param dest The destination image. + * @param source The source image. If the image has a border, the + * function sets all border pixel values to 0. + * @param connectivity8 specifies the type of connectivity used by the algorithm + * for particle detection. The connectivity mode directly + * determines whether an adjacent pixel belongs to the same + * particle or a different particle. Set to TRUE to use + * connectivity-8 to determine whether particles are + * touching. Set to FALSE to use connectivity-4 to + * determine whether particles are touching. The default + * setting for the simplified call is TRUE + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcRejectBorder(Image* dest, Image* source) { return imaqRejectBorder(dest, source, TRUE); } @@ -409,31 +409,32 @@ int frcRejectBorder(Image* dest, Image* source, int connectivity8) { } /** -* @brief Counts the number of particles in a binary image. -* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL. -* @param image binary (thresholded) image -* @param numParticles On return, the number of particles. -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Counts the number of particles in a binary image. + * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL. + * @param image binary (thresholded) image + * @param numParticles On return, the number of particles. + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcCountParticles(Image* image, int* numParticles) { return imaqCountParticles(image, 1, numParticles); } /** -* @brief Conduct measurements for a single particle in an images. -* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL. -* -* @param image image with the particle to analyze. This function modifies the -* source image. -* If you need the original image, create a copy of the image using frcCopy() -* before using this function. -* @param particleNumber The number of the particle to get information on -* @param par on return, a particle analysis report containing information about -* the particle. This structure must be created by the caller. -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Conduct measurements for a single particle in an images. + * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL. + * + * @param image image with the particle to analyze. This function + * modifies the source image. If you need the original + * image, create a copy of the image using frcCopy() + * before using this function. + * @param particleNumber The number of the particle to get information on + * @param par on return, a particle analysis report containing + * information about the particle. This structure must be + * created by the caller. + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcParticleAnalysis(Image* image, int particleNumber, ParticleAnalysisReport* par) { int success = 0; @@ -447,7 +448,7 @@ int frcParticleAnalysis(Image* image, int particleNumber, par->imageHeight = height; par->particleIndex = particleNumber; - /* center of mass point of the largest particle */ + /* center of mass point of the largest particle */ double returnDouble; success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_CENTER_OF_MASS_X, &returnDouble); @@ -522,33 +523,36 @@ int frcParticleAnalysis(Image* image, int particleNumber, return success; } -/* Image Enhancement functions */ +/* Image Enhancement functions */ /** -* @brief Improves contrast on a grayscale image. -* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16. -* @param dest The destination image. -* @param source The image to equalize -* @param min the smallest value used for processing. After processing, all pixel -* values that are less than or equal to the Minimum in the original image are set -* to 0 for an 8-bit image. In 16-bit and floating-point images, these pixel -* values are set to the smallest pixel value found in the original image. -* @param max the largest value used for processing. After processing, all pixel -* values that are greater than or equal to the Maximum in the original image are -* set to 255 for an 8-bit image. In 16-bit and floating-point images, these pixel -* values are set to the largest pixel value found in the original image. -* @param mask an 8-bit image that specifies the region of the small image that -* will be copied. Only those pixels in the Image Src (Small) image that -* correspond to an equivalent non-zero pixel in the mask image are copied. All -* other pixels keep their original values. The entire image is processed if Image -* Mask is nullptr or this parameter is omitted. -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -* -* option defaults: -* searchRect = IMAQ_NO_RECT -* minMatchScore = DEFAULT_MINMAX_SCORE (800) -*/ + * @brief Improves contrast on a grayscale image. + * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16. + * @param dest The destination image. + * @param source The image to equalize + * @param min the smallest value used for processing. After processing, all + * pixel values that are less than or equal to the Minimum in the + * original image are set to 0 for an 8-bit image. In 16-bit and + * floating-point images, these pixel values are set to the + * smallest pixel value found in the original image. + * @param max the largest value used for processing. After processing, all + * pixel values that are greater than or equal to the Maximum in + * the original image are set to 255 for an 8-bit image. In 16-bit + * and floating-point images, these pixel values are set to the + * largest pixel value found in the original image. + * @param mask an 8-bit image that specifies the region of the small image + * that will be copied. Only those pixels in the Image Src (Small) + * image that correspond to an equivalent non-zero pixel in the + * mask image are copied. All other pixels keep their original + * values. The entire image is processed if Image Mask is nullptr + * or this parameter is omitted. + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + * + * option defaults: + * searchRect = IMAQ_NO_RECT + * minMatchScore = DEFAULT_MINMAX_SCORE (800) + */ int frcEqualize(Image* dest, const Image* source, float min, float max) { return frcEqualize(dest, source, min, max, nullptr); } @@ -559,19 +563,19 @@ int frcEqualize(Image* dest, const Image* source, float min, float max, } /** -* @brief Improves contrast on a color image. -* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL -* -* option defaults: colorEqualization = TRUE to equalize all three planes of the -* image -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -* @param dest The destination image. -* @param source The image to equalize -* @param colorEqualization Set this parameter to TRUE to equalize all three -* planes of the image (the default). Set this parameter to FALSE to equalize only -* the luminance plane. -*/ + * @brief Improves contrast on a color image. + * Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL + * + * option defaults: colorEqualization = TRUE to equalize all three planes of the + * image + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + * @param dest The destination image. + * @param source The image to equalize + * @param colorEqualization Set this parameter to TRUE to equalize all three + * planes of the image (the default). Set this parameter to FALSE to equalize + * only the luminance plane. + */ int frcColorEqualize(Image* dest, const Image* source) { return imaqColorEqualize(dest, source, TRUE); } @@ -580,47 +584,47 @@ int frcColorEqualize(Image* dest, const Image* source, int colorEqualization) { return imaqColorEqualize(dest, source, TRUE); } -/* Image Conversion functions */ +/* Image Conversion functions */ /** -* @brief Automatically thresholds a grayscale image into a binary image for -* Particle Analysis based on a smart threshold. -* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_I16 -* @param dest The destination image. -* @param source The image to threshold -* @param windowWidth The width of the rectangular window around the pixel on -* which the function -* performs the local threshold. This number must be at least 3 and cannot be -* larger than the width of source -* @param windowHeight The height of the rectangular window around the pixel on -* which the function -* performs the local threshold. This number must be at least 3 and cannot be -* larger than the height of source -* @param method Specifies the local thresholding method the function uses. Value -* can be IMAQ_NIBLACK -* (which computes thresholds for each pixel based on its local statistics using -* the Niblack local thresholding -* algorithm.), or IMAQ_BACKGROUND_CORRECTION (which does background correction -* first to eliminate non-uniform -* lighting effects, then performs thresholding using the Otsu thresholding -* algorithm) -* @param deviationWeight Specifies the k constant used in the Niblack local -* thresholding algorithm, which -* determines the weight applied to the variance calculation. Valid k constants -* range from 0 to 1. Setting -* this value to 0 will increase the performance of the function because the -* function will not calculate the -* variance for any of the pixels. The function ignores this value if method is -* not set to IMAQ_NIBLACK -* @param type Specifies the type of objects for which you want to look. Values -* can be IMAQ_BRIGHT_OBJECTS -* or IMAQ_DARK_OBJECTS. -* @param replaceValue Specifies the replacement value the function uses for the -* pixels of the kept objects -* in the destination image. -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Automatically thresholds a grayscale image into a binary image for + * Particle Analysis based on a smart threshold. + * Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_I16 + * @param dest The destination image. + * @param source The image to threshold + * @param windowWidth The width of the rectangular window around the pixel + * on which the function performs the local threshold. + * This number must be at least 3 and cannot be larger + * than the width of source + * @param windowHeight The height of the rectangular window around the pixel + * on which the function performs the local threshold. + * This number must be at least 3 and cannot be larger + * than the height of source + * @param method Specifies the local thresholding method the function + * uses. Value can be IMAQ_NIBLACK (which computes + * thresholds for each pixel based on its local + * statistics using the Niblack local thresholding + * algorithm.), or IMAQ_BACKGROUND_CORRECTION (which + * does background correction first to eliminate + * non-uniform lighting effects, then performs + * thresholding using the Otsu thresholding algorithm) + * @param deviationWeight Specifies the k constant used in the Niblack local + * thresholding algorithm, which determines the weight + * applied to the variance calculation. Valid k constants + * range from 0 to 1. Setting this value to 0 will + * increase the performance of the function because the + * function will not calculate the variance for any of + * the pixels. The function ignores this value if method + * is not set to IMAQ_NIBLACK + * @param type Specifies the type of objects for which you want to + * look. Values can be IMAQ_BRIGHT_OBJECTS or + * IMAQ_DARK_OBJECTS. + * @param replaceValue Specifies the replacement value the function uses for + * the pixels of the kept objects in the destination + * image. + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcSmartThreshold(Image* dest, const Image* source, unsigned int windowWidth, unsigned int windowHeight, LocalThresholdMethod method, double deviationWeight, @@ -639,22 +643,21 @@ int frcSmartThreshold(Image* dest, const Image* source, } /** -* @brief Converts a grayscale image to a binary image for Particle Analysis -* based on a fixed threshold. -* The function sets pixels values outside of the given range to 0. The function -* sets pixel values -* within the range to a given value or leaves the values unchanged. -* Use the simplified call to leave pixel values unchanged. -* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_I16. -* -* @param dest The destination image. -* @param source The image to threshold -* @param rangeMin The lower boundary of the range of pixel values to keep -* @param rangeMax The upper boundary of the range of pixel values to keep. -* -* @return int - error code: 0 = error. To get extended error information, call -* GetLastError(). -*/ + * @brief Converts a grayscale image to a binary image for Particle Analysis + * based on a fixed threshold. + * The function sets pixels values outside of the given range to 0. The function + * sets pixel values within the range to a given value or leaves the values + * unchanged. Use the simplified call to leave pixel values unchanged. + * Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_I16. + * + * @param dest The destination image. + * @param source The image to threshold + * @param rangeMin The lower boundary of the range of pixel values to keep + * @param rangeMax The upper boundary of the range of pixel values to keep. + * + * @return int - error code: 0 = error. To get extended error information, call + * GetLastError(). + */ int frcSimpleThreshold(Image* dest, const Image* source, float rangeMin, float rangeMax) { int newValue = 255; @@ -662,23 +665,22 @@ int frcSimpleThreshold(Image* dest, const Image* source, float rangeMin, } /** -* @brief Converts a grayscale image to a binary image for Particle Analysis -* based on a fixed threshold. -* The function sets pixels values outside of the given range to 0. The function -* sets -* pixel values within the range to the given value. -* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_I16. -* -* @param dest The destination image. -* @param source The image to threshold -* @param rangeMin The lower boundary of the range of pixel values to keep -* @param rangeMax The upper boundary of the range of pixel values to keep. -* @param newValue The replacement value for pixels within the range. Use the -* simplified call to leave the pixel values unchanged -* -* @return int - error code: 0 = error. To get extended error information, call -* GetLastError(). -*/ + * @brief Converts a grayscale image to a binary image for Particle Analysis + * based on a fixed threshold. + * The function sets pixels values outside of the given range to 0. The function + * sets pixel values within the range to the given value. + * Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_I16. + * + * @param dest The destination image. + * @param source The image to threshold + * @param rangeMin The lower boundary of the range of pixel values to keep + * @param rangeMax The upper boundary of the range of pixel values to keep. + * @param newValue The replacement value for pixels within the range. Use the + * simplified call to leave the pixel values unchanged + * + * @return int - error code: 0 = error. To get extended error information, call + * GetLastError(). + */ int frcSimpleThreshold(Image* dest, const Image* source, float rangeMin, float rangeMax, float newValue) { int useNewValue = TRUE; @@ -686,26 +688,28 @@ int frcSimpleThreshold(Image* dest, const Image* source, float rangeMin, } /** -* @brief Applies a threshold to the Red, Green, and Blue values of a RGB image -* or the Hue, -* Saturation, Luminance values for a HSL image. -* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL. -* This simpler version filters based on a hue range in the HSL mode. -* -* @param dest The destination image. This must be a IMAQ_IMAGE_U8 image. -* @param source The image to threshold -* @param mode The color space to perform the threshold in. valid values are: -* IMAQ_RGB, IMAQ_HSL. -* @param plane1Range The selection range for the first plane of the image. Set -* this parameter to nullptr to use a selection range from 0 to 255. -* @param plane2Range The selection range for the second plane of the image. Set -* this parameter to nullptr to use a selection range from 0 to 255. -* @param plane3Range The selection range for the third plane of the image. Set -* this parameter to nullptr to use a selection range from 0 to 255. -* -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -* */ + * @brief Applies a threshold to the Red, Green, and Blue values of a RGB image + * or the Hue, Saturation, Luminance values for a HSL image. + * Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL. + * This simpler version filters based on a hue range in the HSL mode. + * + * @param dest The destination image. This must be a IMAQ_IMAGE_U8 image. + * @param source The image to threshold + * @param mode The color space to perform the threshold in. valid values + * are: IMAQ_RGB, IMAQ_HSL. + * @param plane1Range The selection range for the first plane of the image. Set + * this parameter to nullptr to use a selection range from 0 + * to 255. + * @param plane2Range The selection range for the second plane of the image. Set + * this parameter to nullptr to use a selection range from 0 + * to 255. + * @param plane3Range The selection range for the third plane of the image. Set + * this parameter to nullptr to use a selection range from 0 + * to 255. + * + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcColorThreshold(Image* dest, const Image* source, ColorMode mode, const Range* plane1Range, const Range* plane2Range, const Range* plane3Range) { @@ -715,28 +719,31 @@ int frcColorThreshold(Image* dest, const Image* source, ColorMode mode, } /** -* @brief Applies a threshold to the Red, Green, and Blue values of a RGB image -* or the Hue, -* Saturation, Luminance values for a HSL image. -* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL. -* The simpler version filters based on a hue range in the HSL mode. -* -* @param dest The destination image. This must be a IMAQ_IMAGE_U8 image. -* @param source The image to threshold -* @param replaceValue Value to assign to selected pixels. Defaults to 1 if -* simplified call is used. -* @param mode The color space to perform the threshold in. valid values are: -* IMAQ_RGB, IMAQ_HSL. -* @param plane1Range The selection range for the first plane of the image. Set -* this parameter to nullptr to use a selection range from 0 to 255. -* @param plane2Range The selection range for the second plane of the image. Set -* this parameter to nullptr to use a selection range from 0 to 255. -* @param plane3Range The selection range for the third plane of the image. Set -* this parameter to nullptr to use a selection range from 0 to 255. -* -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Applies a threshold to the Red, Green, and Blue values of a RGB image + * or the Hue, Saturation, Luminance values for a HSL image. + * Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL. + * The simpler version filters based on a hue range in the HSL mode. + * + * @param dest The destination image. This must be a IMAQ_IMAGE_U8 + * image. + * @param source The image to threshold + * @param replaceValue Value to assign to selected pixels. Defaults to 1 if + * simplified call is used. + * @param mode The color space to perform the threshold in. valid values + * are: IMAQ_RGB, IMAQ_HSL. + * @param plane1Range The selection range for the first plane of the image. + * Set this parameter to nullptr to use a selection range + * from 0 to 255. + * @param plane2Range The selection range for the second plane of the image. + * Set this parameter to nullptr to use a selection range + * from 0 to 255. + * @param plane3Range The selection range for the third plane of the image. + * Set this parameter to nullptr to use a selection range + * from 0 to 255. + * + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcColorThreshold(Image* dest, const Image* source, int replaceValue, ColorMode mode, const Range* plane1Range, const Range* plane2Range, const Range* plane3Range) { @@ -745,17 +752,17 @@ int frcColorThreshold(Image* dest, const Image* source, int replaceValue, } /** -* @brief A simpler version of ColorThreshold that thresholds hue range in the -* HSL mode. Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL. -* @param dest The destination image. -* @param source The image to threshold -* @param hueRange The selection range for the hue (color). -* @param minSaturation The minimum saturation value (1-255). If not used, -* DEFAULT_SATURATION_THRESHOLD is the default. -* -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief A simpler version of ColorThreshold that thresholds hue range in the + * HSL mode. Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL. + * @param dest The destination image. + * @param source The image to threshold + * @param hueRange The selection range for the hue (color). + * @param minSaturation The minimum saturation value (1-255). If not used, + * DEFAULT_SATURATION_THRESHOLD is the default. + * + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcHueThreshold(Image* dest, const Image* source, const Range* hueRange) { return frcHueThreshold(dest, source, hueRange, DEFAULT_SATURATION_THRESHOLD); } @@ -779,40 +786,43 @@ int frcHueThreshold(Image* dest, const Image* source, const Range* hueRange, } /** -* @brief Extracts the Red, Green, Blue, or Hue, Saturation or Luminance -* information from a color image. -* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64. -* -* @param image The source image that the function extracts the planes from. -* @param mode The color space that the function extracts the planes from. Valid -* values are IMAQ_RGB, IMAQ_HSL, IMAQ_HSV, IMAQ_HSI. -* @param plane1 On return, the first extracted plane. Set this parameter to nullptr -* if you do not need this information. RGB-Red, HSL/HSV/HSI-Hue. -* @param plane2 On return, the second extracted plane. Set this parameter to -* nullptr if you do not need this information. RGB-Green, HSL/HSV/HSI-Saturation. -* @param plane3 On return, the third extracted plane. Set this parameter to nullptr -* if you do not need this information. RGB-Blue, HSL-Luminance, HSV-Value, -* HSI-Intensity. -* -* @return error code: 0 = error. To get extended error information, call -* GetLastError(). -*/ + * @brief Extracts the Red, Green, Blue, or Hue, Saturation or Luminance + * information from a color image. + * Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64. + * + * @param image The source image that the function extracts the planes from. + * @param mode The color space that the function extracts the planes from. + * Valid values are IMAQ_RGB, IMAQ_HSL, IMAQ_HSV, IMAQ_HSI. + * @param plane1 On return, the first extracted plane. Set this parameter to + * nullptr if you do not need this information. RGB-Red, + * HSL/HSV/HSI-Hue. + * @param plane2 On return, the second extracted plane. Set this parameter to + * nullptr if you do not need this information. RGB-Green, + * HSL/HSV/HSI-Saturation. + * @param plane3 On return, the third extracted plane. Set this parameter to + * nullptr if you do not need this information. RGB-Blue, + * HSL-Luminance, HSV-Value, HSI-Intensity. + * + * @return error code: 0 = error. To get extended error information, call + * GetLastError(). + */ int frcExtractColorPlanes(const Image* image, ColorMode mode, Image* plane1, Image* plane2, Image* plane3) { return imaqExtractColorPlanes(image, mode, plane1, plane2, plane3); } /** -* @brief Extracts the Hue information from a color image. Supports -* IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64 -* -* @param image The source image that the function extracts the plane from. -* @param huePlane On return, the extracted hue plane. -* @param minSaturation the minimum saturation level required 0-255 (try 50) -* -* @return On success: 1. On failure: 0. To get extended error information, call -* GetLastError(). -*/ + * @brief Extracts the Hue information from a color image. Supports + * IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64 + * + * @param image The source image that the function extracts the plane + * from. + * @param huePlane On return, the extracted hue plane. + * @param minSaturation the minimum saturation level required 0-255 (try 50) + * + * @return On success: 1. On failure: 0. To get extended error information, call + * GetLastError(). + */ int frcExtractHuePlane(const Image* image, Image* huePlane) { return frcExtractHuePlane(image, huePlane, DEFAULT_SATURATION_THRESHOLD); } diff --git a/wpilibc/shared/include/Base.h b/wpilibc/shared/include/Base.h index 8bb8d4ccc6..87c36a6e7a 100644 --- a/wpilibc/shared/include/Base.h +++ b/wpilibc/shared/include/Base.h @@ -11,55 +11,54 @@ // (currently) only actually using the move constructors in non-MSVC situations // (ie, wpilibC++Devices), we can just ignore it in MSVC. #if !defined(_MSC_VER) -#define DEFAULT_MOVE_CONSTRUCTOR(ClassName) \ -ClassName(ClassName &&) = default +#define DEFAULT_MOVE_CONSTRUCTOR(ClassName) ClassName(ClassName&&) = default #else #define DEFAULT_MOVE_CONSTRUCTOR(ClassName) #endif #if (__cplusplus < 201103L) - #if !defined(_MSC_VER) - #define nullptr NULL - #endif - #define constexpr const +#if !defined(_MSC_VER) +#define nullptr NULL +#endif +#define constexpr const #endif #if defined(_MSC_VER) - #define noexcept throw() +#define noexcept throw() #endif // [[deprecated(msg)]] is a C++14 feature not supported by MSVC or GCC < 4.9. // We provide an equivalent warning implementation for those compilers here. #if defined(_MSC_VER) - #define DEPRECATED(msg) __declspec(deprecated(msg)) +#define DEPRECATED(msg) __declspec(deprecated(msg)) #elif defined(__GNUC__) - #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 8) - #define DEPRECATED(msg) [[deprecated(msg)]] - #else - #define DEPRECATED(msg) __attribute__((deprecated(msg))) - #endif -#elif __cplusplus > 201103L - #define DEPRECATED(msg) [[deprecated(msg)]] +#if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 8) +#define DEPRECATED(msg) [[deprecated(msg)]] #else - #define DEPRECATED(msg) /*nothing*/ +#define DEPRECATED(msg) __attribute__((deprecated(msg))) +#endif +#elif __cplusplus > 201103L +#define DEPRECATED(msg) [[deprecated(msg)]] +#else +#define DEPRECATED(msg) /*nothing*/ #endif // Provide std::decay_t when using GCC < 4.9 #if defined(__GNUC__) - #if __GNUC__ == 4 && __GNUC_MINOR__ < 9 - #include - namespace std { - template using decay_t = typename decay::type; - } - #endif +#if __GNUC__ == 4 && __GNUC_MINOR__ < 9 +#include +namespace std { +template +using decay_t = typename decay::type; +} +#endif #endif // A struct to use as a deleter when a std::shared_ptr must wrap a raw pointer // that is being deleted by someone else. -template -struct -NullDeleter { - void operator()(T *) const noexcept {}; +template +struct NullDeleter { + void operator()(T*) const noexcept {}; }; #include @@ -101,7 +100,7 @@ struct _Unique_if { }; template -typename _Unique_if::_Single_object make_unique(Args &&... args) { +typename _Unique_if::_Single_object make_unique(Args&&... args) { return unique_ptr(new T(std::forward(args)...)); } @@ -112,6 +111,6 @@ typename _Unique_if::_Unknown_bound make_unique(size_t n) { } template -typename _Unique_if::_Known_bound make_unique(Args &&...) = delete; +typename _Unique_if::_Known_bound make_unique(Args&&...) = delete; } // namespace std #endif diff --git a/wpilibc/shared/include/Buttons/Button.h b/wpilibc/shared/include/Buttons/Button.h index 3c77886d04..995c67a048 100644 --- a/wpilibc/shared/include/Buttons/Button.h +++ b/wpilibc/shared/include/Buttons/Button.h @@ -27,11 +27,11 @@ */ class Button : public Trigger { public: - virtual void WhenPressed(Command *command); - virtual void WhileHeld(Command *command); - virtual void WhenReleased(Command *command); - virtual void CancelWhenPressed(Command *command); - virtual void ToggleWhenPressed(Command *command); + virtual void WhenPressed(Command* command); + virtual void WhileHeld(Command* command); + virtual void WhenReleased(Command* command); + virtual void CancelWhenPressed(Command* command); + virtual void ToggleWhenPressed(Command* command); }; #endif diff --git a/wpilibc/shared/include/Buttons/ButtonScheduler.h b/wpilibc/shared/include/Buttons/ButtonScheduler.h index f05274d097..6e95a303e8 100644 --- a/wpilibc/shared/include/Buttons/ButtonScheduler.h +++ b/wpilibc/shared/include/Buttons/ButtonScheduler.h @@ -13,15 +13,15 @@ class Command; class ButtonScheduler { public: - ButtonScheduler(bool last, Trigger *button, Command *orders); + ButtonScheduler(bool last, Trigger* button, Command* orders); virtual ~ButtonScheduler() = default; virtual void Execute() = 0; void Start(); protected: bool m_pressedLast; - Trigger *m_button; - Command *m_command; + Trigger* m_button; + Command* m_command; }; #endif diff --git a/wpilibc/shared/include/Buttons/CancelButtonScheduler.h b/wpilibc/shared/include/Buttons/CancelButtonScheduler.h index 1c5d32816c..17517e8568 100644 --- a/wpilibc/shared/include/Buttons/CancelButtonScheduler.h +++ b/wpilibc/shared/include/Buttons/CancelButtonScheduler.h @@ -15,7 +15,7 @@ class Command; class CancelButtonScheduler : public ButtonScheduler { public: - CancelButtonScheduler(bool last, Trigger *button, Command *orders); + CancelButtonScheduler(bool last, Trigger* button, Command* orders); virtual ~CancelButtonScheduler() = default; virtual void Execute(); diff --git a/wpilibc/shared/include/Buttons/HeldButtonScheduler.h b/wpilibc/shared/include/Buttons/HeldButtonScheduler.h index 0a29105c86..db90dab5cd 100644 --- a/wpilibc/shared/include/Buttons/HeldButtonScheduler.h +++ b/wpilibc/shared/include/Buttons/HeldButtonScheduler.h @@ -15,7 +15,7 @@ class Command; class HeldButtonScheduler : public ButtonScheduler { public: - HeldButtonScheduler(bool last, Trigger *button, Command *orders); + HeldButtonScheduler(bool last, Trigger* button, Command* orders); virtual ~HeldButtonScheduler() = default; virtual void Execute(); }; diff --git a/wpilibc/shared/include/Buttons/JoystickButton.h b/wpilibc/shared/include/Buttons/JoystickButton.h index b1163f0793..7b9e8c0408 100644 --- a/wpilibc/shared/include/Buttons/JoystickButton.h +++ b/wpilibc/shared/include/Buttons/JoystickButton.h @@ -8,18 +8,18 @@ #ifndef __JOYSTICK_BUTTON_H__ #define __JOYSTICK_BUTTON_H__ -#include "GenericHID.h" #include "Buttons/Button.h" +#include "GenericHID.h" class JoystickButton : public Button { public: - JoystickButton(GenericHID *joystick, int buttonNumber); + JoystickButton(GenericHID* joystick, int buttonNumber); virtual ~JoystickButton() = default; virtual bool Get(); private: - GenericHID *m_joystick; + GenericHID* m_joystick; int m_buttonNumber; }; diff --git a/wpilibc/shared/include/Buttons/NetworkButton.h b/wpilibc/shared/include/Buttons/NetworkButton.h index b534e0095a..1ea56a5d01 100644 --- a/wpilibc/shared/include/Buttons/NetworkButton.h +++ b/wpilibc/shared/include/Buttons/NetworkButton.h @@ -8,14 +8,14 @@ #ifndef __NETWORK_BUTTON_H__ #define __NETWORK_BUTTON_H__ -#include "Buttons/Button.h" -#include #include +#include +#include "Buttons/Button.h" class NetworkButton : public Button { public: - NetworkButton(const std::string &tableName, const std::string &field); - NetworkButton(std::shared_ptr table, const std::string &field); + NetworkButton(const std::string& tableName, const std::string& field); + NetworkButton(std::shared_ptr table, const std::string& field); virtual ~NetworkButton() = default; virtual bool Get(); diff --git a/wpilibc/shared/include/Buttons/PressedButtonScheduler.h b/wpilibc/shared/include/Buttons/PressedButtonScheduler.h index 7a2e477493..468e3ba2ba 100644 --- a/wpilibc/shared/include/Buttons/PressedButtonScheduler.h +++ b/wpilibc/shared/include/Buttons/PressedButtonScheduler.h @@ -15,7 +15,7 @@ class Command; class PressedButtonScheduler : public ButtonScheduler { public: - PressedButtonScheduler(bool last, Trigger *button, Command *orders); + PressedButtonScheduler(bool last, Trigger* button, Command* orders); virtual ~PressedButtonScheduler() = default; virtual void Execute(); }; diff --git a/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h b/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h index a9ee3c8e47..3e17a7df50 100644 --- a/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h +++ b/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h @@ -15,7 +15,7 @@ class Command; class ReleasedButtonScheduler : public ButtonScheduler { public: - ReleasedButtonScheduler(bool last, Trigger *button, Command *orders); + ReleasedButtonScheduler(bool last, Trigger* button, Command* orders); virtual ~ReleasedButtonScheduler() = default; virtual void Execute(); }; diff --git a/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h b/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h index 4c2b5eb76d..19d4207b4f 100644 --- a/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h +++ b/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h @@ -15,7 +15,7 @@ class Command; class ToggleButtonScheduler : public ButtonScheduler { public: - ToggleButtonScheduler(bool last, Trigger *button, Command *orders); + ToggleButtonScheduler(bool last, Trigger* button, Command* orders); virtual ~ToggleButtonScheduler() = default; virtual void Execute(); diff --git a/wpilibc/shared/include/Buttons/Trigger.h b/wpilibc/shared/include/Buttons/Trigger.h index 719b072ebe..34c739a9b7 100644 --- a/wpilibc/shared/include/Buttons/Trigger.h +++ b/wpilibc/shared/include/Buttons/Trigger.h @@ -8,8 +8,8 @@ #ifndef __TRIGGER_H__ #define __TRIGGER_H__ -#include "SmartDashboard/Sendable.h" #include +#include "SmartDashboard/Sendable.h" class Command; @@ -36,11 +36,11 @@ class Trigger : public Sendable { virtual ~Trigger() = default; bool Grab(); virtual bool Get() = 0; - void WhenActive(Command *command); - void WhileActive(Command *command); - void WhenInactive(Command *command); - void CancelWhenActive(Command *command); - void ToggleWhenActive(Command *command); + void WhenActive(Command* command); + void WhileActive(Command* command); + void WhenInactive(Command* command); + void CancelWhenActive(Command* command); + void ToggleWhenActive(Command* command); virtual void InitTable(std::shared_ptr table); virtual std::shared_ptr GetTable() const; diff --git a/wpilibc/shared/include/CircularBuffer.h b/wpilibc/shared/include/CircularBuffer.h index e4c773954e..749fbdc09b 100644 --- a/wpilibc/shared/include/CircularBuffer.h +++ b/wpilibc/shared/include/CircularBuffer.h @@ -7,8 +7,8 @@ #pragma once -#include #include +#include /** * This is a simple circular buffer so we don't need to "bucket brigade" copy diff --git a/wpilibc/shared/include/Commands/Command.h b/wpilibc/shared/include/Commands/Command.h index 47fe7f1deb..64b78bfade 100644 --- a/wpilibc/shared/include/Commands/Command.h +++ b/wpilibc/shared/include/Commands/Command.h @@ -8,12 +8,12 @@ #ifndef __COMMAND_H__ #define __COMMAND_H__ +#include +#include +#include #include "ErrorBase.h" #include "SmartDashboard/NamedSendable.h" #include "tables/ITableListener.h" -#include -#include -#include class CommandGroup; class Subsystem; @@ -22,31 +22,27 @@ class Subsystem; * The Command class is at the very core of the entire command framework. * Every command can be started with a call to {@link Command#Start() Start()}. * Once a command is started it will call {@link Command#Initialize() - * Initialize()}, and then - * will repeatedly call {@link Command#Execute() Execute()} until the {@link - *Command#IsFinished() IsFinished()} - * returns true. Once it does, {@link Command#End() End()} will be called. + * Initialize()}, and then will repeatedly call + * {@link Command#Execute() Execute()} until the + * {@link Command#IsFinished() IsFinished()} returns true. Once it does, + * {@link Command#End() End()} will be called. * *

However, if at any point while it is running {@link Command#Cancel() - * Cancel()} is called, then - * the command will be stopped and {@link Command#Interrupted() Interrupted()} - * will be called.

+ * Cancel()} is called, then the command will be stopped and + * {@link Command#Interrupted() Interrupted()} will be called.

* *

If a command uses a {@link Subsystem}, then it should specify that it does - * so by - * calling the {@link Command#Requires(Subsystem) Requires(...)} method + * so by calling the {@link Command#Requires(Subsystem) Requires(...)} method * in its constructor. Note that a Command may have multiple requirements, and - * {@link Command#Requires(Subsystem) Requires(...)} should be - * called for each one.

+ * {@link Command#Requires(Subsystem) Requires(...)} should be called for each + * one.

* *

If a command is running and a new command with shared requirements is - * started, - * then one of two things will happen. If the active command is interruptible, - * then {@link Command#Cancel() Cancel()} will be called and the command will be - * removed - * to make way for the new one. If the active command is not interruptible, the - * other one will not even be started, and the active one will continue - * functioning.

+ * started, then one of two things will happen. If the active command is + * interruptible, then {@link Command#Cancel() Cancel()} will be called and the + * command will be removed to make way for the new one. If the active command + * is not interruptible, the other one will not even be started, and the active + * one will continue functioning.

* * @see CommandGroup * @see Subsystem @@ -57,12 +53,12 @@ class Command : public ErrorBase, public NamedSendable, public ITableListener { public: Command(); - Command(const std::string &name); + Command(const std::string& name); Command(double timeout); - Command(const std::string &name, double timeout); + Command(const std::string& name, double timeout); virtual ~Command(); double TimeSinceInitialized() const; - void Requires(Subsystem *s); + void Requires(Subsystem* s); bool IsCanceled() const; void Start(); bool Run(); @@ -70,10 +66,10 @@ class Command : public ErrorBase, public NamedSendable, public ITableListener { bool IsRunning() const; bool IsInterruptible() const; void SetInterruptible(bool interruptible); - bool DoesRequire(Subsystem *subsystem) const; - typedef std::set SubsystemSet; + bool DoesRequire(Subsystem* subsystem) const; + typedef std::set SubsystemSet; SubsystemSet GetRequirements() const; - CommandGroup *GetGroup() const; + CommandGroup* GetGroup() const; void SetRunWhenDisabled(bool run); bool WillRunWhenDisabled() const; int GetID() const; @@ -81,8 +77,8 @@ class Command : public ErrorBase, public NamedSendable, public ITableListener { protected: void SetTimeout(double timeout); bool IsTimedOut() const; - bool AssertUnlocked(const std::string &message); - void SetParent(CommandGroup *parent); + bool AssertUnlocked(const std::string& message); + void SetParent(CommandGroup* parent); /** * The initialize method is called the first time this Command is run after * being started. @@ -95,12 +91,11 @@ class Command : public ErrorBase, public NamedSendable, public ITableListener { virtual void Execute() = 0; /** * Returns whether this command is finished. - * If it is, then the command will be removed - * and {@link Command#end() end()} will be called. + * If it is, then the command will be removed and {@link Command#end() end()} + * will be called. * *

It may be useful for a team to reference the {@link Command#isTimedOut() - * isTimedOut()} method - * for time-sensitive commands.

+ * isTimedOut()} method for time-sensitive commands.

* @return whether this command is finished. * @see Command#isTimedOut() isTimedOut() */ @@ -112,18 +107,15 @@ class Command : public ErrorBase, public NamedSendable, public ITableListener { */ virtual void End() = 0; /** - * Called when the command ends because somebody called {@link - *Command#cancel() cancel()} - * or another command shared the same requirements as this one, and booted - * it out. + * Called when the command ends because somebody called + * {@link Command#cancel() cancel()} or another command shared the same + * requirements as this one, and booted it out. * - *

This is where you may want - * to wrap up loose ends, like shutting off a motor that was being used - * in the command.

+ *

This is where you may want to wrap up loose ends, like shutting off a + * motor that was being used in the command.

* *

Generally, it is useful to simply call the {@link Command#end() end()} - * method - * within this method

+ * method within this method

*/ virtual void Interrupted() = 0; virtual void _Initialize(); @@ -170,7 +162,7 @@ class Command : public ErrorBase, public NamedSendable, public ITableListener { bool m_runWhenDisabled = false; /** The {@link CommandGroup} this is in */ - CommandGroup *m_parent = nullptr; + CommandGroup* m_parent = nullptr; int m_commandID = m_commandCounter++; static int m_commandCounter; diff --git a/wpilibc/shared/include/Commands/CommandGroup.h b/wpilibc/shared/include/Commands/CommandGroup.h index 309d591787..1f7454bcde 100644 --- a/wpilibc/shared/include/Commands/CommandGroup.h +++ b/wpilibc/shared/include/Commands/CommandGroup.h @@ -8,25 +8,24 @@ #ifndef __COMMAND_GROUP_H__ #define __COMMAND_GROUP_H__ -#include "Commands/Command.h" -#include "Commands/CommandGroupEntry.h" #include #include +#include "Commands/Command.h" +#include "Commands/CommandGroupEntry.h" /** * A {@link CommandGroup} is a list of commands which are executed in sequence. * *

Commands in a {@link CommandGroup} are added using the {@link - * CommandGroup#AddSequential(Command) AddSequential(...)} method - * and are called sequentially. - * {@link CommandGroup CommandGroups} are themselves {@link Command Commands} - * and can be given to other {@link CommandGroup CommandGroups}.

+ * CommandGroup#AddSequential(Command) AddSequential(...)} method and are + * called sequentially. {@link CommandGroup CommandGroups} are themselves + * {@link Command Commands} and can be given to other + * {@link CommandGroup CommandGroups}.

* *

{@link CommandGroup CommandGroups} will carry all of the requirements of - * their {@link Command subcommands}. Additional - * requirements can be specified by calling {@link - *CommandGroup#Requires(Subsystem) Requires(...)} - * normally in the constructor.

+ * their {@link Command subcommands}. Additional requirements can be specified + * by calling {@link CommandGroup#Requires(Subsystem) Requires(...)} normally + * in the constructor.

* *

CommandGroups can also execute commands in parallel, simply by adding them * using {@link CommandGroup#AddParallel(Command) AddParallel(...)}.

@@ -37,13 +36,13 @@ class CommandGroup : public Command { public: CommandGroup() = default; - CommandGroup(const std::string &name); + CommandGroup(const std::string& name); virtual ~CommandGroup() = default; - void AddSequential(Command *command); - void AddSequential(Command *command, double timeout); - void AddParallel(Command *command); - void AddParallel(Command *command, double timeout); + void AddSequential(Command* command); + void AddSequential(Command* command, double timeout); + void AddParallel(Command* command); + void AddParallel(Command* command, double timeout); bool IsInterruptible() const; int GetSize() const; @@ -59,7 +58,7 @@ class CommandGroup : public Command { virtual void _End(); private: - void CancelConflicts(Command *command); + void CancelConflicts(Command* command); /** The commands in this group (stored in entries) */ std::vector m_commands; diff --git a/wpilibc/shared/include/Commands/CommandGroupEntry.h b/wpilibc/shared/include/Commands/CommandGroupEntry.h index fd9b387807..17fff6da2a 100644 --- a/wpilibc/shared/include/Commands/CommandGroupEntry.h +++ b/wpilibc/shared/include/Commands/CommandGroupEntry.h @@ -19,11 +19,11 @@ class CommandGroupEntry { } Sequence; CommandGroupEntry() = default; - CommandGroupEntry(Command *command, Sequence state, double timeout = -1.0); + CommandGroupEntry(Command* command, Sequence state, double timeout = -1.0); bool IsTimedOut() const; double m_timeout = -1.0; - Command *m_command = nullptr; + Command* m_command = nullptr; Sequence m_state = kSequence_InSequence; }; diff --git a/wpilibc/shared/include/Commands/PIDCommand.h b/wpilibc/shared/include/Commands/PIDCommand.h index b9fb2ca49a..eac95baa34 100644 --- a/wpilibc/shared/include/Commands/PIDCommand.h +++ b/wpilibc/shared/include/Commands/PIDCommand.h @@ -10,16 +10,17 @@ #include "Commands/Command.h" #include "PIDController.h" -#include "PIDSource.h" #include "PIDOutput.h" +#include "PIDSource.h" #include class PIDCommand : public Command, public PIDOutput, public PIDSource { public: - PIDCommand(const std::string &name, double p, double i, double d); - PIDCommand(const std::string &name, double p, double i, double d, double period); - PIDCommand(const std::string &name, double p, double i, double d, double f, + PIDCommand(const std::string& name, double p, double i, double d); + PIDCommand(const std::string& name, double p, double i, double d, + double period); + PIDCommand(const std::string& name, double p, double i, double d, double f, double period); PIDCommand(double p, double i, double d); PIDCommand(double p, double i, double d, double period); diff --git a/wpilibc/shared/include/Commands/PIDSubsystem.h b/wpilibc/shared/include/Commands/PIDSubsystem.h index fea18471d4..133ddd4291 100644 --- a/wpilibc/shared/include/Commands/PIDSubsystem.h +++ b/wpilibc/shared/include/Commands/PIDSubsystem.h @@ -10,8 +10,8 @@ #include "Commands/Subsystem.h" #include "PIDController.h" -#include "PIDSource.h" #include "PIDOutput.h" +#include "PIDSource.h" #include @@ -21,17 +21,15 @@ * an elevator which attempts to stay at a constant height). * *

It provides some convenience methods to run an internal {@link - * PIDController}. - * It also allows access to the internal {@link PIDController} in order to give - * total control - * to the programmer.

+ * PIDController}. It also allows access to the internal {@link PIDController} + * in order to give total control to the programmer.

* */ class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource { public: - PIDSubsystem(const std::string &name, double p, double i, double d); - PIDSubsystem(const std::string &name, double p, double i, double d, double f); - PIDSubsystem(const std::string &name, double p, double i, double d, double f, + PIDSubsystem(const std::string& name, double p, double i, double d); + PIDSubsystem(const std::string& name, double p, double i, double d, double f); + PIDSubsystem(const std::string& name, double p, double i, double d, double f, double period); PIDSubsystem(double p, double i, double d); PIDSubsystem(double p, double i, double d, double f); diff --git a/wpilibc/shared/include/Commands/PrintCommand.h b/wpilibc/shared/include/Commands/PrintCommand.h index 577252dc0b..7d4a82c1e2 100644 --- a/wpilibc/shared/include/Commands/PrintCommand.h +++ b/wpilibc/shared/include/Commands/PrintCommand.h @@ -8,12 +8,12 @@ #ifndef __PRINT_COMMAND_H__ #define __PRINT_COMMAND_H__ -#include "Commands/Command.h" #include +#include "Commands/Command.h" class PrintCommand : public Command { public: - PrintCommand(const std::string &message); + PrintCommand(const std::string& message); virtual ~PrintCommand() = default; protected: diff --git a/wpilibc/shared/include/Commands/Scheduler.h b/wpilibc/shared/include/Commands/Scheduler.h index fc55256405..935facd14f 100644 --- a/wpilibc/shared/include/Commands/Scheduler.h +++ b/wpilibc/shared/include/Commands/Scheduler.h @@ -8,31 +8,31 @@ #ifndef __SCHEDULER_H__ #define __SCHEDULER_H__ -#include "Commands/Command.h" -#include "ErrorBase.h" -#include "SmartDashboard/NamedSendable.h" -#include "networktables/NetworkTable.h" -#include "SmartDashboard/SmartDashboard.h" #include #include #include #include #include #include +#include "Commands/Command.h" +#include "ErrorBase.h" #include "HAL/cpp/priority_mutex.h" +#include "SmartDashboard/NamedSendable.h" +#include "SmartDashboard/SmartDashboard.h" +#include "networktables/NetworkTable.h" class ButtonScheduler; class Subsystem; class Scheduler : public ErrorBase, public NamedSendable { public: - static Scheduler *GetInstance(); + static Scheduler* GetInstance(); - void AddCommand(Command *command); - void AddButton(ButtonScheduler *button); - void RegisterSubsystem(Subsystem *subsystem); + void AddCommand(Command* command); + void AddButton(ButtonScheduler* button); + void RegisterSubsystem(Subsystem* subsystem); void Run(); - void Remove(Command *command); + void Remove(Command* command); void RemoveAll(); void ResetAll(); void SetEnabled(bool enabled); @@ -48,16 +48,16 @@ class Scheduler : public ErrorBase, public NamedSendable { Scheduler(); virtual ~Scheduler() = default; - void ProcessCommandAddition(Command *command); + void ProcessCommandAddition(Command* command); Command::SubsystemSet m_subsystems; priority_mutex m_buttonsLock; - typedef std::vector ButtonVector; + typedef std::vector ButtonVector; ButtonVector m_buttons; - typedef std::vector CommandVector; + typedef std::vector CommandVector; priority_mutex m_additionsLock; CommandVector m_additions; - typedef std::set CommandSet; + typedef std::set CommandSet; CommandSet m_commands; bool m_adding = false; bool m_enabled = true; diff --git a/wpilibc/shared/include/Commands/StartCommand.h b/wpilibc/shared/include/Commands/StartCommand.h index 4f97971546..2a1251cb97 100644 --- a/wpilibc/shared/include/Commands/StartCommand.h +++ b/wpilibc/shared/include/Commands/StartCommand.h @@ -12,7 +12,7 @@ class StartCommand : public Command { public: - StartCommand(Command *commandToStart); + StartCommand(Command* commandToStart); virtual ~StartCommand() = default; protected: @@ -23,7 +23,7 @@ class StartCommand : public Command { virtual void Interrupted(); private: - Command *m_commandToFork; + Command* m_commandToFork; }; #endif diff --git a/wpilibc/shared/include/Commands/Subsystem.h b/wpilibc/shared/include/Commands/Subsystem.h index 070af48089..0baa42d216 100644 --- a/wpilibc/shared/include/Commands/Subsystem.h +++ b/wpilibc/shared/include/Commands/Subsystem.h @@ -8,10 +8,10 @@ #ifndef __SUBSYSTEM_H__ #define __SUBSYSTEM_H__ +#include +#include #include "ErrorBase.h" #include "SmartDashboard/NamedSendable.h" -#include -#include class Command; @@ -19,21 +19,21 @@ class Subsystem : public ErrorBase, public NamedSendable { friend class Scheduler; public: - Subsystem(const std::string &name); + Subsystem(const std::string& name); virtual ~Subsystem() = default; - void SetDefaultCommand(Command *command); - Command *GetDefaultCommand(); - void SetCurrentCommand(Command *command); - Command *GetCurrentCommand() const; + void SetDefaultCommand(Command* command); + Command* GetDefaultCommand(); + void SetCurrentCommand(Command* command); + Command* GetCurrentCommand() const; virtual void InitDefaultCommand(); private: void ConfirmCommand(); - Command *m_currentCommand = nullptr; + Command* m_currentCommand = nullptr; bool m_currentCommandChanged = true; - Command *m_defaultCommand = nullptr; + Command* m_defaultCommand = nullptr; std::string m_name; bool m_initializedDefaultCommand = false; diff --git a/wpilibc/shared/include/Commands/WaitCommand.h b/wpilibc/shared/include/Commands/WaitCommand.h index fa64b75153..3cfafd0af0 100644 --- a/wpilibc/shared/include/Commands/WaitCommand.h +++ b/wpilibc/shared/include/Commands/WaitCommand.h @@ -13,7 +13,7 @@ class WaitCommand : public Command { public: WaitCommand(double timeout); - WaitCommand(const std::string &name, double timeout); + WaitCommand(const std::string& name, double timeout); virtual ~WaitCommand() = default; protected: diff --git a/wpilibc/shared/include/Commands/WaitForChildren.h b/wpilibc/shared/include/Commands/WaitForChildren.h index 5028cdbca0..c041e6fa99 100644 --- a/wpilibc/shared/include/Commands/WaitForChildren.h +++ b/wpilibc/shared/include/Commands/WaitForChildren.h @@ -13,7 +13,7 @@ class WaitForChildren : public Command { public: WaitForChildren(double timeout); - WaitForChildren(const std::string &name, double timeout); + WaitForChildren(const std::string& name, double timeout); virtual ~WaitForChildren() = default; protected: diff --git a/wpilibc/shared/include/Commands/WaitUntilCommand.h b/wpilibc/shared/include/Commands/WaitUntilCommand.h index 2512a20a62..9ce9ab4cc5 100644 --- a/wpilibc/shared/include/Commands/WaitUntilCommand.h +++ b/wpilibc/shared/include/Commands/WaitUntilCommand.h @@ -13,7 +13,7 @@ class WaitUntilCommand : public Command { public: WaitUntilCommand(double time); - WaitUntilCommand(const std::string &name, double time); + WaitUntilCommand(const std::string& name, double time); virtual ~WaitUntilCommand() = default; protected: diff --git a/wpilibc/shared/include/Controller.h b/wpilibc/shared/include/Controller.h index bad16d6f2a..95f035878f 100644 --- a/wpilibc/shared/include/Controller.h +++ b/wpilibc/shared/include/Controller.h @@ -8,12 +8,10 @@ #pragma once /** - * Interface for Controllers + * Interface for Controllers. * Common interface for controllers. Controllers run control loops, the most - * common - * are PID controllers and their variants, but this includes anything that is - * controlling - * an actuator in a separate thread. + * common are PID controllers and their variants, but this includes anything + * that is controlling an actuator in a separate thread. */ class Controller { public: diff --git a/wpilibc/shared/include/Error.h b/wpilibc/shared/include/Error.h index a00114573e..73f12acb41 100644 --- a/wpilibc/shared/include/Error.h +++ b/wpilibc/shared/include/Error.h @@ -10,13 +10,14 @@ #include "Base.h" #ifdef _WIN32 - #include - //Windows.h defines #define GetMessage GetMessageW, which is stupid and we don't want it. - #undef GetMessage +#include +// Windows.h defines #define GetMessage GetMessageW, which is stupid and we +// don't want it. +#undef GetMessage #endif -#include #include +#include #include "llvm/StringRef.h" // Forward declarations @@ -43,9 +44,9 @@ class Error { const ErrorBase* GetOriginatingObject() const; double GetTimestamp() const; void Clear(); - void Set(Code code, llvm::StringRef contextMessage, - llvm::StringRef filename, llvm::StringRef function, - uint32_t lineNumber, const ErrorBase* originatingObject); + void Set(Code code, llvm::StringRef contextMessage, llvm::StringRef filename, + llvm::StringRef function, uint32_t lineNumber, + const ErrorBase* originatingObject); private: void Report(); diff --git a/wpilibc/shared/include/ErrorBase.h b/wpilibc/shared/include/ErrorBase.h index 756442dbe5..2f04c7378b 100644 --- a/wpilibc/shared/include/ErrorBase.h +++ b/wpilibc/shared/include/ErrorBase.h @@ -53,8 +53,7 @@ #define wpi_setGlobalWPIErrorWithContext(error, context) \ ErrorBase::SetGlobalWPIError((wpi_error_s_##error), (context), __FILE__, \ __FUNCTION__, __LINE__) -#define wpi_setGlobalWPIError(error) \ - wpi_setGlobalWPIErrorWithContext(error, "") +#define wpi_setGlobalWPIError(error) wpi_setGlobalWPIErrorWithContext(error, "") /** * Base class for most objects. diff --git a/wpilibc/shared/include/Filters/LinearDigitalFilter.h b/wpilibc/shared/include/Filters/LinearDigitalFilter.h index b6dbce7a8e..3ad0eba969 100644 --- a/wpilibc/shared/include/Filters/LinearDigitalFilter.h +++ b/wpilibc/shared/include/Filters/LinearDigitalFilter.h @@ -10,8 +10,8 @@ #include #include #include -#include "Filter.h" #include "CircularBuffer.h" +#include "Filter.h" /** * This class implements a linear, digital filter. All types of FIR and IIR @@ -19,7 +19,8 @@ * used types of filters. * * Filters are of the form: - * y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P) - (a0*y[n-1] + a2*y[n-2] + ... + aQ*y[n-Q]) + * y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P) - (a0*y[n-1] + a2*y[n-2] + ... + * + aQ*y[n-Q]) * * Where: * y[n] is the output at time "n" diff --git a/wpilibc/shared/include/GenericHID.h b/wpilibc/shared/include/GenericHID.h index 17a959f80f..57497f2b16 100644 --- a/wpilibc/shared/include/GenericHID.h +++ b/wpilibc/shared/include/GenericHID.h @@ -9,7 +9,8 @@ #include -/** GenericHID Interface +/** + * GenericHID Interface. */ class GenericHID { public: diff --git a/wpilibc/shared/include/GyroBase.h b/wpilibc/shared/include/GyroBase.h index 5d93fc4ca8..558a1902b3 100644 --- a/wpilibc/shared/include/GyroBase.h +++ b/wpilibc/shared/include/GyroBase.h @@ -7,10 +7,10 @@ #pragma once -#include "SensorBase.h" -#include "PIDSource.h" -#include "interfaces/Gyro.h" #include "LiveWindow/LiveWindowSendable.h" +#include "PIDSource.h" +#include "SensorBase.h" +#include "interfaces/Gyro.h" #include @@ -18,7 +18,10 @@ * GyroBase is the common base class for Gyro implementations such as * AnalogGyro. */ -class GyroBase : public Gyro, public SensorBase, public PIDSource, public LiveWindowSendable { +class GyroBase : public Gyro, + public SensorBase, + public PIDSource, + public LiveWindowSendable { public: virtual ~GyroBase() = default; diff --git a/wpilibc/shared/include/LiveWindow/LiveWindow.h b/wpilibc/shared/include/LiveWindow/LiveWindow.h index 17817a9aec..f06e77c6d5 100644 --- a/wpilibc/shared/include/LiveWindow/LiveWindow.h +++ b/wpilibc/shared/include/LiveWindow/LiveWindow.h @@ -8,12 +8,12 @@ #ifndef _LIVE_WINDOW_H #define _LIVE_WINDOW_H -#include "LiveWindow/LiveWindowSendable.h" -#include "tables/ITable.h" -#include "Commands/Scheduler.h" -#include #include #include +#include +#include "Commands/Scheduler.h" +#include "LiveWindow/LiveWindowSendable.h" +#include "tables/ITable.h" struct LiveWindowComponent { std::string subsystem; @@ -37,26 +37,26 @@ struct LiveWindowComponent { */ class LiveWindow { public: - static LiveWindow *GetInstance(); + static LiveWindow* GetInstance(); void Run(); - void AddSensor(const std::string &subsystem, const std::string &name, - LiveWindowSendable *component); - void AddSensor(const std::string &subsystem, const std::string &name, - LiveWindowSendable &component); - void AddSensor(const std::string &subsystem, const std::string &name, + void AddSensor(const std::string& subsystem, const std::string& name, + LiveWindowSendable* component); + void AddSensor(const std::string& subsystem, const std::string& name, + LiveWindowSendable& component); + void AddSensor(const std::string& subsystem, const std::string& name, std::shared_ptr component); - void AddActuator(const std::string &subsystem, const std::string &name, - LiveWindowSendable *component); - void AddActuator(const std::string &subsystem, const std::string &name, - LiveWindowSendable &component); - void AddActuator(const std::string &subsystem, const std::string &name, + void AddActuator(const std::string& subsystem, const std::string& name, + LiveWindowSendable* component); + void AddActuator(const std::string& subsystem, const std::string& name, + LiveWindowSendable& component); + void AddActuator(const std::string& subsystem, const std::string& name, std::shared_ptr component); - void AddSensor(std::string type, int channel, LiveWindowSendable *component); + void AddSensor(std::string type, int channel, LiveWindowSendable* component); void AddActuator(std::string type, int channel, - LiveWindowSendable *component); + LiveWindowSendable* component); void AddActuator(std::string type, int module, int channel, - LiveWindowSendable *component); + LiveWindowSendable* component); bool IsEnabled() const { return m_enabled; } void SetEnabled(bool enabled); @@ -71,12 +71,13 @@ class LiveWindow { void InitializeLiveWindowComponents(); std::vector> m_sensors; - std::map, LiveWindowComponent> m_components; + std::map, LiveWindowComponent> + m_components; std::shared_ptr m_liveWindowTable; std::shared_ptr m_statusTable; - Scheduler *m_scheduler; + Scheduler* m_scheduler; bool m_enabled = false; bool m_firstTime = true; diff --git a/wpilibc/shared/include/PIDController.h b/wpilibc/shared/include/PIDController.h index 5e743894e2..b3a6496bd9 100644 --- a/wpilibc/shared/include/PIDController.h +++ b/wpilibc/shared/include/PIDController.h @@ -9,11 +9,11 @@ #include "Base.h" #include "Controller.h" +#include "HAL/cpp/priority_mutex.h" #include "LiveWindow/LiveWindow.h" +#include "Notifier.h" #include "PIDInterface.h" #include "PIDSource.h" -#include "Notifier.h" -#include "HAL/cpp/priority_mutex.h" #include "Timer.h" #include @@ -27,17 +27,16 @@ class PIDOutput; * Class implements a PID Control Loop. * * Creates a separate thread which reads the given PIDSource and takes - * care of the integral calculations, as well as writing the given - * PIDOutput + * care of the integral calculations, as well as writing the given PIDOutput. */ class PIDController : public LiveWindowSendable, public PIDInterface, public ITableListener { public: - PIDController(float p, float i, float d, PIDSource *source, PIDOutput *output, + PIDController(float p, float i, float d, PIDSource* source, PIDOutput* output, float period = 0.05); - PIDController(float p, float i, float d, float f, PIDSource *source, - PIDOutput *output, float period = 0.05); + PIDController(float p, float i, float d, float f, PIDSource* source, + PIDOutput* output, float period = 0.05); virtual ~PIDController(); PIDController(const PIDController&) = delete; @@ -79,26 +78,38 @@ class PIDController : public LiveWindowSendable, virtual void InitTable(std::shared_ptr table) override; protected: - PIDSource *m_pidInput; - PIDOutput *m_pidOutput; + PIDSource* m_pidInput; + PIDOutput* m_pidOutput; std::shared_ptr m_table; virtual void Calculate(); virtual double CalculateFeedForward(); private: - float m_P; // factor for "proportional" control - float m_I; // factor for "integral" control - float m_D; // factor for "derivative" control - float m_F; // factor for "feed forward" control - float m_maximumOutput = 1.0; // |maximum output| - float m_minimumOutput = -1.0; // |minimum output| - float m_maximumInput = 0; // maximum input - limit setpoint to this - float m_minimumInput = 0; // minimum input - limit setpoint to this - bool m_continuous = false; // do the endpoints wrap around? eg. Absolute encoder - bool m_enabled = false; // is the pid controller enabled - float m_prevError = 0; // the prior error (used to compute velocity) - double m_totalError = 0; // the sum of the errors for use in the integral calc + // factor for "proportional" control + float m_P; + // factor for "integral" control + float m_I; + // factor for "derivative" control + float m_D; + // factor for "feed forward" control + float m_F; + // |maximum output| + float m_maximumOutput = 1.0; + // |minimum output| + float m_minimumOutput = -1.0; + // maximum input - limit setpoint to this + float m_maximumInput = 0; + // minimum input - limit setpoint to this + float m_minimumInput = 0; + // do the endpoints wrap around? eg. Absolute encoder + bool m_continuous = false; + // is the pid controller enabled + bool m_enabled = false; + // the prior error (used to compute velocity) + float m_prevError = 0; + // the sum of the errors for use in the integral calc + double m_totalError = 0; enum { kAbsoluteTolerance, kPercentTolerance, @@ -123,12 +134,12 @@ class PIDController : public LiveWindowSendable, std::unique_ptr m_controlLoop; Timer m_setpointTimer; - void Initialize(float p, float i, float d, float f, PIDSource *source, - PIDOutput *output, float period = 0.05); + void Initialize(float p, float i, float d, float f, PIDSource* source, + PIDOutput* output, float period = 0.05); virtual std::shared_ptr GetTable() const override; virtual std::string GetSmartDashboardType() const override; - virtual void ValueChanged(ITable *source, llvm::StringRef key, + virtual void ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) override; virtual void UpdateTable() override; diff --git a/wpilibc/shared/include/PIDOutput.h b/wpilibc/shared/include/PIDOutput.h index ad720dca32..a6660b2e16 100644 --- a/wpilibc/shared/include/PIDOutput.h +++ b/wpilibc/shared/include/PIDOutput.h @@ -13,7 +13,7 @@ * PIDOutput interface is a generic output for the PID class. * PWMs use this class. * Users implement this interface to allow for a PIDController to - * read directly from the inputs + * read directly from the inputs. */ class PIDOutput { public: diff --git a/wpilibc/shared/include/PIDSource.h b/wpilibc/shared/include/PIDSource.h index 1a2be9c862..c8a4c05e4f 100644 --- a/wpilibc/shared/include/PIDSource.h +++ b/wpilibc/shared/include/PIDSource.h @@ -12,8 +12,7 @@ enum class PIDSourceType { kDisplacement, kRate }; /** * PIDSource interface is a generic sensor source for the PID class. * All sensors that can be used with the PID class will implement the PIDSource - * that - * returns a standard value that will be used in the PID code. + * that returns a standard value that will be used in the PID code. */ class PIDSource { public: diff --git a/wpilibc/shared/include/Resource.h b/wpilibc/shared/include/Resource.h index 8d26442d66..427ab1d6e1 100644 --- a/wpilibc/shared/include/Resource.h +++ b/wpilibc/shared/include/Resource.h @@ -7,10 +7,10 @@ #pragma once -#include "ErrorBase.h" #include #include #include +#include "ErrorBase.h" #include "HAL/cpp/priority_mutex.h" @@ -30,10 +30,11 @@ class Resource : public ErrorBase { Resource(const Resource&) = delete; Resource& operator=(const Resource&) = delete; - static void CreateResourceObject(std::unique_ptr& r, uint32_t elements); + static void CreateResourceObject(std::unique_ptr& r, + uint32_t elements); explicit Resource(uint32_t size); - uint32_t Allocate(const std::string &resourceDesc); - uint32_t Allocate(uint32_t index, const std::string &resourceDesc); + uint32_t Allocate(const std::string& resourceDesc); + uint32_t Allocate(uint32_t index, const std::string& resourceDesc); void Free(uint32_t index); private: diff --git a/wpilibc/shared/include/SensorBase.h b/wpilibc/shared/include/SensorBase.h index 51fd94ce25..1689012bd1 100644 --- a/wpilibc/shared/include/SensorBase.h +++ b/wpilibc/shared/include/SensorBase.h @@ -7,15 +7,14 @@ #pragma once -#include "ErrorBase.h" #include #include "Base.h" +#include "ErrorBase.h" /** * Base class for all sensors. * Stores most recent status information as well as containing utility functions - * for checking - * channels and error processing. + * for checking channels and error processing. */ class SensorBase : public ErrorBase { public: diff --git a/wpilibc/shared/include/SmartDashboard/NamedSendable.h b/wpilibc/shared/include/SmartDashboard/NamedSendable.h index 0dd1b248d9..4bc3f8838d 100644 --- a/wpilibc/shared/include/SmartDashboard/NamedSendable.h +++ b/wpilibc/shared/include/SmartDashboard/NamedSendable.h @@ -20,7 +20,7 @@ class NamedSendable : public Sendable { public: /** * @return the name of the subtable of SmartDashboard that the Sendable object - * will use + * will use */ virtual std::string GetName() const = 0; }; diff --git a/wpilibc/shared/include/SmartDashboard/Sendable.h b/wpilibc/shared/include/SmartDashboard/Sendable.h index 78206d0839..f38e56d45f 100644 --- a/wpilibc/shared/include/SmartDashboard/Sendable.h +++ b/wpilibc/shared/include/SmartDashboard/Sendable.h @@ -8,8 +8,8 @@ #ifndef __SMART_DASHBOARD_DATA__ #define __SMART_DASHBOARD_DATA__ -#include #include +#include #include "tables/ITable.h" class Sendable { @@ -27,7 +27,7 @@ class Sendable { /** * @return the string representation of the named data type that will be used - * by the smart dashboard for this sendable + * by the smart dashboard for this sendable */ virtual std::string GetSmartDashboardType() const = 0; }; diff --git a/wpilibc/shared/include/SmartDashboard/SendableChooser.h b/wpilibc/shared/include/SmartDashboard/SendableChooser.h index 9560746e03..8ce5c2bb42 100644 --- a/wpilibc/shared/include/SmartDashboard/SendableChooser.h +++ b/wpilibc/shared/include/SmartDashboard/SendableChooser.h @@ -8,26 +8,22 @@ #ifndef __SENDABLE_CHOOSER_H__ #define __SENDABLE_CHOOSER_H__ -#include "SmartDashboard/Sendable.h" -#include "tables/ITable.h" #include #include #include +#include "SmartDashboard/Sendable.h" +#include "tables/ITable.h" /** * The {@link SendableChooser} class is a useful tool for presenting a selection - * of options - * to the {@link SmartDashboard}. + * of options to the {@link SmartDashboard}. * *

For instance, you may wish to be able to select between multiple - * autonomous modes. - * You can do this by putting every possible {@link Command} you want to run as - * an autonomous into - * a {@link SendableChooser} and then put it into the {@link SmartDashboard} to - * have a list of options - * appear on the laptop. Once autonomous starts, simply ask the {@link - * SendableChooser} what the selected - * value is.

+ * autonomous modes. You can do this by putting every possible {@link Command} + * you want to run as an autonomous into a {@link SendableChooser} and then put + * it into the {@link SmartDashboard} to have a list of options appear on the + * laptop. Once autonomous starts, simply ask the {@link SendableChooser} what + * the selected value is.

* * @see SmartDashboard */ @@ -35,9 +31,9 @@ class SendableChooser : public Sendable { public: virtual ~SendableChooser() = default; - void AddObject(const std::string &name, void *object); - void AddDefault(const std::string &name, void *object); - void *GetSelected(); + void AddObject(const std::string& name, void* object); + void AddDefault(const std::string& name, void* object); + void* GetSelected(); virtual void InitTable(std::shared_ptr subtable); virtual std::shared_ptr GetTable() const; @@ -45,7 +41,7 @@ class SendableChooser : public Sendable { private: std::string m_defaultChoice; - std::map m_choices; + std::map m_choices; std::shared_ptr m_table; }; diff --git a/wpilibc/shared/include/SmartDashboard/SmartDashboard.h b/wpilibc/shared/include/SmartDashboard/SmartDashboard.h index 51e51d1ae5..2d5932ef50 100644 --- a/wpilibc/shared/include/SmartDashboard/SmartDashboard.h +++ b/wpilibc/shared/include/SmartDashboard/SmartDashboard.h @@ -8,20 +8,20 @@ #ifndef __SMART_DASHBOARD_H__ #define __SMART_DASHBOARD_H__ -#include "SensorBase.h" #include #include -#include "SmartDashboard/Sendable.h" +#include "SensorBase.h" #include "SmartDashboard/NamedSendable.h" +#include "SmartDashboard/Sendable.h" #include "tables/ITable.h" class SmartDashboard : public SensorBase { public: static void init(); - static void PutData(llvm::StringRef key, Sendable *data); - static void PutData(NamedSendable *value); - static Sendable *GetData(llvm::StringRef keyName); + static void PutData(llvm::StringRef key, Sendable* data); + static void PutData(NamedSendable* value); + static Sendable* GetData(llvm::StringRef keyName); static void PutBoolean(llvm::StringRef keyName, bool value); static bool GetBoolean(llvm::StringRef keyName, bool defaultValue); @@ -44,11 +44,10 @@ class SmartDashboard : public SensorBase { static std::shared_ptr m_table; /** - * A map linking tables in the SmartDashboard to the {@link - * SmartDashboardData} objects - * they came from. + * A map linking tables in the SmartDashboard to the + * {@link SmartDashboardData} objects they came from. */ - static std::map , Sendable *> m_tablesToData; + static std::map, Sendable*> m_tablesToData; }; #endif diff --git a/wpilibc/shared/include/Task.h b/wpilibc/shared/include/Task.h index 2735f486e7..4e8fe2ab68 100644 --- a/wpilibc/shared/include/Task.h +++ b/wpilibc/shared/include/Task.h @@ -7,11 +7,11 @@ #pragma once -#include "ErrorBase.h" -#include "HAL/Task.hpp" #include #include #include +#include "ErrorBase.h" +#include "HAL/Task.hpp" /** * Wrapper class around std::thread that allows changing thread priority diff --git a/wpilibc/shared/include/Task.inc b/wpilibc/shared/include/Task.inc index 3c90aba8e9..9eed6cc81d 100644 --- a/wpilibc/shared/include/Task.inc +++ b/wpilibc/shared/include/Task.inc @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include "HAL/HAL.hpp" #include +#include "HAL/HAL.hpp" /** * Create and launch a task. @@ -24,10 +24,11 @@ Task::Task(const std::string& name, Function&& function, Args&&... args) { m_thread = std::thread(std::forward>(function), std::forward(args)...); - //TODO: lvuser does not currently have permissions to set the priority. - //SetPriority(kDefaultPriority); + // TODO: lvuser does not currently have permissions to set the priority. + // SetPriority(kDefaultPriority); static std::atomic instances{0}; instances++; - HALReport(HALUsageReporting::kResourceType_Task, instances, 0, m_taskName.c_str()); + HALReport(HALUsageReporting::kResourceType_Task, instances, 0, + m_taskName.c_str()); } diff --git a/wpilibc/shared/include/Timer.h b/wpilibc/shared/include/Timer.h index b3fe77da1a..4971933ac0 100644 --- a/wpilibc/shared/include/Timer.h +++ b/wpilibc/shared/include/Timer.h @@ -10,7 +10,7 @@ #include "Base.h" #include "HAL/cpp/priority_mutex.h" -typedef void (*TimerInterruptHandler)(void *param); +typedef void (*TimerInterruptHandler)(void* param); void Wait(double seconds); double GetClock(); @@ -19,12 +19,10 @@ double GetTime(); /** * Timer objects measure accumulated time in seconds. * The timer object functions like a stopwatch. It can be started, stopped, and - * cleared. When the - * timer is running its value counts up in seconds. When stopped, the timer - * holds the current - * value. The implementation simply records the time when started and subtracts - * the current time - * whenever the value is requested. + * cleared. When the timer is running its value counts up in seconds. When + * stopped, the timer holds the current value. The implementation simply records + * the time when started and subtracts the current time whenever the value is + * requested. */ class Timer { public: diff --git a/wpilibc/shared/include/Utility.h b/wpilibc/shared/include/Utility.h index 89b97296a8..8f67fc3341 100644 --- a/wpilibc/shared/include/Utility.h +++ b/wpilibc/shared/include/Utility.h @@ -31,17 +31,17 @@ wpi_assertNotEqual_impl(a, b, #a, #b, message, __FILE__, __LINE__, \ __FUNCTION__) -bool wpi_assert_impl(bool conditionValue, const char *conditionText, - const char *message, const char *fileName, - uint32_t lineNumber, const char *funcName); -bool wpi_assertEqual_impl(int valueA, int valueB, const char *valueAString, - const char *valueBString, const char *message, - const char *fileName, uint32_t lineNumber, - const char *funcName); -bool wpi_assertNotEqual_impl(int valueA, int valueB, const char *valueAString, - const char *valueBString, const char *message, - const char *fileName, uint32_t lineNumber, - const char *funcName); +bool wpi_assert_impl(bool conditionValue, const char* conditionText, + const char* message, const char* fileName, + uint32_t lineNumber, const char* funcName); +bool wpi_assertEqual_impl(int valueA, int valueB, const char* valueAString, + const char* valueBString, const char* message, + const char* fileName, uint32_t lineNumber, + const char* funcName); +bool wpi_assertNotEqual_impl(int valueA, int valueB, const char* valueAString, + const char* valueBString, const char* message, + const char* fileName, uint32_t lineNumber, + const char* funcName); void wpi_suspendOnAssertEnabled(bool enabled); diff --git a/wpilibc/shared/include/WPIErrors.h b/wpilibc/shared/include/WPIErrors.h index ccf6dae2d6..c16ae92def 100644 --- a/wpilibc/shared/include/WPIErrors.h +++ b/wpilibc/shared/include/WPIErrors.h @@ -11,11 +11,11 @@ #ifdef WPI_ERRORS_DEFINE_STRINGS #define S(label, offset, message) \ - const char * wpi_error_s_##label = message; \ + const char* wpi_error_s_##label = message; \ const int32_t wpi_error_value_##label = offset #else #define S(label, offset, message) \ - extern const char * wpi_error_s_##label; \ + extern const char* wpi_error_s_##label; \ const int32_t wpi_error_value_##label = offset #endif diff --git a/wpilibc/shared/src/Buttons/Button.cpp b/wpilibc/shared/src/Buttons/Button.cpp index c4bd5ec014..ff83172a15 100644 --- a/wpilibc/shared/src/Buttons/Button.cpp +++ b/wpilibc/shared/src/Buttons/Button.cpp @@ -8,34 +8,41 @@ #include "Buttons/Button.h" /** - * Specifies the command to run when a button is first pressed + * Specifies the command to run when a button is first pressed. + * * @param command The pointer to the command to run */ -void Button::WhenPressed(Command *command) { WhenActive(command); } +void Button::WhenPressed(Command* command) { WhenActive(command); } /** - * Specifies the command to be scheduled while the button is pressed + * Specifies the command to be scheduled while the button is pressed. + * * The command will be scheduled repeatedly while the button is pressed and will * be canceled when the button is released. + * * @param command The pointer to the command to run */ -void Button::WhileHeld(Command *command) { WhileActive(command); } +void Button::WhileHeld(Command* command) { WhileActive(command); } /** - * Specifies the command to run when the button is released + * Specifies the command to run when the button is released. + * * The command will be scheduled a single time. - * @param The pointer to the command to run + * + * @param command The pointer to the command to run */ -void Button::WhenReleased(Command *command) { WhenInactive(command); } +void Button::WhenReleased(Command* command) { WhenInactive(command); } /** - * Cancels the specificed command when the button is pressed - * @param The command to be canceled + * Cancels the specificed command when the button is pressed. + * + * @param command The command to be canceled */ -void Button::CancelWhenPressed(Command *command) { CancelWhenActive(command); } +void Button::CancelWhenPressed(Command* command) { CancelWhenActive(command); } /** - * Toggle the specified command when the button is pressed - * @param The command to be toggled + * Toggle the specified command when the button is pressed. + * + * @param command The command to be toggled */ -void Button::ToggleWhenPressed(Command *command) { ToggleWhenActive(command); } +void Button::ToggleWhenPressed(Command* command) { ToggleWhenActive(command); } diff --git a/wpilibc/shared/src/Buttons/ButtonScheduler.cpp b/wpilibc/shared/src/Buttons/ButtonScheduler.cpp index 318fd5f711..6a700b32f7 100644 --- a/wpilibc/shared/src/Buttons/ButtonScheduler.cpp +++ b/wpilibc/shared/src/Buttons/ButtonScheduler.cpp @@ -9,7 +9,7 @@ #include "Commands/Scheduler.h" -ButtonScheduler::ButtonScheduler(bool last, Trigger *button, Command *orders) +ButtonScheduler::ButtonScheduler(bool last, Trigger* button, Command* orders) : m_pressedLast(last), m_button(button), m_command(orders) {} void ButtonScheduler::Start() { Scheduler::GetInstance()->AddButton(this); } diff --git a/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp b/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp index 52544e535d..8371a877fb 100644 --- a/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp +++ b/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp @@ -10,8 +10,8 @@ #include "Buttons/Button.h" #include "Commands/Command.h" -CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger *button, - Command *orders) +CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger* button, + Command* orders) : ButtonScheduler(last, button, orders) { pressedLast = m_button->Grab(); } diff --git a/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp b/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp index 37de3858d4..923f4e310b 100644 --- a/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp +++ b/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp @@ -10,8 +10,8 @@ #include "Buttons/Button.h" #include "Commands/Command.h" -HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger *button, - Command *orders) +HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger* button, + Command* orders) : ButtonScheduler(last, button, orders) {} void HeldButtonScheduler::Execute() { diff --git a/wpilibc/shared/src/Buttons/JoystickButton.cpp b/wpilibc/shared/src/Buttons/JoystickButton.cpp index a7a31e6bb2..a9f4c663eb 100644 --- a/wpilibc/shared/src/Buttons/JoystickButton.cpp +++ b/wpilibc/shared/src/Buttons/JoystickButton.cpp @@ -7,7 +7,7 @@ #include "Buttons/JoystickButton.h" -JoystickButton::JoystickButton(GenericHID *joystick, int buttonNumber) +JoystickButton::JoystickButton(GenericHID* joystick, int buttonNumber) : m_joystick(joystick), m_buttonNumber(buttonNumber) {} bool JoystickButton::Get() { return m_joystick->GetRawButton(m_buttonNumber); } diff --git a/wpilibc/shared/src/Buttons/NetworkButton.cpp b/wpilibc/shared/src/Buttons/NetworkButton.cpp index 54604edace..208c282331 100644 --- a/wpilibc/shared/src/Buttons/NetworkButton.cpp +++ b/wpilibc/shared/src/Buttons/NetworkButton.cpp @@ -8,12 +8,14 @@ #include "Buttons/NetworkButton.h" #include "networktables/NetworkTable.h" -NetworkButton::NetworkButton(const std::string &tableName, const std::string &field) +NetworkButton::NetworkButton(const std::string& tableName, + const std::string& field) : // TODO how is this supposed to work??? m_netTable(NetworkTable::GetTable(tableName)), m_field(field) {} -NetworkButton::NetworkButton(std::shared_ptr table, const std::string &field) +NetworkButton::NetworkButton(std::shared_ptr table, + const std::string& field) : m_netTable(table), m_field(field) {} bool NetworkButton::Get() { diff --git a/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp b/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp index 60792dc2cc..eec0197d0f 100644 --- a/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp +++ b/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp @@ -10,8 +10,8 @@ #include "Buttons/Button.h" #include "Commands/Command.h" -PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger *button, - Command *orders) +PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger* button, + Command* orders) : ButtonScheduler(last, button, orders) {} void PressedButtonScheduler::Execute() { diff --git a/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp b/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp index dde271e348..33a2381556 100644 --- a/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp +++ b/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp @@ -10,8 +10,8 @@ #include "Buttons/Button.h" #include "Commands/Command.h" -ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger *button, - Command *orders) +ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger* button, + Command* orders) : ButtonScheduler(last, button, orders) {} void ReleasedButtonScheduler::Execute() { diff --git a/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp b/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp index dc2f0586aa..fc45828600 100644 --- a/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp +++ b/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp @@ -10,8 +10,8 @@ #include "Buttons/Button.h" #include "Commands/Command.h" -ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger *button, - Command *orders) +ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger* button, + Command* orders) : ButtonScheduler(last, button, orders) { pressedLast = m_button->Grab(); } diff --git a/wpilibc/shared/src/Buttons/Trigger.cpp b/wpilibc/shared/src/Buttons/Trigger.cpp index 3f15b7ed5c..cd2bfd5911 100644 --- a/wpilibc/shared/src/Buttons/Trigger.cpp +++ b/wpilibc/shared/src/Buttons/Trigger.cpp @@ -7,11 +7,11 @@ #include "Buttons/Button.h" +#include "Buttons/CancelButtonScheduler.h" #include "Buttons/HeldButtonScheduler.h" #include "Buttons/PressedButtonScheduler.h" #include "Buttons/ReleasedButtonScheduler.h" #include "Buttons/ToggleButtonScheduler.h" -#include "Buttons/CancelButtonScheduler.h" bool Trigger::Grab() { if (Get()) @@ -25,27 +25,27 @@ bool Trigger::Grab() { return false; } -void Trigger::WhenActive(Command *command) { +void Trigger::WhenActive(Command* command) { auto pbs = new PressedButtonScheduler(Grab(), this, command); pbs->Start(); } -void Trigger::WhileActive(Command *command) { +void Trigger::WhileActive(Command* command) { auto hbs = new HeldButtonScheduler(Grab(), this, command); hbs->Start(); } -void Trigger::WhenInactive(Command *command) { +void Trigger::WhenInactive(Command* command) { auto rbs = new ReleasedButtonScheduler(Grab(), this, command); rbs->Start(); } -void Trigger::CancelWhenActive(Command *command) { +void Trigger::CancelWhenActive(Command* command) { auto cbs = new CancelButtonScheduler(Grab(), this, command); cbs->Start(); } -void Trigger::ToggleWhenActive(Command *command) { +void Trigger::ToggleWhenActive(Command* command) { auto tbs = new ToggleButtonScheduler(Grab(), this, command); tbs->Start(); } diff --git a/wpilibc/shared/src/Commands/Command.cpp b/wpilibc/shared/src/Commands/Command.cpp index f965712e64..5298261806 100644 --- a/wpilibc/shared/src/Commands/Command.cpp +++ b/wpilibc/shared/src/Commands/Command.cpp @@ -6,12 +6,12 @@ /*----------------------------------------------------------------------------*/ #include "Commands/Command.h" +#include #include "Commands/CommandGroup.h" #include "Commands/Scheduler.h" #include "RobotState.h" #include "Timer.h" #include "WPIErrors.h" -#include static const std::string kName = "name"; static const std::string kRunning = "running"; @@ -27,12 +27,14 @@ Command::Command() : Command("", -1.0) {} /** * Creates a new command with the given name and no timeout. + * * @param name the name for this command */ -Command::Command(const std::string &name) : Command(name, -1.0) {} +Command::Command(const std::string& name) : Command(name, -1.0) {} /** * Creates a new command with the given timeout and a default name. + * * @param timeout the time (in seconds) before this command "times out" * @see Command#isTimedOut() isTimedOut() */ @@ -40,11 +42,12 @@ Command::Command(double timeout) : Command("", timeout) {} /** * Creates a new command with the given name and timeout. - * @param name the name of the command + * + * @param name the name of the command * @param timeout the time (in seconds) before this command "times out" * @see Command#isTimedOut() isTimedOut() */ -Command::Command(const std::string &name, double timeout) { +Command::Command(const std::string& name, double timeout) { // We use -1.0 to indicate no timeout. if (timeout < 0.0 && timeout != -1.0) wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0"); @@ -54,8 +57,7 @@ Command::Command(const std::string &name, double timeout) { // If name contains an empty string if (name.length() == 0) { m_name = std::string("Command_") + std::string(typeid(*this).name()); - } - else { + } else { m_name = name; } } @@ -65,14 +67,17 @@ Command::~Command() { } /** - * Get the ID (sequence number) for this command + * Get the ID (sequence number) for this command. + * * The ID is a unique sequence number that is incremented for each command. + * * @return the ID of this command */ int Command::GetID() const { return m_commandID; } /** * Sets the timeout of this command. + * * @param timeout the timeout (in seconds) * @see Command#isTimedOut() isTimedOut() */ @@ -85,7 +90,9 @@ void Command::SetTimeout(double timeout) { /** * Returns the time since this command was initialized (in seconds). + * * This function will work even if there is no specified timeout. + * * @return the time since this command was initialized (in seconds). */ double Command::TimeSinceInitialized() const { @@ -98,6 +105,7 @@ double Command::TimeSinceInitialized() const { /** * This method specifies that the given {@link Subsystem} is used by this * command. + * * This method is crucial to the functioning of the Command System in general. * *

Note that the recommended way to call this method is in the @@ -106,7 +114,7 @@ double Command::TimeSinceInitialized() const { * @param subsystem the {@link Subsystem} required * @see Subsystem */ -void Command::Requires(Subsystem *subsystem) { +void Command::Requires(Subsystem* subsystem) { if (!AssertUnlocked("Can not add new requirement to command")) return; if (subsystem != nullptr) @@ -117,8 +125,9 @@ void Command::Requires(Subsystem *subsystem) { /** * Called when the command has been removed. - * This will call {@link Command#interrupted() interrupted()} or {@link - * Command#end() end()}. + * + * This will call {@link Command#interrupted() interrupted()} or + * {@link Command#end() end()}. */ void Command::Removed() { if (m_initialized) { @@ -138,10 +147,10 @@ void Command::Removed() { /** * Starts up the command. Gets the command ready to start. + * *

Note that the command will eventually start, however it will not - * necessarily - * do so immediately, and may in fact be canceled before initialize is even - * called.

+ * necessarily do so immediately, and may in fact be canceled before initialize + * is even called.

*/ void Command::Start() { LockChanges(); @@ -155,6 +164,7 @@ void Command::Start() { /** * The run method is used internally to actually run the commands. + * * @return whether or not the command should stay within the {@link Scheduler}. */ bool Command::Run() { @@ -184,18 +194,19 @@ void Command::_End() {} /** * Called to indicate that the timer should start. + * * This is called right before {@link Command#initialize() initialize()} is, - * inside the - * {@link Command#run() run()} method. + * inside the {@link Command#run() run()} method. */ void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); } /** - * Returns whether or not the {@link Command#timeSinceInitialized() - * timeSinceInitialized()} - * method returns a number which is greater than or equal to the timeout for the - * command. + * Returns whether or not the + * {@link Command#timeSinceInitialized() timeSinceInitialized()} method returns + * a number which is greater than or equal to the timeout for the command. + * * If there is no timeout, this will always return false. + * * @return whether the time has expired */ bool Command::IsTimedOut() const { @@ -204,28 +215,31 @@ bool Command::IsTimedOut() const { /** * Returns the requirements (as an std::set of {@link Subsystem Subsystems} - * pointers) of this command + * pointers) of this command. + * * @return the requirements (as an std::set of {@link Subsystem Subsystems} - * pointers) of this command + * pointers) of this command */ Command::SubsystemSet Command::GetRequirements() const { return m_requirements; } /** - * Prevents further changes from being made + * Prevents further changes from being made. */ void Command::LockChanges() { m_locked = true; } /** * If changes are locked, then this will generate a CommandIllegalUse error. + * * @param message the message to report on error (it is appended by a default - * message) + * message) * @return true if assert passed, false if assert failed */ -bool Command::AssertUnlocked(const std::string &message) { +bool Command::AssertUnlocked(const std::string& message) { if (m_locked) { - std::string buf = message + " after being started or being added to a command group"; + std::string buf = + message + " after being started or being added to a command group"; wpi_setWPIErrorWithContext(CommandIllegalUse, buf); return false; } @@ -234,9 +248,10 @@ bool Command::AssertUnlocked(const std::string &message) { /** * Sets the parent of this command. No actual change is made to the group. + * * @param parent the parent */ -void Command::SetParent(CommandGroup *parent) { +void Command::SetParent(CommandGroup* parent) { if (parent == nullptr) { wpi_setWPIErrorWithContext(NullParameter, "parent"); } else if (m_parent != nullptr) { @@ -254,6 +269,7 @@ void Command::SetParent(CommandGroup *parent) { /** * This is used internally to mark that the command has been started. + * * The lifecycle of a command is: * * startRunning() is called. @@ -261,8 +277,7 @@ void Command::SetParent(CommandGroup *parent) { * removed() is called * * It is very important that startRunning and removed be called in order or some - * assumptions - * of the code will be broken. + * assumptions of the code will be broken. */ void Command::StartRunning() { m_running = true; @@ -272,22 +287,24 @@ void Command::StartRunning() { /** * Returns whether or not the command is running. + * * This may return true even if the command has just been canceled, as it may * not have yet called {@link Command#interrupted()}. + * * @return whether or not the command is running */ bool Command::IsRunning() const { return m_running; } /** * This will cancel the current command. + * *

This will cancel the current command eventually. It can be called - * multiple times. - * And it can be called when the command is not running. If the command is - * running though, - * then the command will be marked as canceled and eventually removed.

- *

A command can not be canceled - * if it is a part of a command group, you must cancel the command group - * instead.

+ * multiple times. And it can be called when the command is not running. If + * the command is running though, then the command will be marked as canceled + * and eventually removed.

+ * + *

A command can not be canceled if it is a part of a command group, you + * must cancel the command group instead.

*/ void Command::Cancel() { if (m_parent != nullptr) @@ -300,8 +317,9 @@ void Command::Cancel() { /** * This works like cancel(), except that it doesn't throw an exception if it is - * a part - * of a command group. Should only be called by the parent command group. + * a part of a command group. + * + * Should only be called by the parent command group. */ void Command::_Cancel() { if (IsRunning()) m_canceled = true; @@ -309,18 +327,21 @@ void Command::_Cancel() { /** * Returns whether or not this has been canceled. + * * @return whether or not this has been canceled */ bool Command::IsCanceled() const { return m_canceled; } /** * Returns whether or not this command can be interrupted. + * * @return whether or not this command can be interrupted */ bool Command::IsInterruptible() const { return m_interruptible; } /** * Sets whether or not this command can be interrupted. + * * @param interruptible whether or not this command can be interrupted */ void Command::SetInterruptible(bool interruptible) { @@ -329,20 +350,23 @@ void Command::SetInterruptible(bool interruptible) { /** * Checks if the command requires the given {@link Subsystem}. + * * @param system the system * @return whether or not the subsystem is required (false if given nullptr) */ -bool Command::DoesRequire(Subsystem *system) const { +bool Command::DoesRequire(Subsystem* system) const { return m_requirements.count(system) > 0; } /** * Returns the {@link CommandGroup} that this command is a part of. + * * Will return null if this {@link Command} is not in a group. + * * @return the {@link CommandGroup} that this command is a part of (or null if - * not in group) + * not in group) */ -CommandGroup *Command::GetGroup() const { return m_parent; } +CommandGroup* Command::GetGroup() const { return m_parent; } /** * Sets whether or not this {@link Command} should run when the robot is @@ -350,6 +374,7 @@ CommandGroup *Command::GetGroup() const { return m_parent; } * *

By default a command will not run when the robot is disabled, and will in * fact be canceled.

+ * * @param run whether or not this command should run when the robot is disabled */ void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; } @@ -357,14 +382,13 @@ void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; } /** * Returns whether or not this {@link Command} will run when the robot is * disabled, or if it will cancel itself. + * * @return whether or not this {@link Command} will run when the robot is - * disabled, or if it will cancel itself + * disabled, or if it will cancel itself */ bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; } -std::string Command::GetName() const { - return m_name; -} +std::string Command::GetName() const { return m_name; } std::string Command::GetSmartDashboardType() const { return "Command"; } diff --git a/wpilibc/shared/src/Commands/CommandGroup.cpp b/wpilibc/shared/src/Commands/CommandGroup.cpp index 91cf788a47..f2fe97b5f6 100644 --- a/wpilibc/shared/src/Commands/CommandGroup.cpp +++ b/wpilibc/shared/src/Commands/CommandGroup.cpp @@ -12,24 +12,21 @@ * Creates a new {@link CommandGroup CommandGroup} with the given name. * @param name the name for this command group */ -CommandGroup::CommandGroup(const std::string &name) : Command(name) {} +CommandGroup::CommandGroup(const std::string& name) : Command(name) {} /** * Adds a new {@link Command Command} to the group. The {@link Command Command} - * will be started after - * all the previously added {@link Command Commands}. + * will be started after all the previously added {@link Command Commands}. * *

Note that any requirements the given {@link Command Command} has will be - * added to the - * group. For this reason, a {@link Command Command's} requirements can not be - * changed after - * being added to a group.

+ * added to the group. For this reason, a {@link Command Command's} + * requirements can not be changed after being added to a group.

* *

It is recommended that this method be called in the constructor.

* * @param command The {@link Command Command} to be added */ -void CommandGroup::AddSequential(Command *command) { +void CommandGroup::AddSequential(Command* command) { if (command == nullptr) { wpi_setWPIErrorWithContext(NullParameter, "command"); return; @@ -53,23 +50,19 @@ void CommandGroup::AddSequential(Command *command) { * commands. * *

Once the {@link Command Command} is started, it will be run until it - * finishes or the time - * expires, whichever is sooner. Note that the given {@link Command Command} - * will have no - * knowledge that it is on a timer.

+ * finishes or the time expires, whichever is sooner. Note that the given + * {@link Command Command} will have no knowledge that it is on a timer.

* *

Note that any requirements the given {@link Command Command} has will be - * added to the - * group. For this reason, a {@link Command Command's} requirements can not be - * changed after - * being added to a group.

+ * added to the group. For this reason, a {@link Command Command's} + * requirements can not be changed after being added to a group.

* *

It is recommended that this method be called in the constructor.

* * @param command The {@link Command Command} to be added * @param timeout The timeout (in seconds) */ -void CommandGroup::AddSequential(Command *command, double timeout) { +void CommandGroup::AddSequential(Command* command, double timeout) { if (command == nullptr) { wpi_setWPIErrorWithContext(NullParameter, "command"); return; @@ -93,30 +86,24 @@ void CommandGroup::AddSequential(Command *command, double timeout) { /** * Adds a new child {@link Command} to the group. The {@link Command} will be - * started after - * all the previously added {@link Command Commands}. + * started after all the previously added {@link Command Commands}. * *

Instead of waiting for the child to finish, a {@link CommandGroup} will - * have it - * run at the same time as the subsequent {@link Command Commands}. The child - * will run until either - * it finishes, a new child with conflicting requirements is started, or - * the main sequence runs a {@link Command} with conflicting requirements. In - * the latter - * two cases, the child will be canceled even if it says it can't be - * interrupted.

+ * have it run at the same time as the subsequent {@link Command Commands}. + * The child will run until either it finishes, a new child with conflicting + * requirements is started, or the main sequence runs a {@link Command} with + * conflicting requirements. In the latter two cases, the child will be + * canceled even if it says it can't be interrupted.

* *

Note that any requirements the given {@link Command Command} has will be - * added to the - * group. For this reason, a {@link Command Command's} requirements can not be - * changed after - * being added to a group.

+ * added to the group. For this reason, a {@link Command Command's} + * requirements can not be changed after being added to a group.

* *

It is recommended that this method be called in the constructor.

* * @param command The command to be added */ -void CommandGroup::AddParallel(Command *command) { +void CommandGroup::AddParallel(Command* command) { if (command == nullptr) { wpi_setWPIErrorWithContext(NullParameter, "command"); return; @@ -136,38 +123,31 @@ void CommandGroup::AddParallel(Command *command) { /** * Adds a new child {@link Command} to the group with the given timeout. The - * {@link Command} will be started after - * all the previously added {@link Command Commands}. + * {@link Command} will be started after all the previously added + * {@link Command Commands}. * *

Once the {@link Command Command} is started, it will run until it - * finishes, is interrupted, - * or the time expires, whichever is sooner. Note that the given {@link Command - * Command} will have no - * knowledge that it is on a timer.

+ * finishes, is interrupted, or the time expires, whichever is sooner. Note + * that the given {@link Command Command} will have no knowledge that it is on + * a timer.

* *

Instead of waiting for the child to finish, a {@link CommandGroup} will - * have it - * run at the same time as the subsequent {@link Command Commands}. The child - * will run until either - * it finishes, the timeout expires, a new child with conflicting requirements - * is started, or - * the main sequence runs a {@link Command} with conflicting requirements. In - * the latter - * two cases, the child will be canceled even if it says it can't be - * interrupted.

+ * have it run at the same time as the subsequent {@link Command Commands}. + * The child will run until either it finishes, the timeout expires, a new + * child with conflicting requirements is started, or the main sequence runs a + * {@link Command} with conflicting requirements. In the latter two cases, the + * child will be canceled even if it says it can't be interrupted.

* *

Note that any requirements the given {@link Command Command} has will be - * added to the - * group. For this reason, a {@link Command Command's} requirements can not be - * changed after - * being added to a group.

+ * added to the group. For this reason, a {@link Command Command's} + * requirements can not be changed after being added to a group.

* *

It is recommended that this method be called in the constructor.

* * @param command The command to be added * @param timeout The timeout (in seconds) */ -void CommandGroup::AddParallel(Command *command, double timeout) { +void CommandGroup::AddParallel(Command* command, double timeout) { if (command == nullptr) { wpi_setWPIErrorWithContext(NullParameter, "command"); return; @@ -193,7 +173,7 @@ void CommandGroup::_Initialize() { m_currentCommandIndex = -1; } void CommandGroup::_Execute() { CommandGroupEntry entry; - Command *cmd = nullptr; + Command* cmd = nullptr; bool firstRun = false; if (m_currentCommandIndex == -1) { @@ -247,7 +227,7 @@ void CommandGroup::_Execute() { auto iter = m_children.begin(); for (; iter != m_children.end();) { entry = *iter; - Command *child = entry.m_command; + Command* child = entry.m_command; if (entry.IsTimedOut()) child->_Cancel(); if (!child->Run()) { @@ -264,14 +244,14 @@ void CommandGroup::_End() { // IsFinished method if (m_currentCommandIndex != -1 && (unsigned)m_currentCommandIndex < m_commands.size()) { - Command *cmd = m_commands[m_currentCommandIndex].m_command; + Command* cmd = m_commands[m_currentCommandIndex].m_command; cmd->_Cancel(); cmd->Removed(); } auto iter = m_children.begin(); for (; iter != m_children.end(); iter++) { - Command *cmd = iter->m_command; + Command* cmd = iter->m_command; cmd->_Cancel(); cmd->Removed(); } @@ -302,7 +282,7 @@ bool CommandGroup::IsInterruptible() const { if (m_currentCommandIndex != -1 && (unsigned)m_currentCommandIndex < m_commands.size()) { - Command *cmd = m_commands[m_currentCommandIndex].m_command; + Command* cmd = m_commands[m_currentCommandIndex].m_command; if (!cmd->IsInterruptible()) return false; } @@ -314,10 +294,10 @@ bool CommandGroup::IsInterruptible() const { return true; } -void CommandGroup::CancelConflicts(Command *command) { +void CommandGroup::CancelConflicts(Command* command) { auto childIter = m_children.begin(); for (; childIter != m_children.end();) { - Command *child = childIter->m_command; + Command* child = childIter->m_command; bool erased = false; Command::SubsystemSet requirements = command->GetRequirements(); diff --git a/wpilibc/shared/src/Commands/CommandGroupEntry.cpp b/wpilibc/shared/src/Commands/CommandGroupEntry.cpp index 0534a71c98..a2b851dc13 100644 --- a/wpilibc/shared/src/Commands/CommandGroupEntry.cpp +++ b/wpilibc/shared/src/Commands/CommandGroupEntry.cpp @@ -9,7 +9,7 @@ #include "Commands/Command.h" -CommandGroupEntry::CommandGroupEntry(Command *command, Sequence state, +CommandGroupEntry::CommandGroupEntry(Command* command, Sequence state, double timeout) : m_timeout(timeout), m_command(command), m_state(state) {} diff --git a/wpilibc/shared/src/Commands/PIDCommand.cpp b/wpilibc/shared/src/Commands/PIDCommand.cpp index b6f426f39c..816a86fd13 100644 --- a/wpilibc/shared/src/Commands/PIDCommand.cpp +++ b/wpilibc/shared/src/Commands/PIDCommand.cpp @@ -9,22 +9,23 @@ #include "float.h" -PIDCommand::PIDCommand(const std::string &name, double p, double i, double d, double f, - double period) +PIDCommand::PIDCommand(const std::string& name, double p, double i, double d, + double f, double period) : Command(name) { m_controller = std::make_shared(p, i, d, this, this, period); } PIDCommand::PIDCommand(double p, double i, double d, double f, double period) { - m_controller = std::make_shared(p, i, d, f, this, this, period); + m_controller = + std::make_shared(p, i, d, f, this, this, period); } -PIDCommand::PIDCommand(const std::string &name, double p, double i, double d) +PIDCommand::PIDCommand(const std::string& name, double p, double i, double d) : Command(name) { m_controller = std::make_shared(p, i, d, this, this); } -PIDCommand::PIDCommand(const std::string &name, double p, double i, double d, +PIDCommand::PIDCommand(const std::string& name, double p, double i, double d, double period) : Command(name) { m_controller = std::make_shared(p, i, d, this, this, period); diff --git a/wpilibc/shared/src/Commands/PIDSubsystem.cpp b/wpilibc/shared/src/Commands/PIDSubsystem.cpp index e478073eff..39b54edd95 100644 --- a/wpilibc/shared/src/Commands/PIDSubsystem.cpp +++ b/wpilibc/shared/src/Commands/PIDSubsystem.cpp @@ -12,12 +12,14 @@ /** * Instantiates a {@link PIDSubsystem} that will use the given p, i and d * values. + * * @param name the name - * @param p the proportional value - * @param i the integral value - * @param d the derivative value + * @param p the proportional value + * @param i the integral value + * @param d the derivative value */ -PIDSubsystem::PIDSubsystem(const std::string &name, double p, double i, double d) +PIDSubsystem::PIDSubsystem(const std::string& name, double p, double i, + double d) : Subsystem(name) { m_controller = std::make_shared(p, i, d, this, this); } @@ -25,39 +27,46 @@ PIDSubsystem::PIDSubsystem(const std::string &name, double p, double i, double d /** * Instantiates a {@link PIDSubsystem} that will use the given p, i and d * values. + * * @param name the name - * @param p the proportional value - * @param i the integral value - * @param d the derivative value - * @param f the feedforward value + * @param p the proportional value + * @param i the integral value + * @param d the derivative value + * @param f the feedforward value */ -PIDSubsystem::PIDSubsystem(const std::string &name, double p, double i, double d, - double f) +PIDSubsystem::PIDSubsystem(const std::string& name, double p, double i, + double d, double f) : Subsystem(name) { m_controller = std::make_shared(p, i, d, f, this, this); } /** * Instantiates a {@link PIDSubsystem} that will use the given p, i and d - * values. It will also space the time - * between PID loop calculations to be equal to the given period. - * @param name the name - * @param p the proportional value - * @param i the integral value - * @param d the derivative value - * @param f the feedfoward value + * values. + * + * It will also space the time between PID loop calculations to be equal to the + * given period. + * + * @param name the name + * @param p the proportional value + * @param i the integral value + * @param d the derivative value + * @param f the feedfoward value * @param period the time (in seconds) between calculations */ -PIDSubsystem::PIDSubsystem(const std::string &name, double p, double i, double d, - double f, double period) +PIDSubsystem::PIDSubsystem(const std::string& name, double p, double i, + double d, double f, double period) : Subsystem(name) { - m_controller = std::make_shared(p, i, d, f, this, this, period); + m_controller = + std::make_shared(p, i, d, f, this, this, period); } /** * Instantiates a {@link PIDSubsystem} that will use the given p, i and d * values. + * * It will use the class name as its name. + * * @param p the proportional value * @param i the integral value * @param d the derivative value @@ -70,7 +79,9 @@ PIDSubsystem::PIDSubsystem(double p, double i, double d) /** * Instantiates a {@link PIDSubsystem} that will use the given p, i and d * values. + * * It will use the class name as its name. + * * @param p the proportional value * @param i the integral value * @param d the derivative value @@ -84,33 +95,36 @@ PIDSubsystem::PIDSubsystem(double p, double i, double d, double f) /** * Instantiates a {@link PIDSubsystem} that will use the given p, i and d * values. - * It will use the class name as its name. - * It will also space the time + * + * It will use the class name as its name. It will also space the time * between PID loop calculations to be equal to the given period. - * @param p the proportional value - * @param i the integral value - * @param d the derivative value - * @param f the feedforward value + * + * @param p the proportional value + * @param i the integral value + * @param d the derivative value + * @param f the feedforward value * @param period the time (in seconds) between calculations */ PIDSubsystem::PIDSubsystem(double p, double i, double d, double f, double period) : Subsystem("PIDSubsystem") { - m_controller = std::make_shared(p, i, d, f, this, this, period); + m_controller = + std::make_shared(p, i, d, f, this, this, period); } /** - * Enables the internal {@link PIDController} + * Enables the internal {@link PIDController}. */ void PIDSubsystem::Enable() { m_controller->Enable(); } /** - * Disables the internal {@link PIDController} + * Disables the internal {@link PIDController}. */ void PIDSubsystem::Disable() { m_controller->Disable(); } /** * Returns the {@link PIDController} used by this {@link PIDSubsystem}. + * * Use this if you would like to fine tune the pid loop. * * @return the {@link PIDController} used by this {@link PIDSubsystem} @@ -120,11 +134,11 @@ std::shared_ptr PIDSubsystem::GetPIDController() { } /** - * Sets the setpoint to the given value. If {@link PIDCommand#SetRange(double, - * double) SetRange(...)} - * was called, - * then the given setpoint - * will be trimmed to fit within the range. + * Sets the setpoint to the given value. + * + * If {@link PIDCommand#SetRange(double, double) SetRange(...)} was called, + * then the given setpoint will be trimmed to fit within the range. + * * @param setpoint the new setpoint */ void PIDSubsystem::SetSetpoint(double setpoint) { @@ -133,8 +147,10 @@ void PIDSubsystem::SetSetpoint(double setpoint) { /** * Adds the given value to the setpoint. + * * If {@link PIDCommand#SetRange(double, double) SetRange(...)} was used, * then the bounds will still be honored by this method. + * * @param deltaSetpoint the change in the setpoint */ void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) { @@ -142,7 +158,8 @@ void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) { } /** - * Return the current setpoint + * Return the current setpoint. + * * @return The current setpoint */ double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); } @@ -167,49 +184,52 @@ void PIDSubsystem::SetOutputRange(float minimumOutput, float maximumOutput) { m_controller->SetOutputRange(minimumOutput, maximumOutput); } -/* +/** * Set the absolute error which is considered tolerable for use with * OnTarget. - * @param percentage error which is tolerable + * + * @param absValue absolute error which is tolerable */ void PIDSubsystem::SetAbsoluteTolerance(float absValue) { m_controller->SetAbsoluteTolerance(absValue); } -/* - * Set the percentage error which is considered tolerable for use with - * OnTarget. - * @param percentage error which is tolerable +/** + * Set the percentage error which is considered tolerable for use with OnTarget. + * + * @param percent percentage error which is tolerable */ void PIDSubsystem::SetPercentTolerance(float percent) { m_controller->SetPercentTolerance(percent); } -/* +/** * Return true if the error is within the percentage of the total input range, - * determined by SetTolerance. This asssumes that the maximum and minimum input - * were set using SetInput. Use OnTarget() in the IsFinished() method of - * commands - * that use this subsystem. + * determined by SetTolerance. + * + * This asssumes that the maximum and minimum input were set using SetInput. + * Use OnTarget() in the IsFinished() method of commands that use this + * subsystem. * * Currently this just reports on target as the actual value passes through the - * setpoint. - * Ideally it should be based on being within the tolerance for some period of - * time. + * setpoint. Ideally it should be based on being within the tolerance for some + * period of time. * * @return true if the error is within the percentage tolerance of the input - * range + * range */ bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); } /** - * Returns the current position + * Returns the current position. + * * @return the current position */ double PIDSubsystem::GetPosition() { return ReturnPIDInput(); } /** - * Returns the current rate + * Returns the current rate. + * * @return the current rate */ double PIDSubsystem::GetRate() { return ReturnPIDInput(); } diff --git a/wpilibc/shared/src/Commands/PrintCommand.cpp b/wpilibc/shared/src/Commands/PrintCommand.cpp index 1d94694482..ba4cad1868 100644 --- a/wpilibc/shared/src/Commands/PrintCommand.cpp +++ b/wpilibc/shared/src/Commands/PrintCommand.cpp @@ -6,12 +6,12 @@ /*----------------------------------------------------------------------------*/ #include "Commands/PrintCommand.h" -#include "stdio.h" #include +#include "stdio.h" -PrintCommand::PrintCommand(const std::string &message) - : Command(((std::stringstream &)(std::stringstream("Print \"") << message - << "\"")) +PrintCommand::PrintCommand(const std::string& message) + : Command(((std::stringstream&)(std::stringstream("Print \"") << message + << "\"")) .str() .c_str()) { m_message = message; diff --git a/wpilibc/shared/src/Commands/Scheduler.cpp b/wpilibc/shared/src/Commands/Scheduler.cpp index 955b33713e..300f07adce 100644 --- a/wpilibc/shared/src/Commands/Scheduler.cpp +++ b/wpilibc/shared/src/Commands/Scheduler.cpp @@ -7,23 +7,22 @@ #include "Commands/Scheduler.h" +#include +#include +#include #include "Buttons/ButtonScheduler.h" #include "Commands/Subsystem.h" #include "HLUsageReporting.h" #include "WPIErrors.h" -#include -#include -#include -Scheduler::Scheduler() { - HLUsageReporting::ReportScheduler(); -} +Scheduler::Scheduler() { HLUsageReporting::ReportScheduler(); } /** * Returns the {@link Scheduler}, creating it if one does not exist. + * * @return the {@link Scheduler} */ -Scheduler *Scheduler::GetInstance() { +Scheduler* Scheduler::GetInstance() { static Scheduler instance; return &instance; } @@ -32,12 +31,13 @@ void Scheduler::SetEnabled(bool enabled) { m_enabled = enabled; } /** * Add a command to be scheduled later. + * * In any pass through the scheduler, all commands are added to the additions - * list, then - * at the end of the pass, they are all scheduled. + * list, then at the end of the pass, they are all scheduled. + * * @param command The command to be scheduled */ -void Scheduler::AddCommand(Command *command) { +void Scheduler::AddCommand(Command* command) { std::lock_guard sync(m_additionsLock); if (std::find(m_additions.begin(), m_additions.end(), command) != m_additions.end()) @@ -45,12 +45,12 @@ void Scheduler::AddCommand(Command *command) { m_additions.push_back(command); } -void Scheduler::AddButton(ButtonScheduler *button) { +void Scheduler::AddButton(ButtonScheduler* button) { std::lock_guard sync(m_buttonsLock); m_buttons.push_back(button); } -void Scheduler::ProcessCommandAddition(Command *command) { +void Scheduler::ProcessCommandAddition(Command* command) { if (command == nullptr) return; // Check to make sure no adding during adding @@ -67,7 +67,7 @@ void Scheduler::ProcessCommandAddition(Command *command) { Command::SubsystemSet requirements = command->GetRequirements(); Command::SubsystemSet::iterator iter; for (iter = requirements.begin(); iter != requirements.end(); iter++) { - Subsystem *lock = *iter; + Subsystem* lock = *iter; if (lock->GetCurrentCommand() != nullptr && !lock->GetCurrentCommand()->IsInterruptible()) return; @@ -76,7 +76,7 @@ void Scheduler::ProcessCommandAddition(Command *command) { // Give it the requirements m_adding = true; for (iter = requirements.begin(); iter != requirements.end(); iter++) { - Subsystem *lock = *iter; + Subsystem* lock = *iter; if (lock->GetCurrentCommand() != nullptr) { lock->GetCurrentCommand()->Cancel(); Remove(lock->GetCurrentCommand()); @@ -93,8 +93,9 @@ void Scheduler::ProcessCommandAddition(Command *command) { } /** - * Runs a single iteration of the loop. This method should be called often in - * order to have a functioning + * Runs a single iteration of the loop. + * + * This method should be called often in order to have a functioning * {@link Command} system. The loop has five stages: * *
    @@ -122,7 +123,7 @@ void Scheduler::Run() { // Loop through the commands auto commandIter = m_commands.begin(); for (; commandIter != m_commands.end();) { - Command *command = *commandIter; + Command* command = *commandIter; // Increment before potentially removing to keep the iterator valid commandIter++; if (!command->Run()) { @@ -144,7 +145,7 @@ void Scheduler::Run() { // Add in the defaults auto subsystemIter = m_subsystems.begin(); for (; subsystemIter != m_subsystems.end(); subsystemIter++) { - Subsystem *lock = *subsystemIter; + Subsystem* lock = *subsystemIter; if (lock->GetCurrentCommand() == nullptr) { ProcessCommandAddition(lock->GetDefaultCommand()); } @@ -156,12 +157,13 @@ void Scheduler::Run() { /** * Registers a {@link Subsystem} to this {@link Scheduler}, so that the {@link - * Scheduler} might know - * if a default {@link Command} needs to be run. All {@link Subsystem - * Subsystems} should call this. + * Scheduler} might know if a default {@link Command} needs to be run. + * + * All {@link Subsystem Subsystems} should call this. + * * @param system the system */ -void Scheduler::RegisterSubsystem(Subsystem *subsystem) { +void Scheduler::RegisterSubsystem(Subsystem* subsystem) { if (subsystem == nullptr) { wpi_setWPIErrorWithContext(NullParameter, "subsystem"); return; @@ -171,9 +173,10 @@ void Scheduler::RegisterSubsystem(Subsystem *subsystem) { /** * Removes the {@link Command} from the {@link Scheduler}. + * * @param command the command to remove */ -void Scheduler::Remove(Command *command) { +void Scheduler::Remove(Command* command) { if (command == nullptr) { wpi_setWPIErrorWithContext(NullParameter, "command"); return; @@ -184,7 +187,7 @@ void Scheduler::Remove(Command *command) { Command::SubsystemSet requirements = command->GetRequirements(); auto iter = requirements.begin(); for (; iter != requirements.end(); iter++) { - Subsystem *lock = *iter; + Subsystem* lock = *iter; lock->SetCurrentCommand(nullptr); } @@ -211,7 +214,7 @@ void Scheduler::ResetAll() { /** * Update the network tables associated with the Scheduler object on the - * SmartDashboard + * SmartDashboard. */ void Scheduler::UpdateTable() { CommandSet::iterator commandIter; @@ -230,7 +233,7 @@ void Scheduler::UpdateTable() { for (commandIter = m_commands.begin(); commandIter != m_commands.end(); ++commandIter) { for (unsigned i = 0; i < toCancel.size(); i++) { - Command *c = *commandIter; + Command* c = *commandIter; if (c->GetID() == toCancel[i]) { c->Cancel(); } @@ -246,7 +249,7 @@ void Scheduler::UpdateTable() { ids.resize(0); for (commandIter = m_commands.begin(); commandIter != m_commands.end(); ++commandIter) { - Command *c = *commandIter; + Command* c = *commandIter; commands.push_back(c->GetName()); ids.push_back(c->GetID()); } diff --git a/wpilibc/shared/src/Commands/StartCommand.cpp b/wpilibc/shared/src/Commands/StartCommand.cpp index ca5f67b17d..c724108daf 100644 --- a/wpilibc/shared/src/Commands/StartCommand.cpp +++ b/wpilibc/shared/src/Commands/StartCommand.cpp @@ -7,7 +7,7 @@ #include "Commands/StartCommand.h" -StartCommand::StartCommand(Command *commandToStart) : Command("StartCommand") { +StartCommand::StartCommand(Command* commandToStart) : Command("StartCommand") { m_commandToFork = commandToStart; } diff --git a/wpilibc/shared/src/Commands/Subsystem.cpp b/wpilibc/shared/src/Commands/Subsystem.cpp index 7e3ef611ba..fe63983732 100644 --- a/wpilibc/shared/src/Commands/Subsystem.cpp +++ b/wpilibc/shared/src/Commands/Subsystem.cpp @@ -12,20 +12,21 @@ #include "WPIErrors.h" /** - * Creates a subsystem with the given name + * Creates a subsystem with the given name. + * * @param name the name of the subsystem */ -Subsystem::Subsystem(const std::string &name) { +Subsystem::Subsystem(const std::string& name) { m_name = name; Scheduler::GetInstance()->RegisterSubsystem(this); } /** - * Initialize the default command for this subsystem + * Initialize the default command for this subsystem. + * * This is meant to be the place to call SetDefaultCommand in a subsystem and - * will be called - * on all the subsystems by the CommandBase method before the program starts - * running by using - * the list of all registered Subsystems inside the Scheduler. + * will be called on all the subsystems by the CommandBase method before the + * program starts running by using the list of all registered Subsystems inside + * the Scheduler. * * This should be overridden by a Subsystem that has a default Command */ @@ -36,12 +37,11 @@ void Subsystem::InitDefaultCommand() {} * then there will be no default command for the subsystem. * *

    WARNING: This should NOT be called in a constructor if the - * subsystem is a - * singleton.

    + * subsystem is a singleton.

    * * @param command the default command (or null if there should be none) */ -void Subsystem::SetDefaultCommand(Command *command) { +void Subsystem::SetDefaultCommand(Command* command) { if (command == nullptr) { m_defaultCommand = nullptr; } else { @@ -75,9 +75,10 @@ void Subsystem::SetDefaultCommand(Command *command) { /** * Returns the default command (or null if there is none). + * * @return the default command */ -Command *Subsystem::GetDefaultCommand() { +Command* Subsystem::GetDefaultCommand() { if (!m_initializedDefaultCommand) { m_initializedDefaultCommand = true; InitDefaultCommand(); @@ -86,27 +87,29 @@ Command *Subsystem::GetDefaultCommand() { } /** - * Sets the current command + * Sets the current command. + * * @param command the new current command */ -void Subsystem::SetCurrentCommand(Command *command) { +void Subsystem::SetCurrentCommand(Command* command) { m_currentCommand = command; m_currentCommandChanged = true; } /** * Returns the command which currently claims this subsystem. + * * @return the command which currently claims this subsystem */ -Command *Subsystem::GetCurrentCommand() const { return m_currentCommand; } +Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; } /** * Call this to alert Subsystem that the current command is actually the * command. + * * Sometimes, the {@link Subsystem} is told that it has no command while the - * {@link Scheduler} - * is going through the loop, only to be soon after given a new one. This will - * avoid that situation. + * {@link Scheduler} is going through the loop, only to be soon after given a + * new one. This will avoid that situation. */ void Subsystem::ConfirmCommand() { if (m_currentCommandChanged) { diff --git a/wpilibc/shared/src/Commands/WaitCommand.cpp b/wpilibc/shared/src/Commands/WaitCommand.cpp index 8c5be65e96..0f85ce0a18 100644 --- a/wpilibc/shared/src/Commands/WaitCommand.cpp +++ b/wpilibc/shared/src/Commands/WaitCommand.cpp @@ -10,12 +10,12 @@ WaitCommand::WaitCommand(double timeout) : Command( - ((std::stringstream &)(std::stringstream("Wait(") << timeout << ")")) + ((std::stringstream&)(std::stringstream("Wait(") << timeout << ")")) .str() .c_str(), timeout) {} -WaitCommand::WaitCommand(const std::string &name, double timeout) +WaitCommand::WaitCommand(const std::string& name, double timeout) : Command(name, timeout) {} void WaitCommand::Initialize() {} diff --git a/wpilibc/shared/src/Commands/WaitForChildren.cpp b/wpilibc/shared/src/Commands/WaitForChildren.cpp index e08dfe02fe..ad8f12815f 100644 --- a/wpilibc/shared/src/Commands/WaitForChildren.cpp +++ b/wpilibc/shared/src/Commands/WaitForChildren.cpp @@ -11,7 +11,7 @@ WaitForChildren::WaitForChildren(double timeout) : Command("WaitForChildren", timeout) {} -WaitForChildren::WaitForChildren(const std::string &name, double timeout) +WaitForChildren::WaitForChildren(const std::string& name, double timeout) : Command(name, timeout) {} void WaitForChildren::Initialize() {} diff --git a/wpilibc/shared/src/Commands/WaitUntilCommand.cpp b/wpilibc/shared/src/Commands/WaitUntilCommand.cpp index 17d6f3eabb..fb47a65e01 100644 --- a/wpilibc/shared/src/Commands/WaitUntilCommand.cpp +++ b/wpilibc/shared/src/Commands/WaitUntilCommand.cpp @@ -10,6 +10,7 @@ /** * A {@link WaitCommand} will wait until a certain match time before finishing. + * * This will wait until the game clock reaches some value, then continue to the * next command. * @see CommandGroup @@ -19,7 +20,7 @@ WaitUntilCommand::WaitUntilCommand(double time) m_time = time; } -WaitUntilCommand::WaitUntilCommand(const std::string &name, double time) +WaitUntilCommand::WaitUntilCommand(const std::string& name, double time) : Command(name, time) { m_time = time; } diff --git a/wpilibc/shared/src/Error.cpp b/wpilibc/shared/src/Error.cpp index 5237a923b3..74e84fcca7 100644 --- a/wpilibc/shared/src/Error.cpp +++ b/wpilibc/shared/src/Error.cpp @@ -7,11 +7,11 @@ #include "Error.h" +#include +#include +#include #include #include -#include -#include -#include #include "DriverStation.h" #include "Timer.h" @@ -70,12 +70,13 @@ void Error::Report() { locStream << m_function << " ["; #if defined(_WIN32) - const int MAX_DIR = 100; - char basename[MAX_DIR]; - _splitpath_s(m_filename.c_str(), NULL, 0, basename, MAX_DIR, NULL, 0, NULL, 0); - locStream << basename; + const int MAX_DIR = 100; + char basename[MAX_DIR]; + _splitpath_s(m_filename.c_str(), NULL, 0, basename, MAX_DIR, NULL, 0, NULL, + 0); + locStream << basename; #else - locStream << basename(m_filename.c_str()); + locStream << basename(m_filename.c_str()); #endif locStream << ":" << m_lineNumber << "]"; diff --git a/wpilibc/shared/src/ErrorBase.cpp b/wpilibc/shared/src/ErrorBase.cpp index c1e41d85b3..35394b361e 100644 --- a/wpilibc/shared/src/ErrorBase.cpp +++ b/wpilibc/shared/src/ErrorBase.cpp @@ -10,8 +10,8 @@ #include "WPIErrors.h" #include -#include #include +#include #include priority_mutex ErrorBase::_globalErrorMutex; @@ -35,9 +35,9 @@ void ErrorBase::ClearError() const { m_error.Clear(); } * error to the "errno" global variable. * * @param contextMessage A custom message from the code that set the error. - * @param filename Filename of the error source - * @param function Function of the error source - * @param lineNumber Line number of the error source + * @param filename Filename of the error source + * @param function Function of the error source + * @param lineNumber Line number of the error source */ void ErrorBase::SetErrnoError(llvm::StringRef contextMessage, llvm::StringRef filename, @@ -55,7 +55,7 @@ void ErrorBase::SetErrnoError(llvm::StringRef contextMessage, err = buf; } - // Set the current error information for this object. + // Set the current error information for this object. m_error.Set(-1, err, filename, function, lineNumber, this); // Update the global error if there is not one already set. @@ -69,21 +69,21 @@ void ErrorBase::SetErrnoError(llvm::StringRef contextMessage, * @brief Set the current error information associated from the nivision Imaq * API. * - * @param success The return from the function + * @param success The return from the function * @param contextMessage A custom message from the code that set the error. - * @param filename Filename of the error source - * @param function Function of the error source - * @param lineNumber Line number of the error source + * @param filename Filename of the error source + * @param function Function of the error source + * @param lineNumber Line number of the error source */ void ErrorBase::SetImaqError(int success, llvm::StringRef contextMessage, llvm::StringRef filename, llvm::StringRef function, uint32_t lineNumber) const { - // If there was an error + // If there was an error if (success <= 0) { std::stringstream err; err << success << ": " << contextMessage; - // Set the current error information for this object. + // Set the current error information for this object. m_error.Set(success, err.str(), filename, function, lineNumber, this); // Update the global error if there is not one already set. @@ -97,11 +97,11 @@ void ErrorBase::SetImaqError(int success, llvm::StringRef contextMessage, /** * @brief Set the current error information associated with this sensor. * - * @param code The error code + * @param code The error code * @param contextMessage A custom message from the code that set the error. - * @param filename Filename of the error source - * @param function Function of the error source - * @param lineNumber Line number of the error source + * @param filename Filename of the error source + * @param function Function of the error source + * @param lineNumber Line number of the error source */ void ErrorBase::SetError(Error::Code code, llvm::StringRef contextMessage, llvm::StringRef filename, llvm::StringRef function, @@ -122,11 +122,11 @@ void ErrorBase::SetError(Error::Code code, llvm::StringRef contextMessage, /** * @brief Set the current error information associated with this sensor. * - * @param errorMessage The error message from WPIErrors.h + * @param errorMessage The error message from WPIErrors.h * @param contextMessage A custom message from the code that set the error. - * @param filename Filename of the error source - * @param function Function of the error source - * @param lineNumber Line number of the error source + * @param filename Filename of the error source + * @param function Function of the error source + * @param lineNumber Line number of the error source */ void ErrorBase::SetWPIError(llvm::StringRef errorMessage, Error::Code code, llvm::StringRef contextMessage, @@ -149,20 +149,20 @@ void ErrorBase::CloneError(const ErrorBase& rhs) const { } /** -@brief Check if the current error code represents a fatal error. - -@return true if the current error is fatal. -*/ + * @brief Check if the current error code represents a fatal error. + * + * @return true if the current error is fatal. + */ bool ErrorBase::StatusIsFatal() const { return m_error.GetCode() < 0; } void ErrorBase::SetGlobalError(Error::Code code, llvm::StringRef contextMessage, llvm::StringRef filename, llvm::StringRef function, uint32_t lineNumber) { - // If there was an error + // If there was an error if (code != 0) { std::lock_guard mutex(_globalErrorMutex); - // Set the current error information for this object. + // Set the current error information for this object. _globalError.Set(code, contextMessage, filename, function, lineNumber, nullptr); } @@ -183,8 +183,8 @@ void ErrorBase::SetGlobalWPIError(llvm::StringRef errorMessage, } /** - * Retrieve the current global error. -*/ + * Retrieve the current global error. + */ Error& ErrorBase::GetGlobalError() { std::lock_guard mutex(_globalErrorMutex); return _globalError; diff --git a/wpilibc/shared/src/Filters/Filter.cpp b/wpilibc/shared/src/Filters/Filter.cpp index d6042d75a3..325e6c113d 100644 --- a/wpilibc/shared/src/Filters/Filter.cpp +++ b/wpilibc/shared/src/Filters/Filter.cpp @@ -7,9 +7,7 @@ #include "Filters/Filter.h" -Filter::Filter(std::shared_ptr source) { - m_source = source; -} +Filter::Filter(std::shared_ptr source) { m_source = source; } void Filter::SetPIDSourceType(PIDSourceType pidSource) { m_source->SetPIDSourceType(pidSource); @@ -19,6 +17,4 @@ PIDSourceType Filter::GetPIDSourceType() const { return m_source->GetPIDSourceType(); } -double Filter::PIDGetSource() { - return m_source->PIDGet(); -} +double Filter::PIDGetSource() { return m_source->PIDGet(); } diff --git a/wpilibc/shared/src/Filters/LinearDigitalFilter.cpp b/wpilibc/shared/src/Filters/LinearDigitalFilter.cpp index e6e7474f8a..81f46588a3 100644 --- a/wpilibc/shared/src/Filters/LinearDigitalFilter.cpp +++ b/wpilibc/shared/src/Filters/LinearDigitalFilter.cpp @@ -10,56 +10,68 @@ #include /** - * Create a linear FIR or IIR filter + * Create a linear FIR or IIR filter. * - * @param source The PIDSource object that is used to get values + * @param source The PIDSource object that is used to get values * @param ffGains The "feed forward" or FIR gains * @param fbGains The "feed back" or IIR gains */ LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr source, std::initializer_list ffGains, - std::initializer_list fbGains) : - Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()), - m_inputGains(ffGains), m_outputGains(fbGains) {} + std::initializer_list fbGains) + : Filter(source), + m_inputs(ffGains.size()), + m_outputs(fbGains.size()), + m_inputGains(ffGains), + m_outputGains(fbGains) {} /** - * Create a linear FIR or IIR filter + * Create a linear FIR or IIR filter. * - * @param source The PIDSource object that is used to get values + * @param source The PIDSource object that is used to get values * @param ffGains The "feed forward" or FIR gains * @param fbGains The "feed back" or IIR gains */ LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr source, std::initializer_list ffGains, - const std::vector& fbGains) : - Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()), - m_inputGains(ffGains), m_outputGains(fbGains) {} + const std::vector& fbGains) + : Filter(source), + m_inputs(ffGains.size()), + m_outputs(fbGains.size()), + m_inputGains(ffGains), + m_outputGains(fbGains) {} /** - * Create a linear FIR or IIR filter + * Create a linear FIR or IIR filter. * - * @param source The PIDSource object that is used to get values + * @param source The PIDSource object that is used to get values * @param ffGains The "feed forward" or FIR gains * @param fbGains The "feed back" or IIR gains */ LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr source, const std::vector& ffGains, - std::initializer_list fbGains) : - Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()), - m_inputGains(ffGains), m_outputGains(fbGains) {} + std::initializer_list fbGains) + : Filter(source), + m_inputs(ffGains.size()), + m_outputs(fbGains.size()), + m_inputGains(ffGains), + m_outputGains(fbGains) {} /** - * Create a linear FIR or IIR filter + * Create a linear FIR or IIR filter. * - * @param source The PIDSource object that is used to get values + * @param source The PIDSource object that is used to get values * @param ffGains The "feed forward" or FIR gains * @param fbGains The "feed back" or IIR gains */ LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr source, const std::vector& ffGains, - const std::vector& fbGains) : - Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()), - m_inputGains(ffGains), m_outputGains(fbGains) {} + const std::vector& fbGains) + : Filter(source), + m_inputs(ffGains.size()), + m_outputs(fbGains.size()), + m_inputGains(ffGains), + m_outputGains(fbGains) {} /** * Creates a one-pole IIR low-pass filter of the form: @@ -68,13 +80,12 @@ LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr source, * * This filter is stable for time constants greater than zero * - * @param source The PIDSource object that is used to get values + * @param source The PIDSource object that is used to get values * @param timeConstant The discrete-time time constant in seconds - * @param period The period in seconds between samples taken by the user + * @param period The period in seconds between samples taken by the user */ -LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(std::shared_ptr source, - double timeConstant, - double period) { +LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR( + std::shared_ptr source, double timeConstant, double period) { double gain = std::exp(-period / timeConstant); return LinearDigitalFilter(std::move(source), {1.0 - gain}, {-gain}); } @@ -86,13 +97,12 @@ LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(std::shared_ptr source, - double timeConstant, - double period) { +LinearDigitalFilter LinearDigitalFilter::HighPass( + std::shared_ptr source, double timeConstant, double period) { double gain = std::exp(-period / timeConstant); return LinearDigitalFilter(std::move(source), {gain, -gain}, {-gain}); } @@ -104,11 +114,11 @@ LinearDigitalFilter LinearDigitalFilter::HighPass(std::shared_ptr sou * This filter is always stable. * * @param source The PIDSource object that is used to get values - * @param taps The number of samples to average over. Higher = smoother but - * slower + * @param taps The number of samples to average over. Higher = smoother but + * slower */ -LinearDigitalFilter LinearDigitalFilter::MovingAverage(std::shared_ptr source, - unsigned int taps) { +LinearDigitalFilter LinearDigitalFilter::MovingAverage( + std::shared_ptr source, unsigned int taps) { assert(taps > 0); std::vector gains(taps, 1.0 / taps); diff --git a/wpilibc/shared/src/GyroBase.cpp b/wpilibc/shared/src/GyroBase.cpp index f1044b7361..18018cc6a7 100644 --- a/wpilibc/shared/src/GyroBase.cpp +++ b/wpilibc/shared/src/GyroBase.cpp @@ -6,8 +6,8 @@ /*----------------------------------------------------------------------------*/ #include "GyroBase.h" -#include "WPIErrors.h" #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" /** * Get the PIDOutput for the PIDSource base object. Can be set to return diff --git a/wpilibc/shared/src/LiveWindow/LiveWindow.cpp b/wpilibc/shared/src/LiveWindow/LiveWindow.cpp index 1dfd50753e..db28352e4f 100644 --- a/wpilibc/shared/src/LiveWindow/LiveWindow.cpp +++ b/wpilibc/shared/src/LiveWindow/LiveWindow.cpp @@ -6,23 +6,24 @@ /*----------------------------------------------------------------------------*/ #include "LiveWindow/LiveWindow.h" -#include "networktables/NetworkTable.h" #include #include +#include "networktables/NetworkTable.h" /** - * Get an instance of the LiveWindow main class + * Get an instance of the LiveWindow main class. + * * This is a singleton to guarantee that there is only a single instance - * regardless of - * how many times GetInstance is called. + * regardless of how many times GetInstance is called. */ -LiveWindow *LiveWindow::GetInstance() { +LiveWindow* LiveWindow::GetInstance() { static LiveWindow instance; return &instance; } /** * LiveWindow constructor. + * * Allocate the necessary tables. */ LiveWindow::LiveWindow() : m_scheduler(Scheduler::GetInstance()) { @@ -31,7 +32,8 @@ LiveWindow::LiveWindow() : m_scheduler(Scheduler::GetInstance()) { } /** - * Change the enabled status of LiveWindow + * Change the enabled status of LiveWindow. + * * If it changes to enabled, start livewindow running otherwise stop it */ void LiveWindow::SetEnabled(bool enabled) { @@ -58,16 +60,19 @@ void LiveWindow::SetEnabled(bool enabled) { /** * @name AddSensor(subsystem, name, component) + * * Add a Sensor associated with the subsystem and call it by the given name. + * * @param subsystem The subsystem this component is part of. - * @param name The name of this component. + * @param name The name of this component. * @param component A LiveWindowSendable component that represents a sensor. */ //@{ /** * @brief Use a STL smart pointer to share ownership of component. */ -void LiveWindow::AddSensor(const std::string &subsystem, const std::string &name, +void LiveWindow::AddSensor(const std::string& subsystem, + const std::string& name, std::shared_ptr component) { m_components[component].subsystem = subsystem; m_components[component].name = name; @@ -77,20 +82,20 @@ void LiveWindow::AddSensor(const std::string &subsystem, const std::string &name /** * @brief Pass a reference to LiveWindow and retain ownership of the component. */ -void LiveWindow::AddSensor(const std::string &subsystem, - const std::string &name, - LiveWindowSendable &component) { +void LiveWindow::AddSensor(const std::string& subsystem, + const std::string& name, + LiveWindowSendable& component) { AddSensor(subsystem, name, std::shared_ptr( - &component, [](LiveWindowSendable*){})); + &component, [](LiveWindowSendable*) {})); } /** * @brief Use a raw pointer to the LiveWindow. * @deprecated Prefer smart pointers or references. */ -void LiveWindow::AddSensor(const std::string &subsystem, - const std::string &name, - LiveWindowSendable *component) { +void LiveWindow::AddSensor(const std::string& subsystem, + const std::string& name, + LiveWindowSendable* component) { AddSensor(subsystem, name, std::shared_ptr( component, NullDeleter())); } @@ -98,16 +103,19 @@ void LiveWindow::AddSensor(const std::string &subsystem, /** * @name AddActuator(subsystem, name, component) + * * Add an Actuator associated with the subsystem and call it by the given name. + * * @param subsystem The subsystem this component is part of. - * @param name The name of this component. + * @param name The name of this component. * @param component A LiveWindowSendable component that represents a actuator. */ //@{ /** * @brief Use a STL smart pointer to share ownership of component. */ -void LiveWindow::AddActuator(const std::string &subsystem, const std::string &name, +void LiveWindow::AddActuator(const std::string& subsystem, + const std::string& name, std::shared_ptr component) { m_components[component].subsystem = subsystem; m_components[component].name = name; @@ -117,19 +125,20 @@ void LiveWindow::AddActuator(const std::string &subsystem, const std::string &na /** * @brief Pass a reference to LiveWindow and retain ownership of the component. */ -void LiveWindow::AddActuator(const std::string &subsystem, - const std::string &name, - LiveWindowSendable &component) { +void LiveWindow::AddActuator(const std::string& subsystem, + const std::string& name, + LiveWindowSendable& component) { AddActuator(subsystem, name, std::shared_ptr( - &component, [](LiveWindowSendable*){})); + &component, [](LiveWindowSendable*) {})); } /** * @brief Use a raw pointer to the LiveWindow. * @deprecated Prefer smart pointers or references. */ -void LiveWindow::AddActuator(const std::string &subsystem, const std::string &name, - LiveWindowSendable *component) { +void LiveWindow::AddActuator(const std::string& subsystem, + const std::string& name, + LiveWindowSendable* component) { AddActuator(subsystem, name, std::shared_ptr( component, NullDeleter())); @@ -140,7 +149,7 @@ void LiveWindow::AddActuator(const std::string &subsystem, const std::string &na * Meant for internal use in other WPILib classes. */ void LiveWindow::AddSensor(std::string type, int channel, - LiveWindowSendable *component) { + LiveWindowSendable* component) { std::ostringstream oss; oss << type << "[" << channel << "]"; AddSensor("Ungrouped", oss.str(), component); @@ -155,26 +164,29 @@ void LiveWindow::AddSensor(std::string type, int channel, * Meant for internal use in other WPILib classes. */ void LiveWindow::AddActuator(std::string type, int channel, - LiveWindowSendable *component) { + LiveWindowSendable* component) { std::ostringstream oss; oss << type << "[" << channel << "]"; - AddActuator("Ungrouped", oss.str(), std::shared_ptr( - component, [](LiveWindowSendable *) {})); + AddActuator("Ungrouped", oss.str(), + std::shared_ptr(component, + [](LiveWindowSendable*) {})); } /** * Meant for internal use in other WPILib classes. */ void LiveWindow::AddActuator(std::string type, int module, int channel, - LiveWindowSendable *component) { + LiveWindowSendable* component) { std::ostringstream oss; oss << type << "[" << module << "," << channel << "]"; - AddActuator("Ungrouped", oss.str(), std::shared_ptr( - component, [](LiveWindowSendable *) {})); + AddActuator("Ungrouped", oss.str(), + std::shared_ptr(component, + [](LiveWindowSendable*) {})); } /** - * Tell all the sensors to update (send) their values + * Tell all the sensors to update (send) their values. + * * Actuators are handled through callbacks on their value changing from the * SmartDashboard widgets. */ @@ -207,8 +219,8 @@ void LiveWindow::InitializeLiveWindowComponents() { LiveWindowComponent c = elem.second; std::string subsystem = c.subsystem; std::string name = c.name; - m_liveWindowTable->GetSubTable(subsystem) - ->PutString("~TYPE~", "LW Subsystem"); + m_liveWindowTable->GetSubTable(subsystem)->PutString("~TYPE~", + "LW Subsystem"); std::shared_ptr table( m_liveWindowTable->GetSubTable(subsystem)->GetSubTable(name)); table->PutString("~TYPE~", component->GetSmartDashboardType()); diff --git a/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp b/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp index 31e5db6e22..5b02dc8489 100644 --- a/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp +++ b/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp @@ -8,6 +8,6 @@ #include "LiveWindow/LiveWindowStatusListener.h" #include "Commands/Scheduler.h" -void LiveWindowStatusListener::ValueChanged(ITable *source, llvm::StringRef key, +void LiveWindowStatusListener::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) {} diff --git a/wpilibc/shared/src/PIDSource.cpp b/wpilibc/shared/src/PIDSource.cpp index 4e44b69594..66aa68a671 100644 --- a/wpilibc/shared/src/PIDSource.cpp +++ b/wpilibc/shared/src/PIDSource.cpp @@ -16,6 +16,4 @@ void PIDSource::SetPIDSourceType(PIDSourceType pidSource) { m_pidSource = pidSource; } -PIDSourceType PIDSource::GetPIDSourceType() const { - return m_pidSource; -} +PIDSourceType PIDSource::GetPIDSourceType() const { return m_pidSource; } diff --git a/wpilibc/shared/src/Resource.cpp b/wpilibc/shared/src/Resource.cpp index a68122e360..c6df9e2d14 100644 --- a/wpilibc/shared/src/Resource.cpp +++ b/wpilibc/shared/src/Resource.cpp @@ -6,17 +6,17 @@ /*----------------------------------------------------------------------------*/ #include "Resource.h" -#include "WPIErrors.h" #include "ErrorBase.h" +#include "WPIErrors.h" priority_recursive_mutex Resource::m_createLock; /** * Allocate storage for a new instance of Resource. + * * Allocate a bool array of values that will get initialized to indicate that no - * resources - * have been allocated yet. The indicies of the resources are [0 .. elements - - * 1]. + * resources have been allocated yet. The indicies of the resources are [0 .. + * elements - 1]. */ Resource::Resource(uint32_t elements) { std::lock_guard sync(m_createLock); @@ -26,12 +26,13 @@ Resource::Resource(uint32_t elements) { /** * Factory method to create a Resource allocation-tracker *if* needed. * - * @param r -- address of the caller's Resource pointer. If *r == nullptr, this - * will construct a Resource and make *r point to it. If *r != nullptr, i.e. - * the caller already has a Resource instance, this won't do anything. - * @param elements -- the number of elements for this Resource allocator to - * track, that is, it will allocate resource numbers in the range - * [0 .. elements - 1]. + * @param r address of the caller's Resource pointer. If *r == nullptr, + * this will construct a Resource and make *r point to it. If + * *r != nullptr, i.e. the caller already has a Resource + * instance, this won't do anything. + * @param elements the number of elements for this Resource allocator to + * track, that is, it will allocate resource numbers in the + * range [0 .. elements - 1]. */ /*static*/ void Resource::CreateResourceObject(std::unique_ptr& r, uint32_t elements) { @@ -43,11 +44,12 @@ Resource::Resource(uint32_t elements) { /** * Allocate a resource. + * * When a resource is requested, mark it allocated. In this case, a free - * resource value - * within the range is located and returned after it is marked allocated. + * resource value within the range is located and returned after it is marked + * allocated. */ -uint32_t Resource::Allocate(const std::string &resourceDesc) { +uint32_t Resource::Allocate(const std::string& resourceDesc) { std::lock_guard sync(m_allocateLock); for (uint32_t i = 0; i < m_isAllocated.size(); i++) { if (!m_isAllocated[i]) { @@ -61,11 +63,11 @@ uint32_t Resource::Allocate(const std::string &resourceDesc) { /** * Allocate a specific resource value. + * * The user requests a specific resource value, i.e. channel number and it is - * verified - * unallocated, then returned. + * verified unallocated, then returned. */ -uint32_t Resource::Allocate(uint32_t index, const std::string &resourceDesc) { +uint32_t Resource::Allocate(uint32_t index, const std::string& resourceDesc) { std::lock_guard sync(m_allocateLock); if (index >= m_isAllocated.size()) { wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc); @@ -81,10 +83,10 @@ uint32_t Resource::Allocate(uint32_t index, const std::string &resourceDesc) { /** * Free an allocated resource. + * * After a resource is no longer needed, for example a destructor is called for - * a channel assignment - * class, Free will release the resource value so it can be reused somewhere - * else in the program. + * a channel assignment class, Free will release the resource value so it can + * be reused somewhere else in the program. */ void Resource::Free(uint32_t index) { std::unique_lock sync(m_allocateLock); diff --git a/wpilibc/shared/src/RobotState.cpp b/wpilibc/shared/src/RobotState.cpp index f3db1dadc3..6add6649c2 100644 --- a/wpilibc/shared/src/RobotState.cpp +++ b/wpilibc/shared/src/RobotState.cpp @@ -16,8 +16,7 @@ void RobotState::SetImplementation(RobotStateInterface& i) { &i, NullDeleter()); } -void RobotState::SetImplementation( - std::shared_ptr i) { +void RobotState::SetImplementation(std::shared_ptr i) { impl = i; } diff --git a/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp b/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp index cc6543f2f7..6e7cd0f04a 100644 --- a/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp +++ b/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp @@ -14,37 +14,42 @@ static const std::string kOptions = "options"; static const std::string kSelected = "selected"; /** - * Adds the given object to the list of options. On the {@link SmartDashboard} - * on the desktop, - * the object will appear as the given name. - * @param name the name of the option + * Adds the given object to the list of options. + * + * On the {@link SmartDashboard} on the desktop, the object will appear as the + * given name. + * + * @param name the name of the option * @param object the option */ -void SendableChooser::AddObject(const std::string &name, void *object) { +void SendableChooser::AddObject(const std::string& name, void* object) { m_choices[name] = object; } /** * Add the given object to the list of options and marks it as the default. + * * Functionally, this is very close to {@link SendableChooser#AddObject(const - * char *name, void *object) AddObject(...)} - * except that it will use this as the default option if none other is - * explicitly selected. - * @param name the name of the option + * char *name, void *object) AddObject(...)} except that it will use this as + * the default option if none other is explicitly selected. + * + * @param name the name of the option * @param object the option */ -void SendableChooser::AddDefault(const std::string &name, void *object) { +void SendableChooser::AddDefault(const std::string& name, void* object) { m_defaultChoice = name; AddObject(name, object); } /** - * Returns the selected option. If there is none selected, it will return the - * default. If there is none selected - * and no default, then it will return {@code nullptr}. + * Returns the selected option. + * + * If there is none selected, it will return the default. If there is none + * selected and no default, then it will return {@code nullptr}. + * * @return the option selected */ -void *SendableChooser::GetSelected() { +void* SendableChooser::GetSelected() { std::string selected = m_table->GetString(kSelected, m_defaultChoice); if (selected == "") return nullptr; @@ -56,7 +61,7 @@ void SendableChooser::InitTable(std::shared_ptr subtable) { std::vector keys; m_table = subtable; if (m_table != nullptr) { - std::map::iterator iter; + std::map::iterator iter; for (iter = m_choices.begin(); iter != m_choices.end(); iter++) { keys.push_back(iter->first); } diff --git a/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp b/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp index 9fbf60551b..18f63d472f 100644 --- a/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp +++ b/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp @@ -8,13 +8,13 @@ #include "SmartDashboard/SmartDashboard.h" //#include "NetworkCommunication/UsageReporting.h" +#include "HLUsageReporting.h" #include "SmartDashboard/NamedSendable.h" #include "WPIErrors.h" #include "networktables/NetworkTable.h" -#include "HLUsageReporting.h" std::shared_ptr SmartDashboard::m_table; -std::map , Sendable *> SmartDashboard::m_tablesToData; +std::map, Sendable*> SmartDashboard::m_tablesToData; void SmartDashboard::init() { m_table = NetworkTable::GetTable("SmartDashboard"); @@ -24,13 +24,14 @@ void SmartDashboard::init() { /** * Maps the specified key to the specified value in this table. - * The key can not be nullptr. + * * The value can be retrieved by calling the get method with a key that is equal * to the original key. + * * @param keyName the key - * @param value the value + * @param value the value */ -void SmartDashboard::PutData(llvm::StringRef key, Sendable *data) { +void SmartDashboard::PutData(llvm::StringRef key, Sendable* data) { if (data == nullptr) { wpi_setGlobalWPIErrorWithContext(NullParameter, "value"); return; @@ -42,14 +43,15 @@ void SmartDashboard::PutData(llvm::StringRef key, Sendable *data) { } /** - * Maps the specified key (where the key is the name of the {@link - * SmartDashboardNamedData} - * to the specified value in this table. + * Maps the specified key (where the key is the name of the + * {@link SmartDashboardNamedData} to the specified value in this table. + * * The value can be retrieved by calling the get method with a key that is equal * to the original key. + * * @param value the value */ -void SmartDashboard::PutData(NamedSendable *value) { +void SmartDashboard::PutData(NamedSendable* value) { if (value == nullptr) { wpi_setGlobalWPIErrorWithContext(NullParameter, "value"); return; @@ -59,12 +61,13 @@ void SmartDashboard::PutData(NamedSendable *value) { /** * Returns the value at the specified key. + * * @param keyName the key * @return the value */ -Sendable *SmartDashboard::GetData(llvm::StringRef key) { +Sendable* SmartDashboard::GetData(llvm::StringRef key) { std::shared_ptr subtable(m_table->GetSubTable(key)); - Sendable *data = m_tablesToData[subtable]; + Sendable* data = m_tablesToData[subtable]; if (data == nullptr) { wpi_setGlobalWPIErrorWithContext(SmartDashboardMissingKey, key); return nullptr; @@ -75,11 +78,12 @@ Sendable *SmartDashboard::GetData(llvm::StringRef key) { /** * Maps the specified key to the specified complex value (such as an array) in * this table. - * The key can not be nullptr. + * * The value can be retrieved by calling the RetrieveValue method with a key * that is equal to the original key. + * * @param keyName the key - * @param value the value + * @param value the value */ void SmartDashboard::PutValue(llvm::StringRef keyName, std::shared_ptr value) { @@ -88,10 +92,10 @@ void SmartDashboard::PutValue(llvm::StringRef keyName, /** * Retrieves the complex value (such as an array) in this table into the complex - * data object - * The key can not be nullptr. + * data object. + * * @param keyName the key - * @param value the object to retrieve the value into + * @param value the object to retrieve the value into */ std::shared_ptr SmartDashboard::GetValue(llvm::StringRef keyName) { return m_table->GetValue(keyName); @@ -99,19 +103,22 @@ std::shared_ptr SmartDashboard::GetValue(llvm::StringRef keyName) { /** * Maps the specified key to the specified value in this table. - * The key can not be nullptr. + * * The value can be retrieved by calling the get method with a key that is equal * to the original key. + * * @param keyName the key - * @param value the value + * @param value the value */ void SmartDashboard::PutBoolean(llvm::StringRef keyName, bool value) { m_table->PutBoolean(keyName, value); } /** - * Returns the value at the specified key. If the key is not found, returns the - * default value. + * Returns the value at the specified key. + * + * If the key is not found, returns the default value. + * * @param keyName the key * @return the value */ @@ -121,19 +128,22 @@ bool SmartDashboard::GetBoolean(llvm::StringRef keyName, bool defaultValue) { /** * Maps the specified key to the specified value in this table. - * The key can not be nullptr. + * * The value can be retrieved by calling the get method with a key that is equal * to the original key. + * * @param keyName the key - * @param value the value + * @param value the value */ void SmartDashboard::PutNumber(llvm::StringRef keyName, double value) { m_table->PutNumber(keyName, value); } /** - * Returns the value at the specified key. If the key is not found, returns the - * default value. + * Returns the value at the specified key. + * + * If the key is not found, returns the default value. + * * @param keyName the key * @return the value */ @@ -143,19 +153,22 @@ double SmartDashboard::GetNumber(llvm::StringRef keyName, double defaultValue) { /** * Maps the specified key to the specified value in this table. - * Neither the key nor the value can be nullptr. + * * The value can be retrieved by calling the get method with a key that is equal * to the original key. + * * @param keyName the key - * @param value the value + * @param value the value */ void SmartDashboard::PutString(llvm::StringRef keyName, llvm::StringRef value) { m_table->PutString(keyName, value); } /** - * Returns the value at the specified key. If the key is not found, returns the - * default value. + * Returns the value at the specified key. + * + * If the key is not found, returns the default value. + * * @param keyName the key * @return the value */ diff --git a/wpilibc/shared/src/interfaces/Potentiometer.cpp b/wpilibc/shared/src/interfaces/Potentiometer.cpp index 9ac097be13..fb95aa333e 100644 --- a/wpilibc/shared/src/interfaces/Potentiometer.cpp +++ b/wpilibc/shared/src/interfaces/Potentiometer.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include void Potentiometer::SetPIDSourceType(PIDSourceType pidSource) { if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) { diff --git a/wpilibc/simulation/include/AnalogGyro.h b/wpilibc/simulation/include/AnalogGyro.h index e117330291..8e17b146f1 100644 --- a/wpilibc/simulation/include/AnalogGyro.h +++ b/wpilibc/simulation/include/AnalogGyro.h @@ -17,34 +17,35 @@ class AnalogModule; /** * Use a rate gyro to return the robots heading relative to a starting position. - * The AnalogGyro class tracks the robots heading based on the starting position. As the robot - * rotates the new heading is computed by integrating the rate of rotation returned - * by the sensor. When the class is instantiated, it does a short calibration routine - * where it samples the gyro while at rest to determine the default offset. This is - * subtracted from each sample to determine the heading. This gyro class must be used - * with a channel that is assigned one of the Analog accumulators from the FPGA. See - * AnalogInput for the current accumulator assignments. + * + * The AnalogGyro class tracks the robots heading based on the starting + * position. As the robot rotates the new heading is computed by integrating + * the rate of rotation returned by the sensor. When the class is instantiated, + * it does a short calibration routine where it samples the gyro while at rest + * to determine the default offset. This is subtracted from each sample to + * determine the heading. This gyro class must be used with a channel that is + * assigned one of the Analog accumulators from the FPGA. See AnalogInput for + * the current accumulator assignments. */ -class AnalogGyro : public GyroBase -{ -public: - static const uint32_t kOversampleBits; - static const uint32_t kAverageBits; - static const float kSamplesPerSecond; - static const float kCalibrationSampleTime; - static const float kDefaultVoltsPerDegreePerSecond; +class AnalogGyro : public GyroBase { + public: + static const uint32_t kOversampleBits; + static const uint32_t kAverageBits; + static const float kSamplesPerSecond; + static const float kCalibrationSampleTime; + static const float kDefaultVoltsPerDegreePerSecond; - explicit AnalogGyro(uint32_t channel); - virtual ~AnalogGyro() = default; - float GetAngle() const; - void Calibrate() override; - double GetRate() const; - void Reset(); + explicit AnalogGyro(uint32_t channel); + virtual ~AnalogGyro() = default; + float GetAngle() const; + void Calibrate() override; + double GetRate() const; + void Reset(); -private: - void InitAnalogGyro(int channel); + private: + void InitAnalogGyro(int channel); - SimGyro* impl; + SimGyro* impl; - std::shared_ptr m_table; + std::shared_ptr m_table; }; diff --git a/wpilibc/simulation/include/AnalogInput.h b/wpilibc/simulation/include/AnalogInput.h index 8e857b4acb..b0859257cb 100644 --- a/wpilibc/simulation/include/AnalogInput.h +++ b/wpilibc/simulation/include/AnalogInput.h @@ -7,51 +7,54 @@ #pragma once -#include "simulation/SimFloatInput.h" -#include "SensorBase.h" -#include "PIDSource.h" #include "LiveWindow/LiveWindowSendable.h" +#include "PIDSource.h" +#include "SensorBase.h" +#include "simulation/SimFloatInput.h" #include /** * Analog input class. * - * Connected to each analog channel is an averaging and oversampling engine. This engine accumulates - * the specified ( by SetAverageBits() and SetOversampleBits() ) number of samples before returning a new - * value. This is not a sliding window average. The only difference between the oversampled samples and - * the averaged samples is that the oversampled samples are simply accumulated effectively increasing the - * resolution, while the averaged samples are divided by the number of samples to retain the resolution, - * but get more stable values. + * Connected to each analog channel is an averaging and oversampling engine. + * This engine accumulates the specified ( by SetAverageBits() and + * SetOversampleBits() ) number of samples before returning a new value. This + * is not a sliding window average. The only difference between the + * oversampled samples and the averaged samples is that the oversampled samples + * are simply accumulated effectively increasing the resolution, while the + * averaged samples are divided by the number of samples to retain the + * resolution, but get more stable values. */ -class AnalogInput : public SensorBase, public PIDSource, public LiveWindowSendable -{ -public: - static const uint8_t kAccumulatorModuleNumber = 1; - static const uint32_t kAccumulatorNumChannels = 2; - static const uint32_t kAccumulatorChannels[kAccumulatorNumChannels]; +class AnalogInput : public SensorBase, + public PIDSource, + public LiveWindowSendable { + public: + static const uint8_t kAccumulatorModuleNumber = 1; + static const uint32_t kAccumulatorNumChannels = 2; + static const uint32_t kAccumulatorChannels[kAccumulatorNumChannels]; - explicit AnalogInput(uint32_t channel); - virtual ~AnalogInput() = default; + explicit AnalogInput(uint32_t channel); + virtual ~AnalogInput() = default; - float GetVoltage() const; - float GetAverageVoltage() const; + float GetVoltage() const; + float GetAverageVoltage() const; - uint32_t GetChannel() const; + uint32_t GetChannel() const; - double PIDGet() override; + double PIDGet() override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - std::shared_ptr GetTable() const override; + void UpdateTable() override; + void StartLiveWindowMode() override; + void StopLiveWindowMode() override; + std::string GetSmartDashboardType() const override; + void InitTable(std::shared_ptr subTable) override; + std::shared_ptr GetTable() const override; -private: - uint32_t m_channel; - SimFloatInput* m_impl; - int64_t m_accumulatorOffset; + private: + uint32_t m_channel; + SimFloatInput* m_impl; + int64_t m_accumulatorOffset; - std::shared_ptr m_table; + std::shared_ptr m_table; }; diff --git a/wpilibc/simulation/include/AnalogPotentiometer.h b/wpilibc/simulation/include/AnalogPotentiometer.h index 2dbcb3e9c5..70359f9248 100644 --- a/wpilibc/simulation/include/AnalogPotentiometer.h +++ b/wpilibc/simulation/include/AnalogPotentiometer.h @@ -8,8 +8,8 @@ #pragma once #include "AnalogInput.h" -#include "interfaces/Potentiometer.h" #include "LiveWindow/LiveWindowSendable.h" +#include "interfaces/Potentiometer.h" #include @@ -22,71 +22,74 @@ * @author Alex Henning */ class AnalogPotentiometer : public Potentiometer, public LiveWindowSendable { -public: - /** - * AnalogPotentiometer constructor. - * - * Use the scaling and offset values so that the output produces - * meaningful values. I.E: you have a 270 degree potentiometer and - * you want the output to be degrees with the halfway point as 0 - * degrees. The scale value is 270.0(degrees)/5.0(volts) and the - * offset is -135.0 since the halfway point after scaling is 135 - * degrees. - * - * @param channel The analog channel this potentiometer is plugged into. - * @param scale The scaling to multiply the voltage by to get a meaningful unit. - * @param offset The offset to add to the scaled value for controlling the zero value - */ - AnalogPotentiometer(int channel, double scale = 1.0, double offset = 0.0); + public: + /** + * AnalogPotentiometer constructor. + * + * Use the scaling and offset values so that the output produces meaningful + * values. I.E: you have a 270 degree potentiometer and you want the output + * to be degrees with the halfway point as 0 degrees. The scale value is + * 270.0(degrees)/5.0(volts) and the offset is -135.0 since the halfway point + * after scaling is 135 degrees. + * + * @param channel The analog channel this potentiometer is plugged into. + * @param scale The scaling to multiply the voltage by to get a meaningful + * unit. + * @param offset The offset to add to the scaled value for controlling the + * zero value + */ + AnalogPotentiometer(int channel, double scale = 1.0, double offset = 0.0); - AnalogPotentiometer(AnalogInput *input, double scale = 1.0, double offset = 0.0); + AnalogPotentiometer(AnalogInput* input, double scale = 1.0, + double offset = 0.0); - AnalogPotentiometer(AnalogInput &input, double scale = 1.0, double offset = 0.0); + AnalogPotentiometer(AnalogInput& input, double scale = 1.0, + double offset = 0.0); - virtual ~AnalogPotentiometer(); + virtual ~AnalogPotentiometer(); - /** - * Get the current reading of the potentiomere. - * - * @return The current position of the potentiometer. - */ - virtual double Get() const; + /** + * Get the current reading of the potentiomere. + * + * @return The current position of the potentiometer. + */ + virtual double Get() const; + /** + * Implement the PIDSource interface. + * + * @return The current reading. + */ + virtual double PIDGet() override; - /** - * Implement the PIDSource interface. - * - * @return The current reading. - */ - virtual double PIDGet() override; + /* + * Live Window code, only does anything if live window is activated. + */ + virtual std::string GetSmartDashboardType() const override; + virtual void InitTable(std::shared_ptr subtable) override; + virtual void UpdateTable() override; + virtual std::shared_ptr GetTable() const override; + /** + * AnalogPotentiometers don't have to do anything special when entering the + * LiveWindow. + */ + virtual void StartLiveWindowMode() override {} - /* - * Live Window code, only does anything if live window is activated. - */ - virtual std::string GetSmartDashboardType() const override; - virtual void InitTable(std::shared_ptr subtable) override; - virtual void UpdateTable() override; - virtual std::shared_ptr GetTable() const override; + /** + * AnalogPotentiometers don't have to do anything special when exiting the + * LiveWindow. + */ + virtual void StopLiveWindowMode() override {} - /** - * AnalogPotentiometers don't have to do anything special when entering the LiveWindow. - */ - virtual void StartLiveWindowMode() override {} + private: + double m_scale, m_offset; + AnalogInput* m_analog_input; + std::shared_ptr m_table; + bool m_init_analog_input; - /** - * AnalogPotentiometers don't have to do anything special when exiting the LiveWindow. - */ - virtual void StopLiveWindowMode() override {} - -private: - double m_scale, m_offset; - AnalogInput* m_analog_input; - std::shared_ptr m_table; - bool m_init_analog_input; - - /** - * Common initialization code called by all constructors. - */ - void initPot(AnalogInput *input, double scale, double offset); + /** + * Common initialization code called by all constructors. + */ + void initPot(AnalogInput* input, double scale, double offset); }; diff --git a/wpilibc/simulation/include/Counter.h b/wpilibc/simulation/include/Counter.h index 2accacf4a2..737daccb5a 100644 --- a/wpilibc/simulation/include/Counter.h +++ b/wpilibc/simulation/include/Counter.h @@ -7,89 +7,94 @@ #pragma once -#include "HAL/HAL.hpp" #include "CounterBase.h" -#include "SensorBase.h" +#include "HAL/HAL.hpp" #include "LiveWindow/LiveWindowSendable.h" +#include "SensorBase.h" #include /** * Class for counting the number of ticks on a digital input channel. - * This is a general purpose class for counting repetitive events. It can return the number - * of counts, the period of the most recent cycle, and detect when the signal being counted - * has stopped by supplying a maximum cycle time. + * + * This is a general purpose class for counting repetitive events. It can return + * the number of counts, the period of the most recent cycle, and detect when + * the signal being counted has stopped by supplying a maximum cycle time. * * All counters will immediately start counting - Reset() them if you need them * to be zeroed before use. */ -class Counter : public SensorBase, public CounterBase, public LiveWindowSendable -{ -public: +class Counter : public SensorBase, + public CounterBase, + public LiveWindowSendable { + public: + explicit Counter(Mode mode = kTwoPulse); + explicit Counter(uint32_t channel); + // TODO: [Not Supported] explicit Counter(DigitalSource *source); + // TODO: [Not Supported] explicit Counter(DigitalSource &source); + // TODO: [Not Supported] explicit Counter(AnalogTrigger *source); + // TODO: [Not Supported] explicit Counter(AnalogTrigger &source); + // TODO: [Not Supported] Counter(EncodingType encodingType, DigitalSource + // *upSource, DigitalSource *downSource, bool inverted); + virtual ~Counter(); - explicit Counter(Mode mode = kTwoPulse); - explicit Counter(uint32_t channel); - // TODO: [Not Supported] explicit Counter(DigitalSource *source); - // TODO: [Not Supported] explicit Counter(DigitalSource &source); - // TODO: [Not Supported] explicit Counter(AnalogTrigger *source); - // TODO: [Not Supported] explicit Counter(AnalogTrigger &source); - // TODO: [Not Supported] Counter(EncodingType encodingType, DigitalSource *upSource, DigitalSource *downSource, bool inverted); - virtual ~Counter(); + void SetUpSource(uint32_t channel); + // TODO: [Not Supported] void SetUpSource(AnalogTrigger *analogTrigger, + // AnalogTriggerType triggerType); + // TODO: [Not Supported] void SetUpSource(AnalogTrigger &analogTrigger, + // AnalogTriggerType triggerType); + // TODO: [Not Supported] void SetUpSource(DigitalSource *source); + // TODO: [Not Supported] void SetUpSource(DigitalSource &source); + void SetUpSourceEdge(bool risingEdge, bool fallingEdge); + void ClearUpSource(); - void SetUpSource(uint32_t channel); - // TODO: [Not Supported] void SetUpSource(AnalogTrigger *analogTrigger, AnalogTriggerType triggerType); - // TODO: [Not Supported] void SetUpSource(AnalogTrigger &analogTrigger, AnalogTriggerType triggerType); - // TODO: [Not Supported] void SetUpSource(DigitalSource *source); - // TODO: [Not Supported] void SetUpSource(DigitalSource &source); - void SetUpSourceEdge(bool risingEdge, bool fallingEdge); - void ClearUpSource(); + void SetDownSource(uint32_t channel); + // TODO: [Not Supported] void SetDownSource(AnalogTrigger *analogTrigger, + // AnalogTriggerType triggerType); + // TODO: [Not Supported] void SetDownSource(AnalogTrigger &analogTrigger, + // AnalogTriggerType triggerType); + // TODO: [Not Supported] void SetDownSource(DigitalSource *source); + // TODO: [Not Supported] void SetDownSource(DigitalSource &source); + void SetDownSourceEdge(bool risingEdge, bool fallingEdge); + void ClearDownSource(); - void SetDownSource(uint32_t channel); - // TODO: [Not Supported] void SetDownSource(AnalogTrigger *analogTrigger, AnalogTriggerType triggerType); - // TODO: [Not Supported] void SetDownSource(AnalogTrigger &analogTrigger, AnalogTriggerType triggerType); - // TODO: [Not Supported] void SetDownSource(DigitalSource *source); - // TODO: [Not Supported] void SetDownSource(DigitalSource &source); - void SetDownSourceEdge(bool risingEdge, bool fallingEdge); - void ClearDownSource(); + void SetUpDownCounterMode(); + void SetExternalDirectionMode(); + void SetSemiPeriodMode(bool highSemiPeriod); + void SetPulseLengthMode(float threshold); - void SetUpDownCounterMode(); - void SetExternalDirectionMode(); - void SetSemiPeriodMode(bool highSemiPeriod); - void SetPulseLengthMode(float threshold); + void SetReverseDirection(bool reverseDirection); - void SetReverseDirection(bool reverseDirection); + // CounterBase interface + int32_t Get() const override; + void Reset() override; + double GetPeriod() const override; + void SetMaxPeriod(double maxPeriod) override; + void SetUpdateWhenEmpty(bool enabled); + bool GetStopped() const override; + bool GetDirection() const override; - // CounterBase interface - int32_t Get() const override; - void Reset() override; - double GetPeriod() const override; - void SetMaxPeriod(double maxPeriod) override; - void SetUpdateWhenEmpty(bool enabled); - bool GetStopped() const override; - bool GetDirection() const override; + void SetSamplesToAverage(int samplesToAverage); + int GetSamplesToAverage() const; + uint32_t GetFPGAIndex() const { return m_index; } - void SetSamplesToAverage(int samplesToAverage); - int GetSamplesToAverage() const; - uint32_t GetFPGAIndex() const - { - return m_index; - } + void UpdateTable() override; + void StartLiveWindowMode() override; + void StopLiveWindowMode() override; + virtual std::string GetSmartDashboardType() const override; + void InitTable(std::shared_ptr subTable) override; + std::shared_ptr GetTable() const override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - virtual std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - std::shared_ptr GetTable() const override; -protected: - // TODO: [Not Supported] DigitalSource *m_upSource; ///< What makes the counter count up. - // TODO: [Not Supported] DigitalSource *m_downSource; ///< What makes the counter count down. - void* m_counter; ///< The FPGA counter object. -private: + protected: + // What makes the counter count up. + // TODO: [Not Supported] DigitalSource *m_upSource; + // What makes the counter count down. + // TODO: [Not Supported] DigitalSource *m_downSource; + void* m_counter; ///< The FPGA counter object. + private: + bool m_allocatedUpSource; ///< Was the upSource allocated locally? + bool m_allocatedDownSource; ///< Was the downSource allocated locally? + uint32_t m_index; ///< The index of this counter. - bool m_allocatedUpSource; ///< Was the upSource allocated locally? - bool m_allocatedDownSource; ///< Was the downSource allocated locally? - uint32_t m_index; ///< The index of this counter. - - std::shared_ptr m_table; + std::shared_ptr m_table; }; diff --git a/wpilibc/simulation/include/CounterBase.h b/wpilibc/simulation/include/CounterBase.h index 683b681c43..09a71356ad 100644 --- a/wpilibc/simulation/include/CounterBase.h +++ b/wpilibc/simulation/include/CounterBase.h @@ -9,27 +9,22 @@ /** * Interface for counting the number of ticks on a digital input channel. - * Encoders, Gear tooth sensors, and counters should all subclass this so it can be used to - * build more advanced classes for control and driving. + * + * Encoders, Gear tooth sensors, and counters should all subclass this so it can + * be used to build more advanced classes for control and driving. * * All counters will immediately start counting - Reset() them if you need them * to be zeroed before use. */ -class CounterBase -{ -public: - enum EncodingType - { - k1X, - k2X, - k4X - }; +class CounterBase { + public: + enum EncodingType { k1X, k2X, k4X }; - virtual ~CounterBase() = default; - virtual int32_t Get() const = 0; - virtual void Reset() = 0; - virtual double GetPeriod() const = 0; - virtual void SetMaxPeriod(double maxPeriod) = 0; - virtual bool GetStopped() const = 0; - virtual bool GetDirection() const = 0; + virtual ~CounterBase() = default; + virtual int32_t Get() const = 0; + virtual void Reset() = 0; + virtual double GetPeriod() const = 0; + virtual void SetMaxPeriod(double maxPeriod) = 0; + virtual bool GetStopped() const = 0; + virtual bool GetDirection() const = 0; }; diff --git a/wpilibc/simulation/include/DigitalInput.h b/wpilibc/simulation/include/DigitalInput.h index 92c4742a8e..554400628d 100644 --- a/wpilibc/simulation/include/DigitalInput.h +++ b/wpilibc/simulation/include/DigitalInput.h @@ -7,36 +7,38 @@ #pragma once -#include "simulation/SimDigitalInput.h" #include "LiveWindow/LiveWindowSendable.h" +#include "simulation/SimDigitalInput.h" #include /** * Class to read a digital input. - * This class will read digital inputs and return the current value on the channel. Other devices - * such as encoders, gear tooth sensors, etc. that are implemented elsewhere will automatically - * allocate digital inputs and outputs as required. This class is only for devices like switches - * etc. that aren't implemented anywhere else. + * + * This class will read digital inputs and return the current value on the + * channel. Other devices such as encoders, gear tooth sensors, etc. that are + * implemented elsewhere will automatically allocate digital inputs and outputs + * as required. This class is only for devices like switches etc. that aren't + * implemented anywhere else. */ class DigitalInput : public LiveWindowSendable { -public: - explicit DigitalInput(uint32_t channel); - virtual ~DigitalInput() = default; - uint32_t Get() const; - uint32_t GetChannel() const; + public: + explicit DigitalInput(uint32_t channel); + virtual ~DigitalInput() = default; + uint32_t Get() const; + uint32_t GetChannel() const; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - std::shared_ptr GetTable() const override; + void UpdateTable() override; + void StartLiveWindowMode() override; + void StopLiveWindowMode() override; + std::string GetSmartDashboardType() const override; + void InitTable(std::shared_ptr subTable) override; + std::shared_ptr GetTable() const override; -private: - uint32_t m_channel; - bool m_lastValue; - SimDigitalInput *m_impl; + private: + uint32_t m_channel; + bool m_lastValue; + SimDigitalInput* m_impl; - std::shared_ptr m_table; + std::shared_ptr m_table; }; diff --git a/wpilibc/simulation/include/DoubleSolenoid.h b/wpilibc/simulation/include/DoubleSolenoid.h index ca38f1d159..a2685610df 100644 --- a/wpilibc/simulation/include/DoubleSolenoid.h +++ b/wpilibc/simulation/include/DoubleSolenoid.h @@ -7,8 +7,8 @@ #pragma once -#include "simulation/SimContinuousOutput.h" #include "LiveWindow/LiveWindowSendable.h" +#include "simulation/SimContinuousOutput.h" #include "tables/ITableListener.h" #include @@ -20,35 +20,30 @@ * The DoubleSolenoid class is typically used for pneumatics solenoids that * have two positions controlled by two separate channels. */ -class DoubleSolenoid : public LiveWindowSendable, public ITableListener -{ -public: - enum Value - { - kOff, - kForward, - kReverse - }; +class DoubleSolenoid : public LiveWindowSendable, public ITableListener { + public: + enum Value { kOff, kForward, kReverse }; - explicit DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel); - DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, uint32_t reverseChannel); - virtual ~DoubleSolenoid(); - virtual void Set(Value value); - virtual Value Get() const; + explicit DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel); + DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, + uint32_t reverseChannel); + virtual ~DoubleSolenoid(); + virtual void Set(Value value); + virtual Value Get() const; - void ValueChanged(ITable* source, llvm::StringRef key, + void ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - std::shared_ptr GetTable() const override; + void UpdateTable() override; + void StartLiveWindowMode() override; + void StopLiveWindowMode() override; + std::string GetSmartDashboardType() const override; + void InitTable(std::shared_ptr subTable) override; + std::shared_ptr GetTable() const override; -private: - SimContinuousOutput* m_impl; - Value m_value; - bool m_reversed; + private: + SimContinuousOutput* m_impl; + Value m_value; + bool m_reversed; - std::shared_ptr m_table; + std::shared_ptr m_table; }; diff --git a/wpilibc/simulation/include/DriverStation.h b/wpilibc/simulation/include/DriverStation.h index b96f1a7e9e..00f71436a0 100644 --- a/wpilibc/simulation/include/DriverStation.h +++ b/wpilibc/simulation/include/DriverStation.h @@ -10,16 +10,16 @@ #include "simulation/gz_msgs/msgs.h" #ifdef _WIN32 - // Ensure that Winsock2.h is included before Windows.h, which can get - // pulled in by anybody (e.g., Boost). - #include +// Ensure that Winsock2.h is included before Windows.h, which can get +// pulled in by anybody (e.g., Boost). +#include #endif -#include -#include "SensorBase.h" -#include "RobotState.h" -#include #include +#include +#include +#include "RobotState.h" +#include "SensorBase.h" struct HALCommonControlData; class AnalogInput; @@ -27,120 +27,115 @@ class AnalogInput; using namespace gazebo; /** - * Provide access to the network communication data to / from the Driver Station. + * Provide access to the network communication data to / from the Driver + * Station. */ -class DriverStation : public SensorBase, public RobotStateInterface -{ -public: - enum Alliance - { - kRed, - kBlue, - kInvalid - }; +class DriverStation : public SensorBase, public RobotStateInterface { + public: + enum Alliance { kRed, kBlue, kInvalid }; - virtual ~DriverStation() = default; - static DriverStation &GetInstance(); - static void ReportError(std::string error); - static void ReportWarning(std::string error); - static void ReportError(bool is_error, int32_t code, const std::string& error, const std::string& location, const std::string& stack); + virtual ~DriverStation() = default; + static DriverStation& GetInstance(); + static void ReportError(std::string error); + static void ReportWarning(std::string error); + static void ReportError(bool is_error, int32_t code, const std::string& error, + const std::string& location, + const std::string& stack); - static const uint32_t kBatteryChannel = 7; - static const uint32_t kJoystickPorts = 4; - static const uint32_t kJoystickAxes = 6; + static const uint32_t kBatteryChannel = 7; + static const uint32_t kJoystickPorts = 4; + static const uint32_t kJoystickAxes = 6; - float GetStickAxis(uint32_t stick, uint32_t axis); - bool GetStickButton(uint32_t stick, uint32_t button); - short GetStickButtons(uint32_t stick); + float GetStickAxis(uint32_t stick, uint32_t axis); + bool GetStickButton(uint32_t stick, uint32_t button); + short GetStickButtons(uint32_t stick); - float GetAnalogIn(uint32_t channel); - bool GetDigitalIn(uint32_t channel); - void SetDigitalOut(uint32_t channel, bool value); - bool GetDigitalOut(uint32_t channel); + float GetAnalogIn(uint32_t channel); + bool GetDigitalIn(uint32_t channel); + void SetDigitalOut(uint32_t channel, bool value); + bool GetDigitalOut(uint32_t channel); - bool IsEnabled() const; - bool IsDisabled() const; - bool IsAutonomous() const; - bool IsOperatorControl() const; - bool IsTest() const; - bool IsFMSAttached() const; + bool IsEnabled() const; + bool IsDisabled() const; + bool IsAutonomous() const; + bool IsOperatorControl() const; + bool IsTest() const; + bool IsFMSAttached() const; - uint32_t GetPacketNumber() const; - Alliance GetAlliance() const; - uint32_t GetLocation() const; - void WaitForData(); - double GetMatchTime() const; - float GetBatteryVoltage() const; - uint16_t GetTeamNumber() const; + uint32_t GetPacketNumber() const; + Alliance GetAlliance() const; + uint32_t GetLocation() const; + void WaitForData(); + double GetMatchTime() const; + float GetBatteryVoltage() const; + uint16_t GetTeamNumber() const; + void IncrementUpdateNumber() { m_updateNumber++; } + /** + * Only to be used to tell the Driver Station what code you claim to be + * executing for diagnostic purposes only. + * + * @param entering If true, starting disabled code; if false, leaving + * disabled code + */ + void InDisabled(bool entering) { m_userInDisabled = entering; } + /** + * Only to be used to tell the Driver Station what code you claim to be + * executing for diagnostic purposes only. + * + * @param entering If true, starting autonomous code; if false, leaving + * autonomous code + */ + void InAutonomous(bool entering) { m_userInAutonomous = entering; } + /** + * Only to be used to tell the Driver Station what code you claim to be + * executing for diagnostic purposes only. + * + * @param entering If true, starting teleop code; if false, leaving teleop + * code + */ + void InOperatorControl(bool entering) { m_userInTeleop = entering; } + /** + * Only to be used to tell the Driver Station what code you claim to be + * executing for diagnostic purposes only. + * + * @param entering If true, starting test code; if false, leaving test code + */ + void InTest(bool entering) { m_userInTest = entering; } - void IncrementUpdateNumber() - { - m_updateNumber++; - } + protected: + DriverStation(); - /** Only to be used to tell the Driver Station what code you claim to be executing - * for diagnostic purposes only - * @param entering If true, starting disabled code; if false, leaving disabled code */ - void InDisabled(bool entering) - { - m_userInDisabled = entering; - } - /** Only to be used to tell the Driver Station what code you claim to be executing - * for diagnostic purposes only - * @param entering If true, starting autonomous code; if false, leaving autonomous code */ - void InAutonomous(bool entering) - { - m_userInAutonomous = entering; - } - /** Only to be used to tell the Driver Station what code you claim to be executing - * for diagnostic purposes only - * @param entering If true, starting teleop code; if false, leaving teleop code */ - void InOperatorControl(bool entering) - { - m_userInTeleop = entering; - } - /** Only to be used to tell the Driver Station what code you claim to be executing - * for diagnostic purposes only - * @param entering If true, starting test code; if false, leaving test code */ - void InTest(bool entering) - { - m_userInTest = entering; - } + private: + static void InitTask(DriverStation* ds); + static DriverStation* m_instance; + static uint8_t m_updateNumber; + ///< TODO: Get rid of this and use the semaphore signaling + static const float kUpdatePeriod; -protected: - DriverStation(); + void stateCallback(const msgs::ConstDriverStationPtr& msg); + void joystickCallback(const msgs::ConstFRCJoystickPtr& msg, int i); + void joystickCallback0(const msgs::ConstFRCJoystickPtr& msg); + void joystickCallback1(const msgs::ConstFRCJoystickPtr& msg); + void joystickCallback2(const msgs::ConstFRCJoystickPtr& msg); + void joystickCallback3(const msgs::ConstFRCJoystickPtr& msg); + void joystickCallback4(const msgs::ConstFRCJoystickPtr& msg); + void joystickCallback5(const msgs::ConstFRCJoystickPtr& msg); -private: - static void InitTask(DriverStation *ds); - static DriverStation *m_instance; - static uint8_t m_updateNumber; - ///< TODO: Get rid of this and use the semaphore signaling - static const float kUpdatePeriod; - - void stateCallback(const msgs::ConstDriverStationPtr &msg); - void joystickCallback(const msgs::ConstFRCJoystickPtr &msg, int i); - void joystickCallback0(const msgs::ConstFRCJoystickPtr &msg); - void joystickCallback1(const msgs::ConstFRCJoystickPtr &msg); - void joystickCallback2(const msgs::ConstFRCJoystickPtr &msg); - void joystickCallback3(const msgs::ConstFRCJoystickPtr &msg); - void joystickCallback4(const msgs::ConstFRCJoystickPtr &msg); - void joystickCallback5(const msgs::ConstFRCJoystickPtr &msg); - - uint8_t m_digitalOut = 0; - std::condition_variable m_waitForDataCond; - std::mutex m_waitForDataMutex; + uint8_t m_digitalOut = 0; + std::condition_variable m_waitForDataCond; + std::mutex m_waitForDataMutex; mutable std::recursive_mutex m_stateMutex; - std::recursive_mutex m_joystickMutex; - double m_approxMatchTimeOffset = 0; - bool m_userInDisabled = false; - bool m_userInAutonomous = false; - bool m_userInTeleop = false; - bool m_userInTest = false; + std::recursive_mutex m_joystickMutex; + double m_approxMatchTimeOffset = 0; + bool m_userInDisabled = false; + bool m_userInAutonomous = false; + bool m_userInTeleop = false; + bool m_userInTest = false; - transport::SubscriberPtr stateSub; - transport::SubscriberPtr joysticksSub[6]; - msgs::DriverStationPtr state; - msgs::FRCJoystickPtr joysticks[6]; + transport::SubscriberPtr stateSub; + transport::SubscriberPtr joysticksSub[6]; + msgs::DriverStationPtr state; + msgs::FRCJoystickPtr joysticks[6]; }; diff --git a/wpilibc/simulation/include/Encoder.h b/wpilibc/simulation/include/Encoder.h index 22b8657392..9e338b8649 100644 --- a/wpilibc/simulation/include/Encoder.h +++ b/wpilibc/simulation/include/Encoder.h @@ -7,83 +7,91 @@ #pragma once -#include "simulation/SimEncoder.h" -#include "CounterBase.h" -#include "SensorBase.h" -#include "Counter.h" -#include "PIDSource.h" -#include "LiveWindow/LiveWindowSendable.h" - +#include #include +#include "CounterBase.h" +#include "LiveWindow/LiveWindowSendable.h" +#include "PIDSource.h" +#include "SensorBase.h" +#include "simulation/SimEncoder.h" + /** * Class to read quad encoders. - * Quadrature encoders are devices that count shaft rotation and can sense direction. The output of - * the QuadEncoder class is an integer that can count either up or down, and can go negative for - * reverse direction counting. When creating QuadEncoders, a direction is supplied that changes the - * sense of the output to make code more readable if the encoder is mounted such that forward movement - * generates negative values. Quadrature encoders have two digital outputs, an A Channel and a B Channel - * that are out of phase with each other to allow the FPGA to do direction sensing. + * + * Quadrature encoders are devices that count shaft rotation and can sense + * direction. The output of the QuadEncoder class is an integer that can count + * either up or down, and can go negative for reverse direction counting. When + * creating QuadEncoders, a direction is supplied that changes the sense of the + * output to make code more readable if the encoder is mounted such that + * forward movement generates negative values. Quadrature encoders have two + * digital outputs, an A Channel and a B Channel that are out of phase with + * each other to allow the FPGA to do direction sensing. * * All encoders will immediately start counting - Reset() them if you need them * to be zeroed before use. */ -class Encoder : public SensorBase, public CounterBase, public PIDSource, public LiveWindowSendable -{ -public: +class Encoder : public SensorBase, + public CounterBase, + public PIDSource, + public LiveWindowSendable { + public: + Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection = false, + EncodingType encodingType = k4X); + // TODO: [Not Supported] Encoder(DigitalSource *aSource, DigitalSource + // *bSource, bool reverseDirection=false, EncodingType encodingType = k4X); + // TODO: [Not Supported] Encoder(DigitalSource &aSource, DigitalSource + // &bSource, bool reverseDirection=false, EncodingType encodingType = k4X); + virtual ~Encoder() = default; - Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection = false, - EncodingType encodingType = k4X); - // TODO: [Not Supported] Encoder(DigitalSource *aSource, DigitalSource *bSource, bool reverseDirection=false, EncodingType encodingType = k4X); - // TODO: [Not Supported] Encoder(DigitalSource &aSource, DigitalSource &bSource, bool reverseDirection=false, EncodingType encodingType = k4X); - virtual ~Encoder() = default; + // CounterBase interface + int32_t Get() const override; + int32_t GetRaw() const; + int32_t GetEncodingScale() const; + void Reset() override; + double GetPeriod() const override; + void SetMaxPeriod(double maxPeriod) override; + bool GetStopped() const override; + bool GetDirection() const override; - // CounterBase interface - int32_t Get() const override; - int32_t GetRaw() const; - int32_t GetEncodingScale() const; - void Reset() override; - double GetPeriod() const override; - void SetMaxPeriod(double maxPeriod) override; - bool GetStopped() const override; - bool GetDirection() const override; + double GetDistance() const; + double GetRate() const; + void SetMinRate(double minRate); + void SetDistancePerPulse(double distancePerPulse); + void SetReverseDirection(bool reverseDirection); + void SetSamplesToAverage(int samplesToAverage); + int GetSamplesToAverage() const; + void SetPIDSourceType(PIDSourceType pidSource); + double PIDGet() override; - double GetDistance() const; - double GetRate() const; - void SetMinRate(double minRate); - void SetDistancePerPulse(double distancePerPulse); - void SetReverseDirection(bool reverseDirection); - void SetSamplesToAverage(int samplesToAverage); - int GetSamplesToAverage() const; - void SetPIDSourceType(PIDSourceType pidSource); - double PIDGet() override; + void UpdateTable() override; + void StartLiveWindowMode() override; + void StopLiveWindowMode() override; + std::string GetSmartDashboardType() const override; + void InitTable(std::shared_ptr subTable) override; + std::shared_ptr GetTable() const override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - std::shared_ptr GetTable() const override; + int32_t FPGAEncoderIndex() const { return 0; } - int32_t FPGAEncoderIndex() const - { - return 0; - } + private: + void InitEncoder(int channelA, int channelB, bool _reverseDirection, + EncodingType encodingType); + double DecodingScaleFactor() const; -private: - void InitEncoder(int channelA, int channelB, bool _reverseDirection, EncodingType encodingType); - double DecodingScaleFactor() const; + // the A phase of the quad encoder + // TODO: [Not Supported] DigitalSource *m_aSource; + // the B phase of the quad encoder + // TODO: [Not Supported] DigitalSource *m_bSource; + // was the A source allocated locally? + // TODO: [Not Supported] bool m_allocatedASource; + // was the B source allocated locally? + // TODO: [Not Supported] bool m_allocatedBSource; + int channelA, channelB; + double m_distancePerPulse; // distance of travel for each encoder tick + EncodingType m_encodingType; // Encoding type + int32_t m_encodingScale; // 1x, 2x, or 4x, per the encodingType + bool m_reverseDirection; + SimEncoder* impl; - // TODO: [Not Supported] DigitalSource *m_aSource; // the A phase of the quad encoder - // TODO: [Not Supported] DigitalSource *m_bSource; // the B phase of the quad encoder - // TODO: [Not Supported] bool m_allocatedASource; // was the A source allocated locally? - // TODO: [Not Supported] bool m_allocatedBSource; // was the B source allocated locally? - int channelA, channelB; - double m_distancePerPulse; // distance of travel for each encoder tick - EncodingType m_encodingType; // Encoding type - int32_t m_encodingScale; // 1x, 2x, or 4x, per the encodingType - bool m_reverseDirection; - SimEncoder* impl; - - std::shared_ptr m_table; + std::shared_ptr m_table; }; diff --git a/wpilibc/simulation/include/IterativeRobot.h b/wpilibc/simulation/include/IterativeRobot.h index 8a4cd4dbca..0d8ef1c783 100644 --- a/wpilibc/simulation/include/IterativeRobot.h +++ b/wpilibc/simulation/include/IterativeRobot.h @@ -7,30 +7,37 @@ #pragma once -#include "Timer.h" #include "RobotBase.h" +#include "Timer.h" /** - * IterativeRobot implements a specific type of Robot Program framework, extending the RobotBase class. + * IterativeRobot implements a specific type of Robot Program framework, + * extending the RobotBase class. * - * The IterativeRobot class is intended to be subclassed by a user creating a robot program. + * The IterativeRobot class is intended to be subclassed by a user creating a + * robot program. * - * This class is intended to implement the "old style" default code, by providing - * the following functions which are called by the main loop, StartCompetition(), at the appropriate times: + * This class is intended to implement the "old style" default code, by + * providing the following functions which are called by the main loop, + * StartCompetition(), at the appropriate times: * * RobotInit() -- provide for initialization at robot power-on * * Init() functions -- each of the following functions is called once when the * appropriate mode is entered: * - DisabledInit() -- called only when first disabled - * - AutonomousInit() -- called each and every time autonomous is entered from another mode - * - TeleopInit() -- called each and every time teleop is entered from another mode - * - TestInit() -- called each and every time test is entered from another mode + * - AutonomousInit() -- called each and every time autonomous is entered from + * another mode + * - TeleopInit() -- called each and every time teleop is entered from + * another mode + * - TestInit() -- called each and every time test is entered from + * another mode * * Periodic() functions -- each of these functions is called iteratively at the - * appropriate periodic rate (aka the "slow loop"). The default period of - * the iterative robot is synced to the driver station control packets, - * giving a periodic frequency of about 50Hz (50 times per second). + * appropriate periodic rate (aka the "slow loop"). The + * default period of the iterative robot is synced to + * the driver station control packets, giving a periodic + * frequency of about 50Hz (50 times per second). * - DisabledPeriodic() * - AutonomousPeriodic() * - TeleopPeriodic() @@ -38,44 +45,43 @@ * */ -class IterativeRobot : public RobotBase -{ -public: - /* - * The default period for the periodic function calls (seconds) - * Setting the period to 0.0 will cause the periodic functions to follow - * the Driver Station packet rate of about 50Hz. - */ - static const double kDefaultPeriod; +class IterativeRobot : public RobotBase { + public: + /* + * The default period for the periodic function calls (seconds). + * Setting the period to 0.0 will cause the periodic functions to follow + * the Driver Station packet rate of about 50Hz. + */ + static const double kDefaultPeriod; - virtual void StartCompetition(); + virtual void StartCompetition(); - virtual void RobotInit(); - virtual void DisabledInit(); - virtual void AutonomousInit(); - virtual void TeleopInit(); - virtual void TestInit(); + virtual void RobotInit(); + virtual void DisabledInit(); + virtual void AutonomousInit(); + virtual void TeleopInit(); + virtual void TestInit(); - virtual void DisabledPeriodic(); - virtual void AutonomousPeriodic(); - virtual void TeleopPeriodic(); - virtual void TestPeriodic(); + virtual void DisabledPeriodic(); + virtual void AutonomousPeriodic(); + virtual void TeleopPeriodic(); + virtual void TestPeriodic(); - void SetPeriod(double period); - double GetPeriod(); - double GetLoopsPerSec(); + void SetPeriod(double period); + double GetPeriod(); + double GetLoopsPerSec(); -protected: - virtual ~IterativeRobot() = default; - IterativeRobot() = default; + protected: + virtual ~IterativeRobot() = default; + IterativeRobot() = default; -private: - bool NextPeriodReady(); + private: + bool NextPeriodReady(); - bool m_disabledInitialized = false; - bool m_autonomousInitialized = false; - bool m_teleopInitialized = false; - bool m_testInitialized = false; - double m_period = kDefaultPeriod; - Timer m_mainLoopTimer; + bool m_disabledInitialized = false; + bool m_autonomousInitialized = false; + bool m_teleopInitialized = false; + bool m_testInitialized = false; + double m_period = kDefaultPeriod; + Timer m_mainLoopTimer; }; diff --git a/wpilibc/simulation/include/Jaguar.h b/wpilibc/simulation/include/Jaguar.h index 27325af9d1..c71b81890c 100644 --- a/wpilibc/simulation/include/Jaguar.h +++ b/wpilibc/simulation/include/Jaguar.h @@ -7,21 +7,20 @@ #pragma once +#include "PIDOutput.h" #include "SafePWM.h" #include "SpeedController.h" -#include "PIDOutput.h" /** - * Luminary Micro Jaguar Speed Control + * Luminary Micro Jaguar Speed Control. */ -class Jaguar : public SafePWM, public SpeedController -{ -public: - explicit Jaguar(uint32_t channel); - virtual ~Jaguar() = default; - virtual void Set(float value, uint8_t syncGroup = 0); - virtual float Get() const; - virtual void Disable(); +class Jaguar : public SafePWM, public SpeedController { + public: + explicit Jaguar(uint32_t channel); + virtual ~Jaguar() = default; + virtual void Set(float value, uint8_t syncGroup = 0); + virtual float Get() const; + virtual void Disable(); - virtual void PIDWrite(float output) override; + virtual void PIDWrite(float output) override; }; diff --git a/wpilibc/simulation/include/Joystick.h b/wpilibc/simulation/include/Joystick.h index e4e46da77a..c04ddbb82b 100644 --- a/wpilibc/simulation/include/Joystick.h +++ b/wpilibc/simulation/include/Joystick.h @@ -9,71 +9,73 @@ #define JOYSTICK_H_ #include -#include "GenericHID.h" #include "ErrorBase.h" +#include "GenericHID.h" class DriverStation; /** * Handle input from standard Joysticks connected to the Driver Station. - * This class handles standard input that comes from the Driver Station. Each time a value is requested - * the most recent value is returned. There is a single class instance for each joystick and the mapping - * of ports to hardware buttons depends on the code in the driver station. + * + * This class handles standard input that comes from the Driver Station. Each + * time a value is requested the most recent value is returned. There is a + * single class instance for each joystick and the mapping of ports to hardware + * buttons depends on the code in the driver station. */ -class Joystick : public GenericHID, public ErrorBase -{ -public: - static const uint32_t kDefaultXAxis = 1; - static const uint32_t kDefaultYAxis = 2; - static const uint32_t kDefaultZAxis = 3; - static const uint32_t kDefaultTwistAxis = 4; - static const uint32_t kDefaultThrottleAxis = 3; - typedef enum - { - kXAxis, kYAxis, kZAxis, kTwistAxis, kThrottleAxis, kNumAxisTypes - } AxisType; - static const uint32_t kDefaultTriggerButton = 1; - static const uint32_t kDefaultTopButton = 2; - typedef enum - { - kTriggerButton, kTopButton, kNumButtonTypes - } ButtonType; +class Joystick : public GenericHID, public ErrorBase { + public: + static const uint32_t kDefaultXAxis = 1; + static const uint32_t kDefaultYAxis = 2; + static const uint32_t kDefaultZAxis = 3; + static const uint32_t kDefaultTwistAxis = 4; + static const uint32_t kDefaultThrottleAxis = 3; + typedef enum { + kXAxis, + kYAxis, + kZAxis, + kTwistAxis, + kThrottleAxis, + kNumAxisTypes + } AxisType; + static const uint32_t kDefaultTriggerButton = 1; + static const uint32_t kDefaultTopButton = 2; + typedef enum { kTriggerButton, kTopButton, kNumButtonTypes } ButtonType; - explicit Joystick(uint32_t port); - Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes); - virtual ~Joystick() = default; + explicit Joystick(uint32_t port); + Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes); + virtual ~Joystick() = default; - Joystick(const Joystick&) = delete; - Joystick& operator=(const Joystick&) = delete; + Joystick(const Joystick&) = delete; + Joystick& operator=(const Joystick&) = delete; - uint32_t GetAxisChannel(AxisType axis); - void SetAxisChannel(AxisType axis, uint32_t channel); + uint32_t GetAxisChannel(AxisType axis); + void SetAxisChannel(AxisType axis, uint32_t channel); - virtual float GetX(JoystickHand hand = kRightHand) const override; - virtual float GetY(JoystickHand hand = kRightHand) const override; - virtual float GetZ() const override; - virtual float GetTwist() const override; - virtual float GetThrottle() const override; - virtual float GetAxis(AxisType axis) const; - float GetRawAxis(uint32_t axis) const override; + virtual float GetX(JoystickHand hand = kRightHand) const override; + virtual float GetY(JoystickHand hand = kRightHand) const override; + virtual float GetZ() const override; + virtual float GetTwist() const override; + virtual float GetThrottle() const override; + virtual float GetAxis(AxisType axis) const; + float GetRawAxis(uint32_t axis) const override; - virtual bool GetTrigger(JoystickHand hand = kRightHand) const override; - virtual bool GetTop(JoystickHand hand = kRightHand) const override; - virtual bool GetBumper(JoystickHand hand = kRightHand) const override; - virtual bool GetRawButton(uint32_t button) const override; - virtual int GetPOV(uint32_t pov = 1) const override; - bool GetButton(ButtonType button) const; - static Joystick* GetStickForPort(uint32_t port); + virtual bool GetTrigger(JoystickHand hand = kRightHand) const override; + virtual bool GetTop(JoystickHand hand = kRightHand) const override; + virtual bool GetBumper(JoystickHand hand = kRightHand) const override; + virtual bool GetRawButton(uint32_t button) const override; + virtual int GetPOV(uint32_t pov = 1) const override; + bool GetButton(ButtonType button) const; + static Joystick* GetStickForPort(uint32_t port); - virtual float GetMagnitude() const; - virtual float GetDirectionRadians() const; - virtual float GetDirectionDegrees() const; + virtual float GetMagnitude() const; + virtual float GetDirectionRadians() const; + virtual float GetDirectionDegrees() const; -private: - DriverStation &m_ds; - uint32_t m_port; - std::unique_ptr m_axes; - std::unique_ptr m_buttons; + private: + DriverStation& m_ds; + uint32_t m_port; + std::unique_ptr m_axes; + std::unique_ptr m_buttons; }; #endif diff --git a/wpilibc/simulation/include/MotorSafety.h b/wpilibc/simulation/include/MotorSafety.h index 82c77d8f57..373fddf6c4 100644 --- a/wpilibc/simulation/include/MotorSafety.h +++ b/wpilibc/simulation/include/MotorSafety.h @@ -11,14 +11,13 @@ #include -class MotorSafety -{ -public: - virtual void SetExpiration(float timeout) = 0; - virtual float GetExpiration() const = 0; - virtual bool IsAlive() const = 0; - virtual void StopMotor() = 0; - virtual void SetSafetyEnabled(bool enabled) = 0; - virtual bool IsSafetyEnabled() const = 0; - virtual void GetDescription(std::ostringstream& desc) const = 0; +class MotorSafety { + public: + virtual void SetExpiration(float timeout) = 0; + virtual float GetExpiration() const = 0; + virtual bool IsAlive() const = 0; + virtual void StopMotor() = 0; + virtual void SetSafetyEnabled(bool enabled) = 0; + virtual bool IsSafetyEnabled() const = 0; + virtual void GetDescription(std::ostringstream& desc) const = 0; }; diff --git a/wpilibc/simulation/include/MotorSafetyHelper.h b/wpilibc/simulation/include/MotorSafetyHelper.h index f124605df4..35ee64ce68 100644 --- a/wpilibc/simulation/include/MotorSafetyHelper.h +++ b/wpilibc/simulation/include/MotorSafetyHelper.h @@ -16,7 +16,7 @@ class MotorSafety; class MotorSafetyHelper : public ErrorBase { public: - MotorSafetyHelper(MotorSafety *safeObject); + MotorSafetyHelper(MotorSafety* safeObject); ~MotorSafetyHelper(); void Feed(); void SetExpiration(float expirationTime); @@ -28,14 +28,17 @@ class MotorSafetyHelper : public ErrorBase { static void CheckMotors(); private: - double m_expiration; // the expiration time for this object - bool m_enabled; // true if motor safety is enabled for this motor - double m_stopTime; // the FPGA clock value when this motor has expired - mutable priority_recursive_mutex - m_syncMutex; // protect accesses to the state for this object - MotorSafety *m_safeObject; // the object that is using the helper + // the expiration time for this object + double m_expiration; + // true if motor safety is enabled for this motor + bool m_enabled; + // the FPGA clock value when this motor has expired + double m_stopTime; + // protect accesses to the state for this object + mutable priority_recursive_mutex m_syncMutex; + MotorSafety* m_safeObject; // the object that is using the helper // List of all existing MotorSafetyHelper objects. static std::set m_helperList; - static priority_recursive_mutex - m_listMutex; // protect accesses to the list of helpers + // protect accesses to the list of helpers + static priority_recursive_mutex m_listMutex; }; diff --git a/wpilibc/simulation/include/Notifier.h b/wpilibc/simulation/include/Notifier.h index 0160aa4990..a0929eb5f3 100644 --- a/wpilibc/simulation/include/Notifier.h +++ b/wpilibc/simulation/include/Notifier.h @@ -23,8 +23,7 @@ class Notifier : public ErrorBase { template Notifier(Callable&& f, Arg&& arg, Args&&... args) - : Notifier(std::bind(std::forward(f), - std::forward(arg), + : Notifier(std::bind(std::forward(f), std::forward(arg), std::forward(args)...)) {} virtual ~Notifier(); @@ -39,11 +38,11 @@ class Notifier : public ErrorBase { static std::list timerQueue; static priority_recursive_mutex queueMutex; static priority_mutex halMutex; - static void *m_notifier; + static void* m_notifier; static std::atomic refcount; // Process the timer queue on a timer event - static void ProcessQueue(uint32_t mask, void *params); + static void ProcessQueue(uint32_t mask, void* params); // Update the FPGA alarm since the queue has changed static void UpdateAlarm(); diff --git a/wpilibc/simulation/include/PWM.h b/wpilibc/simulation/include/PWM.h index bc0b7da948..61f70cebcb 100644 --- a/wpilibc/simulation/include/PWM.h +++ b/wpilibc/simulation/include/PWM.h @@ -7,9 +7,9 @@ #pragma once +#include "LiveWindow/LiveWindowSendable.h" #include "SensorBase.h" #include "simulation/SimContinuousOutput.h" -#include "LiveWindow/LiveWindowSendable.h" #include "tables/ITableListener.h" #include @@ -17,12 +17,13 @@ /** * Class implements the PWM generation in the FPGA. * - * The values supplied as arguments for PWM outputs range from -1.0 to 1.0. They are mapped - * to the hardware dependent values, in this case 0-255 for the FPGA. + * The values supplied as arguments for PWM outputs range from -1.0 to 1.0. They + * are mapped to the hardware dependent values, in this case 0-255 for the FPGA. * Changes are immediately sent to the FPGA, and the update occurs at the next * FPGA cycle. There is no delay. * - * As of revision 0.1.10 of the FPGA, the FPGA interprets the 0-255 values as follows: + * As of revision 0.1.10 of the FPGA, the FPGA interprets the 0-255 values as + * follows: * - 255 = full "forward" * - 254 to 129 = linear scaling from "full forward" to "center" * - 128 = center value @@ -30,77 +31,77 @@ * - 1 = full "reverse" * - 0 = disabled (i.e. PWM output is held low) */ -class PWM : public SensorBase, public ITableListener, public LiveWindowSendable -{ -public: - enum PeriodMultiplier - { - kPeriodMultiplier_1X = 1, - kPeriodMultiplier_2X = 2, - kPeriodMultiplier_4X = 4 - }; +class PWM : public SensorBase, + public ITableListener, + public LiveWindowSendable { + public: + enum PeriodMultiplier { + kPeriodMultiplier_1X = 1, + kPeriodMultiplier_2X = 2, + kPeriodMultiplier_4X = 4 + }; - explicit PWM(uint32_t channel); - virtual ~PWM(); - virtual void SetRaw(unsigned short value); - void SetPeriodMultiplier(PeriodMultiplier mult); - void EnableDeadbandElimination(bool eliminateDeadband); - void SetBounds(int32_t max, int32_t deadbandMax, int32_t center, int32_t deadbandMin, - int32_t min); - void SetBounds(double max, double deadbandMax, double center, double deadbandMin, double min); - uint32_t GetChannel() const - { - return m_channel; - } + explicit PWM(uint32_t channel); + virtual ~PWM(); + virtual void SetRaw(unsigned short value); + void SetPeriodMultiplier(PeriodMultiplier mult); + void EnableDeadbandElimination(bool eliminateDeadband); + void SetBounds(int32_t max, int32_t deadbandMax, int32_t center, + int32_t deadbandMin, int32_t min); + void SetBounds(double max, double deadbandMax, double center, + double deadbandMin, double min); + uint32_t GetChannel() const { return m_channel; } -protected: - /** - * kDefaultPwmPeriod is in ms - * - * - 20ms periods (50 Hz) are the "safest" setting in that this works for all devices - * - 20ms periods seem to be desirable for Vex Motors - * - 20ms periods are the specified period for HS-322HD servos, but work reliably down - * to 10.0 ms; starting at about 8.5ms, the servo sometimes hums and get hot; - * by 5.0ms the hum is nearly continuous - * - 10ms periods work well for Victor 884 - * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed controllers. - * Due to the shipping firmware on the Jaguar, we can't run the update period less - * than 5.05 ms. - * - * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period scaling is implemented as an - * output squelch to get longer periods for old devices. - */ - static const float kDefaultPwmPeriod; - /** - * kDefaultPwmCenter is the PWM range center in ms - */ - static const float kDefaultPwmCenter; - /** - * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint - */ - static const int32_t kDefaultPwmStepsDown; - static const int32_t kPwmDisabled; + protected: + /** + * kDefaultPwmPeriod is in ms + * + * - 20ms periods (50 Hz) are the "safest" setting in that this works for all + * devices + * - 20ms periods seem to be desirable for Vex Motors + * - 20ms periods are the specified period for HS-322HD servos, but work + * reliably down to 10.0 ms; starting at about 8.5ms, the servo sometimes + * hums and get hot; by 5.0ms the hum is nearly continuous + * - 10ms periods work well for Victor 884 + * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed + * controllers. Due to the shipping firmware on the Jaguar, we can't run + * the update period less than 5.05 ms. + * + * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period + * scaling is implemented as an output squelch to get longer periods for old + * devices. + */ + static const float kDefaultPwmPeriod; + /** + * kDefaultPwmCenter is the PWM range center in ms + */ + static const float kDefaultPwmCenter; + /** + * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint + */ + static const int32_t kDefaultPwmStepsDown; + static const int32_t kPwmDisabled; - virtual void SetPosition(float pos); - virtual float GetPosition() const; - virtual void SetSpeed(float speed); - virtual float GetSpeed() const; + virtual void SetPosition(float pos); + virtual float GetPosition() const; + virtual void SetSpeed(float speed); + virtual float GetSpeed() const; - bool m_eliminateDeadband; - int32_t m_centerPwm; + bool m_eliminateDeadband; + int32_t m_centerPwm; - void ValueChanged(ITable* source, llvm::StringRef key, + void ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - std::shared_ptr GetTable() const override; + void UpdateTable() override; + void StartLiveWindowMode() override; + void StopLiveWindowMode() override; + std::string GetSmartDashboardType() const override; + void InitTable(std::shared_ptr subTable) override; + std::shared_ptr GetTable() const override; - std::shared_ptr m_table; + std::shared_ptr m_table; -private: - uint32_t m_channel; - SimContinuousOutput* impl; + private: + uint32_t m_channel; + SimContinuousOutput* impl; }; diff --git a/wpilibc/simulation/include/Relay.h b/wpilibc/simulation/include/Relay.h index 5a1c680606..46faffc52a 100644 --- a/wpilibc/simulation/include/Relay.h +++ b/wpilibc/simulation/include/Relay.h @@ -7,12 +7,12 @@ #pragma once +#include "LiveWindow/LiveWindowSendable.h" #include "MotorSafety.h" #include "SensorBase.h" #include "simulation/SimContinuousOutput.h" -#include "tables/ITableListener.h" -#include "LiveWindow/LiveWindowSendable.h" #include "tables/ITable.h" +#include "tables/ITableListener.h" #include @@ -21,38 +21,30 @@ class DigitalModule; /** * Class for Spike style relay outputs. - * Relays are intended to be connected to spikes or similar relays. The relay channels controls - * a pair of pins that are either both off, one on, the other on, or both on. This translates into - * two spike outputs at 0v, one at 12v and one at 0v, one at 0v and the other at 12v, or two - * spike outputs at 12V. This allows off, full forward, or full reverse control of motors without - * variable speed. It also allows the two channels (forward and reverse) to be used independently - * for something that does not care about voltage polatiry (like a solenoid). + * + * Relays are intended to be connected to spikes or similar relays. The relay + * channels controls a pair of pins that are either both off, one on, the other + * on, or both on. This translates into two spike outputs at 0v, one at 12v and + * one at 0v, one at 0v and the other at 12v, or two spike outputs at 12V. This + * allows off, full forward, or full reverse control of motors without variable + * speed. It also allows the two channels (forward and reverse) to be used + * independently for something that does not care about voltage polatiry (like + * a solenoid). */ class Relay : public MotorSafety, public SensorBase, public ITableListener, public LiveWindowSendable { -public: - enum Value - { - kOff, - kOn, - kForward, - kReverse - }; - enum Direction - { - kBothDirections, - kForwardOnly, - kReverseOnly - }; + public: + enum Value { kOff, kOn, kForward, kReverse }; + enum Direction { kBothDirections, kForwardOnly, kReverseOnly }; - Relay(uint32_t channel, Direction direction = kBothDirections); - virtual ~Relay(); + Relay(uint32_t channel, Direction direction = kBothDirections); + virtual ~Relay(); - void Set(Value value); - Value Get() const; - uint32_t GetChannel() const; + void Set(Value value); + Value Get() const; + uint32_t GetChannel() const; void SetExpiration(float timeout) override; float GetExpiration() const override; @@ -62,21 +54,21 @@ public: void SetSafetyEnabled(bool enabled) override; void GetDescription(std::ostringstream& desc) const override; - void ValueChanged(ITable* source, llvm::StringRef key, + void ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - std::shared_ptr GetTable() const override; + void UpdateTable() override; + void StartLiveWindowMode() override; + void StopLiveWindowMode() override; + std::string GetSmartDashboardType() const override; + void InitTable(std::shared_ptr subTable) override; + std::shared_ptr GetTable() const override; - std::shared_ptr m_table; + std::shared_ptr m_table; -private: - uint32_t m_channel; - Direction m_direction; - std::unique_ptr m_safetyHelper; - SimContinuousOutput* impl; - bool go_pos, go_neg; + private: + uint32_t m_channel; + Direction m_direction; + std::unique_ptr m_safetyHelper; + SimContinuousOutput* impl; + bool go_pos, go_neg; }; diff --git a/wpilibc/simulation/include/RobotBase.h b/wpilibc/simulation/include/RobotBase.h index ff9c015942..588a3bcc22 100644 --- a/wpilibc/simulation/include/RobotBase.h +++ b/wpilibc/simulation/include/RobotBase.h @@ -9,48 +9,49 @@ #include "Base.h" #include "DriverStation.h" -#include "simulation/simTime.h" #include "simulation/MainNode.h" +#include "simulation/simTime.h" -#define START_ROBOT_CLASS(_ClassName_) \ - int main() \ - { \ - (new _ClassName_())->StartCompetition(); \ - return 0; \ - } +#define START_ROBOT_CLASS(_ClassName_) \ + int main() { \ + (new _ClassName_())->StartCompetition(); \ + return 0; \ + } /** * Implement a Robot Program framework. - * The RobotBase class is intended to be subclassed by a user creating a robot program. - * Overridden Autonomous() and OperatorControl() methods are called at the appropriate time - * as the match proceeds. In the current implementation, the Autonomous code will run to - * completion before the OperatorControl code could start. In the future the Autonomous code - * might be spawned as a task, then killed at the end of the Autonomous period. + * + * The RobotBase class is intended to be subclassed by a user creating a robot + * program. Overridden Autonomous() and OperatorControl() methods are called at + * the appropriate time as the match proceeds. In the current implementation, + * the Autonomous code will run to completion before the OperatorControl code + * could start. In the future the Autonomous code might be spawned as a task, + * then killed at the end of the Autonomous period. */ -class RobotBase -{ - friend class RobotDeleter; -public: - static RobotBase &getInstance(); - static void setInstance(RobotBase* robot); +class RobotBase { + friend class RobotDeleter; - bool IsEnabled() const; - bool IsDisabled() const; - bool IsAutonomous() const; - bool IsOperatorControl() const; - bool IsTest() const; - virtual void StartCompetition() = 0; + public: + static RobotBase& getInstance(); + static void setInstance(RobotBase* robot); -protected: - RobotBase(); - virtual ~RobotBase() = default; + bool IsEnabled() const; + bool IsDisabled() const; + bool IsAutonomous() const; + bool IsOperatorControl() const; + bool IsTest() const; + virtual void StartCompetition() = 0; - RobotBase(const RobotBase&) = delete; - RobotBase& operator=(const RobotBase&) = delete; + protected: + RobotBase(); + virtual ~RobotBase() = default; - DriverStation &m_ds; + RobotBase(const RobotBase&) = delete; + RobotBase& operator=(const RobotBase&) = delete; + + DriverStation& m_ds; transport::SubscriberPtr time_sub; -private: - static RobotBase *m_instance; + private: + static RobotBase* m_instance; }; diff --git a/wpilibc/simulation/include/RobotDrive.h b/wpilibc/simulation/include/RobotDrive.h index 8e9f8caed9..3081deb7d5 100644 --- a/wpilibc/simulation/include/RobotDrive.h +++ b/wpilibc/simulation/include/RobotDrive.h @@ -7,9 +7,9 @@ #pragma once -#include "ErrorBase.h" #include #include +#include "ErrorBase.h" #include "MotorSafety.h" #include "MotorSafetyHelper.h" @@ -17,105 +17,110 @@ class SpeedController; class GenericHID; /** - * Utility class for handling Robot drive based on a definition of the motor configuration. - * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor standard - * drive trains are supported. In the future other drive types like swerve and meccanum might - * be implemented. Motor channel numbers are passed supplied on creation of the class. Those are - * used for either the Drive function (intended for hand created drive code, such as autonomous) - * or with the Tank/Arcade functions intended to be used for Operator Control driving. + * Utility class for handling Robot drive based on a definition of the motor + * configuration. + * + * The robot drive class handles basic driving for a robot. Currently, 2 and 4 + * motor standard drive trains are supported. In the future other drive types + * like swerve and meccanum might be implemented. Motor channel numbers are + * passed supplied on creation of the class. Those are used for either the + * Drive function (intended for hand created drive code, such as autonomous) + * or with the Tank/Arcade functions intended to be used for Operator Control + * driving. */ -class RobotDrive : public MotorSafety, public ErrorBase -{ -public: - enum MotorType - { - kFrontLeftMotor = 0, - kFrontRightMotor = 1, - kRearLeftMotor = 2, - kRearRightMotor = 3 - }; +class RobotDrive : public MotorSafety, public ErrorBase { + public: + enum MotorType { + kFrontLeftMotor = 0, + kFrontRightMotor = 1, + kRearLeftMotor = 2, + kRearRightMotor = 3 + }; - RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel); - RobotDrive(uint32_t frontLeftMotorChannel, uint32_t rearLeftMotorChannel, - uint32_t frontRightMotorChannel, uint32_t rearRightMotorChannel); - RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor); - RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor); - RobotDrive(std::shared_ptr leftMotor, - std::shared_ptr rightMotor); - RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor, - SpeedController *frontRightMotor, SpeedController *rearRightMotor); - RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor, - SpeedController &frontRightMotor, SpeedController &rearRightMotor); - RobotDrive(std::shared_ptr frontLeftMotor, - std::shared_ptr rearLeftMotor, - std::shared_ptr frontRightMotor, - std::shared_ptr rearRightMotor); - virtual ~RobotDrive() = default; + RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel); + RobotDrive(uint32_t frontLeftMotorChannel, uint32_t rearLeftMotorChannel, + uint32_t frontRightMotorChannel, uint32_t rearRightMotorChannel); + RobotDrive(SpeedController* leftMotor, SpeedController* rightMotor); + RobotDrive(SpeedController& leftMotor, SpeedController& rightMotor); + RobotDrive(std::shared_ptr leftMotor, + std::shared_ptr rightMotor); + RobotDrive(SpeedController* frontLeftMotor, SpeedController* rearLeftMotor, + SpeedController* frontRightMotor, SpeedController* rearRightMotor); + RobotDrive(SpeedController& frontLeftMotor, SpeedController& rearLeftMotor, + SpeedController& frontRightMotor, SpeedController& rearRightMotor); + RobotDrive(std::shared_ptr frontLeftMotor, + std::shared_ptr rearLeftMotor, + std::shared_ptr frontRightMotor, + std::shared_ptr rearRightMotor); + virtual ~RobotDrive() = default; - RobotDrive(const RobotDrive&) = delete; - RobotDrive& operator=(const RobotDrive&) = delete; + RobotDrive(const RobotDrive&) = delete; + RobotDrive& operator=(const RobotDrive&) = delete; - void Drive(float outputMagnitude, float curve); - void TankDrive(GenericHID *leftStick, GenericHID *rightStick, bool squaredInputs = true); - void TankDrive(GenericHID &leftStick, GenericHID &rightStick, bool squaredInputs = true); - void TankDrive(GenericHID *leftStick, uint32_t leftAxis, GenericHID *rightStick, - uint32_t rightAxis, bool squaredInputs = true); - void TankDrive(GenericHID &leftStick, uint32_t leftAxis, GenericHID &rightStick, - uint32_t rightAxis, bool squaredInputs = true); - void TankDrive(float leftValue, float rightValue, bool squaredInputs = true); - void ArcadeDrive(GenericHID *stick, bool squaredInputs = true); - void ArcadeDrive(GenericHID &stick, bool squaredInputs = true); - void ArcadeDrive(GenericHID *moveStick, uint32_t moveChannel, GenericHID *rotateStick, - uint32_t rotateChannel, bool squaredInputs = true); - void ArcadeDrive(GenericHID &moveStick, uint32_t moveChannel, GenericHID &rotateStick, - uint32_t rotateChannel, bool squaredInputs = true); - void ArcadeDrive(float moveValue, float rotateValue, bool squaredInputs = true); - void MecanumDrive_Cartesian(float x, float y, float rotation, float gyroAngle = 0.0); - void MecanumDrive_Polar(float magnitude, float direction, float rotation); - void HolonomicDrive(float magnitude, float direction, float rotation); - virtual void SetLeftRightMotorOutputs(float leftOutput, float rightOutput); - void SetInvertedMotor(MotorType motor, bool isInverted); - void SetSensitivity(float sensitivity); - void SetMaxOutput(double maxOutput); + void Drive(float outputMagnitude, float curve); + void TankDrive(GenericHID* leftStick, GenericHID* rightStick, + bool squaredInputs = true); + void TankDrive(GenericHID& leftStick, GenericHID& rightStick, + bool squaredInputs = true); + void TankDrive(GenericHID* leftStick, uint32_t leftAxis, + GenericHID* rightStick, uint32_t rightAxis, + bool squaredInputs = true); + void TankDrive(GenericHID& leftStick, uint32_t leftAxis, + GenericHID& rightStick, uint32_t rightAxis, + bool squaredInputs = true); + void TankDrive(float leftValue, float rightValue, bool squaredInputs = true); + void ArcadeDrive(GenericHID* stick, bool squaredInputs = true); + void ArcadeDrive(GenericHID& stick, bool squaredInputs = true); + void ArcadeDrive(GenericHID* moveStick, uint32_t moveChannel, + GenericHID* rotateStick, uint32_t rotateChannel, + bool squaredInputs = true); + void ArcadeDrive(GenericHID& moveStick, uint32_t moveChannel, + GenericHID& rotateStick, uint32_t rotateChannel, + bool squaredInputs = true); + void ArcadeDrive(float moveValue, float rotateValue, + bool squaredInputs = true); + void MecanumDrive_Cartesian(float x, float y, float rotation, + float gyroAngle = 0.0); + void MecanumDrive_Polar(float magnitude, float direction, float rotation); + void HolonomicDrive(float magnitude, float direction, float rotation); + virtual void SetLeftRightMotorOutputs(float leftOutput, float rightOutput); + void SetInvertedMotor(MotorType motor, bool isInverted); + void SetSensitivity(float sensitivity); + void SetMaxOutput(double maxOutput); - void SetExpiration(float timeout) override; - float GetExpiration() const override; - bool IsAlive() const override; - void StopMotor() override; - bool IsSafetyEnabled() const override; - void SetSafetyEnabled(bool enabled) override; - void GetDescription(std::ostringstream& desc) const override; + void SetExpiration(float timeout) override; + float GetExpiration() const override; + bool IsAlive() const override; + void StopMotor() override; + bool IsSafetyEnabled() const override; + void SetSafetyEnabled(bool enabled) override; + void GetDescription(std::ostringstream& desc) const override; -protected: - void InitRobotDrive(); - float Limit(float num); - void Normalize(double *wheelSpeeds); - void RotateVector(double &x, double &y, double angle); + protected: + void InitRobotDrive(); + float Limit(float num); + void Normalize(double* wheelSpeeds); + void RotateVector(double& x, double& y, double angle); - static const int32_t kMaxNumberOfMotors = 4; + static const int32_t kMaxNumberOfMotors = 4; - int32_t m_invertedMotors[kMaxNumberOfMotors] = {1,1,1,1}; - float m_sensitivity = 0.5; - double m_maxOutput = 1.0; - bool m_deleteSpeedControllers; - std::shared_ptr m_frontLeftMotor; - std::shared_ptr m_frontRightMotor; - std::shared_ptr m_rearLeftMotor; - std::shared_ptr m_rearRightMotor; - // FIXME: MotorSafetyHelper *m_safetyHelper; + int32_t m_invertedMotors[kMaxNumberOfMotors] = {1, 1, 1, 1}; + float m_sensitivity = 0.5; + double m_maxOutput = 1.0; + bool m_deleteSpeedControllers; + std::shared_ptr m_frontLeftMotor; + std::shared_ptr m_frontRightMotor; + std::shared_ptr m_rearLeftMotor; + std::shared_ptr m_rearRightMotor; + // FIXME: MotorSafetyHelper *m_safetyHelper; -private: - int32_t GetNumMotors() - { - int motors = 0; - if (m_frontLeftMotor) - motors++; - if (m_frontRightMotor) - motors++; - if (m_rearLeftMotor) - motors++; - if (m_rearRightMotor) - motors++; - return motors; - } + private: + int32_t GetNumMotors() { + int motors = 0; + if (m_frontLeftMotor) motors++; + if (m_frontRightMotor) motors++; + if (m_rearLeftMotor) motors++; + if (m_rearRightMotor) motors++; + return motors; + } }; diff --git a/wpilibc/simulation/include/SafePWM.h b/wpilibc/simulation/include/SafePWM.h index 4787cee74f..0c09dd5181 100644 --- a/wpilibc/simulation/include/SafePWM.h +++ b/wpilibc/simulation/include/SafePWM.h @@ -7,33 +7,34 @@ #pragma once -#include "MotorSafety.h" -#include "PWM.h" -#include "MotorSafetyHelper.h" #include +#include "MotorSafety.h" +#include "MotorSafetyHelper.h" +#include "PWM.h" /** * A safe version of the PWM class. - * It is safe because it implements the MotorSafety interface that provides timeouts - * in the event that the motor value is not updated before the expiration time. - * This delegates the actual work to a MotorSafetyHelper object that is used for all - * objects that implement MotorSafety. + * + * It is safe because it implements the MotorSafety interface that provides + * timeouts in the event that the motor value is not updated before the + * expiration time. This delegates the actual work to a MotorSafetyHelper + * object that is used for all objects that implement MotorSafety. */ -class SafePWM : public PWM, public MotorSafety -{ -public: - explicit SafePWM(uint32_t channel); - virtual ~SafePWM() = default; +class SafePWM : public PWM, public MotorSafety { + public: + explicit SafePWM(uint32_t channel); + virtual ~SafePWM() = default; - void SetExpiration(float timeout); - float GetExpiration() const; - bool IsAlive() const; - void StopMotor(); - bool IsSafetyEnabled() const; - void SetSafetyEnabled(bool enabled); - void GetDescription(std::ostringstream& desc) const; + void SetExpiration(float timeout); + float GetExpiration() const; + bool IsAlive() const; + void StopMotor(); + bool IsSafetyEnabled() const; + void SetSafetyEnabled(bool enabled); + void GetDescription(std::ostringstream& desc) const; - virtual void SetSpeed(float speed); -private: - std::unique_ptr m_safetyHelper; + virtual void SetSpeed(float speed); + + private: + std::unique_ptr m_safetyHelper; }; diff --git a/wpilibc/simulation/include/SampleRobot.h b/wpilibc/simulation/include/SampleRobot.h index a87bb4e819..61f8bc0f85 100644 --- a/wpilibc/simulation/include/SampleRobot.h +++ b/wpilibc/simulation/include/SampleRobot.h @@ -9,19 +9,18 @@ #include "RobotBase.h" -class SampleRobot : public RobotBase -{ -public: - SampleRobot(); - virtual ~SampleRobot() = default; - virtual void RobotInit(); - virtual void Disabled(); - virtual void Autonomous(); - virtual void OperatorControl(); - virtual void Test(); - virtual void RobotMain(); - void StartCompetition(); +class SampleRobot : public RobotBase { + public: + SampleRobot(); + virtual ~SampleRobot() = default; + virtual void RobotInit(); + virtual void Disabled(); + virtual void Autonomous(); + virtual void OperatorControl(); + virtual void Test(); + virtual void RobotMain(); + void StartCompetition(); -private: - bool m_robotMainOverridden; + private: + bool m_robotMainOverridden; }; diff --git a/wpilibc/simulation/include/Solenoid.h b/wpilibc/simulation/include/Solenoid.h index 4f6ac3ad68..3aae4399d0 100644 --- a/wpilibc/simulation/include/Solenoid.h +++ b/wpilibc/simulation/include/Solenoid.h @@ -7,8 +7,8 @@ #pragma once -#include "simulation/SimContinuousOutput.h" #include "LiveWindow/LiveWindowSendable.h" +#include "simulation/SimContinuousOutput.h" #include "tables/ITableListener.h" #include @@ -16,30 +16,29 @@ /** * Solenoid class for running high voltage Digital Output (PCM). * - * The Solenoid class is typically used for pneumatics solenoids, but could be used - * for any device within the current spec of the PCM. + * The Solenoid class is typically used for pneumatics solenoids, but could be + * used for any device within the current spec of the PCM. */ -class Solenoid : public LiveWindowSendable, public ITableListener -{ -public: - explicit Solenoid(uint32_t channel); - Solenoid(uint8_t moduleNumber, uint32_t channel); - virtual ~Solenoid(); - virtual void Set(bool on); - virtual bool Get() const; +class Solenoid : public LiveWindowSendable, public ITableListener { + public: + explicit Solenoid(uint32_t channel); + Solenoid(uint8_t moduleNumber, uint32_t channel); + virtual ~Solenoid(); + virtual void Set(bool on); + virtual bool Get() const; - void ValueChanged(ITable* source, llvm::StringRef key, + void ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - std::shared_ptr GetTable() const override; + void UpdateTable() override; + void StartLiveWindowMode() override; + void StopLiveWindowMode() override; + std::string GetSmartDashboardType() const override; + void InitTable(std::shared_ptr subTable) override; + std::shared_ptr GetTable() const override; -private: - SimContinuousOutput* m_impl; - bool m_on; + private: + SimContinuousOutput* m_impl; + bool m_on; - std::shared_ptr m_table; + std::shared_ptr m_table; }; diff --git a/wpilibc/simulation/include/SpeedController.h b/wpilibc/simulation/include/SpeedController.h index e665d06d10..b102981d0c 100644 --- a/wpilibc/simulation/include/SpeedController.h +++ b/wpilibc/simulation/include/SpeedController.h @@ -12,25 +12,25 @@ /** * Interface for speed controlling devices. */ -class SpeedController : public PIDOutput -{ -public: - virtual ~SpeedController() = default; - /** - * Common interface for setting the speed of a speed controller. - * - * @param speed The speed to set. Value should be between -1.0 and 1.0. - * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup(). If 0, update immediately. - */ - virtual void Set(float speed, uint8_t syncGroup = 0) = 0; - /** - * Common interface for getting the current set speed of a speed controller. - * - * @return The current set speed. Value is between -1.0 and 1.0. - */ - virtual float Get() const = 0; - /** - * Common interface for disabling a motor. - */ - virtual void Disable() = 0; +class SpeedController : public PIDOutput { + public: + virtual ~SpeedController() = default; + /** + * Common interface for setting the speed of a speed controller. + * + * @param speed The speed to set. Value should be between -1.0 and 1.0. + * @param syncGroup The update group to add this Set() to, pending + * UpdateSyncGroup(). If 0, update immediately. + */ + virtual void Set(float speed, uint8_t syncGroup = 0) = 0; + /** + * Common interface for getting the current set speed of a speed controller. + * + * @return The current set speed. Value is between -1.0 and 1.0. + */ + virtual float Get() const = 0; + /** + * Common interface for disabling a motor. + */ + virtual void Disable() = 0; }; diff --git a/wpilibc/simulation/include/Talon.h b/wpilibc/simulation/include/Talon.h index d56d5ba29c..e257235989 100644 --- a/wpilibc/simulation/include/Talon.h +++ b/wpilibc/simulation/include/Talon.h @@ -7,21 +7,20 @@ #pragma once +#include "PIDOutput.h" #include "SafePWM.h" #include "SpeedController.h" -#include "PIDOutput.h" /** - * CTRE Talon Speed Controller + * CTRE Talon Speed Controller. */ -class Talon : public SafePWM, public SpeedController -{ -public: - explicit Talon(uint32_t channel); - virtual ~Talon() = default; - virtual void Set(float value, uint8_t syncGroup = 0); - virtual float Get() const; - virtual void Disable(); +class Talon : public SafePWM, public SpeedController { + public: + explicit Talon(uint32_t channel); + virtual ~Talon() = default; + virtual void Set(float value, uint8_t syncGroup = 0); + virtual float Get() const; + virtual void Disable(); - virtual void PIDWrite(float output) override; + virtual void PIDWrite(float output) override; }; diff --git a/wpilibc/simulation/include/Victor.h b/wpilibc/simulation/include/Victor.h index ef3646e6ed..8021629696 100644 --- a/wpilibc/simulation/include/Victor.h +++ b/wpilibc/simulation/include/Victor.h @@ -7,21 +7,20 @@ #pragma once +#include "PIDOutput.h" #include "SafePWM.h" #include "SpeedController.h" -#include "PIDOutput.h" /** - * IFI Victor Speed Controller + * IFI Victor Speed Controller. */ -class Victor : public SafePWM, public SpeedController -{ -public: - explicit Victor(uint32_t channel); - virtual ~Victor() = default; - virtual void Set(float value, uint8_t syncGroup = 0); - virtual float Get() const; - virtual void Disable(); +class Victor : public SafePWM, public SpeedController { + public: + explicit Victor(uint32_t channel); + virtual ~Victor() = default; + virtual void Set(float value, uint8_t syncGroup = 0); + virtual float Get() const; + virtual void Disable(); - virtual void PIDWrite(float output) override; + virtual void PIDWrite(float output) override; }; diff --git a/wpilibc/simulation/include/WPILib.h b/wpilibc/simulation/include/WPILib.h index d4e9bf288f..99a76753f2 100644 --- a/wpilibc/simulation/include/WPILib.h +++ b/wpilibc/simulation/include/WPILib.h @@ -8,14 +8,14 @@ #define SIMULATION "gazebo" -#include "string.h" #include +#include "string.h" -#include "Buttons/Trigger.h" #include "Buttons/Button.h" #include "Buttons/InternalButton.h" #include "Buttons/JoystickButton.h" #include "Buttons/NetworkButton.h" +#include "Buttons/Trigger.h" #include "Commands/Command.h" #include "Commands/CommandGroup.h" @@ -32,25 +32,24 @@ #include "SmartDashboard/SendableChooser.h" #include "SmartDashboard/SmartDashboard.h" -#include "RobotBase.h" -#include "SampleRobot.h" -#include "IterativeRobot.h" -#include "SpeedController.h" -#include "Talon.h" -#include "Victor.h" -#include "Jaguar.h" -#include "Solenoid.h" -#include "DoubleSolenoid.h" -#include "interfaces/Potentiometer.h" +#include "AnalogGyro.h" #include "AnalogInput.h" #include "AnalogPotentiometer.h" #include "Counter.h" #include "DigitalInput.h" +#include "DoubleSolenoid.h" #include "Encoder.h" -#include "AnalogGyro.h" #include "GenericHID.h" +#include "IterativeRobot.h" +#include "Jaguar.h" #include "Joystick.h" -#include "PIDController.h" -#include "RobotDrive.h" #include "LiveWindow/LiveWindow.h" - +#include "PIDController.h" +#include "RobotBase.h" +#include "RobotDrive.h" +#include "SampleRobot.h" +#include "Solenoid.h" +#include "SpeedController.h" +#include "Talon.h" +#include "Victor.h" +#include "interfaces/Potentiometer.h" diff --git a/wpilibc/simulation/include/simulation/MainNode.h b/wpilibc/simulation/include/simulation/MainNode.h index 416c7a5bfa..9f17452ef7 100644 --- a/wpilibc/simulation/include/simulation/MainNode.h +++ b/wpilibc/simulation/include/simulation/MainNode.h @@ -8,54 +8,56 @@ #ifndef _SIM_MAIN_NODE_H #define _SIM_MAIN_NODE_H -#include "simulation/gz_msgs/msgs.h" -#include #include +#include +#include "simulation/gz_msgs/msgs.h" using namespace gazebo; class MainNode { -public: - static MainNode* GetInstance() { - static MainNode instance; - return &instance; - } + public: + static MainNode* GetInstance() { + static MainNode instance; + return &instance; + } - template - static transport::PublisherPtr Advertise(const std::string &topic, - unsigned int _queueLimit = 10, - bool _latch = false) { - return GetInstance()->main->Advertise(topic, _queueLimit, _latch); - } + template + static transport::PublisherPtr Advertise(const std::string& topic, + unsigned int _queueLimit = 10, + bool _latch = false) { + return GetInstance()->main->Advertise(topic, _queueLimit, _latch); + } - template - static transport::SubscriberPtr Subscribe(const std::string &topic, - void(T::*fp)(const boost::shared_ptr &), T *obj, - bool _latching = false) { - return GetInstance()->main->Subscribe(topic, fp, obj, _latching); - } + template + static transport::SubscriberPtr Subscribe( + const std::string& topic, + void (T::*fp)(const boost::shared_ptr&), T* obj, + bool _latching = false) { + return GetInstance()->main->Subscribe(topic, fp, obj, _latching); + } - template - static transport::SubscriberPtr Subscribe(const std::string &topic, - void(*fp)(const boost::shared_ptr &), - bool _latching = false) { - return GetInstance()->main->Subscribe(topic, fp, _latching); - } + template + static transport::SubscriberPtr Subscribe( + const std::string& topic, void (*fp)(const boost::shared_ptr&), + bool _latching = false) { + return GetInstance()->main->Subscribe(topic, fp, _latching); + } - transport::NodePtr main; -private: - MainNode() { - bool success = gazebo::client::setup(); + transport::NodePtr main; - if (success){ - main = transport::NodePtr(new transport::Node()); - main->Init("frc"); - gazebo::transport::run(); + private: + MainNode() { + bool success = gazebo::client::setup(); + + if (success) { + main = transport::NodePtr(new transport::Node()); + main->Init("frc"); + gazebo::transport::run(); + } else { + std::cout << "An error has occured setting up gazebo_client!" + << std::endl; } - else { - std::cout << "An error has occured setting up gazebo_client!" << std::endl; - } - } + } }; #endif diff --git a/wpilibc/simulation/include/simulation/SimContinuousOutput.h b/wpilibc/simulation/include/simulation/SimContinuousOutput.h index 2c0d5da5c4..b1c81ed3e0 100644 --- a/wpilibc/simulation/include/simulation/SimContinuousOutput.h +++ b/wpilibc/simulation/include/simulation/SimContinuousOutput.h @@ -5,14 +5,13 @@ /* the project. */ /*----------------------------------------------------------------------------*/ - #ifndef _SIM_SPEED_CONTROLLER_H #define _SIM_SPEED_CONTROLLER_H #ifdef _WIN32 - // Ensure that Winsock2.h is included before Windows.h, which can get - // pulled in by anybody (e.g., Boost). - #include +// Ensure that Winsock2.h is included before Windows.h, which can get +// pulled in by anybody (e.g., Boost). +#include #endif #include @@ -21,27 +20,27 @@ using namespace gazebo; class SimContinuousOutput { -private: - transport::PublisherPtr pub; - float speed; + private: + transport::PublisherPtr pub; + float speed; -public: - SimContinuousOutput(std::string topic); + public: + SimContinuousOutput(std::string topic); - /** - * Set the output value. - * - * The value is set using a range of -1.0 to 1.0, appropriately - * scaling the value. - * - * @param value The value between -1.0 and 1.0 to set. - */ - void Set(float value); + /** + * Set the output value. + * + * The value is set using a range of -1.0 to 1.0, appropriately + * scaling the value. + * + * @param value The value between -1.0 and 1.0 to set. + */ + void Set(float value); - /** - * @return The most recently set value. - */ - float Get(); + /** + * @return The most recently set value. + */ + float Get(); }; #endif diff --git a/wpilibc/simulation/include/simulation/SimDigitalInput.h b/wpilibc/simulation/include/simulation/SimDigitalInput.h index ca14b0f9ad..5bc5542f46 100644 --- a/wpilibc/simulation/include/simulation/SimDigitalInput.h +++ b/wpilibc/simulation/include/simulation/SimDigitalInput.h @@ -5,28 +5,27 @@ /* the project. */ /*----------------------------------------------------------------------------*/ - #ifndef _SIM_DIGITAL_INPUT_H #define _SIM_DIGITAL_INPUT_H -#include "simulation/gz_msgs/msgs.h" #include +#include "simulation/gz_msgs/msgs.h" using namespace gazebo; class SimDigitalInput { -public: - SimDigitalInput(std::string topic); + public: + SimDigitalInput(std::string topic); - /** - * @return The value of the potentiometer. - */ - bool Get(); + /** + * @return The value of the potentiometer. + */ + bool Get(); -private: - bool value; - transport::SubscriberPtr sub; - void callback(const msgs::ConstBoolPtr &msg); + private: + bool value; + transport::SubscriberPtr sub; + void callback(const msgs::ConstBoolPtr& msg); }; #endif diff --git a/wpilibc/simulation/include/simulation/SimEncoder.h b/wpilibc/simulation/include/simulation/SimEncoder.h index 254cbe41ac..ea3eb976cb 100644 --- a/wpilibc/simulation/include/simulation/SimEncoder.h +++ b/wpilibc/simulation/include/simulation/SimEncoder.h @@ -5,34 +5,33 @@ /* the project. */ /*----------------------------------------------------------------------------*/ - #ifndef _SIM_ENCODER_H #define _SIM_ENCODER_H -#include "simulation/gz_msgs/msgs.h" -#include #include +#include +#include "simulation/gz_msgs/msgs.h" using namespace gazebo; class SimEncoder { -public: - SimEncoder(std::string topic); + public: + SimEncoder(std::string topic); - void Reset(); - void Start(); - void Stop(); - double GetPosition(); - double GetVelocity(); + void Reset(); + void Start(); + void Stop(); + double GetPosition(); + double GetVelocity(); -private: - void sendCommand(std::string cmd); + private: + void sendCommand(std::string cmd); - double position, velocity; - transport::SubscriberPtr posSub, velSub; - transport::PublisherPtr commandPub; - void positionCallback(const msgs::ConstFloat64Ptr &msg); - void velocityCallback(const msgs::ConstFloat64Ptr &msg); + double position, velocity; + transport::SubscriberPtr posSub, velSub; + transport::PublisherPtr commandPub; + void positionCallback(const msgs::ConstFloat64Ptr& msg); + void velocityCallback(const msgs::ConstFloat64Ptr& msg); }; #endif diff --git a/wpilibc/simulation/include/simulation/SimFloatInput.h b/wpilibc/simulation/include/simulation/SimFloatInput.h index bdc1c4ced3..f43be1d229 100644 --- a/wpilibc/simulation/include/simulation/SimFloatInput.h +++ b/wpilibc/simulation/include/simulation/SimFloatInput.h @@ -5,28 +5,27 @@ /* the project. */ /*----------------------------------------------------------------------------*/ - #ifndef _SIM_FLOAT_INPUT_H #define _SIM_FLOAT_INPUT_H -#include "simulation/gz_msgs/msgs.h" #include +#include "simulation/gz_msgs/msgs.h" using namespace gazebo; class SimFloatInput { -public: - SimFloatInput(std::string topic); + public: + SimFloatInput(std::string topic); - /** - * @return The value of the potentiometer. - */ - double Get(); + /** + * @return The value of the potentiometer. + */ + double Get(); -private: - double value; - transport::SubscriberPtr sub; - void callback(const msgs::ConstFloat64Ptr &msg); + private: + double value; + transport::SubscriberPtr sub; + void callback(const msgs::ConstFloat64Ptr& msg); }; #endif diff --git a/wpilibc/simulation/include/simulation/SimGyro.h b/wpilibc/simulation/include/simulation/SimGyro.h index 85d9d2782d..48a7697703 100644 --- a/wpilibc/simulation/include/simulation/SimGyro.h +++ b/wpilibc/simulation/include/simulation/SimGyro.h @@ -5,31 +5,30 @@ /* the project. */ /*----------------------------------------------------------------------------*/ - #ifndef _SIM_GYRO_H #define _SIM_GYRO_H -#include "simulation/gz_msgs/msgs.h" #include +#include "simulation/gz_msgs/msgs.h" using namespace gazebo; class SimGyro { -public: - SimGyro(std::string topic); + public: + SimGyro(std::string topic); - void Reset(); - double GetAngle(); - double GetVelocity(); + void Reset(); + double GetAngle(); + double GetVelocity(); -private: - void sendCommand(std::string cmd); + private: + void sendCommand(std::string cmd); - double position, velocity; - transport::SubscriberPtr posSub, velSub; - transport::PublisherPtr commandPub; - void positionCallback(const msgs::ConstFloat64Ptr &msg); - void velocityCallback(const msgs::ConstFloat64Ptr &msg); + double position, velocity; + transport::SubscriberPtr posSub, velSub; + transport::PublisherPtr commandPub; + void positionCallback(const msgs::ConstFloat64Ptr& msg); + void velocityCallback(const msgs::ConstFloat64Ptr& msg); }; #endif diff --git a/wpilibc/simulation/include/simulation/simTime.h b/wpilibc/simulation/include/simulation/simTime.h index bd87ffa4ca..1208c46f14 100644 --- a/wpilibc/simulation/include/simulation/simTime.h +++ b/wpilibc/simulation/include/simulation/simTime.h @@ -7,11 +7,10 @@ #pragma once - #ifdef _WIN32 - // Ensure that Winsock2.h is included before Windows.h, which can get - // pulled in by anybody (e.g., Boost). - #include +// Ensure that Winsock2.h is included before Windows.h, which can get +// pulled in by anybody (e.g., Boost). +#include #endif #include "simulation/SimFloatInput.h" @@ -19,7 +18,9 @@ #include #include -namespace wpilib { namespace internal { - extern double simTime; - extern void time_callback(const msgs::ConstFloat64Ptr &msg); -}} +namespace wpilib { +namespace internal { +extern double simTime; +extern void time_callback(const msgs::ConstFloat64Ptr& msg); +} +} diff --git a/wpilibc/simulation/src/AnalogGyro.cpp b/wpilibc/simulation/src/AnalogGyro.cpp index 32196a86a8..9063f2c162 100644 --- a/wpilibc/simulation/src/AnalogGyro.cpp +++ b/wpilibc/simulation/src/AnalogGyro.cpp @@ -6,9 +6,9 @@ /*----------------------------------------------------------------------------*/ #include "AnalogGyro.h" +#include "LiveWindow/LiveWindow.h" #include "Timer.h" #include "WPIErrors.h" -#include "LiveWindow/LiveWindow.h" const uint32_t AnalogGyro::kOversampleBits = 10; const uint32_t AnalogGyro::kAverageBits = 0; @@ -18,63 +18,55 @@ const float AnalogGyro::kDefaultVoltsPerDegreePerSecond = 0.007; /** * Initialize the gyro. - * Calibrate the gyro by running for a number of samples and computing the center value for this - * part. Then use the center value as the Accumulator center value for subsequent measurements. - * It's important to make sure that the robot is not moving while the centering calculations are - * in progress, this is typically done when the robot is first turned on while it's sitting at - * rest before the competition starts. + * + * Calibrate the gyro by running for a number of samples and computing the + * center value for this part. Then use the center value as the Accumulator + * center value for subsequent measurements. It's important to make sure that + * the robot is not moving while the centering calculations are in progress, + * this is typically done when the robot is first turned on while it's sitting + * at rest before the competition starts. */ -void AnalogGyro::InitAnalogGyro(int channel) -{ - SetPIDSourceType(PIDSourceType::kDisplacement); +void AnalogGyro::InitAnalogGyro(int channel) { + SetPIDSourceType(PIDSourceType::kDisplacement); - char buffer[50]; - int n = sprintf(buffer, "analog/%d", channel); - impl = new SimGyro(buffer); + char buffer[50]; + int n = sprintf(buffer, "analog/%d", channel); + impl = new SimGyro(buffer); - LiveWindow::GetInstance()->AddSensor("AnalogGyro", channel, this); + LiveWindow::GetInstance()->AddSensor("AnalogGyro", channel, this); } /** - * AnalogGyro constructor with only a channel.. + * AnalogGyro constructor with only a channel. * * @param channel The analog channel the gyro is connected to. */ -AnalogGyro::AnalogGyro(uint32_t channel) -{ - InitAnalogGyro(channel); -} +AnalogGyro::AnalogGyro(uint32_t channel) { InitAnalogGyro(channel); } /** * Reset the gyro. - * Resets the gyro to a heading of zero. This can be used if there is significant - * drift in the gyro and it needs to be recalibrated after it has been running. + * + * Resets the gyro to a heading of zero. This can be used if there is + * significant drift in the gyro and it needs to be recalibrated after it has + * been running. */ -void AnalogGyro::Reset() -{ - impl->Reset(); -} +void AnalogGyro::Reset() { impl->Reset(); } -void AnalogGyro::Calibrate(){ - Reset(); -} +void AnalogGyro::Calibrate() { Reset(); } /** * Return the actual angle in degrees that the robot is currently facing. * - * The angle is based on the current accumulator value corrected by the oversampling rate, the - * gyro type and the A/D calibration values. - * The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't - * want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around. + * The angle is based on the current accumulator value corrected by the + * oversampling rate, the gyro type and the A/D calibration values. The angle + * is continuous, that is can go beyond 360 degrees. This make algorithms that + * wouldn't want to see a discontinuity in the gyro output as it sweeps past 0 + * on the second time around. * - * @return the current heading of the robot in degrees. This heading is based on integration - * of the returned rate from the gyro. + * @return the current heading of the robot in degrees. This heading is based on + * integration of the returned rate from the gyro. */ -float AnalogGyro::GetAngle() const -{ - return impl->GetAngle(); -} - +float AnalogGyro::GetAngle() const { return impl->GetAngle(); } /** * Return the rate of rotation of the gyro @@ -83,7 +75,4 @@ float AnalogGyro::GetAngle() const * * @return the current rate in degrees per second */ -double AnalogGyro::GetRate() const -{ - return impl->GetVelocity(); -} +double AnalogGyro::GetRate() const { return impl->GetVelocity(); } diff --git a/wpilibc/simulation/src/AnalogInput.cpp b/wpilibc/simulation/src/AnalogInput.cpp index 7979edf35e..5d74bf0f6b 100644 --- a/wpilibc/simulation/src/AnalogInput.cpp +++ b/wpilibc/simulation/src/AnalogInput.cpp @@ -6,88 +6,78 @@ /*----------------------------------------------------------------------------*/ #include "AnalogInput.h" -#include "WPIErrors.h" #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" /** * Construct an analog input. - * + * * @param channel The channel number to represent. */ -AnalogInput::AnalogInput(uint32_t channel) -{ - m_channel = channel; - char buffer[50]; - int n = sprintf(buffer, "analog/%d", channel); - m_impl = new SimFloatInput(buffer); +AnalogInput::AnalogInput(uint32_t channel) { + m_channel = channel; + char buffer[50]; + int n = sprintf(buffer, "analog/%d", channel); + m_impl = new SimFloatInput(buffer); - LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this); + LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this); } /** * Get a scaled sample straight from this channel. - * The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset(). + * + * The value is scaled to units of Volts using the calibrated scaling data from + * GetLSBWeight() and GetOffset(). + * * @return A scaled sample straight from this channel. */ -float AnalogInput::GetVoltage() const -{ - return m_impl->Get(); -} +float AnalogInput::GetVoltage() const { return m_impl->Get(); } /** - * Get a scaled sample from the output of the oversample and average engine for this channel. - * The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset(). - * Using oversampling will cause this value to be higher resolution, but it will update more slowly. - * Using averaging will cause this value to be more stable, but it will update more slowly. - * @return A scaled sample from the output of the oversample and average engine for this channel. + * Get a scaled sample from the output of the oversample and average engine for + * this channel. + * + * The value is scaled to units of Volts using the calibrated scaling data from + * GetLSBWeight() and GetOffset(). Using oversampling will cause this value to + * be higher resolution, but it will update more slowly. Using averaging will + * cause this value to be more stable, but it will update more slowly. + * + * @return A scaled sample from the output of the oversample and average engine + * for this channel. */ -float AnalogInput::GetAverageVoltage() const -{ - return m_impl->Get(); -} +float AnalogInput::GetAverageVoltage() const { return m_impl->Get(); } /** * Get the channel number. + * * @return The channel number. */ -uint32_t AnalogInput::GetChannel() const -{ - return m_channel; -} +uint32_t AnalogInput::GetChannel() const { return m_channel; } /** * Get the Average value for the PID Source base object. - * + * * @return The average voltage. */ -double AnalogInput::PIDGet() -{ - return GetAverageVoltage(); -} +double AnalogInput::PIDGet() { return GetAverageVoltage(); } void AnalogInput::UpdateTable() { - if (m_table != nullptr) { - m_table->PutNumber("Value", GetAverageVoltage()); - } + if (m_table != nullptr) { + m_table->PutNumber("Value", GetAverageVoltage()); + } } -void AnalogInput::StartLiveWindowMode() { - -} +void AnalogInput::StartLiveWindowMode() {} -void AnalogInput::StopLiveWindowMode() { - -} +void AnalogInput::StopLiveWindowMode() {} std::string AnalogInput::GetSmartDashboardType() const { - return "Analog Input"; + return "Analog Input"; } void AnalogInput::InitTable(std::shared_ptr subTable) { - m_table = subTable; - UpdateTable(); + m_table = subTable; + UpdateTable(); } -std::shared_ptr AnalogInput::GetTable() const { - return m_table; -} +std::shared_ptr AnalogInput::GetTable() const { return m_table; } diff --git a/wpilibc/simulation/src/AnalogPotentiometer.cpp b/wpilibc/simulation/src/AnalogPotentiometer.cpp index c71fda9462..2ed990b463 100644 --- a/wpilibc/simulation/src/AnalogPotentiometer.cpp +++ b/wpilibc/simulation/src/AnalogPotentiometer.cpp @@ -10,29 +10,33 @@ /** * Common initialization code called by all constructors. */ -void AnalogPotentiometer::initPot(AnalogInput *input, double scale, double offset) { - m_scale = scale; - m_offset = offset; - m_analog_input = input; +void AnalogPotentiometer::initPot(AnalogInput* input, double scale, + double offset) { + m_scale = scale; + m_offset = offset; + m_analog_input = input; } -AnalogPotentiometer::AnalogPotentiometer(int channel, double scale, double offset) { - m_init_analog_input = true; - initPot(new AnalogInput(channel), scale, offset); +AnalogPotentiometer::AnalogPotentiometer(int channel, double scale, + double offset) { + m_init_analog_input = true; + initPot(new AnalogInput(channel), scale, offset); } -AnalogPotentiometer::AnalogPotentiometer(AnalogInput *input, double scale, double offset) { - m_init_analog_input = false; - initPot(input, scale, offset); +AnalogPotentiometer::AnalogPotentiometer(AnalogInput* input, double scale, + double offset) { + m_init_analog_input = false; + initPot(input, scale, offset); } -AnalogPotentiometer::AnalogPotentiometer(AnalogInput &input, double scale, double offset) { - m_init_analog_input = false; - initPot(&input, scale, offset); +AnalogPotentiometer::AnalogPotentiometer(AnalogInput& input, double scale, + double offset) { + m_init_analog_input = false; + initPot(&input, scale, offset); } AnalogPotentiometer::~AnalogPotentiometer() { - if(m_init_analog_input){ + if (m_init_analog_input) { delete m_analog_input; m_init_analog_input = false; } @@ -44,7 +48,7 @@ AnalogPotentiometer::~AnalogPotentiometer() { * @return The current position of the potentiometer. */ double AnalogPotentiometer::Get() const { - return m_analog_input->GetVoltage() * m_scale + m_offset; + return m_analog_input->GetVoltage() * m_scale + m_offset; } /** @@ -52,32 +56,29 @@ double AnalogPotentiometer::Get() const { * * @return The current reading. */ -double AnalogPotentiometer::PIDGet() { - return Get(); -} - +double AnalogPotentiometer::PIDGet() { return Get(); } /** * @return the Smart Dashboard Type */ std::string AnalogPotentiometer::GetSmartDashboardType() const { - return "Analog Input"; + return "Analog Input"; } /** * Live Window code, only does anything if live window is activated. */ void AnalogPotentiometer::InitTable(std::shared_ptr subtable) { - m_table = subtable; - UpdateTable(); + m_table = subtable; + UpdateTable(); } void AnalogPotentiometer::UpdateTable() { - if (m_table != nullptr) { - m_table->PutNumber("Value", Get()); - } + if (m_table != nullptr) { + m_table->PutNumber("Value", Get()); + } } std::shared_ptr AnalogPotentiometer::GetTable() const { - return m_table; + return m_table; } diff --git a/wpilibc/simulation/src/DigitalInput.cpp b/wpilibc/simulation/src/DigitalInput.cpp index 5911fb3bf9..f59305fb2f 100644 --- a/wpilibc/simulation/src/DigitalInput.cpp +++ b/wpilibc/simulation/src/DigitalInput.cpp @@ -14,54 +14,41 @@ * * @param channel The digital channel (1..14). */ -DigitalInput::DigitalInput(uint32_t channel) -{ - char buf[64]; - m_channel = channel; - int n = sprintf(buf, "dio/%d", channel); - m_impl = new SimDigitalInput(buf); +DigitalInput::DigitalInput(uint32_t channel) { + char buf[64]; + m_channel = channel; + int n = sprintf(buf, "dio/%d", channel); + m_impl = new SimDigitalInput(buf); } -/* +/** * Get the value from a digital input channel. * Retrieve the value of a single digital input channel from the FPGA. */ -uint32_t DigitalInput::Get() const -{ - return m_impl->Get(); -} +uint32_t DigitalInput::Get() const { return m_impl->Get(); } /** * @return The GPIO channel number that this object represents. */ -uint32_t DigitalInput::GetChannel() const -{ - return m_channel; -} +uint32_t DigitalInput::GetChannel() const { return m_channel; } void DigitalInput::UpdateTable() { - if (m_table != nullptr) { - m_table->PutBoolean("Value", Get()); - } + if (m_table != nullptr) { + m_table->PutBoolean("Value", Get()); + } } -void DigitalInput::StartLiveWindowMode() { +void DigitalInput::StartLiveWindowMode() {} -} - -void DigitalInput::StopLiveWindowMode() { - -} +void DigitalInput::StopLiveWindowMode() {} std::string DigitalInput::GetSmartDashboardType() const { - return "DigitalInput"; + return "DigitalInput"; } void DigitalInput::InitTable(std::shared_ptr subTable) { - m_table = subTable; - UpdateTable(); + m_table = subTable; + UpdateTable(); } -std::shared_ptr DigitalInput::GetTable() const { - return m_table; -} +std::shared_ptr DigitalInput::GetTable() const { return m_table; } diff --git a/wpilibc/simulation/src/DoubleSolenoid.cpp b/wpilibc/simulation/src/DoubleSolenoid.cpp index 6180fb400a..0afe2bf921 100644 --- a/wpilibc/simulation/src/DoubleSolenoid.cpp +++ b/wpilibc/simulation/src/DoubleSolenoid.cpp @@ -6,9 +6,9 @@ /*----------------------------------------------------------------------------*/ #include "DoubleSolenoid.h" -#include "WPIErrors.h" #include #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" /** * Constructor. @@ -17,35 +17,35 @@ * @param reverseChannel The reverse channel on the module to control. */ DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel) - : DoubleSolenoid(1, forwardChannel, reverseChannel) {} + : DoubleSolenoid(1, forwardChannel, reverseChannel) {} /** * Constructor. * - * @param moduleNumber The solenoid module (1 or 2). + * @param moduleNumber The solenoid module (1 or 2). * @param forwardChannel The forward channel on the module to control. * @param reverseChannel The reverse channel on the module to control. */ -DoubleSolenoid::DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, uint32_t reverseChannel) -{ - m_reversed = false; - if (reverseChannel < forwardChannel) { // Swap ports to get the right address - int channel = reverseChannel; - reverseChannel = forwardChannel; - forwardChannel = channel; - m_reversed = true; - } - char buffer[50]; - int n = sprintf(buffer, "pneumatic/%d/%d/%d/%d", moduleNumber, - forwardChannel, moduleNumber, reverseChannel); - m_impl = new SimContinuousOutput(buffer); - - LiveWindow::GetInstance()->AddActuator("DoubleSolenoid", moduleNumber, - forwardChannel, this); +DoubleSolenoid::DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, + uint32_t reverseChannel) { + m_reversed = false; + if (reverseChannel < forwardChannel) { // Swap ports to get the right address + int channel = reverseChannel; + reverseChannel = forwardChannel; + forwardChannel = channel; + m_reversed = true; + } + char buffer[50]; + int n = sprintf(buffer, "pneumatic/%d/%d/%d/%d", moduleNumber, forwardChannel, + moduleNumber, reverseChannel); + m_impl = new SimContinuousOutput(buffer); + + LiveWindow::GetInstance()->AddActuator("DoubleSolenoid", moduleNumber, + forwardChannel, this); } DoubleSolenoid::~DoubleSolenoid() { - if (m_table != nullptr) m_table->RemoveTableListener(this); + if (m_table != nullptr) m_table->RemoveTableListener(this); } /** @@ -53,21 +53,19 @@ DoubleSolenoid::~DoubleSolenoid() { * * @param value Move the solenoid to forward, reverse, or don't move it. */ -void DoubleSolenoid::Set(Value value) -{ - m_value = value; - switch(value) - { - case kOff: - m_impl->Set(0); - break; - case kForward: - m_impl->Set(m_reversed ? -1 : 1); - break; - case kReverse: - m_impl->Set(m_reversed ? 1 : -1); - break; - } +void DoubleSolenoid::Set(Value value) { + m_value = value; + switch (value) { + case kOff: + m_impl->Set(0); + break; + case kForward: + m_impl->Set(m_reversed ? -1 : 1); + break; + case kReverse: + m_impl->Set(m_reversed ? 1 : -1); + break; + } } /** @@ -75,52 +73,49 @@ void DoubleSolenoid::Set(Value value) * * @return The current value of the solenoid. */ -DoubleSolenoid::Value DoubleSolenoid::Get() const -{ - return m_value; -} +DoubleSolenoid::Value DoubleSolenoid::Get() const { return m_value; } -void DoubleSolenoid::ValueChanged(ITable *source, llvm::StringRef key, +void DoubleSolenoid::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) { if (!value->IsString()) return; Value lvalue = kOff; - if (value->GetString() == "Forward") - lvalue = kForward; - else if (value->GetString() == "Reverse") - lvalue = kReverse; - Set(lvalue); + if (value->GetString() == "Forward") + lvalue = kForward; + else if (value->GetString() == "Reverse") + lvalue = kReverse; + Set(lvalue); } void DoubleSolenoid::UpdateTable() { - if (m_table != nullptr) { - m_table->PutString("Value", (Get() == kForward ? "Forward" : (Get() == kReverse ? "Reverse" : "Off"))); - } + if (m_table != nullptr) { + m_table->PutString( + "Value", (Get() == kForward ? "Forward" + : (Get() == kReverse ? "Reverse" : "Off"))); + } } void DoubleSolenoid::StartLiveWindowMode() { - Set(kOff); - if (m_table != nullptr) { - m_table->AddTableListener("Value", this, true); - } + Set(kOff); + if (m_table != nullptr) { + m_table->AddTableListener("Value", this, true); + } } void DoubleSolenoid::StopLiveWindowMode() { - Set(kOff); - if (m_table != nullptr) { - m_table->RemoveTableListener(this); - } + Set(kOff); + if (m_table != nullptr) { + m_table->RemoveTableListener(this); + } } std::string DoubleSolenoid::GetSmartDashboardType() const { - return "Double Solenoid"; + return "Double Solenoid"; } void DoubleSolenoid::InitTable(std::shared_ptr subTable) { - m_table = subTable; - UpdateTable(); + m_table = subTable; + UpdateTable(); } -std::shared_ptr DoubleSolenoid::GetTable() const { - return m_table; -} +std::shared_ptr DoubleSolenoid::GetTable() const { return m_table; } diff --git a/wpilibc/simulation/src/DriverStation.cpp b/wpilibc/simulation/src/DriverStation.cpp index 274cb1870f..3418af6204 100644 --- a/wpilibc/simulation/src/DriverStation.cpp +++ b/wpilibc/simulation/src/DriverStation.cpp @@ -9,18 +9,20 @@ #include "Timer.h" #include "simulation/MainNode.h" //#include "MotorSafetyHelper.h" -#include "Utility.h" -#include "WPIErrors.h" #include #include "Log.hpp" +#include "Utility.h" +#include "WPIErrors.h" #include "boost/mem_fn.hpp" // set the logging level TLogLevel dsLogLevel = logDEBUG; -#define DS_LOG(level) \ - if (level > dsLogLevel) ; \ - else Log().Get(level) +#define DS_LOG(level) \ + if (level > dsLogLevel) \ + ; \ + else \ + Log().Get(level) const uint32_t DriverStation::kBatteryChannel; const uint32_t DriverStation::kJoystickPorts; @@ -34,39 +36,38 @@ uint8_t DriverStation::m_updateNumber = 0; * This is only called once the first time GetInstance() is called */ DriverStation::DriverStation() { - state = msgs::DriverStationPtr(new msgs::DriverStation()); - stateSub = MainNode::Subscribe("~/ds/state", - &DriverStation::stateCallback, this); - // TODO: for loop + boost bind - joysticks[0] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); - joysticksSub[0] = MainNode::Subscribe("~/ds/joysticks/0", - &DriverStation::joystickCallback0, this); - joysticks[1] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); - joysticksSub[1] = MainNode::Subscribe("~/ds/joysticks/1", - &DriverStation::joystickCallback1, this); - joysticks[2] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); - joysticksSub[2] = MainNode::Subscribe("~/ds/joysticks/2", - &DriverStation::joystickCallback2, this); - joysticks[3] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); - joysticksSub[3] = MainNode::Subscribe("~/ds/joysticks/5", - &DriverStation::joystickCallback3, this); - joysticks[4] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); - joysticksSub[4] = MainNode::Subscribe("~/ds/joysticks/4", - &DriverStation::joystickCallback4, this); - joysticks[5] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); - joysticksSub[5] = MainNode::Subscribe("~/ds/joysticks/5", - &DriverStation::joystickCallback5, this); + state = msgs::DriverStationPtr(new msgs::DriverStation()); + stateSub = + MainNode::Subscribe("~/ds/state", &DriverStation::stateCallback, this); + // TODO: for loop + boost bind + joysticks[0] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); + joysticksSub[0] = MainNode::Subscribe( + "~/ds/joysticks/0", &DriverStation::joystickCallback0, this); + joysticks[1] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); + joysticksSub[1] = MainNode::Subscribe( + "~/ds/joysticks/1", &DriverStation::joystickCallback1, this); + joysticks[2] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); + joysticksSub[2] = MainNode::Subscribe( + "~/ds/joysticks/2", &DriverStation::joystickCallback2, this); + joysticks[3] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); + joysticksSub[3] = MainNode::Subscribe( + "~/ds/joysticks/5", &DriverStation::joystickCallback3, this); + joysticks[4] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); + joysticksSub[4] = MainNode::Subscribe( + "~/ds/joysticks/4", &DriverStation::joystickCallback4, this); + joysticks[5] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); + joysticksSub[5] = MainNode::Subscribe( + "~/ds/joysticks/5", &DriverStation::joystickCallback5, this); - AddToSingletonList(); + AddToSingletonList(); } /** * Return a pointer to the singleton DriverStation. */ -DriverStation& DriverStation::GetInstance() -{ - static DriverStation instance; - return instance; +DriverStation& DriverStation::GetInstance() { + static DriverStation instance; + return instance; } /** @@ -74,9 +75,8 @@ DriverStation& DriverStation::GetInstance() * * @return The battery voltage. */ -float DriverStation::GetBatteryVoltage() const -{ - return 12.0; // 12 volts all the time! +float DriverStation::GetBatteryVoltage() const { + return 12.0; // 12 volts all the time! } /** @@ -84,80 +84,75 @@ float DriverStation::GetBatteryVoltage() const * This depends on the mapping of the joystick connected to the specified port. * * @param stick The joystick to read. - * @param axis The analog axis value to read from the joystick. + * @param axis The analog axis value to read from the joystick. * @return The value of the axis on the joystick. */ -float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) -{ - if (axis < 0 || axis > (kJoystickAxes - 1)) - { - wpi_setWPIError(BadJoystickAxis); - return 0.0; - } - if (stick < 0 || stick > 5) - { - wpi_setWPIError(BadJoystickIndex); - return 0.0; - } +float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) { + if (axis < 0 || axis > (kJoystickAxes - 1)) { + wpi_setWPIError(BadJoystickAxis); + return 0.0; + } + if (stick < 0 || stick > 5) { + wpi_setWPIError(BadJoystickIndex); + return 0.0; + } - std::unique_lock lock(m_joystickMutex); - if (joysticks[stick] == nullptr || axis >= joysticks[stick]->axes().size()) - { - return 0.0; - } - return joysticks[stick]->axes(axis); + std::unique_lock lock(m_joystickMutex); + if (joysticks[stick] == nullptr || axis >= joysticks[stick]->axes().size()) { + return 0.0; + } + return joysticks[stick]->axes(axis); } /** * The state of a specific button (1 - 12) on the joystick. - * This method only works in simulation, but is more efficient than GetStickButtons. * - * @param stick The joystick to read. + * This method only works in simulation, but is more efficient than + * GetStickButtons. + * + * @param stick The joystick to read. * @param button The button number to check. * @return If the button is pressed. */ -bool DriverStation::GetStickButton(uint32_t stick, uint32_t button) -{ - if (stick < 0 || stick >= 6) - { - wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5"); - return false; - } +bool DriverStation::GetStickButton(uint32_t stick, uint32_t button) { + if (stick < 0 || stick >= 6) { + wpi_setWPIErrorWithContext(ParameterOutOfRange, + "stick must be between 0 and 5"); + return false; + } - std::unique_lock lock(m_joystickMutex); - if (joysticks[stick] == nullptr || button >= joysticks[stick]->buttons().size()) - { - return false; - } - return joysticks[stick]->buttons(button-1); + std::unique_lock lock(m_joystickMutex); + if (joysticks[stick] == nullptr || + button >= joysticks[stick]->buttons().size()) { + return false; + } + return joysticks[stick]->buttons(button - 1); } /** * The state of the buttons on the joystick. + * * 12 buttons (4 msb are unused) from the joystick. * * @param stick The joystick to read. * @return The state of the buttons on the joystick. */ -short DriverStation::GetStickButtons(uint32_t stick) -{ - if (stick < 0 || stick >= 6) - { - wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5"); - return false; - } - short btns = 0, btnid; +short DriverStation::GetStickButtons(uint32_t stick) { + if (stick < 0 || stick >= 6) { + wpi_setWPIErrorWithContext(ParameterOutOfRange, + "stick must be between 0 and 5"); + return false; + } + short btns = 0, btnid; - std::unique_lock lock(m_joystickMutex); - msgs::FRCJoystickPtr joy = joysticks[stick]; - for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++) - { - if (joysticks[stick]->buttons(btnid)) - { - btns |= (1 << btnid); - } - } - return btns; + std::unique_lock lock(m_joystickMutex); + msgs::FRCJoystickPtr joy = joysticks[stick]; + for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++) { + if (joysticks[stick]->buttons(btnid)) { + btns |= (1 << btnid); + } + } + return btns; } // 5V divided by 10 bits @@ -165,163 +160,153 @@ short DriverStation::GetStickButtons(uint32_t stick) /** * Get an analog voltage from the Driver Station. - * The analog values are returned as voltage values for the Driver Station analog inputs. - * These inputs are typically used for advanced operator interfaces consisting of potentiometers - * or resistor networks representing values on a rotary switch. * - * @param channel The analog input channel on the driver station to read from. Valid range is 1 - 4. + * The analog values are returned as voltage values for the Driver Station + * analog inputs. These inputs are typically used for advanced operator + * interfaces consisting of potentiometers or resistor networks representing + * values on a rotary switch. + * + * @param channel The analog input channel on the driver station to read from. + * Valid range is 1 - 4. * @return The analog voltage on the input. */ -float DriverStation::GetAnalogIn(uint32_t channel) -{ - wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetAnalogIn"); - return 0.0; +float DriverStation::GetAnalogIn(uint32_t channel) { + wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetAnalogIn"); + return 0.0; } /** * Get values from the digital inputs on the Driver Station. - * Return digital values from the Drivers Station. These values are typically used for buttons - * and switches on advanced operator interfaces. + * + * Return digital values from the Drivers Station. These values are typically + * used for buttons and switches on advanced operator interfaces. + * * @param channel The digital input to get. Valid range is 1 - 8. */ -bool DriverStation::GetDigitalIn(uint32_t channel) -{ - wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalIn"); - return false; +bool DriverStation::GetDigitalIn(uint32_t channel) { + wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalIn"); + return false; } /** * Set a value for the digital outputs on the Driver Station. * - * Control digital outputs on the Drivers Station. These values are typically used for - * giving feedback on a custom operator station such as LEDs. + * Control digital outputs on the Drivers Station. These values are typically + * used for giving feedback on a custom operator station such as LEDs. * * @param channel The digital output to set. Valid range is 1 - 8. - * @param value The state to set the digital output. + * @param value The state to set the digital output. */ -void DriverStation::SetDigitalOut(uint32_t channel, bool value) -{ - wpi_setWPIErrorWithContext(UnsupportedInSimulation, "SetDigitalOut"); +void DriverStation::SetDigitalOut(uint32_t channel, bool value) { + wpi_setWPIErrorWithContext(UnsupportedInSimulation, "SetDigitalOut"); } /** * Get a value that was set for the digital outputs on the Driver Station. + * * @param channel The digital ouput to monitor. Valid range is 1 through 8. * @return A digital value being output on the Drivers Station. */ -bool DriverStation::GetDigitalOut(uint32_t channel) -{ - wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalOut"); - return false; +bool DriverStation::GetDigitalOut(uint32_t channel) { + wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalOut"); + return false; } -bool DriverStation::IsEnabled() const -{ - std::unique_lock lock(m_stateMutex); - return state != nullptr ? state->enabled() : false; +bool DriverStation::IsEnabled() const { + std::unique_lock lock(m_stateMutex); + return state != nullptr ? state->enabled() : false; } -bool DriverStation::IsDisabled() const -{ - return !IsEnabled(); +bool DriverStation::IsDisabled() const { return !IsEnabled(); } + +bool DriverStation::IsAutonomous() const { + std::unique_lock lock(m_stateMutex); + return state != nullptr ? state->state() == msgs::DriverStation_State_AUTO + : false; } -bool DriverStation::IsAutonomous() const -{ - std::unique_lock lock(m_stateMutex); - return state != nullptr ? - state->state() == msgs::DriverStation_State_AUTO : false; +bool DriverStation::IsOperatorControl() const { + return !(IsAutonomous() || IsTest()); } -bool DriverStation::IsOperatorControl() const -{ - return !(IsAutonomous() || IsTest()); -} - -bool DriverStation::IsTest() const -{ - std::unique_lock lock(m_stateMutex); - return state != nullptr ? - state->state() == msgs::DriverStation_State_TEST : false; +bool DriverStation::IsTest() const { + std::unique_lock lock(m_stateMutex); + return state != nullptr ? state->state() == msgs::DriverStation_State_TEST + : false; } /** * Is the driver station attached to a Field Management System? * Note: This does not work with the Blue DS. - * @return True if the robot is competing on a field being controlled by a Field Management System + * @return True if the robot is competing on a field being controlled by a Field + * Management System */ -bool DriverStation::IsFMSAttached() const -{ - return false; // No FMS in simulation +bool DriverStation::IsFMSAttached() const { + return false; // No FMS in simulation } /** * Return the alliance that the driver station says it is on. - * This could return kRed or kBlue + * This could return kRed or kBlue. * @return The Alliance enum */ -DriverStation::Alliance DriverStation::GetAlliance() const -{ - // if (m_controlData->dsID_Alliance == 'R') return kRed; - // if (m_controlData->dsID_Alliance == 'B') return kBlue; - // wpi_assert(false); - return kInvalid; // TODO: Support alliance colors +DriverStation::Alliance DriverStation::GetAlliance() const { + // if (m_controlData->dsID_Alliance == 'R') return kRed; + // if (m_controlData->dsID_Alliance == 'B') return kBlue; + // wpi_assert(false); + return kInvalid; // TODO: Support alliance colors } /** - * Return the driver station location on the field - * This could return 1, 2, or 3 + * Return the driver station location on the field. + * This could return 1, 2, or 3. * @return The location of the driver station */ -uint32_t DriverStation::GetLocation() const -{ - return -1; // TODO: Support locations +uint32_t DriverStation::GetLocation() const { + return -1; // TODO: Support locations } /** - * Wait until a new packet comes from the driver station + * Wait until a new packet comes from the driver station. * This blocks on a semaphore, so the waiting is efficient. - * This is a good way to delay processing until there is new driver station data to act on + * This is a good way to delay processing until there is new driver station data + * to act on. */ -void DriverStation::WaitForData() -{ - std::unique_lock lock(m_waitForDataMutex); - m_waitForDataCond.wait(lock); +void DriverStation::WaitForData() { + std::unique_lock lock(m_waitForDataMutex); + m_waitForDataCond.wait(lock); } /** - * Return the approximate match time + * Return the approximate match time. * The FMS does not currently send the official match time to the robots * This returns the time since the enable signal sent from the Driver Station * At the beginning of autonomous, the time is reset to 0.0 seconds * At the beginning of teleop, the time is reset to +15.0 seconds * If the robot is disabled, this returns 0.0 seconds - * Warning: This is not an official time (so it cannot be used to argue with referees) + * Warning: This is not an official time (so it cannot be used to argue with + * referees) * @return Match time in seconds since the beginning of autonomous */ -double DriverStation::GetMatchTime() const -{ - if (m_approxMatchTimeOffset < 0.0) - return 0.0; - return Timer::GetFPGATimestamp() - m_approxMatchTimeOffset; +double DriverStation::GetMatchTime() const { + if (m_approxMatchTimeOffset < 0.0) return 0.0; + return Timer::GetFPGATimestamp() - m_approxMatchTimeOffset; } /** * Report an error to the DriverStation messages window. * The error is also printed to the program console. */ -void DriverStation::ReportError(std::string error) -{ - std::cout << error << std::endl; +void DriverStation::ReportError(std::string error) { + std::cout << error << std::endl; } /** * Report a warning to the DriverStation messages window. * The warning is also printed to the program console. */ -void DriverStation::ReportWarning(std::string error) -{ - std::cout << error << std::endl; +void DriverStation::ReportWarning(std::string error) { + std::cout << error << std::endl; } /** @@ -331,66 +316,53 @@ void DriverStation::ReportWarning(std::string error) void DriverStation::ReportError(bool is_error, int32_t code, const std::string& error, const std::string& location, - const std::string& stack) -{ - if (!location.empty()) - std::cout << (is_error ? "Error" : "Warning") << " at " << location << ": "; - std::cout << error << std::endl; - if (!stack.empty()) - std::cout << stack << std::endl; + const std::string& stack) { + if (!location.empty()) + std::cout << (is_error ? "Error" : "Warning") << " at " << location << ": "; + std::cout << error << std::endl; + if (!stack.empty()) std::cout << stack << std::endl; } /** - * Return the team number that the Driver Station is configured for + * Return the team number that the Driver Station is configured for. * @return The team number */ -uint16_t DriverStation::GetTeamNumber() const -{ - return 348; +uint16_t DriverStation::GetTeamNumber() const { return 348; } + +void DriverStation::stateCallback(const msgs::ConstDriverStationPtr& msg) { + { + std::unique_lock lock(m_stateMutex); + *state = *msg; + } + m_waitForDataCond.notify_all(); } -void DriverStation::stateCallback(const msgs::ConstDriverStationPtr &msg) -{ - { - std::unique_lock lock(m_stateMutex); - *state = *msg; - } - m_waitForDataCond.notify_all(); +void DriverStation::joystickCallback(const msgs::ConstFRCJoystickPtr& msg, + int i) { + std::unique_lock lock(m_joystickMutex); + *(joysticks[i]) = *msg; } -void DriverStation::joystickCallback(const msgs::ConstFRCJoystickPtr &msg, - int i) -{ - std::unique_lock lock(m_joystickMutex); - *(joysticks[i]) = *msg; +void DriverStation::joystickCallback0(const msgs::ConstFRCJoystickPtr& msg) { + joystickCallback(msg, 0); } -void DriverStation::joystickCallback0(const msgs::ConstFRCJoystickPtr &msg) -{ - joystickCallback(msg, 0); +void DriverStation::joystickCallback1(const msgs::ConstFRCJoystickPtr& msg) { + joystickCallback(msg, 1); } -void DriverStation::joystickCallback1(const msgs::ConstFRCJoystickPtr &msg) -{ - joystickCallback(msg, 1); +void DriverStation::joystickCallback2(const msgs::ConstFRCJoystickPtr& msg) { + joystickCallback(msg, 2); } -void DriverStation::joystickCallback2(const msgs::ConstFRCJoystickPtr &msg) -{ - joystickCallback(msg, 2); +void DriverStation::joystickCallback3(const msgs::ConstFRCJoystickPtr& msg) { + joystickCallback(msg, 3); } -void DriverStation::joystickCallback3(const msgs::ConstFRCJoystickPtr &msg) -{ - joystickCallback(msg, 3); +void DriverStation::joystickCallback4(const msgs::ConstFRCJoystickPtr& msg) { + joystickCallback(msg, 4); } -void DriverStation::joystickCallback4(const msgs::ConstFRCJoystickPtr &msg) -{ - joystickCallback(msg, 4); -} - -void DriverStation::joystickCallback5(const msgs::ConstFRCJoystickPtr &msg) -{ - joystickCallback(msg, 5); +void DriverStation::joystickCallback5(const msgs::ConstFRCJoystickPtr& msg) { + joystickCallback(msg, 5); } diff --git a/wpilibc/simulation/src/Encoder.cpp b/wpilibc/simulation/src/Encoder.cpp index 565a8eb70e..579d2cad48 100644 --- a/wpilibc/simulation/src/Encoder.cpp +++ b/wpilibc/simulation/src/Encoder.cpp @@ -6,9 +6,9 @@ /*----------------------------------------------------------------------------*/ #include "Encoder.h" +#include "LiveWindow/LiveWindow.h" #include "Resource.h" #include "WPIErrors.h" -#include "LiveWindow/LiveWindow.h" /** * Common initialization code for Encoders. @@ -16,302 +16,327 @@ * * The counter will start counting immediately. * - * @param reverseDirection If true, counts down instead of up (this is all relative) - * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is - * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder - * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then - * a counter object will be used and the returned value will either exactly match the spec'd count - * or be double (2x) the spec'd count. + * @param reverseDirection If true, counts down instead of up (this is all + * relative) + * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X + * decoding. If 4X is selected, then an encoder FPGA + * object is used and the returned counts will be 4x + * the encoder spec'd value since all rising and + * falling edges are counted. If 1X or 2X are selected + * then a counter object will be used and the returned + * value will either exactly match the spec'd count or + * be double (2x) the spec'd count. */ -void Encoder::InitEncoder(int channelA, int channelB, bool reverseDirection, EncodingType encodingType) -{ - m_table = nullptr; - this->channelA = channelA; - this->channelB = channelB; - m_encodingType = encodingType; - m_encodingScale = encodingType == k4X ? 4 - : encodingType == k2X ? 2 - : 1; +void Encoder::InitEncoder(int channelA, int channelB, bool reverseDirection, + EncodingType encodingType) { + m_table = nullptr; + this->channelA = channelA; + this->channelB = channelB; + m_encodingType = encodingType; + m_encodingScale = encodingType == k4X ? 4 : encodingType == k2X ? 2 : 1; - int32_t index = 0; - m_distancePerPulse = 1.0; + int32_t index = 0; + m_distancePerPulse = 1.0; - LiveWindow::GetInstance()->AddSensor("Encoder", channelA, this); + LiveWindow::GetInstance()->AddSensor("Encoder", channelA, this); - if (channelB < channelA) { // Swap ports - int channel = channelB; - channelB = channelA; - channelA = channel; - m_reverseDirection = !reverseDirection; - } else { - m_reverseDirection = reverseDirection; - } - char buffer[50]; - int n = sprintf(buffer, "dio/%d/%d", channelA, channelB); - impl = new SimEncoder(buffer); - impl->Start(); + if (channelB < channelA) { // Swap ports + int channel = channelB; + channelB = channelA; + channelA = channel; + m_reverseDirection = !reverseDirection; + } else { + m_reverseDirection = reverseDirection; + } + char buffer[50]; + int n = sprintf(buffer, "dio/%d/%d", channelA, channelB); + impl = new SimEncoder(buffer); + impl->Start(); } /** * Encoder constructor. + * * Construct a Encoder given a and b channels. * * The counter will start counting immediately. * - * @param aChannel The a channel digital input channel. - * @param bChannel The b channel digital input channel. - * @param reverseDirection represents the orientation of the encoder and inverts the output values - * if necessary so forward represents positive values. - * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is - * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder - * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then - * a counter object will be used and the returned value will either exactly match the spec'd count - * or be double (2x) the spec'd count. + * @param aChannel The a channel digital input channel. + * @param bChannel The b channel digital input channel. + * @param reverseDirection If true, counts down instead of up (this is all + * relative) + * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X + * decoding. If 4X is selected, then an encoder FPGA + * object is used and the returned counts will be 4x + * the encoder spec'd value since all rising and + * falling edges are counted. If 1X or 2X are selected + * then a counter object will be used and the returned + * value will either exactly match the spec'd count or + * be double (2x) the spec'd count. */ -Encoder::Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection, EncodingType encodingType) -{ - InitEncoder(aChannel, bChannel, reverseDirection, encodingType); +Encoder::Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection, + EncodingType encodingType) { + InitEncoder(aChannel, bChannel, reverseDirection, encodingType); } /** * Encoder constructor. - * Construct a Encoder given a and b channels as digital inputs. This is used in the case - * where the digital inputs are shared. The Encoder class will not allocate the digital inputs - * and assume that they already are counted. + * + * Construct a Encoder given a and b channels as digital inputs. This is used in + * the case where the digital inputs are shared. The Encoder class will not + * allocate the digital inputs and assume that they already are counted. * * The counter will start counting immediately. * - * @param aSource The source that should be used for the a channel. - * @param bSource the source that should be used for the b channel. - * @param reverseDirection represents the orientation of the encoder and inverts the output values - * if necessary so forward represents positive values. - * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is - * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder - * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then - * a counter object will be used and the returned value will either exactly match the spec'd count - * or be double (2x) the spec'd count. + * @param aSource The source that should be used for the a channel. + * @param bSource the source that should be used for the b channel. + * @param reverseDirection If true, counts down instead of up (this is all + * relative) + * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X + * decoding. If 4X is selected, then an encoder FPGA + * object is used and the returned counts will be 4x + * the encoder spec'd value since all rising and + * falling edges are counted. If 1X or 2X are selected + * then a counter object will be used and the returned + * value will either exactly match the spec'd count or + * be double (2x) the spec'd count. */ -/* TODO: [Not Supported] Encoder::Encoder(DigitalSource *aSource, DigitalSource *bSource, bool reverseDirection, EncodingType encodingType) : - m_encoder(nullptr), - m_counter(nullptr) +/* TODO: [Not Supported] Encoder::Encoder(DigitalSource *aSource, DigitalSource +*bSource, bool reverseDirection, EncodingType encodingType) : + m_encoder(nullptr), + m_counter(nullptr) { - m_aSource = aSource; - m_bSource = bSource; - m_allocatedASource = false; - m_allocatedBSource = false; - if (m_aSource == nullptr || m_bSource == nullptr) - wpi_setWPIError(NullParameter); - else - InitEncoder(reverseDirection, encodingType); + m_aSource = aSource; + m_bSource = bSource; + m_allocatedASource = false; + m_allocatedBSource = false; + if (m_aSource == nullptr || m_bSource == nullptr) + wpi_setWPIError(NullParameter); + else + InitEncoder(reverseDirection, encodingType); }*/ /** * Encoder constructor. - * Construct a Encoder given a and b channels as digital inputs. This is used in the case - * where the digital inputs are shared. The Encoder class will not allocate the digital inputs - * and assume that they already are counted. + * + * Construct a Encoder given a and b channels as digital inputs. This is used in + * the case where the digital inputs are shared. The Encoder class will not + * allocate the digital inputs and assume that they already are counted. * * The counter will start counting immediately. * - * @param aSource The source that should be used for the a channel. - * @param bSource the source that should be used for the b channel. - * @param reverseDirection represents the orientation of the encoder and inverts the output values - * if necessary so forward represents positive values. - * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is - * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder - * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then - * a counter object will be used and the returned value will either exactly match the spec'd count - * or be double (2x) the spec'd count. + * @param aSource The source that should be used for the a channel. + * @param bSource the source that should be used for the b channel. + * @param reverseDirection If true, counts down instead of up (this is all + * relative) + * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X + * decoding. If 4X is selected, then an encoder FPGA + * object is used and the returned counts will be 4x + * the encoder spec'd value since all rising and + * falling edges are counted. If 1X or 2X are selected + * then a counter object will be used and the returned + * value will either exactly match the spec'd count or + * be double (2x) the spec'd count. */ -/*// TODO: [Not Supported] Encoder::Encoder(DigitalSource &aSource, DigitalSource &bSource, bool reverseDirection, EncodingType encodingType) : - m_encoder(nullptr), - m_counter(nullptr) +/*// TODO: [Not Supported] Encoder::Encoder(DigitalSource &aSource, +DigitalSource &bSource, bool reverseDirection, EncodingType encodingType) : + m_encoder(nullptr), + m_counter(nullptr) { - m_aSource = &aSource; - m_bSource = &bSource; - m_allocatedASource = false; - m_allocatedBSource = false; - InitEncoder(reverseDirection, encodingType); + m_aSource = &aSource; + m_bSource = &bSource; + m_allocatedASource = false; + m_allocatedBSource = false; + InitEncoder(reverseDirection, encodingType); }*/ /** * Reset the Encoder distance to zero. + * * Resets the current count to zero on the encoder. */ -void Encoder::Reset() -{ - impl->Reset(); -} +void Encoder::Reset() { impl->Reset(); } /** * Determine if the encoder is stopped. - * Using the MaxPeriod value, a boolean is returned that is true if the encoder is considered - * stopped and false if it is still moving. A stopped encoder is one where the most recent pulse - * width exceeds the MaxPeriod. + * + * Using the MaxPeriod value, a boolean is returned that is true if the encoder + * is considered stopped and false if it is still moving. A stopped encoder is + * one where the most recent pulse width exceeds the MaxPeriod. + * * @return True if the encoder is considered stopped. */ -bool Encoder::GetStopped() const -{ - throw "Simulation doesn't currently support this method."; +bool Encoder::GetStopped() const { + throw "Simulation doesn't currently support this method."; } /** * The last direction the encoder value changed. + * * @return The last direction the encoder value changed. */ -bool Encoder::GetDirection() const -{ - throw "Simulation doesn't currently support this method."; +bool Encoder::GetDirection() const { + throw "Simulation doesn't currently support this method."; } /** - * The scale needed to convert a raw counter value into a number of encoder pulses. + * The scale needed to convert a raw counter value into a number of encoder + * pulses. */ -double Encoder::DecodingScaleFactor() const -{ - switch (m_encodingType) - { - case k1X: - return 1.0; - case k2X: - return 0.5; - case k4X: - return 0.25; - default: - return 0.0; - } +double Encoder::DecodingScaleFactor() const { + switch (m_encodingType) { + case k1X: + return 1.0; + case k2X: + return 0.5; + case k4X: + return 0.25; + default: + return 0.0; + } } /** * The encoding scale factor 1x, 2x, or 4x, per the requested encodingType. + * * Used to divide raw edge counts down to spec'd counts. */ int32_t Encoder::GetEncodingScale() const { return m_encodingScale; } /** * Gets the raw value from the encoder. + * * The raw value is the actual count unscaled by the 1x, 2x, or 4x scale * factor. + * * @return Current raw count from the encoder */ -int32_t Encoder::GetRaw() const -{ - throw "Simulation doesn't currently support this method."; +int32_t Encoder::GetRaw() const { + throw "Simulation doesn't currently support this method."; } /** * Gets the current count. + * * Returns the current count on the Encoder. * This method compensates for the decoding type. * - * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor. + * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale + * factor. */ -int32_t Encoder::Get() const -{ - throw "Simulation doesn't currently support this method."; +int32_t Encoder::Get() const { + throw "Simulation doesn't currently support this method."; } /** * Returns the period of the most recent pulse. + * * Returns the period of the most recent Encoder pulse in seconds. * This method compenstates for the decoding type. * - * @deprecated Use GetRate() in favor of this method. This returns unscaled periods and GetRate() scales using value from SetDistancePerPulse(). + * @deprecated Use GetRate() in favor of this method. This returns unscaled + * periods and GetRate() scales using value from + * SetDistancePerPulse(). * * @return Period in seconds of the most recent pulse. */ -double Encoder::GetPeriod() const -{ - throw "Simulation doesn't currently support this method."; +double Encoder::GetPeriod() const { + throw "Simulation doesn't currently support this method."; } /** * Sets the maximum period for stopped detection. - * Sets the value that represents the maximum period of the Encoder before it will assume - * that the attached device is stopped. This timeout allows users to determine if the wheels or - * other shaft has stopped rotating. + * + * Sets the value that represents the maximum period of the Encoder before it + * will assume that the attached device is stopped. This timeout allows users + * to determine if the wheels or other shaft has stopped rotating. * This method compensates for the decoding type. * - * @deprecated Use SetMinRate() in favor of this method. This takes unscaled periods and SetMinRate() scales using value from SetDistancePerPulse(). + * @deprecated Use SetMinRate() in favor of this method. This takes unscaled + * periods and SetMinRate() scales using value from + * SetDistancePerPulse(). * - * @param maxPeriod The maximum time between rising and falling edges before the FPGA will - * report the device stopped. This is expressed in seconds. + * @param maxPeriod The maximum time between rising and falling edges before the + * FPGA will report the device stopped. This is expressed in + * seconds. */ -void Encoder::SetMaxPeriod(double maxPeriod) -{ - throw "Simulation doesn't currently support this method."; +void Encoder::SetMaxPeriod(double maxPeriod) { + throw "Simulation doesn't currently support this method."; } /** * Get the distance the robot has driven since the last reset. * - * @return The distance driven since the last reset as scaled by the value from SetDistancePerPulse(). + * @return The distance driven since the last reset as scaled by the value from + * SetDistancePerPulse(). */ -double Encoder::GetDistance() const -{ - return m_distancePerPulse * impl->GetPosition(); +double Encoder::GetDistance() const { + return m_distancePerPulse * impl->GetPosition(); } /** * Get the current rate of the encoder. - * Units are distance per second as scaled by the value from SetDistancePerPulse(). + * + * Units are distance per second as scaled by the value from + * SetDistancePerPulse(). * * @return The current rate of the encoder. */ -double Encoder::GetRate() const -{ - return m_distancePerPulse * impl->GetVelocity(); +double Encoder::GetRate() const { + return m_distancePerPulse * impl->GetVelocity(); } /** * Set the minimum rate of the device before the hardware reports it stopped. * - * @param minRate The minimum rate. The units are in distance per second as scaled by the value from SetDistancePerPulse(). + * @param minRate The minimum rate. The units are in distance per second as + * scaled by the value from SetDistancePerPulse(). */ -void Encoder::SetMinRate(double minRate) -{ - throw "Simulation doesn't currently support this method."; +void Encoder::SetMinRate(double minRate) { + throw "Simulation doesn't currently support this method."; } /** * Set the distance per pulse for this encoder. - * This sets the multiplier used to determine the distance driven based on the count value - * from the encoder. - * Do not include the decoding type in this scale. The library already compensates for the decoding type. - * Set this value based on the encoder's rated Pulses per Revolution and - * factor in gearing reductions following the encoder shaft. - * This distance can be in any units you like, linear or angular. * - * @param distancePerPulse The scale factor that will be used to convert pulses to useful units. + * This sets the multiplier used to determine the distance driven based on the + * count value from the encoder. Do not include the decoding type in this scale. + * The library already compensates for the decoding type. Set this value based + * on the encoder's rated Pulses per Revolution and factor in gearing reductions + * following the encoder shaft. This distance can be in any units you like, + * linear or angular. + * + * @param distancePerPulse The scale factor that will be used to convert pulses + * to useful units. */ -void Encoder::SetDistancePerPulse(double distancePerPulse) -{ - if (m_reverseDirection) { - m_distancePerPulse = -distancePerPulse; - } else { - m_distancePerPulse = distancePerPulse; - } +void Encoder::SetDistancePerPulse(double distancePerPulse) { + if (m_reverseDirection) { + m_distancePerPulse = -distancePerPulse; + } else { + m_distancePerPulse = distancePerPulse; + } } /** * Set the direction sensing for this encoder. - * This sets the direction sensing on the encoder so that it could count in the correct - * software direction regardless of the mounting. + * + * This sets the direction sensing on the encoder so that it could count in the + * correct software direction regardless of the mounting. + * * @param reverseDirection true if the encoder direction should be reversed */ -void Encoder::SetReverseDirection(bool reverseDirection) -{ - throw "Simulation doesn't currently support this method."; +void Encoder::SetReverseDirection(bool reverseDirection) { + throw "Simulation doesn't currently support this method."; } /** - * Set which parameter of the encoder you are using as a process control variable. + * Set which parameter of the encoder you are using as a process control + * variable. * * @param pidSource An enum to select the parameter. */ -void Encoder::SetPIDSourceType(PIDSourceType pidSource) -{ - m_pidSource = pidSource; +void Encoder::SetPIDSourceType(PIDSourceType pidSource) { + m_pidSource = pidSource; } /** @@ -319,47 +344,41 @@ void Encoder::SetPIDSourceType(PIDSourceType pidSource) * * @return The current value of the selected source parameter. */ -double Encoder::PIDGet() -{ - switch (m_pidSource) - { - case PIDSourceType::kDisplacement: - return GetDistance(); - case PIDSourceType::kRate: - return GetRate(); - default: - return 0.0; - } +double Encoder::PIDGet() { + switch (m_pidSource) { + case PIDSourceType::kDisplacement: + return GetDistance(); + case PIDSourceType::kRate: + return GetRate(); + default: + return 0.0; + } } void Encoder::UpdateTable() { - if (m_table != nullptr) { - m_table->PutNumber("Speed", GetRate()); - m_table->PutNumber("Distance", GetDistance()); - m_table->PutNumber("Distance per Tick", m_reverseDirection ? -m_distancePerPulse : m_distancePerPulse); - } + if (m_table != nullptr) { + m_table->PutNumber("Speed", GetRate()); + m_table->PutNumber("Distance", GetDistance()); + m_table->PutNumber("Distance per Tick", m_reverseDirection + ? -m_distancePerPulse + : m_distancePerPulse); + } } -void Encoder::StartLiveWindowMode() { +void Encoder::StartLiveWindowMode() {} -} - -void Encoder::StopLiveWindowMode() { - -} +void Encoder::StopLiveWindowMode() {} std::string Encoder::GetSmartDashboardType() const { - if (m_encodingType == k4X) - return "Quadrature Encoder"; - else - return "Encoder"; + if (m_encodingType == k4X) + return "Quadrature Encoder"; + else + return "Encoder"; } void Encoder::InitTable(std::shared_ptr subTable) { - m_table = subTable; - UpdateTable(); + m_table = subTable; + UpdateTable(); } -std::shared_ptr Encoder::GetTable() const { - return m_table; -} +std::shared_ptr Encoder::GetTable() const { return m_table; } diff --git a/wpilibc/simulation/src/IterativeRobot.cpp b/wpilibc/simulation/src/IterativeRobot.cpp index d78e9f33bb..aeca814d7a 100644 --- a/wpilibc/simulation/src/IterativeRobot.cpp +++ b/wpilibc/simulation/src/IterativeRobot.cpp @@ -8,13 +8,13 @@ #include "IterativeRobot.h" #include "DriverStation.h" -#include "SmartDashboard/SmartDashboard.h" #include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SmartDashboard.h" #include "networktables/NetworkTable.h" -//not sure what this is used for yet. +// not sure what this is used for yet. #ifdef _UNIX - #include +#include #endif const double IterativeRobot::kDefaultPeriod = 0; @@ -22,152 +22,132 @@ const double IterativeRobot::kDefaultPeriod = 0; /** * Set the period for the periodic functions. * - * @param period The period of the periodic function calls. 0.0 means sync to driver station control data. + * @param period The period of the periodic function calls. 0.0 means sync to + * driver station control data. */ -void IterativeRobot::SetPeriod(double period) -{ - if (period > 0.0) - { - // Not syncing with the DS, so start the timer for the main loop - m_mainLoopTimer.Reset(); - m_mainLoopTimer.Start(); - } - else - { - // Syncing with the DS, don't need the timer - m_mainLoopTimer.Stop(); - } - m_period = period; +void IterativeRobot::SetPeriod(double period) { + if (period > 0.0) { + // Not syncing with the DS, so start the timer for the main loop + m_mainLoopTimer.Reset(); + m_mainLoopTimer.Start(); + } else { + // Syncing with the DS, don't need the timer + m_mainLoopTimer.Stop(); + } + m_period = period; } /** * Get the period for the periodic functions. + * * Returns 0.0 if configured to syncronize with DS control data packets. + * * @return Period of the periodic function calls */ -double IterativeRobot::GetPeriod() -{ - return m_period; -} +double IterativeRobot::GetPeriod() { return m_period; } /** - * Get the number of loops per second for the IterativeRobot + * Get the number of loops per second for the IterativeRobot. + * * @return Frequency of the periodic function calls */ -double IterativeRobot::GetLoopsPerSec() -{ - // If syncing to the driver station, we don't know the rate, - // so guess something close. - if (m_period <= 0.0) - return 50.0; - return 1.0 / m_period; +double IterativeRobot::GetLoopsPerSec() { + // If syncing to the driver station, we don't know the rate, + // so guess something close. + if (m_period <= 0.0) return 50.0; + return 1.0 / m_period; } /** * Provide an alternate "main loop" via StartCompetition(). * - * This specific StartCompetition() implements "main loop" behavior like that of the FRC - * control system in 2008 and earlier, with a primary (slow) loop that is - * called periodically, and a "fast loop" (a.k.a. "spin loop") that is + * This specific StartCompetition() implements "main loop" behavior like that of + * the FRC control system in 2008 and earlier, with a primary (slow) loop that + * is called periodically, and a "fast loop" (a.k.a. "spin loop") that is * called as fast as possible with no delay between calls. */ -void IterativeRobot::StartCompetition() -{ - LiveWindow *lw = LiveWindow::GetInstance(); - // first and one-time initialization - SmartDashboard::init(); - NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false); - RobotInit(); +void IterativeRobot::StartCompetition() { + LiveWindow* lw = LiveWindow::GetInstance(); + // first and one-time initialization + SmartDashboard::init(); + NetworkTable::GetTable("LiveWindow") + ->GetSubTable("~STATUS~") + ->PutBoolean("LW Enabled", false); + RobotInit(); - // loop forever, calling the appropriate mode-dependent function - lw->SetEnabled(false); - while (true) - { - // Call the appropriate function depending upon the current robot mode - if (IsDisabled()) - { - // call DisabledInit() if we are now just entering disabled mode from - // either a different mode or from power-on - if(!m_disabledInitialized) - { - lw->SetEnabled(false); - DisabledInit(); - m_disabledInitialized = true; - // reset the initialization flags for the other modes - m_autonomousInitialized = false; - m_teleopInitialized = false; - m_testInitialized = false; - } - if (NextPeriodReady()) - { - // TODO: HALNetworkCommunicationObserveUserProgramDisabled(); - DisabledPeriodic(); - } - } - else if (IsAutonomous()) - { - // call AutonomousInit() if we are now just entering autonomous mode from - // either a different mode or from power-on - if(!m_autonomousInitialized) - { - lw->SetEnabled(false); - AutonomousInit(); - m_autonomousInitialized = true; - // reset the initialization flags for the other modes - m_disabledInitialized = false; - m_teleopInitialized = false; - m_testInitialized = false; - } - if (NextPeriodReady()) - { - // TODO: HALNetworkCommunicationObserveUserProgramAutonomous(); - AutonomousPeriodic(); - } - } - else if (IsTest()) - { - // call TestInit() if we are now just entering test mode from - // either a different mode or from power-on - if(!m_testInitialized) - { - lw->SetEnabled(true); - TestInit(); - m_testInitialized = true; - // reset the initialization flags for the other modes - m_disabledInitialized = false; - m_autonomousInitialized = false; - m_teleopInitialized = false; - } - if (NextPeriodReady()) - { - // TODO: HALNetworkCommunicationObserveUserProgramTest(); - TestPeriodic(); - } - } - else - { - // call TeleopInit() if we are now just entering teleop mode from - // either a different mode or from power-on - if(!m_teleopInitialized) - { - lw->SetEnabled(false); - TeleopInit(); - m_teleopInitialized = true; - // reset the initialization flags for the other modes - m_disabledInitialized = false; - m_autonomousInitialized = false; - m_testInitialized = false; - Scheduler::GetInstance()->SetEnabled(true); - } - if (NextPeriodReady()) - { - // TODO: HALNetworkCommunicationObserveUserProgramTeleop(); - TeleopPeriodic(); - } - } - // wait for driver station data so the loop doesn't hog the CPU - m_ds.WaitForData(); - } + // loop forever, calling the appropriate mode-dependent function + lw->SetEnabled(false); + while (true) { + // Call the appropriate function depending upon the current robot mode + if (IsDisabled()) { + // call DisabledInit() if we are now just entering disabled mode from + // either a different mode or from power-on + if (!m_disabledInitialized) { + lw->SetEnabled(false); + DisabledInit(); + m_disabledInitialized = true; + // reset the initialization flags for the other modes + m_autonomousInitialized = false; + m_teleopInitialized = false; + m_testInitialized = false; + } + if (NextPeriodReady()) { + // TODO: HALNetworkCommunicationObserveUserProgramDisabled(); + DisabledPeriodic(); + } + } else if (IsAutonomous()) { + // call AutonomousInit() if we are now just entering autonomous mode from + // either a different mode or from power-on + if (!m_autonomousInitialized) { + lw->SetEnabled(false); + AutonomousInit(); + m_autonomousInitialized = true; + // reset the initialization flags for the other modes + m_disabledInitialized = false; + m_teleopInitialized = false; + m_testInitialized = false; + } + if (NextPeriodReady()) { + // TODO: HALNetworkCommunicationObserveUserProgramAutonomous(); + AutonomousPeriodic(); + } + } else if (IsTest()) { + // call TestInit() if we are now just entering test mode from + // either a different mode or from power-on + if (!m_testInitialized) { + lw->SetEnabled(true); + TestInit(); + m_testInitialized = true; + // reset the initialization flags for the other modes + m_disabledInitialized = false; + m_autonomousInitialized = false; + m_teleopInitialized = false; + } + if (NextPeriodReady()) { + // TODO: HALNetworkCommunicationObserveUserProgramTest(); + TestPeriodic(); + } + } else { + // call TeleopInit() if we are now just entering teleop mode from + // either a different mode or from power-on + if (!m_teleopInitialized) { + lw->SetEnabled(false); + TeleopInit(); + m_teleopInitialized = true; + // reset the initialization flags for the other modes + m_disabledInitialized = false; + m_autonomousInitialized = false; + m_testInitialized = false; + Scheduler::GetInstance()->SetEnabled(true); + } + if (NextPeriodReady()) { + // TODO: HALNetworkCommunicationObserveUserProgramTeleop(); + TeleopPeriodic(); + } + } + // wait for driver station data so the loop doesn't hog the CPU + m_ds.WaitForData(); + } } /** @@ -180,135 +160,118 @@ void IterativeRobot::StartCompetition() * @todo Decide what this should do if it slips more than one cycle. */ -bool IterativeRobot::NextPeriodReady() -{ - if (m_period > 0.0) - { - return m_mainLoopTimer.HasPeriodPassed(m_period); - } - else - { - // XXX: BROKEN! return m_ds->IsNewControlData(); - } - return true; +bool IterativeRobot::NextPeriodReady() { + if (m_period > 0.0) { + return m_mainLoopTimer.HasPeriodPassed(m_period); + } else { + // XXX: BROKEN! return m_ds->IsNewControlData(); + } + return true; } /** * Robot-wide initialization code should go here. * - * Users should override this method for default Robot-wide initialization which will - * be called when the robot is first powered on. It will be called exactly 1 time. + * Users should override this method for default Robot-wide initialization which + * will be called when the robot is first powered on. It will be called + * exactly 1 time. */ -void IterativeRobot::RobotInit() -{ - printf("Default %s() method... Overload me!\n", __FUNCTION__); +void IterativeRobot::RobotInit() { + printf("Default %s() method... Overload me!\n", __FUNCTION__); } /** * Initialization code for disabled mode should go here. * - * Users should override this method for initialization code which will be called each time - * the robot enters disabled mode. + * Users should override this method for initialization code which will be + * called each time the robot enters disabled mode. */ -void IterativeRobot::DisabledInit() -{ - printf("Default %s() method... Overload me!\n", __FUNCTION__); +void IterativeRobot::DisabledInit() { + printf("Default %s() method... Overload me!\n", __FUNCTION__); } /** * Initialization code for autonomous mode should go here. * - * Users should override this method for initialization code which will be called each time - * the robot enters autonomous mode. + * Users should override this method for initialization code which will be + * called each time the robot enters autonomous mode. */ -void IterativeRobot::AutonomousInit() -{ - printf("Default %s() method... Overload me!\n", __FUNCTION__); +void IterativeRobot::AutonomousInit() { + printf("Default %s() method... Overload me!\n", __FUNCTION__); } /** * Initialization code for teleop mode should go here. * - * Users should override this method for initialization code which will be called each time - * the robot enters teleop mode. + * Users should override this method for initialization code which will be + * called each time the robot enters teleop mode. */ -void IterativeRobot::TeleopInit() -{ - printf("Default %s() method... Overload me!\n", __FUNCTION__); +void IterativeRobot::TeleopInit() { + printf("Default %s() method... Overload me!\n", __FUNCTION__); } /** * Initialization code for test mode should go here. * - * Users should override this method for initialization code which will be called each time - * the robot enters test mode. + * Users should override this method for initialization code which will be + * called each time the robot enters test mode. */ -void IterativeRobot::TestInit() -{ - printf("Default %s() method... Overload me!\n", __FUNCTION__); +void IterativeRobot::TestInit() { + printf("Default %s() method... Overload me!\n", __FUNCTION__); } /** * Periodic code for disabled mode should go here. * - * Users should override this method for code which will be called periodically at a regular - * rate while the robot is in disabled mode. + * Users should override this method for code which will be called periodically + * at a regular rate while the robot is in disabled mode. */ -void IterativeRobot::DisabledPeriodic() -{ - static bool firstRun = true; - if (firstRun) - { - printf("Default %s() method... Overload me!\n", __FUNCTION__); - firstRun = false; - } +void IterativeRobot::DisabledPeriodic() { + static bool firstRun = true; + if (firstRun) { + printf("Default %s() method... Overload me!\n", __FUNCTION__); + firstRun = false; + } } /** * Periodic code for autonomous mode should go here. * - * Users should override this method for code which will be called periodically at a regular - * rate while the robot is in autonomous mode. + * Users should override this method for code which will be called periodically + * at a regular rate while the robot is in autonomous mode. */ -void IterativeRobot::AutonomousPeriodic() -{ - static bool firstRun = true; - if (firstRun) - { - printf("Default %s() method... Overload me!\n", __FUNCTION__); - firstRun = false; - } +void IterativeRobot::AutonomousPeriodic() { + static bool firstRun = true; + if (firstRun) { + printf("Default %s() method... Overload me!\n", __FUNCTION__); + firstRun = false; + } } /** * Periodic code for teleop mode should go here. * - * Users should override this method for code which will be called periodically at a regular - * rate while the robot is in teleop mode. + * Users should override this method for code which will be called periodically + * at a regular rate while the robot is in teleop mode. */ -void IterativeRobot::TeleopPeriodic() -{ - static bool firstRun = true; - if (firstRun) - { - printf("Default %s() method... Overload me!\n", __FUNCTION__); - firstRun = false; - } +void IterativeRobot::TeleopPeriodic() { + static bool firstRun = true; + if (firstRun) { + printf("Default %s() method... Overload me!\n", __FUNCTION__); + firstRun = false; + } } /** * Periodic code for test mode should go here. * - * Users should override this method for code which will be called periodically at a regular - * rate while the robot is in test mode. + * Users should override this method for code which will be called periodically + * at a regular rate while the robot is in test mode. */ -void IterativeRobot::TestPeriodic() -{ - static bool firstRun = true; - if (firstRun) - { - printf("Default %s() method... Overload me!\n", __FUNCTION__); - firstRun = false; - } +void IterativeRobot::TestPeriodic() { + static bool firstRun = true; + if (firstRun) { + printf("Default %s() method... Overload me!\n", __FUNCTION__); + firstRun = false; + } } - diff --git a/wpilibc/simulation/src/Jaguar.cpp b/wpilibc/simulation/src/Jaguar.cpp index 148f62929f..01627fec1a 100644 --- a/wpilibc/simulation/src/Jaguar.cpp +++ b/wpilibc/simulation/src/Jaguar.cpp @@ -5,7 +5,6 @@ /* the project. */ /*----------------------------------------------------------------------------*/ - #include "Jaguar.h" //#include "NetworkCommunication/UsageReporting.h" #include "LiveWindow/LiveWindow.h" @@ -13,22 +12,21 @@ /** * @param channel The PWM channel that the Jaguar is attached to. */ -Jaguar::Jaguar(uint32_t channel) : SafePWM(channel) -{ - /* - * Input profile defined by Luminary Micro. - * - * Full reverse ranges from 0.671325ms to 0.6972211ms - * Proportional reverse ranges from 0.6972211ms to 1.4482078ms - * Neutral ranges from 1.4482078ms to 1.5517922ms - * Proportional forward ranges from 1.5517922ms to 2.3027789ms - * Full forward ranges from 2.3027789ms to 2.328675ms - */ - SetBounds(2.31, 1.55, 1.507, 1.454, .697); - SetPeriodMultiplier(kPeriodMultiplier_1X); - SetRaw(m_centerPwm); +Jaguar::Jaguar(uint32_t channel) : SafePWM(channel) { + /* + * Input profile defined by Luminary Micro. + * + * Full reverse ranges from 0.671325ms to 0.6972211ms + * Proportional reverse ranges from 0.6972211ms to 1.4482078ms + * Neutral ranges from 1.4482078ms to 1.5517922ms + * Proportional forward ranges from 1.5517922ms to 2.3027789ms + * Full forward ranges from 2.3027789ms to 2.328675ms + */ + SetBounds(2.31, 1.55, 1.507, 1.454, .697); + SetPeriodMultiplier(kPeriodMultiplier_1X); + SetRaw(m_centerPwm); - LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this); + LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this); } /** @@ -37,38 +35,26 @@ Jaguar::Jaguar(uint32_t channel) : SafePWM(channel) * The PWM value is set using a range of -1.0 to 1.0, appropriately * scaling the value for the FPGA. * - * @param speed The speed value between -1.0 and 1.0 to set. + * @param speed The speed value between -1.0 and 1.0 to set. * @param syncGroup Unused interface. */ -void Jaguar::Set(float speed, uint8_t syncGroup) -{ - SetSpeed(speed); -} +void Jaguar::Set(float speed, uint8_t syncGroup) { SetSpeed(speed); } /** * Get the recently set value of the PWM. * * @return The most recently set value for the PWM between -1.0 and 1.0. */ -float Jaguar::Get() const -{ - return GetSpeed(); -} +float Jaguar::Get() const { return GetSpeed(); } /** * Common interface for disabling a motor. */ -void Jaguar::Disable() -{ - SetRaw(kPwmDisabled); -} +void Jaguar::Disable() { SetRaw(kPwmDisabled); } /** * Write out the PID value as seen in the PIDOutput base object. * * @param output Write out the PWM value as was found in the PIDController */ -void Jaguar::PIDWrite(float output) -{ - Set(output); -} +void Jaguar::PIDWrite(float output) { Set(output); } diff --git a/wpilibc/simulation/src/Joystick.cpp b/wpilibc/simulation/src/Joystick.cpp index 3c32e86494..b878178fd7 100644 --- a/wpilibc/simulation/src/Joystick.cpp +++ b/wpilibc/simulation/src/Joystick.cpp @@ -6,10 +6,10 @@ /*----------------------------------------------------------------------------*/ #include "Joystick.h" -#include "DriverStation.h" -#include "WPIErrors.h" #include #include +#include "DriverStation.h" +#include "WPIErrors.h" const uint32_t Joystick::kDefaultXAxis; const uint32_t Joystick::kDefaultYAxis; @@ -18,26 +18,26 @@ const uint32_t Joystick::kDefaultTwistAxis; const uint32_t Joystick::kDefaultThrottleAxis; const uint32_t Joystick::kDefaultTriggerButton; const uint32_t Joystick::kDefaultTopButton; -static Joystick *joysticks[DriverStation::kJoystickPorts]; +static Joystick* joysticks[DriverStation::kJoystickPorts]; static bool joySticksInitialized = false; /** * Construct an instance of a joystick. + * * The joystick index is the usb port on the drivers station. * * @param port The port on the driver station that the joystick is plugged into. */ Joystick::Joystick(uint32_t port) - : Joystick(port, kNumAxisTypes, kNumButtonTypes) -{ - m_axes[kXAxis] = kDefaultXAxis; - m_axes[kYAxis] = kDefaultYAxis; - m_axes[kZAxis] = kDefaultZAxis; - m_axes[kTwistAxis] = kDefaultTwistAxis; - m_axes[kThrottleAxis] = kDefaultThrottleAxis; + : Joystick(port, kNumAxisTypes, kNumButtonTypes) { + m_axes[kXAxis] = kDefaultXAxis; + m_axes[kYAxis] = kDefaultYAxis; + m_axes[kZAxis] = kDefaultZAxis; + m_axes[kTwistAxis] = kDefaultTwistAxis; + m_axes[kThrottleAxis] = kDefaultThrottleAxis; - m_buttons[kTriggerButton] = kDefaultTriggerButton; - m_buttons[kTopButton] = kDefaultTopButton; + m_buttons[kTriggerButton] = kDefaultTriggerButton; + m_buttons[kTopButton] = kDefaultTopButton; } /** @@ -46,80 +46,73 @@ Joystick::Joystick(uint32_t port) * This constructor allows the subclass to configure the number of constants * for axes and buttons. * - * @param port The port on the driver station that the joystick is plugged into. - * @param numAxisTypes The number of axis types in the enum. + * @param port The port on the driver station that the joystick is + * plugged into. + * @param numAxisTypes The number of axis types in the enum. * @param numButtonTypes The number of button types in the enum. */ -Joystick::Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes) - : m_port (port), - m_ds(DriverStation::GetInstance()) -{ - if ( !joySticksInitialized ) - { - for (unsigned i = 0; i < DriverStation::kJoystickPorts; i++) - joysticks[i] = nullptr; - joySticksInitialized = true; - } - joysticks[m_port] = this; +Joystick::Joystick(uint32_t port, uint32_t numAxisTypes, + uint32_t numButtonTypes) + : m_port(port), m_ds(DriverStation::GetInstance()) { + if (!joySticksInitialized) { + for (unsigned i = 0; i < DriverStation::kJoystickPorts; i++) + joysticks[i] = nullptr; + joySticksInitialized = true; + } + joysticks[m_port] = this; - m_axes = std::make_unique(numAxisTypes); - m_buttons = std::make_unique(numButtonTypes); + m_axes = std::make_unique(numAxisTypes); + m_buttons = std::make_unique(numButtonTypes); } -Joystick * Joystick::GetStickForPort(uint32_t port) -{ - Joystick *stick = joysticks[port]; - if (stick == nullptr) - { - stick = new Joystick(port); - joysticks[port] = stick; - } - return stick; +Joystick* Joystick::GetStickForPort(uint32_t port) { + Joystick* stick = joysticks[port]; + if (stick == nullptr) { + stick = new Joystick(port); + joysticks[port] = stick; + } + return stick; } /** * Get the X value of the joystick. + * * This depends on the mapping of the joystick connected to the current port. */ -float Joystick::GetX(JoystickHand hand) const -{ - return GetRawAxis(m_axes[kXAxis]); +float Joystick::GetX(JoystickHand hand) const { + return GetRawAxis(m_axes[kXAxis]); } /** * Get the Y value of the joystick. + * * This depends on the mapping of the joystick connected to the current port. */ -float Joystick::GetY(JoystickHand hand) const -{ - return GetRawAxis(m_axes[kYAxis]); +float Joystick::GetY(JoystickHand hand) const { + return GetRawAxis(m_axes[kYAxis]); } /** * Get the Z value of the current joystick. + * * This depends on the mapping of the joystick connected to the current port. */ -float Joystick::GetZ() const -{ - return GetRawAxis(m_axes[kZAxis]); -} +float Joystick::GetZ() const { return GetRawAxis(m_axes[kZAxis]); } /** * Get the twist value of the current joystick. + * * This depends on the mapping of the joystick connected to the current port. */ -float Joystick::GetTwist() const -{ - return GetRawAxis(m_axes[kTwistAxis]); -} +float Joystick::GetTwist() const { return GetRawAxis(m_axes[kTwistAxis]); } /** * Get the throttle value of the current joystick. + * * This depends on the mapping of the joystick connected to the current port. */ -float Joystick::GetThrottle() const -{ - return GetRawAxis(m_axes[kThrottleAxis]); +float Joystick::GetThrottle() const { + return GetRawAxis(m_axes[kThrottleAxis]); } /** @@ -128,33 +121,36 @@ float Joystick::GetThrottle() const * @param axis The axis to read [1-6]. * @return The value of the axis. */ -float Joystick::GetRawAxis(uint32_t axis) const -{ - return m_ds.GetStickAxis(m_port, axis); +float Joystick::GetRawAxis(uint32_t axis) const { + return m_ds.GetStickAxis(m_port, axis); } /** * For the current joystick, return the axis determined by the argument. * - * This is for cases where the joystick axis is returned programatically, otherwise one of the - * previous functions would be preferable (for example GetX()). + * This is for cases where the joystick axis is returned programatically, + * otherwise one of the previous functions would be preferable (for example + * GetX()). * * @param axis The axis to read. * @return The value of the axis. */ -float Joystick::GetAxis(AxisType axis) const -{ - switch(axis) - { - case kXAxis: return this->GetX(); - case kYAxis: return this->GetY(); - case kZAxis: return this->GetZ(); - case kTwistAxis: return this->GetTwist(); - case kThrottleAxis: return this->GetThrottle(); - default: - wpi_setWPIError(BadJoystickAxis); - return 0.0; - } +float Joystick::GetAxis(AxisType axis) const { + switch (axis) { + case kXAxis: + return this->GetX(); + case kYAxis: + return this->GetY(); + case kZAxis: + return this->GetZ(); + case kTwistAxis: + return this->GetTwist(); + case kThrottleAxis: + return this->GetThrottle(); + default: + wpi_setWPIError(BadJoystickAxis); + return 0.0; + } } /** @@ -162,12 +158,12 @@ float Joystick::GetAxis(AxisType axis) const * * Look up which button has been assigned to the trigger and read its state. * - * @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface. + * @param hand This parameter is ignored for the Joystick class and is only here + * to complete the GenericHID interface. * @return The state of the trigger. */ -bool Joystick::GetTrigger(JoystickHand hand) const -{ - return GetRawButton(m_buttons[kTriggerButton]); +bool Joystick::GetTrigger(JoystickHand hand) const { + return GetRawButton(m_buttons[kTriggerButton]); } /** @@ -175,45 +171,45 @@ bool Joystick::GetTrigger(JoystickHand hand) const * * Look up which button has been assigned to the top and read its state. * - * @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface. + * @param hand This parameter is ignored for the Joystick class and is only here + * to complete the GenericHID interface. * @return The state of the top button. */ -bool Joystick::GetTop(JoystickHand hand) const -{ - return GetRawButton(m_buttons[kTopButton]); +bool Joystick::GetTop(JoystickHand hand) const { + return GetRawButton(m_buttons[kTopButton]); } /** * This is not supported for the Joystick. + * * This method is only here to complete the GenericHID interface. */ -bool Joystick::GetBumper(JoystickHand hand) const -{ - // Joysticks don't have bumpers. - return false; +bool Joystick::GetBumper(JoystickHand hand) const { + // Joysticks don't have bumpers. + return false; } /** * Get the button value for buttons 1 through 12. * - * The buttons are returned in a single 16 bit value with one bit representing the state - * of each button. The appropriate button is returned as a boolean value. + * The buttons are returned in a single 16 bit value with one bit representing + * the state of each button. The appropriate button is returned as a boolean + * value. * * @param button The button number to be read. * @return The state of the button. - **/ -bool Joystick::GetRawButton(uint32_t button) const -{ - return m_ds.GetStickButton(m_port, button); + */ +bool Joystick::GetRawButton(uint32_t button) const { + return m_ds.GetStickButton(m_port, button); } /** -* Get the state of a POV on the joystick. -* -* @return the angle of the POV in degrees, or -1 if the POV is not pressed. -*/ + * Get the state of a POV on the joystick. + * + * @return the angle of the POV in degrees, or -1 if the POV is not pressed. + */ int Joystick::GetPOV(uint32_t pov) const { - return 0; // TODO + return 0; // TODO } /** @@ -224,15 +220,15 @@ int Joystick::GetPOV(uint32_t pov) const { * @param button The type of button to read. * @return The state of the button. */ -bool Joystick::GetButton(ButtonType button) const -{ - switch (button) - { - case kTriggerButton: return GetTrigger(); - case kTopButton: return GetTop(); - default: - return false; - } +bool Joystick::GetButton(ButtonType button) const { + switch (button) { + case kTriggerButton: + return GetTrigger(); + case kTopButton: + return GetTop(); + default: + return false; + } } /** @@ -241,10 +237,7 @@ bool Joystick::GetButton(ButtonType button) const * @param axis The axis to look up the channel for. * @return The channel fr the axis. */ -uint32_t Joystick::GetAxisChannel(AxisType axis) -{ - return m_axes[axis]; -} +uint32_t Joystick::GetAxisChannel(AxisType axis) { return m_axes[axis]; } /** * Set the channel associated with a specified axis. @@ -252,40 +245,37 @@ uint32_t Joystick::GetAxisChannel(AxisType axis) * @param axis The axis to set the channel for. * @param channel The channel to set the axis to. */ -void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) -{ - m_axes[axis] = channel; +void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) { + m_axes[axis] = channel; } /** * Get the magnitude of the direction vector formed by the joystick's - * current position relative to its origin + * current position relative to its origin. * * @return The magnitude of the direction vector */ float Joystick::GetMagnitude() const { - return sqrt(pow(GetX(),2) + pow(GetY(),2) ); + return sqrt(pow(GetX(), 2) + pow(GetY(), 2)); } /** * Get the direction of the vector formed by the joystick and its origin - * in radians + * in radians. * * @return The direction of the vector in radians */ -float Joystick::GetDirectionRadians() const { - return atan2(GetX(), -GetY()); -} +float Joystick::GetDirectionRadians() const { return atan2(GetX(), -GetY()); } /** * Get the direction of the vector formed by the joystick and its origin - * in degrees + * in degrees. * - * uses acos(-1) to represent Pi due to absence of readily accessable Pi + * uses acos(-1) to represent Pi due to absence of readily accessable PI * constant in C++ * * @return The direction of the vector in degrees */ float Joystick::GetDirectionDegrees() const { - return (180/acos(-1))*GetDirectionRadians(); + return (180 / acos(-1)) * GetDirectionRadians(); } diff --git a/wpilibc/simulation/src/MotorSafetyHelper.cpp b/wpilibc/simulation/src/MotorSafetyHelper.cpp index 47db177142..14e3e271ff 100644 --- a/wpilibc/simulation/src/MotorSafetyHelper.cpp +++ b/wpilibc/simulation/src/MotorSafetyHelper.cpp @@ -20,18 +20,17 @@ priority_recursive_mutex MotorSafetyHelper::m_listMutex; /** * The constructor for a MotorSafetyHelper object. + * * The helper object is constructed for every object that wants to implement the - * Motor - * Safety protocol. The helper object has the code to actually do the timing and - * call the - * motors Stop() method when the timeout expires. The motor object is expected - * to call the - * Feed() method whenever the motors value is updated. + * Motor Safety protocol. The helper object has the code to actually do the + * timing and call the motors Stop() method when the timeout expires. The motor + * object is expected to call the Feed() method whenever the motors value is + * updated. + * * @param safeObject a pointer to the motor object implementing MotorSafety. - * This is used - * to call the Stop() method on the motor. + * This is used to call the Stop() method on the motor. */ -MotorSafetyHelper::MotorSafetyHelper(MotorSafety *safeObject) +MotorSafetyHelper::MotorSafetyHelper(MotorSafety* safeObject) : m_safeObject(safeObject) { m_enabled = false; m_expiration = DEFAULT_SAFETY_EXPIRATION; @@ -48,6 +47,7 @@ MotorSafetyHelper::~MotorSafetyHelper() { /** * Feed the motor safety object. + * * Resets the timer on this object that is used to do the timeouts. */ void MotorSafetyHelper::Feed() { @@ -57,6 +57,7 @@ void MotorSafetyHelper::Feed() { /** * Set the expiration time for the corresponding motor safety object. + * * @param expirationTime The timeout value in seconds. */ void MotorSafetyHelper::SetExpiration(float expirationTime) { @@ -66,6 +67,7 @@ void MotorSafetyHelper::SetExpiration(float expirationTime) { /** * Retrieve the timeout value for the corresponding motor safety object. + * * @return the timeout value in seconds. */ float MotorSafetyHelper::GetExpiration() const { @@ -75,8 +77,9 @@ float MotorSafetyHelper::GetExpiration() const { /** * Determine if the motor is still operating or has timed out. + * * @return a true value if the motor is still operating normally and hasn't - * timed out. + * timed out. */ bool MotorSafetyHelper::IsAlive() const { std::lock_guard sync(m_syncMutex); @@ -85,30 +88,30 @@ bool MotorSafetyHelper::IsAlive() const { /** * Check if this motor has exceeded its timeout. + * * This method is called periodically to determine if this motor has exceeded - * its timeout - * value. If it has, the stop method is called, and the motor is shut down until - * its value is - * updated again. + * its timeout value. If it has, the stop method is called, and the motor is + * shut down until its value is updated again. */ -void MotorSafetyHelper::Check() -{ - DriverStation &ds = DriverStation::GetInstance(); - if (!m_enabled || ds.IsDisabled() || ds.IsTest()) return; +void MotorSafetyHelper::Check() { + DriverStation& ds = DriverStation::GetInstance(); + if (!m_enabled || ds.IsDisabled() || ds.IsTest()) return; std::lock_guard sync(m_syncMutex); if (m_stopTime < Timer::GetFPGATimestamp()) { std::ostringstream desc; m_safeObject->GetDescription(desc); - desc << "... Output not updated often enough."; + desc << "... Output not updated often enough."; wpi_setWPIErrorWithContext(Timeout, desc.str().c_str()); m_safeObject->StopMotor(); } } /** - * Enable/disable motor safety for this device + * Enable/disable motor safety for this device. + * * Turn on and off the motor safety option for this PWM object. + * * @param enabled True if motor safety is enforced for this object */ void MotorSafetyHelper::SetSafetyEnabled(bool enabled) { @@ -117,8 +120,10 @@ void MotorSafetyHelper::SetSafetyEnabled(bool enabled) { } /** - * Return the state of the motor safety enabled flag + * Return the state of the motor safety enabled flag. + * * Return if the motor safety is currently enabled for this devicce. + * * @return True if motor safety is enforced for this device */ bool MotorSafetyHelper::IsSafetyEnabled() const { @@ -128,9 +133,9 @@ bool MotorSafetyHelper::IsSafetyEnabled() const { /** * Check the motors to see if any have timed out. - * This static method is called periodically to poll all the motors and stop - * any that have - * timed out. + * + * This static method is called periodically to poll all the motors and stop + * any that have timed out. */ void MotorSafetyHelper::CheckMotors() { std::lock_guard sync(m_listMutex); diff --git a/wpilibc/simulation/src/Notifier.cpp b/wpilibc/simulation/src/Notifier.cpp index eb1948718b..7dd5b833f6 100644 --- a/wpilibc/simulation/src/Notifier.cpp +++ b/wpilibc/simulation/src/Notifier.cpp @@ -18,255 +18,246 @@ std::atomic Notifier::m_stopped(false); /** * Create a Notifier for timer event notification. + * * @param handler The handler is called at the notification time which is set - * using StartSingle or StartPeriodic. + * using StartSingle or StartPeriodic. */ -Notifier::Notifier(TimerEventHandler handler) -{ - if (handler == nullptr) - wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr"); - m_handler = handler; - m_periodic = false; - m_expirationTime = 0; - m_period = 0; - m_queued = false; - { - std::lock_guard sync(queueMutex); - // do the first time intialization of static variables - if (refcount.fetch_add(1) == 0) { - m_task = std::thread(Run); - } - } +Notifier::Notifier(TimerEventHandler handler) { + if (handler == nullptr) + wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr"); + m_handler = handler; + m_periodic = false; + m_expirationTime = 0; + m_period = 0; + m_queued = false; + { + std::lock_guard sync(queueMutex); + // do the first time intialization of static variables + if (refcount.fetch_add(1) == 0) { + m_task = std::thread(Run); + } + } } /** * Free the resources for a timer event. + * * All resources will be freed and the timer event will be removed from the * queue if necessary. */ -Notifier::~Notifier() -{ - { - std::lock_guard sync(queueMutex); - DeleteFromQueue(); +Notifier::~Notifier() { + { + std::lock_guard sync(queueMutex); + DeleteFromQueue(); - // Delete the static variables when the last one is going away - if (refcount.fetch_sub(1) == 1) - { + // Delete the static variables when the last one is going away + if (refcount.fetch_sub(1) == 1) { m_stopped = true; - m_task.join(); - } - } + m_task.join(); + } + } - // Acquire the semaphore; this makes certain that the handler is - // not being executed by the interrupt manager. - std::lock_guard lock(m_handlerMutex); + // Acquire the semaphore; this makes certain that the handler is + // not being executed by the interrupt manager. + std::lock_guard lock(m_handlerMutex); } /** * Update the alarm hardware to reflect the current first element in the queue. - * Compute the time the next alarm should occur based on the current time and the - * period for the first element in the timer queue. - * WARNING: this method does not do synchronization! It must be called from somewhere - * that is taking care of synchronizing access to the queue. + * + * Compute the time the next alarm should occur based on the current time and + * the period for the first element in the timer queue. + * + * WARNING: this method does not do synchronization! It must be called from + * somewhere that is taking care of synchronizing access to the queue. */ -void Notifier::UpdateAlarm() -{ -} +void Notifier::UpdateAlarm() {} /** * ProcessQueue is called whenever there is a timer interrupt. - * We need to wake up and process the current top item in the timer queue as long - * as its scheduled time is after the current time. Then the item is removed or - * rescheduled (repetitive events) in the queue. + * + * We need to wake up and process the current top item in the timer queue as + * long as its scheduled time is after the current time. Then the item is + * removed or rescheduled (repetitive events) in the queue. */ -void Notifier::ProcessQueue(uint32_t mask, void *params) -{ - Notifier *current; - while (true) // keep processing past events until no more - { - { - std::lock_guard sync(queueMutex); - double currentTime = GetClock(); +void Notifier::ProcessQueue(uint32_t mask, void* params) { + Notifier* current; + while (true) // keep processing past events until no more + { + { + std::lock_guard sync(queueMutex); + double currentTime = GetClock(); - if (timerQueue.empty()) - { - break; - } - current = timerQueue.front(); - if (current->m_expirationTime > currentTime) - { - break; // no more timer events to process - } - // remove next entry before processing it - timerQueue.pop_front(); + if (timerQueue.empty()) { + break; + } + current = timerQueue.front(); + if (current->m_expirationTime > currentTime) { + break; // no more timer events to process + } + // remove next entry before processing it + timerQueue.pop_front(); - current->m_queued = false; - if (current->m_periodic) - { - // if periodic, requeue the event - // compute when to put into queue - current->InsertInQueue(true); - } - else - { - // not periodic; removed from queue - current->m_queued = false; - } - // Take handler mutex while holding queue semaphore to make sure - // the handler will execute to completion in case we are being deleted. - current->m_handlerMutex.lock(); - } + current->m_queued = false; + if (current->m_periodic) { + // if periodic, requeue the event + // compute when to put into queue + current->InsertInQueue(true); + } else { + // not periodic; removed from queue + current->m_queued = false; + } + // Take handler mutex while holding queue semaphore to make sure + // the handler will execute to completion in case we are being deleted. + current->m_handlerMutex.lock(); + } - current->m_handler(); // call the event handler - current->m_handlerMutex.unlock(); - } - // reschedule the first item in the queue - std::lock_guard sync(queueMutex); - UpdateAlarm(); + current->m_handler(); // call the event handler + current->m_handlerMutex.unlock(); + } + // reschedule the first item in the queue + std::lock_guard sync(queueMutex); + UpdateAlarm(); } /** * Insert this Notifier into the timer queue in right place. - * WARNING: this method does not do synchronization! It must be called from somewhere - * that is taking care of synchronizing access to the queue. - * @param reschedule If false, the scheduled alarm is based on the curent time and UpdateAlarm - * method is called which will enable the alarm if necessary. - * If true, update the time by adding the period (no drift) when rescheduled periodic from ProcessQueue. - * This ensures that the public methods only update the queue after finishing inserting. + * + * WARNING: this method does not do synchronization! It must be called from + * somewhere that is taking care of synchronizing access to the queue. + * + * @param reschedule If false, the scheduled alarm is based on the curent time + * and UpdateAlarm method is called which will enable the + * alarm if necessary. If true, update the time by adding the + * period (no drift) when rescheduled periodic from + * ProcessQueue. + * + * This ensures that the public methods only update the queue after finishing + * inserting. */ -void Notifier::InsertInQueue(bool reschedule) -{ - if (reschedule) - { - m_expirationTime += m_period; - } - else - { - m_expirationTime = GetClock() + m_period; - } +void Notifier::InsertInQueue(bool reschedule) { + if (reschedule) { + m_expirationTime += m_period; + } else { + m_expirationTime = GetClock() + m_period; + } - // Attempt to insert new entry into queue - for (auto i = timerQueue.begin(); i != timerQueue.end(); i++) - { - if ((*i)->m_expirationTime > m_expirationTime) - { - timerQueue.insert(i, this); - m_queued = true; - } - } + // Attempt to insert new entry into queue + for (auto i = timerQueue.begin(); i != timerQueue.end(); i++) { + if ((*i)->m_expirationTime > m_expirationTime) { + timerQueue.insert(i, this); + m_queued = true; + } + } - /* If the new entry wasn't queued, either the queue was empty or the first - * element was greater than the new entry. - */ - if (!m_queued) - { - timerQueue.push_front(this); + /* If the new entry wasn't queued, either the queue was empty or the first + * element was greater than the new entry. + */ + if (!m_queued) { + timerQueue.push_front(this); - if (!reschedule) - { - /* Since the first element changed, update alarm, unless we already - * plan to - */ - UpdateAlarm(); - } + if (!reschedule) { + /* Since the first element changed, update alarm, unless we already + * plan to + */ + UpdateAlarm(); + } - m_queued = true; - } + m_queued = true; + } } /** * Delete this Notifier from the timer queue. - * WARNING: this method does not do synchronization! It must be called from somewhere - * that is taking care of synchronizing access to the queue. - * Remove this Notifier from the timer queue and adjust the next interrupt time to reflect - * the current top of the queue. + * + * WARNING: this method does not do synchronization! It must be called from + * somewhere that is taking care of synchronizing access to the queue. + * + * Remove this Notifier from the timer queue and adjust the next interrupt time + * to reflect the current top of the queue. */ -void Notifier::DeleteFromQueue() -{ - if (m_queued) - { - m_queued = false; - wpi_assert(!timerQueue.empty()); - if (timerQueue.front() == this) - { - // remove the first item in the list - update the alarm - timerQueue.pop_front(); - UpdateAlarm(); - } - else - { - timerQueue.remove(this); - } - } +void Notifier::DeleteFromQueue() { + if (m_queued) { + m_queued = false; + wpi_assert(!timerQueue.empty()); + if (timerQueue.front() == this) { + // remove the first item in the list - update the alarm + timerQueue.pop_front(); + UpdateAlarm(); + } else { + timerQueue.remove(this); + } + } } /** * Register for single event notification. + * * A timer event is queued for a single event after the specified delay. + * * @param delay Seconds to wait before the handler is called. */ -void Notifier::StartSingle(double delay) -{ - std::lock_guard sync(queueMutex); - m_periodic = false; - m_period = delay; - DeleteFromQueue(); - InsertInQueue(false); +void Notifier::StartSingle(double delay) { + std::lock_guard sync(queueMutex); + m_periodic = false; + m_period = delay; + DeleteFromQueue(); + InsertInQueue(false); } /** * Register for periodic event notification. - * A timer event is queued for periodic event notification. Each time the interrupt - * occurs, the event will be immediately requeued for the same time interval. - * @param period Period in seconds to call the handler starting one period after the call to this method. + * + * A timer event is queued for periodic event notification. Each time the + * interrupt occurs, the event will be immediately requeued for the same time + * interval. + * + * @param period Period in seconds to call the handler starting one period after + * the call to this method. */ -void Notifier::StartPeriodic(double period) -{ - std::lock_guard sync(queueMutex); - m_periodic = true; - m_period = period; - DeleteFromQueue(); - InsertInQueue(false); +void Notifier::StartPeriodic(double period) { + std::lock_guard sync(queueMutex); + m_periodic = true; + m_period = period; + DeleteFromQueue(); + InsertInQueue(false); } /** * Stop timer events from occuring. - * Stop any repeating timer events from occuring. This will also remove any single - * notification events from the queue. - * If a timer-based call to the registered handler is in progress, this function will - * block until the handler call is complete. + * + * Stop any repeating timer events from occuring. This will also remove any + * single notification events from the queue. If a timer-based call to the + * registered handler is in progress, this function will block until the + * handler call is complete. */ -void Notifier::Stop() -{ - { - std::lock_guard sync(queueMutex); - DeleteFromQueue(); - } - // Wait for a currently executing handler to complete before returning from Stop() - std::lock_guard sync(m_handlerMutex); +void Notifier::Stop() { + { + std::lock_guard sync(queueMutex); + DeleteFromQueue(); + } + // Wait for a currently executing handler to complete before returning from + // Stop() + std::lock_guard sync(m_handlerMutex); } void Notifier::Run() { - while (!m_stopped) { - Notifier::ProcessQueue(0, nullptr); - bool isEmpty; - { - std::lock_guard sync(queueMutex); - isEmpty = timerQueue.empty(); - } - if (!isEmpty) - { - double expirationTime; - { - std::lock_guard sync(queueMutex); - expirationTime = timerQueue.front()->m_expirationTime; - } - Wait(expirationTime - GetClock()); - } - else - { - Wait(0.05); - } + while (!m_stopped) { + Notifier::ProcessQueue(0, nullptr); + bool isEmpty; + { + std::lock_guard sync(queueMutex); + isEmpty = timerQueue.empty(); } + if (!isEmpty) { + double expirationTime; + { + std::lock_guard sync(queueMutex); + expirationTime = timerQueue.front()->m_expirationTime; + } + Wait(expirationTime - GetClock()); + } else { + Wait(0.05); + } + } } diff --git a/wpilibc/simulation/src/PIDController.cpp b/wpilibc/simulation/src/PIDController.cpp index d3c18b5196..d78508a866 100644 --- a/wpilibc/simulation/src/PIDController.cpp +++ b/wpilibc/simulation/src/PIDController.cpp @@ -6,10 +6,10 @@ /*----------------------------------------------------------------------------*/ #include "PIDController.h" -#include "Notifier.h" -#include "PIDSource.h" -#include "PIDOutput.h" #include +#include "Notifier.h" +#include "PIDOutput.h" +#include "PIDSource.h" static const std::string kP = "p"; static const std::string kI = "i"; @@ -18,184 +18,172 @@ static const std::string kF = "f"; static const std::string kSetpoint = "setpoint"; static const std::string kEnabled = "enabled"; - /** - * Allocate a PID object with the given constants for P, I, D - * @param Kp the proportional coefficient - * @param Ki the integral coefficient - * @param Kd the derivative coefficient + * Allocate a PID object with the given constants for P, I, D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient * @param source The PIDSource object that is used to get values * @param output The PIDOutput object that is set to the output value - * @param period the loop time for doing calculations. This particularly effects calculations of the - * integral and differental terms. The default is 50ms. + * @param period the loop time for doing calculations. This particularly effects + * calculations of the integral and differental terms. The + * default is 50ms. */ -PIDController::PIDController(float Kp, float Ki, float Kd, - PIDSource *source, PIDOutput *output, - float period) -{ - Initialize(Kp, Ki, Kd, 0.0f, source, output, period); +PIDController::PIDController(float Kp, float Ki, float Kd, PIDSource* source, + PIDOutput* output, float period) { + Initialize(Kp, Ki, Kd, 0.0f, source, output, period); } /** - * Allocate a PID object with the given constants for P, I, D - * @param Kp the proportional coefficient - * @param Ki the integral coefficient - * @param Kd the derivative coefficient + * Allocate a PID object with the given constants for P, I, D. + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient * @param source The PIDSource object that is used to get values * @param output The PIDOutput object that is set to the output value - * @param period the loop time for doing calculations. This particularly effects calculations of the - * integral and differental terms. The default is 50ms. + * @param period the loop time for doing calculations. This particularly effects + * calculations of the integral and differental terms. The + * default is 50ms. */ PIDController::PIDController(float Kp, float Ki, float Kd, float Kf, - PIDSource *source, PIDOutput *output, - float period) -{ - Initialize(Kp, Ki, Kd, Kf, source, output, period); + PIDSource* source, PIDOutput* output, + float period) { + Initialize(Kp, Ki, Kd, Kf, source, output, period); } - void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf, - PIDSource *source, PIDOutput *output, - float period) -{ - m_table = nullptr; + PIDSource* source, PIDOutput* output, + float period) { + m_table = nullptr; - m_P = Kp; - m_I = Ki; - m_D = Kd; - m_F = Kf; + m_P = Kp; + m_I = Ki; + m_D = Kd; + m_F = Kf; - m_maximumOutput = 1.0; - m_minimumOutput = -1.0; + m_maximumOutput = 1.0; + m_minimumOutput = -1.0; - m_maximumInput = 0; - m_minimumInput = 0; + m_maximumInput = 0; + m_minimumInput = 0; - m_continuous = false; - m_enabled = false; - m_setpoint = 0; + m_continuous = false; + m_enabled = false; + m_setpoint = 0; - m_prevError = 0; - m_totalError = 0; - m_tolerance = .05; + m_prevError = 0; + m_totalError = 0; + m_tolerance = .05; - m_result = 0; + m_result = 0; - m_pidInput = source; - m_pidOutput = output; - m_period = period; + m_pidInput = source; + m_pidOutput = output; + m_period = period; - m_controlLoop = std::make_unique(&PIDController::Calculate, this); - m_controlLoop->StartPeriodic(m_period); + m_controlLoop = std::make_unique(&PIDController::Calculate, this); + m_controlLoop->StartPeriodic(m_period); - static int32_t instances = 0; - instances++; + static int32_t instances = 0; + instances++; - m_toleranceType = kNoTolerance; + m_toleranceType = kNoTolerance; } PIDController::~PIDController() { - if (m_table != nullptr) m_table->RemoveTableListener(this); + if (m_table != nullptr) m_table->RemoveTableListener(this); } /** * Read the input, calculate the output accordingly, and write to the output. + * * This should only be called by the Notifier. */ -void PIDController::Calculate() -{ - bool enabled; - PIDSource *pidInput; +void PIDController::Calculate() { + bool enabled; + PIDSource* pidInput; - { - std::lock_guard lock(m_mutex); - if (m_pidInput == 0) return; - if (m_pidOutput == 0) return; - enabled = m_enabled; - pidInput = m_pidInput; - } + { + std::lock_guard lock(m_mutex); + if (m_pidInput == 0) return; + if (m_pidOutput == 0) return; + enabled = m_enabled; + pidInput = m_pidInput; + } - if (enabled) - { - float input = pidInput->PIDGet(); - float result; - PIDOutput *pidOutput; + if (enabled) { + float input = pidInput->PIDGet(); + float result; + PIDOutput* pidOutput; - { - std::lock_guard sync(m_mutex); - m_error = m_setpoint - input; - if (m_continuous) - { - if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) - { - if (m_error > 0) - { - m_error = m_error - m_maximumInput + m_minimumInput; - } - else - { - m_error = m_error + m_maximumInput - m_minimumInput; - } - } - } + { + std::lock_guard sync(m_mutex); + m_error = m_setpoint - input; + if (m_continuous) { + if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) { + if (m_error > 0) { + m_error = m_error - m_maximumInput + m_minimumInput; + } else { + m_error = m_error + m_maximumInput - m_minimumInput; + } + } + } - if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) { - if (m_P != 0) { - double potentialPGain = (m_totalError + m_error) * m_P; - if (potentialPGain < m_maximumOutput) { - if (potentialPGain > m_minimumOutput) { - m_totalError += m_error; - } - else { - m_totalError = m_minimumOutput / m_P; - } - } - else { - m_totalError = m_maximumOutput / m_P; - } - } - - m_result = m_D * m_error + m_P * m_totalError + - CalculateFeedForward(); + if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) { + if (m_P != 0) { + double potentialPGain = (m_totalError + m_error) * m_P; + if (potentialPGain < m_maximumOutput) { + if (potentialPGain > m_minimumOutput) { + m_totalError += m_error; + } else { + m_totalError = m_minimumOutput / m_P; } - else { - if (m_I != 0) { - double potentialIGain = (m_totalError + m_error) * m_I; - if (potentialIGain < m_maximumOutput) { - if (potentialIGain > m_minimumOutput) { - m_totalError += m_error; - } - else { - m_totalError = m_minimumOutput / m_I; - } - } - else { - m_totalError = m_maximumOutput / m_I; - } - } + } else { + m_totalError = m_maximumOutput / m_P; + } + } - m_result = m_P * m_error + m_I * m_totalError + - m_D * (m_error - m_prevError) + CalculateFeedForward(); + m_result = m_D * m_error + m_P * m_totalError + CalculateFeedForward(); + } else { + if (m_I != 0) { + double potentialIGain = (m_totalError + m_error) * m_I; + if (potentialIGain < m_maximumOutput) { + if (potentialIGain > m_minimumOutput) { + m_totalError += m_error; + } else { + m_totalError = m_minimumOutput / m_I; } - m_prevError = m_error; + } else { + m_totalError = m_maximumOutput / m_I; + } + } - if (m_result > m_maximumOutput) m_result = m_maximumOutput; - else if (m_result < m_minimumOutput) m_result = m_minimumOutput; + m_result = m_P * m_error + m_I * m_totalError + + m_D * (m_error - m_prevError) + CalculateFeedForward(); + } + m_prevError = m_error; - pidOutput = m_pidOutput; - result = m_result; - } + if (m_result > m_maximumOutput) + m_result = m_maximumOutput; + else if (m_result < m_minimumOutput) + m_result = m_minimumOutput; - pidOutput->PIDWrite(result); - } + pidOutput = m_pidOutput; + result = m_result; + } + + pidOutput->PIDWrite(result); + } } /** - * Calculate the feed forward term + * Calculate the feed forward term. * * Both of the provided feed forward calculations are velocity feed forwards. * If a different feed forward calculation is desired, the user can override - * this function and provide his or her own. This function does no + * this function and provide his or her own. This function does no * synchronization because the PIDController class only calls it in synchronized * code, so be careful if calling it oneself. * @@ -208,8 +196,7 @@ void PIDController::Calculate() double PIDController::CalculateFeedForward() { if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) { return m_F * GetSetpoint(); - } - else { + } else { double temp = m_F * GetDeltaSetpoint(); m_prevSetpoint = m_setpoint; m_setpointTimer.Reset(); @@ -219,115 +206,119 @@ double PIDController::CalculateFeedForward() { /** * Set the PID Controller gain parameters. + * * Set the proportional, integral, and differential coefficients. + * * @param p Proportional coefficient * @param i Integral coefficient * @param d Differential coefficient */ -void PIDController::SetPID(double p, double i, double d) -{ - { - std::lock_guard lock(m_mutex); - m_P = p; - m_I = i; - m_D = d; - } +void PIDController::SetPID(double p, double i, double d) { + { + std::lock_guard lock(m_mutex); + m_P = p; + m_I = i; + m_D = d; + } - if (m_table != nullptr) { - m_table->PutNumber("p", m_P); - m_table->PutNumber("i", m_I); - m_table->PutNumber("d", m_D); - } + if (m_table != nullptr) { + m_table->PutNumber("p", m_P); + m_table->PutNumber("i", m_I); + m_table->PutNumber("d", m_D); + } } /** * Set the PID Controller gain parameters. + * * Set the proportional, integral, and differential coefficients. + * * @param p Proportional coefficient * @param i Integral coefficient * @param d Differential coefficient * @param f Feed forward coefficient */ -void PIDController::SetPID(double p, double i, double d, double f) -{ - { - std::lock_guard lock(m_mutex); - m_P = p; - m_I = i; - m_D = d; - m_F = f; - } +void PIDController::SetPID(double p, double i, double d, double f) { + { + std::lock_guard lock(m_mutex); + m_P = p; + m_I = i; + m_D = d; + m_F = f; + } - if (m_table != nullptr) { - m_table->PutNumber("p", m_P); - m_table->PutNumber("i", m_I); - m_table->PutNumber("d", m_D); - m_table->PutNumber("f", m_F); - } + if (m_table != nullptr) { + m_table->PutNumber("p", m_P); + m_table->PutNumber("i", m_I); + m_table->PutNumber("d", m_D); + m_table->PutNumber("f", m_F); + } } /** - * Get the Proportional coefficient + * Get the Proportional coefficient. + * * @return proportional coefficient */ -double PIDController::GetP() const -{ - std::lock_guard lock(m_mutex); - return m_P; +double PIDController::GetP() const { + std::lock_guard lock(m_mutex); + return m_P; } /** - * Get the Integral coefficient + * Get the Integral coefficient. + * * @return integral coefficient */ -double PIDController::GetI() const -{ - std::lock_guard lock(m_mutex); - return m_I; +double PIDController::GetI() const { + std::lock_guard lock(m_mutex); + return m_I; } /** - * Get the Differential coefficient + * Get the Differential coefficient. + * * @return differential coefficient */ -double PIDController::GetD() const -{ - std::lock_guard lock(m_mutex); - return m_D; +double PIDController::GetD() const { + std::lock_guard lock(m_mutex); + return m_D; } /** - * Get the Feed forward coefficient + * Get the Feed forward coefficient. + * * @return Feed forward coefficient */ -double PIDController::GetF() const -{ - std::lock_guard lock(m_mutex); - return m_F; +double PIDController::GetF() const { + std::lock_guard lock(m_mutex); + return m_F; } /** - * Return the current PID result - * This is always centered on zero and constrained the the max and min outs + * Return the current PID result. + * + * This is always centered on zero and constrained the the max and min outs. + * * @return the latest calculated output */ -float PIDController::Get() const -{ - std::lock_guard lock(m_mutex); - return m_result; +float PIDController::Get() const { + std::lock_guard lock(m_mutex); + return m_result; } /** - * Set the PID controller to consider the input to be continuous, - * Rather then using the max and min in as constraints, it considers them to - * be the same point and automatically calculates the shortest route to - * the setpoint. + * Set the PID controller to consider the input to be continuous. + * + * Rather then using the max and min in as constraints, it considers them to + * be the same point and automatically calculates the shortest route to + * the setpoint. + * * @param continuous Set to true turns on continuous, false turns off continuous */ -void PIDController::SetContinuous(bool continuous) -{ - std::lock_guard lock(m_mutex); - m_continuous = continuous; +void PIDController::SetContinuous(bool continuous) { + std::lock_guard lock(m_mutex); + m_continuous = continuous; } /** @@ -336,15 +327,14 @@ void PIDController::SetContinuous(bool continuous) * @param minimumInput the minimum value expected from the input * @param maximumInput the maximum value expected from the output */ -void PIDController::SetInputRange(float minimumInput, float maximumInput) -{ - { - std::lock_guard lock(m_mutex); - m_minimumInput = minimumInput; - m_maximumInput = maximumInput; - } +void PIDController::SetInputRange(float minimumInput, float maximumInput) { + { + std::lock_guard lock(m_mutex); + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; + } - SetSetpoint(m_setpoint); + SetSetpoint(m_setpoint); } /** @@ -353,85 +343,82 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput) * @param minimumOutput the minimum value to write to the output * @param maximumOutput the maximum value to write to the output */ -void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) -{ - std::lock_guard lock(m_mutex); - m_minimumOutput = minimumOutput; - m_maximumOutput = maximumOutput; +void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) { + std::lock_guard lock(m_mutex); + m_minimumOutput = minimumOutput; + m_maximumOutput = maximumOutput; } /** - * Set the setpoint for the PIDController + * Set the setpoint for the PIDController. + * * @param setpoint the desired setpoint */ -void PIDController::SetSetpoint(float setpoint) -{ - { - std::lock_guard lock(m_mutex); +void PIDController::SetSetpoint(float setpoint) { + { + std::lock_guard lock(m_mutex); - if (m_maximumInput > m_minimumInput) - { - if (setpoint > m_maximumInput) - m_setpoint = m_maximumInput; - else if (setpoint < m_minimumInput) - m_setpoint = m_minimumInput; - else - m_setpoint = setpoint; - } - else - { - m_setpoint = setpoint; - } - } + if (m_maximumInput > m_minimumInput) { + if (setpoint > m_maximumInput) + m_setpoint = m_maximumInput; + else if (setpoint < m_minimumInput) + m_setpoint = m_minimumInput; + else + m_setpoint = setpoint; + } else { + m_setpoint = setpoint; + } + } - if (m_table != nullptr) { - m_table->PutNumber("setpoint", m_setpoint); - } + if (m_table != nullptr) { + m_table->PutNumber("setpoint", m_setpoint); + } } /** - * Returns the current setpoint of the PIDController + * Returns the current setpoint of the PIDController. + * * @return the current setpoint */ -double PIDController::GetSetpoint() const -{ - std::lock_guard lock(m_mutex); - return m_setpoint; +double PIDController::GetSetpoint() const { + std::lock_guard lock(m_mutex); + return m_setpoint; } /** - * Returns the change in setpoint over time of the PIDController + * Returns the change in setpoint over time of the PIDController. + * * @return the change in setpoint over time */ -double PIDController::GetDeltaSetpoint() const -{ - std::lock_guard sync(m_mutex); - return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get(); +double PIDController::GetDeltaSetpoint() const { + std::lock_guard sync(m_mutex); + return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get(); } /** - * Retruns the current difference of the input from the setpoint + * Returns the current difference of the input from the setpoint. + * * @return the current error */ -float PIDController::GetError() const -{ - double pidInput; - { - std::lock_guard lock(m_mutex); - pidInput = m_pidInput->PIDGet(); - } - return GetSetpoint() - pidInput; +float PIDController::GetError() const { + double pidInput; + { + std::lock_guard lock(m_mutex); + pidInput = m_pidInput->PIDGet(); + } + return GetSetpoint() - pidInput; } /** - * Sets what type of input the PID controller will use + * Sets what type of input the PID controller will use. */ void PIDController::SetPIDSourceType(PIDSourceType pidSource) { m_pidInput->SetPIDSourceType(pidSource); } /** - * Returns the type of input the PID controller is using + * Returns the type of input the PID controller is using. + * * @return the PID controller input type */ PIDSourceType PIDController::GetPIDSourceType() const { @@ -440,8 +427,10 @@ PIDSourceType PIDController::GetPIDSourceType() const { /** * Returns the current average of the error over the past few iterations. + * * You can specify the number of iterations to average with SetToleranceBuffer() * (defaults to 1). This is the same value that is used for OnTarget(). + * * @return the average error */ float PIDController::GetAvgError() const { @@ -454,45 +443,46 @@ float PIDController::GetAvgError() const { return avgError; } -/* +/** * Set the percentage error which is considered tolerable for use with * OnTarget. - * @param percentage error which is tolerable + * + * @param percent percentage error which is tolerable */ -void PIDController::SetTolerance(float percent) -{ - std::lock_guard lock(m_mutex); - m_toleranceType = kPercentTolerance; - m_tolerance = percent; +void PIDController::SetTolerance(float percent) { + std::lock_guard lock(m_mutex); + m_toleranceType = kPercentTolerance; + m_tolerance = percent; } -/* +/** * Set the percentage error which is considered tolerable for use with * OnTarget. - * @param percentage error which is tolerable + * + * @param percent percentage error which is tolerable */ -void PIDController::SetPercentTolerance(float percent) -{ - std::lock_guard lock(m_mutex); - m_toleranceType = kPercentTolerance; - m_tolerance = percent; +void PIDController::SetPercentTolerance(float percent) { + std::lock_guard lock(m_mutex); + m_toleranceType = kPercentTolerance; + m_tolerance = percent; } -/* +/** * Set the absolute error which is considered tolerable for use with * OnTarget. - * @param percentage error which is tolerable + * + * @param absTolerance absolute error which is tolerable */ -void PIDController::SetAbsoluteTolerance(float absTolerance) -{ - std::lock_guard lock(m_mutex); - m_toleranceType = kAbsoluteTolerance; - m_tolerance = absTolerance; +void PIDController::SetAbsoluteTolerance(float absTolerance) { + std::lock_guard lock(m_mutex); + m_toleranceType = kAbsoluteTolerance; + m_tolerance = absTolerance; } -/* - * Set the number of previous error samples to average for tolerancing. When - * determining whether a mechanism is on target, the user may want to use a +/** + * Set the number of previous error samples to average for tolerancing. + * + * When determining whether a mechanism is on target, the user may want to use a * rolling average of previous measurements instead of a precise position or * velocity. This is useful for noisy sensors which return a few erroneous * measurements when the mechanism is on target. However, the mechanism will @@ -510,106 +500,101 @@ void PIDController::SetToleranceBuffer(unsigned bufLength) { } } -/* +/** * Return true if the error is within the percentage of the total input range, - * determined by SetTolerance. This asssumes that the maximum and minimum input - * were set using SetInput. - * Currently this just reports on target as the actual value passes through the setpoint. - * Ideally it should be based on being within the tolerance for some period of time. + * determined by SetTolerance. + * + * This asssumes that the maximum and minimum input were set using SetInput. + * Currently this just reports on target as the actual value passes through the + * setpoint. Ideally it should be based on being within the tolerance for some + * period of time. */ -bool PIDController::OnTarget() const -{ - std::lock_guard sync(m_mutex); - if (m_buf.size() == 0) return false; - double error = GetError(); - switch (m_toleranceType) { - case kPercentTolerance: - return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput); - break; - case kAbsoluteTolerance: - return fabs(error) < m_tolerance; - break; - case kNoTolerance: // TODO: this case needs an error - return false; - } - return false; +bool PIDController::OnTarget() const { + std::lock_guard sync(m_mutex); + if (m_buf.size() == 0) return false; + double error = GetError(); + switch (m_toleranceType) { + case kPercentTolerance: + return fabs(error) < + m_tolerance / 100 * (m_maximumInput - m_minimumInput); + break; + case kAbsoluteTolerance: + return fabs(error) < m_tolerance; + break; + case kNoTolerance: // TODO: this case needs an error + return false; + } + return false; } /** - * Begin running the PIDController + * Begin running the PIDController. */ -void PIDController::Enable() -{ - { - std::lock_guard lock(m_mutex); - m_enabled = true; - } +void PIDController::Enable() { + { + std::lock_guard lock(m_mutex); + m_enabled = true; + } - if (m_table != nullptr) { - m_table->PutBoolean("enabled", true); - } + if (m_table != nullptr) { + m_table->PutBoolean("enabled", true); + } } /** * Stop running the PIDController, this sets the output to zero before stopping. */ -void PIDController::Disable() -{ - { - std::lock_guard lock(m_mutex); - m_pidOutput->PIDWrite(0); - m_enabled = false; - } +void PIDController::Disable() { + { + std::lock_guard lock(m_mutex); + m_pidOutput->PIDWrite(0); + m_enabled = false; + } - if (m_table != nullptr) { - m_table->PutBoolean("enabled", false); - } + if (m_table != nullptr) { + m_table->PutBoolean("enabled", false); + } } /** * Return true if PIDController is enabled. */ -bool PIDController::IsEnabled() const -{ - std::lock_guard lock(m_mutex); - return m_enabled; +bool PIDController::IsEnabled() const { + std::lock_guard lock(m_mutex); + return m_enabled; } /** * Reset the previous error,, the integral term, and disable the controller. */ -void PIDController::Reset() -{ - Disable(); +void PIDController::Reset() { + Disable(); - std::lock_guard lock(m_mutex); - m_prevError = 0; - m_totalError = 0; - m_result = 0; + std::lock_guard lock(m_mutex); + m_prevError = 0; + m_totalError = 0; + m_result = 0; } std::string PIDController::GetSmartDashboardType() const { - return "PIDController"; + return "PIDController"; } -void PIDController::InitTable(std::shared_ptr table){ - if(m_table!=nullptr) - m_table->RemoveTableListener(this); - m_table = table; - if(m_table!=nullptr){ - m_table->PutNumber(kP, GetP()); - m_table->PutNumber(kI, GetI()); - m_table->PutNumber(kD, GetD()); - m_table->PutNumber(kF, GetF()); - m_table->PutNumber(kSetpoint, GetSetpoint()); - m_table->PutBoolean(kEnabled, IsEnabled()); - m_table->AddTableListener(this, false); - } +void PIDController::InitTable(std::shared_ptr table) { + if (m_table != nullptr) m_table->RemoveTableListener(this); + m_table = table; + if (m_table != nullptr) { + m_table->PutNumber(kP, GetP()); + m_table->PutNumber(kI, GetI()); + m_table->PutNumber(kD, GetD()); + m_table->PutNumber(kF, GetF()); + m_table->PutNumber(kSetpoint, GetSetpoint()); + m_table->PutBoolean(kEnabled, IsEnabled()); + m_table->AddTableListener(this, false); + } } -std::shared_ptr PIDController::GetTable() const { - return m_table; -} +std::shared_ptr PIDController::GetTable() const { return m_table; } void PIDController::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) { @@ -634,14 +619,8 @@ void PIDController::ValueChanged(ITable* source, llvm::StringRef key, } } -void PIDController::UpdateTable() { +void PIDController::UpdateTable() {} -} +void PIDController::StartLiveWindowMode() { Disable(); } -void PIDController::StartLiveWindowMode() { - Disable(); -} - -void PIDController::StopLiveWindowMode() { - -} +void PIDController::StopLiveWindowMode() {} diff --git a/wpilibc/simulation/src/PWM.cpp b/wpilibc/simulation/src/PWM.cpp index 19b30c3b1a..7264e55521 100644 --- a/wpilibc/simulation/src/PWM.cpp +++ b/wpilibc/simulation/src/PWM.cpp @@ -22,72 +22,77 @@ const int32_t PWM::kPwmDisabled = 0; * Checks channel value range and allocates the appropriate channel. * The allocation is only done to help users ensure that they don't double * assign channels. + * * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP - * port + * port */ -PWM::PWM(uint32_t channel) -{ - char buf[64]; +PWM::PWM(uint32_t channel) { + char buf[64]; - if (!CheckPWMChannel(channel)) - { - snprintf(buf, 64, "PWM Channel %d", channel); - wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf); - return; - } + if (!CheckPWMChannel(channel)) { + snprintf(buf, 64, "PWM Channel %d", channel); + wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf); + return; + } - sprintf(buf, "pwm/%d", channel); - impl = new SimContinuousOutput(buf); - m_channel = channel; - m_eliminateDeadband = false; + sprintf(buf, "pwm/%d", channel); + impl = new SimContinuousOutput(buf); + m_channel = channel; + m_eliminateDeadband = false; - m_centerPwm = kPwmDisabled; // In simulation, the same thing. + m_centerPwm = kPwmDisabled; // In simulation, the same thing. } PWM::~PWM() { - if (m_table != nullptr) m_table->RemoveTableListener(this); + if (m_table != nullptr) m_table->RemoveTableListener(this); } /** * Optionally eliminate the deadband from a speed controller. - * @param eliminateDeadband If true, set the motor curve on the Jaguar to eliminate - * the deadband in the middle of the range. Otherwise, keep the full range without - * modifying any values. + * + * @param eliminateDeadband If true, set the motor curve on the Jaguar to + * eliminate the deadband in the middle of the range. + * Otherwise, keep the full range without modifying + * any values. */ -void PWM::EnableDeadbandElimination(bool eliminateDeadband) -{ - m_eliminateDeadband = eliminateDeadband; +void PWM::EnableDeadbandElimination(bool eliminateDeadband) { + m_eliminateDeadband = eliminateDeadband; } /** * Set the bounds on the PWM values. - * This sets the bounds on the PWM values for a particular each type of controller. The values - * determine the upper and lower speeds as well as the deadband bracket. - * @param max The Minimum pwm value + * + * This sets the bounds on the PWM values for a particular each type of + * controller. The values determine the upper and lower speeds as well as the + * deadband bracket. + * + * @param max The Minimum pwm value * @param deadbandMax The high end of the deadband range - * @param center The center speed (off) + * @param center The center speed (off) * @param deadbandMin The low end of the deadband range - * @param min The minimum pwm value + * @param min The minimum pwm value */ -void PWM::SetBounds(int32_t max, int32_t deadbandMax, int32_t center, int32_t deadbandMin, int32_t min) -{ - // Nothing to do in simulation. +void PWM::SetBounds(int32_t max, int32_t deadbandMax, int32_t center, + int32_t deadbandMin, int32_t min) { + // Nothing to do in simulation. } - /** * Set the bounds on the PWM pulse widths. - * This sets the bounds on the PWM values for a particular type of controller. The values - * determine the upper and lower speeds as well as the deadband bracket. - * @param max The max PWM pulse width in ms + * + * This sets the bounds on the PWM values for a particular type of controller. + * The values determine the upper and lower speeds as well as the deadband + * bracket. + * + * @param max The max PWM pulse width in ms * @param deadbandMax The high end of the deadband range pulse width in ms - * @param center The center (off) pulse width in ms + * @param center The center (off) pulse width in ms * @param deadbandMin The low end of the deadband pulse width in ms - * @param min The minimum pulse width in ms + * @param min The minimum pulse width in ms */ -void PWM::SetBounds(double max, double deadbandMax, double center, double deadbandMin, double min) -{ - // Nothing to do in simulation. +void PWM::SetBounds(double max, double deadbandMax, double center, + double deadbandMin, double min) { + // Nothing to do in simulation. } /** @@ -100,18 +105,14 @@ void PWM::SetBounds(double max, double deadbandMax, double center, double deadba * * @param pos The position to set the servo between 0.0 and 1.0. */ -void PWM::SetPosition(float pos) -{ - if (pos < 0.0) - { - pos = 0.0; - } - else if (pos > 1.0) - { - pos = 1.0; - } +void PWM::SetPosition(float pos) { + if (pos < 0.0) { + pos = 0.0; + } else if (pos > 1.0) { + pos = 1.0; + } - impl->Set(pos); + impl->Set(pos); } /** @@ -124,21 +125,15 @@ void PWM::SetPosition(float pos) * * @return The position the servo is set to between 0.0 and 1.0. */ -float PWM::GetPosition() const -{ - float value = impl->Get(); - if (value < 0.0) - { - return 0.0; - } - else if (value > 1.0) - { - return 1.0; - } - else - { - return value; - } +float PWM::GetPosition() const { + float value = impl->Get(); + if (value < 0.0) { + return 0.0; + } else if (value > 1.0) { + return 1.0; + } else { + return value; + } } /** @@ -154,19 +149,15 @@ float PWM::GetPosition() const * * @param speed The speed to set the speed controller between -1.0 and 1.0. */ -void PWM::SetSpeed(float speed) -{ - // clamp speed to be in the range 1.0 >= speed >= -1.0 - if (speed < -1.0) - { - speed = -1.0; - } - else if (speed > 1.0) - { - speed = 1.0; - } +void PWM::SetSpeed(float speed) { + // clamp speed to be in the range 1.0 >= speed >= -1.0 + if (speed < -1.0) { + speed = -1.0; + } else if (speed > 1.0) { + speed = 1.0; + } - impl->Set(speed); + impl->Set(speed); } /** @@ -181,21 +172,15 @@ void PWM::SetSpeed(float speed) * * @return The most recently set speed between -1.0 and 1.0. */ -float PWM::GetSpeed() const -{ - float value = impl->Get(); - if (value > 1.0) - { - return 1.0; - } - else if (value < -1.0) - { - return -1.0; - } - else - { - return value; - } +float PWM::GetSpeed() const { + float value = impl->Get(); + if (value > 1.0) { + return 1.0; + } else if (value < -1.0) { + return -1.0; + } else { + return value; + } } /** @@ -205,10 +190,9 @@ float PWM::GetSpeed() const * * @param value Raw PWM value. */ -void PWM::SetRaw(unsigned short value) -{ - wpi_assert(value == kPwmDisabled); - impl->Set(0); +void PWM::SetRaw(unsigned short value) { + wpi_assert(value == kPwmDisabled); + impl->Set(0); } /** @@ -216,12 +200,10 @@ void PWM::SetRaw(unsigned short value) * * @param mult The period multiplier to apply to this channel */ -void PWM::SetPeriodMultiplier(PeriodMultiplier mult) -{ - // Do nothing in simulation. +void PWM::SetPeriodMultiplier(PeriodMultiplier mult) { + // Do nothing in simulation. } - void PWM::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) { if (!value->IsDouble()) return; @@ -229,34 +211,30 @@ void PWM::ValueChanged(ITable* source, llvm::StringRef key, } void PWM::UpdateTable() { - if (m_table != nullptr) { - m_table->PutNumber("Value", GetSpeed()); - } + if (m_table != nullptr) { + m_table->PutNumber("Value", GetSpeed()); + } } void PWM::StartLiveWindowMode() { - SetSpeed(0); - if (m_table != nullptr) { - m_table->AddTableListener("Value", this, true); - } + SetSpeed(0); + if (m_table != nullptr) { + m_table->AddTableListener("Value", this, true); + } } void PWM::StopLiveWindowMode() { - SetSpeed(0); - if (m_table != nullptr) { - m_table->RemoveTableListener(this); - } + SetSpeed(0); + if (m_table != nullptr) { + m_table->RemoveTableListener(this); + } } -std::string PWM::GetSmartDashboardType() const { - return "Speed Controller"; -} +std::string PWM::GetSmartDashboardType() const { return "Speed Controller"; } void PWM::InitTable(std::shared_ptr subTable) { - m_table = subTable; - UpdateTable(); + m_table = subTable; + UpdateTable(); } -std::shared_ptr PWM::GetTable() const { - return m_table; -} +std::shared_ptr PWM::GetTable() const { return m_table; } diff --git a/wpilibc/simulation/src/Relay.cpp b/wpilibc/simulation/src/Relay.cpp index 7582b529b9..1cbd8ce9ca 100644 --- a/wpilibc/simulation/src/Relay.cpp +++ b/wpilibc/simulation/src/Relay.cpp @@ -9,118 +9,108 @@ #include "MotorSafetyHelper.h" //#include "NetworkCommunication/UsageReporting.h" -#include "WPIErrors.h" #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" /** * Relay constructor given a channel. * * This code initializes the relay and reserves all resources that need to be * locked. Initially the relay is set to both lines at 0v. - * @param channel The channel number (0-3). + * + * @param channel The channel number (0-3). * @param direction The direction that the Relay object will control. */ Relay::Relay(uint32_t channel, Relay::Direction direction) - : m_channel (channel) - , m_direction (direction) -{ - char buf[64]; - if (!SensorBase::CheckRelayChannel(m_channel)) - { - snprintf(buf, 64, "Relay Channel %d", m_channel); - wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf); - return; - } + : m_channel(channel), m_direction(direction) { + char buf[64]; + if (!SensorBase::CheckRelayChannel(m_channel)) { + snprintf(buf, 64, "Relay Channel %d", m_channel); + wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf); + return; + } - m_safetyHelper = std::make_unique(this); - m_safetyHelper->SetSafetyEnabled(false); + m_safetyHelper = std::make_unique(this); + m_safetyHelper->SetSafetyEnabled(false); - sprintf(buf, "relay/%d", m_channel); - impl = new SimContinuousOutput(buf); // TODO: Allow two different relays (targetting the different halves of a relay) to be combined to control one motor. - LiveWindow::GetInstance()->AddActuator("Relay", 1, m_channel, this); - go_pos = go_neg = false; + sprintf(buf, "relay/%d", m_channel); + impl = new SimContinuousOutput(buf); // TODO: Allow two different relays + // (targetting the different halves of a + // relay) to be combined to control one + // motor. + LiveWindow::GetInstance()->AddActuator("Relay", 1, m_channel, this); + go_pos = go_neg = false; } /** * Free the resource associated with a relay. + * * The relay channels are set to free and the relay output is turned off. */ -Relay::~Relay() -{ - impl->Set(0); - if (m_table != nullptr) m_table->RemoveTableListener(this); +Relay::~Relay() { + impl->Set(0); + if (m_table != nullptr) m_table->RemoveTableListener(this); } /** * Set the relay state. * - * Valid values depend on which directions of the relay are controlled by the object. + * Valid values depend on which directions of the relay are controlled by the + * object. * * When set to kBothDirections, the relay can be any of the four states: * 0v-0v, 0v-12v, 12v-0v, 12v-12v * - * When set to kForwardOnly or kReverseOnly, you can specify the constant for the - * direction or you can simply specify kOff and kOn. Using only kOff and kOn is - * recommended. + * When set to kForwardOnly or kReverseOnly, you can specify the constant for + * the direction or you can simply specify kOff and kOn. Using only kOff and + * kOn is recommended. * * @param value The state to set the relay. */ -void Relay::Set(Relay::Value value) -{ - switch (value) - { - case kOff: - if (m_direction == kBothDirections || m_direction == kForwardOnly) - { - go_pos = false; - } - if (m_direction == kBothDirections || m_direction == kReverseOnly) - { - go_neg = false; - } - break; - case kOn: - if (m_direction == kBothDirections || m_direction == kForwardOnly) - { - go_pos = true; - } - if (m_direction == kBothDirections || m_direction == kReverseOnly) - { - go_neg = true; - } - break; - case kForward: - if (m_direction == kReverseOnly) - { - wpi_setWPIError(IncompatibleMode); - break; - } - if (m_direction == kBothDirections || m_direction == kForwardOnly) - { - go_pos = true; - } - if (m_direction == kBothDirections) - { - go_neg = false; - } - break; - case kReverse: - if (m_direction == kForwardOnly) - { - wpi_setWPIError(IncompatibleMode); - break; - } - if (m_direction == kBothDirections) - { - go_pos = false; - } - if (m_direction == kBothDirections || m_direction == kReverseOnly) - { - go_neg = true; - } - break; - } - impl->Set((go_pos ? 1 : 0) + (go_neg ? -1 : 0)); +void Relay::Set(Relay::Value value) { + switch (value) { + case kOff: + if (m_direction == kBothDirections || m_direction == kForwardOnly) { + go_pos = false; + } + if (m_direction == kBothDirections || m_direction == kReverseOnly) { + go_neg = false; + } + break; + case kOn: + if (m_direction == kBothDirections || m_direction == kForwardOnly) { + go_pos = true; + } + if (m_direction == kBothDirections || m_direction == kReverseOnly) { + go_neg = true; + } + break; + case kForward: + if (m_direction == kReverseOnly) { + wpi_setWPIError(IncompatibleMode); + break; + } + if (m_direction == kBothDirections || m_direction == kForwardOnly) { + go_pos = true; + } + if (m_direction == kBothDirections) { + go_neg = false; + } + break; + case kReverse: + if (m_direction == kForwardOnly) { + wpi_setWPIError(IncompatibleMode); + break; + } + if (m_direction == kBothDirections) { + go_pos = false; + } + if (m_direction == kBothDirections || m_direction == kReverseOnly) { + go_neg = true; + } + break; + } + impl->Set((go_pos ? 1 : 0) + (go_neg ? -1 : 0)); } /** @@ -129,29 +119,29 @@ void Relay::Set(Relay::Value value) * Gets the current state of the relay. * * When set to kForwardOnly or kReverseOnly, value is returned as kOn/kOff not - * kForward/kReverse (per the recommendation in Set) + * kForward/kReverse (per the recommendation in Set). * * @return The current state of the relay as a Relay::Value */ Relay::Value Relay::Get() const { - // TODO: Don't assume that the go_pos and go_neg fields are correct? - if ((go_pos || m_direction == kReverseOnly) && (go_neg || m_direction == kForwardOnly)) { - return kOn; - } else if (go_pos) { - return kForward; - } else if (go_neg) { - return kReverse; - } else { - return kOff; - } + // TODO: Don't assume that the go_pos and go_neg fields are correct? + if ((go_pos || m_direction == kReverseOnly) && + (go_neg || m_direction == kForwardOnly)) { + return kOn; + } else if (go_pos) { + return kForward; + } else if (go_neg) { + return kReverse; + } else { + return kOff; + } } -uint32_t Relay::GetChannel() const { - return m_channel; -} +uint32_t Relay::GetChannel() const { return m_channel; } /** - * Set the expiration time for the Relay object + * Set the expiration time for the Relay object. + * * @param timeout The timeout (in seconds) for this relay object */ void Relay::SetExpiration(float timeout) { @@ -160,19 +150,22 @@ void Relay::SetExpiration(float timeout) { /** * Return the expiration time for the relay object. + * * @return The expiration time value. */ float Relay::GetExpiration() const { return m_safetyHelper->GetExpiration(); } /** * Check if the relay object is currently alive or stopped due to a timeout. - * @returns a bool value that is true if the motor has NOT timed out and should - * still be running. + * + * @return a bool value that is true if the motor has NOT timed out and should + * still be running. */ bool Relay::IsAlive() const { return m_safetyHelper->IsAlive(); } /** * Stop the motor associated with this PWM object. + * * This is called by the MotorSafetyHelper object when it has a timeout for this * relay and needs to stop it from running. */ @@ -180,7 +173,9 @@ void Relay::StopMotor() { Set(kOff); } /** * Enable/disable motor safety for this device + * * Turn on and off the motor safety option for this relay object. + * * @param enabled True if motor safety is enforced for this object */ void Relay::SetSafetyEnabled(bool enabled) { @@ -188,8 +183,9 @@ void Relay::SetSafetyEnabled(bool enabled) { } /** - * Check if motor safety is enabled for this object - * @returns True if motor safety is enforced for this object + * Check if motor safety is enabled for this object. + * + * @return True if motor safety is enforced for this object */ bool Relay::IsSafetyEnabled() const { return m_safetyHelper->IsSafetyEnabled(); @@ -202,49 +198,45 @@ void Relay::GetDescription(std::ostringstream& desc) const { void Relay::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) { if (!value->IsString()) return; - if (value->GetString() == "Off") Set(kOff); - else if (value->GetString() == "Forward") Set(kForward); - else if (value->GetString() == "Reverse") Set(kReverse); + if (value->GetString() == "Off") + Set(kOff); + else if (value->GetString() == "Forward") + Set(kForward); + else if (value->GetString() == "Reverse") + Set(kReverse); } void Relay::UpdateTable() { - if(m_table != nullptr){ - if (Get() == kOn) { - m_table->PutString("Value", "On"); - } - else if (Get() == kForward) { - m_table->PutString("Value", "Forward"); - } - else if (Get() == kReverse) { - m_table->PutString("Value", "Reverse"); - } - else { - m_table->PutString("Value", "Off"); - } - } + if (m_table != nullptr) { + if (Get() == kOn) { + m_table->PutString("Value", "On"); + } else if (Get() == kForward) { + m_table->PutString("Value", "Forward"); + } else if (Get() == kReverse) { + m_table->PutString("Value", "Reverse"); + } else { + m_table->PutString("Value", "Off"); + } + } } void Relay::StartLiveWindowMode() { - if(m_table != nullptr){ - m_table->AddTableListener("Value", this, true); - } + if (m_table != nullptr) { + m_table->AddTableListener("Value", this, true); + } } void Relay::StopLiveWindowMode() { - if(m_table != nullptr){ - m_table->RemoveTableListener(this); - } + if (m_table != nullptr) { + m_table->RemoveTableListener(this); + } } -std::string Relay::GetSmartDashboardType() const { - return "Relay"; -} +std::string Relay::GetSmartDashboardType() const { return "Relay"; } void Relay::InitTable(std::shared_ptr subTable) { - m_table = subTable; - UpdateTable(); + m_table = subTable; + UpdateTable(); } -std::shared_ptr Relay::GetTable() const { - return m_table; -} +std::shared_ptr Relay::GetTable() const { return m_table; } diff --git a/wpilibc/simulation/src/RobotBase.cpp b/wpilibc/simulation/src/RobotBase.cpp index d5afb7f4de..7ebaba02f0 100644 --- a/wpilibc/simulation/src/RobotBase.cpp +++ b/wpilibc/simulation/src/RobotBase.cpp @@ -13,88 +13,77 @@ RobotBase* RobotBase::m_instance = nullptr; -void RobotBase::setInstance(RobotBase* robot) -{ - wpi_assert(m_instance == nullptr); - m_instance = robot; +void RobotBase::setInstance(RobotBase* robot) { + wpi_assert(m_instance == nullptr); + m_instance = robot; } -RobotBase &RobotBase::getInstance() -{ - return *m_instance; -} +RobotBase& RobotBase::getInstance() { return *m_instance; } /** * Constructor for a generic robot program. - * User code should be placed in the constuctor that runs before the Autonomous or Operator - * Control period starts. The constructor will run to completion before Autonomous is entered. * - * This must be used to ensure that the communications code starts. In the future it would be - * nice to put this code into it's own task that loads on boot so ensure that it runs. + * User code should be placed in the constuctor that runs before the Autonomous + * or Operator Control period starts. The constructor will run to completion + * before Autonomous is entered. + * + * This must be used to ensure that the communications code starts. In the + * future it would be nice to put this code into it's own task that loads on + * boot so ensure that it runs. */ -RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) -{ - RobotState::SetImplementation(DriverStation::GetInstance()); - time_sub = MainNode::Subscribe("~/time", &wpilib::internal::time_callback); +RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) { + RobotState::SetImplementation(DriverStation::GetInstance()); + time_sub = MainNode::Subscribe("~/time", &wpilib::internal::time_callback); } /** * Determine if the Robot is currently enabled. + * * @return True if the Robot is currently enabled by the field controls. */ -bool RobotBase::IsEnabled() const -{ - return m_ds.IsEnabled(); -} +bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); } /** * Determine if the Robot is currently disabled. + * * @return True if the Robot is currently disabled by the field controls. */ -bool RobotBase::IsDisabled() const -{ - return m_ds.IsDisabled(); -} +bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); } /** * Determine if the robot is currently in Autnomous mode. - * @return True if the robot is currently operating Autonomously as determined by the field controls. + * + * @return True if the robot is currently operating Autonomously as determined + * by the field controls. */ -bool RobotBase::IsAutonomous() const -{ - return m_ds.IsAutonomous(); -} +bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); } /** * Determine if the robot is currently in Operator Control mode. - * @return True if the robot is currently operating in Tele-Op mode as determined by the field controls. + * + * @return True if the robot is currently operating in Tele-Op mode as + * determined by the field controls. */ -bool RobotBase::IsOperatorControl() const -{ - return m_ds.IsOperatorControl(); -} +bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); } /** * Determine if the robot is currently in Test mode. - * @return True if the robot is currently running tests as determined by the field controls. + * + * @return True if the robot is currently running tests as determined by the + * field controls. */ -bool RobotBase::IsTest() const -{ - return m_ds.IsTest(); -} +bool RobotBase::IsTest() const { return m_ds.IsTest(); } /** - * This class exists for the sole purpose of getting its destructor called when the module unloads. - * Before the module is done unloading, we need to delete the RobotBase derived singleton. This should delete - * the other remaining singletons that were registered. This should also stop all tasks that are using - * the Task class. + * This class exists for the sole purpose of getting its destructor called when + * the module unloads. + * + * Before the module is done unloading, we need to delete the RobotBase derived + * singleton. This should delete the other remaining singletons that were + * registered. This should also stop all tasks that are using the Task class. */ -class RobotDeleter -{ -public: - ~RobotDeleter() - { - delete &RobotBase::getInstance(); - } +class RobotDeleter { + public: + ~RobotDeleter() { delete &RobotBase::getInstance(); } }; static RobotDeleter g_robotDeleter; diff --git a/wpilibc/simulation/src/RobotDrive.cpp b/wpilibc/simulation/src/RobotDrive.cpp index b6216d00eb..f591b330a4 100644 --- a/wpilibc/simulation/src/RobotDrive.cpp +++ b/wpilibc/simulation/src/RobotDrive.cpp @@ -7,166 +7,183 @@ #include "RobotDrive.h" //#include "CANJaguar.h" +#include #include "GenericHID.h" #include "Joystick.h" #include "Talon.h" #include "Utility.h" #include "WPIErrors.h" -#include #undef max #include const int32_t RobotDrive::kMaxNumberOfMotors; -static auto make_shared_nodelete(SpeedController *ptr) { - return std::shared_ptr(ptr, NullDeleter()); +static auto make_shared_nodelete(SpeedController* ptr) { + return std::shared_ptr(ptr, NullDeleter()); } /* - * Driving functions - * These functions provide an interface to multiple motors that is used for C programming - * The Drive(speed, direction) function is the main part of the set that makes it easy - * to set speeds and direction independently in one call. + * Driving functions. + * + * These functions provide an interface to multiple motors that is used for C + * programming. + * The Drive(speed, direction) function is the main part of the set that makes + * it easy to set speeds and direction independently in one call. */ /** * Common function to initialize all the robot drive constructors. * Create a motor safety object (the real reason for the common code) and - * initialize all the motor assignments. The default timeout is set for the robot drive. + * initialize all the motor assignments. The default timeout is set for the + * robot drive. */ void RobotDrive::InitRobotDrive() { - // FIXME: m_safetyHelper = new MotorSafetyHelper(this); - // FIXME: m_safetyHelper->SetSafetyEnabled(true); + // FIXME: m_safetyHelper = new MotorSafetyHelper(this); + // FIXME: m_safetyHelper->SetSafetyEnabled(true); } -/** Constructor for RobotDrive with 2 motors specified with channel numbers. +/** + * Constructor for RobotDrive with 2 motors specified with channel numbers. + * * Set up parameters for a two wheel drive system where the * left and right motor pwm channels are specified in the call. - * This call assumes Talosn for controlling the motors. - * @param leftMotorChannel The PWM channel number that drives the left motor. + * This call assumes Talons for controlling the motors. + * + * @param leftMotorChannel The PWM channel number that drives the left motor. * @param rightMotorChannel The PWM channel number that drives the right motor. */ -RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel) -{ - InitRobotDrive(); - m_rearLeftMotor = std::make_shared(leftMotorChannel); - m_rearRightMotor = std::make_shared(rightMotorChannel); - SetLeftRightMotorOutputs(0.0, 0.0); - m_deleteSpeedControllers = true; +RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel) { + InitRobotDrive(); + m_rearLeftMotor = std::make_shared(leftMotorChannel); + m_rearRightMotor = std::make_shared(rightMotorChannel); + SetLeftRightMotorOutputs(0.0, 0.0); + m_deleteSpeedControllers = true; } /** * Constructor for RobotDrive with 4 motors specified with channel numbers. + * * Set up parameters for a four wheel drive system where all four motor * pwm channels are specified in the call. * This call assumes Talons for controlling the motors. - * @param frontLeftMotor Front left motor channel number - * @param rearLeftMotor Rear Left motor channel number + * + * @param frontLeftMotor Front left motor channel number + * @param rearLeftMotor Rear Left motor channel number * @param frontRightMotor Front right motor channel number - * @param rearRightMotor Rear Right motor channel number + * @param rearRightMotor Rear Right motor channel number */ RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor, - uint32_t frontRightMotor, uint32_t rearRightMotor) -{ - InitRobotDrive(); - m_rearLeftMotor = std::make_shared(rearLeftMotor); - m_rearRightMotor = std::make_shared(rearRightMotor); - m_frontLeftMotor = std::make_shared(frontLeftMotor); - m_frontRightMotor = std::make_shared(frontRightMotor); - SetLeftRightMotorOutputs(0.0, 0.0); - m_deleteSpeedControllers = true; + uint32_t frontRightMotor, uint32_t rearRightMotor) { + InitRobotDrive(); + m_rearLeftMotor = std::make_shared(rearLeftMotor); + m_rearRightMotor = std::make_shared(rearRightMotor); + m_frontLeftMotor = std::make_shared(frontLeftMotor); + m_frontRightMotor = std::make_shared(frontRightMotor); + SetLeftRightMotorOutputs(0.0, 0.0); + m_deleteSpeedControllers = true; } /** - * Constructor for RobotDrive with 2 motors specified as SpeedController objects. - * The SpeedController version of the constructor enables programs to use the RobotDrive classes with - * subclasses of the SpeedController objects, for example, versions with ramping or reshaping of - * the curve to suit motor bias or deadband elimination. - * @param leftMotor The left SpeedController object used to drive the robot. + * Constructor for RobotDrive with 2 motors specified as SpeedController + * objects. + * + * The SpeedController version of the constructor enables programs to use the + * RobotDrive classes with subclasses of the SpeedController objects, for + * example, versions with ramping or reshaping of the curve to suit motor bias + * or deadband elimination. + * + * @param leftMotor The left SpeedController object used to drive the robot. * @param rightMotor the right SpeedController object used to drive the robot. */ -RobotDrive::RobotDrive(SpeedController *leftMotor, - SpeedController *rightMotor) { - InitRobotDrive(); - if (leftMotor == nullptr || rightMotor == nullptr) { - wpi_setWPIError(NullParameter); - m_rearLeftMotor = m_rearRightMotor = nullptr; - return; - } - m_rearLeftMotor = make_shared_nodelete(leftMotor); - m_rearRightMotor = make_shared_nodelete(rightMotor); +RobotDrive::RobotDrive(SpeedController* leftMotor, + SpeedController* rightMotor) { + InitRobotDrive(); + if (leftMotor == nullptr || rightMotor == nullptr) { + wpi_setWPIError(NullParameter); + m_rearLeftMotor = m_rearRightMotor = nullptr; + return; + } + m_rearLeftMotor = make_shared_nodelete(leftMotor); + m_rearRightMotor = make_shared_nodelete(rightMotor); } -//TODO: Change to rvalue references & move syntax. -RobotDrive::RobotDrive(SpeedController &leftMotor, - SpeedController &rightMotor) { - InitRobotDrive(); - m_rearLeftMotor = make_shared_nodelete(&leftMotor); - m_rearRightMotor = make_shared_nodelete(&rightMotor); +// TODO: Change to rvalue references & move syntax. +RobotDrive::RobotDrive(SpeedController& leftMotor, + SpeedController& rightMotor) { + InitRobotDrive(); + m_rearLeftMotor = make_shared_nodelete(&leftMotor); + m_rearRightMotor = make_shared_nodelete(&rightMotor); } RobotDrive::RobotDrive(std::shared_ptr leftMotor, std::shared_ptr rightMotor) { - InitRobotDrive(); - if (leftMotor == nullptr || rightMotor == nullptr) { - wpi_setWPIError(NullParameter); - m_rearLeftMotor = m_rearRightMotor = nullptr; - return; - } - m_rearLeftMotor = leftMotor; - m_rearRightMotor = rightMotor; + InitRobotDrive(); + if (leftMotor == nullptr || rightMotor == nullptr) { + wpi_setWPIError(NullParameter); + m_rearLeftMotor = m_rearRightMotor = nullptr; + return; + } + m_rearLeftMotor = leftMotor; + m_rearRightMotor = rightMotor; } /** - * Constructor for RobotDrive with 4 motors specified as SpeedController objects. + * Constructor for RobotDrive with 4 motors specified as SpeedController + * objects. + * * Speed controller input version of RobotDrive (see previous comments). - * @param rearLeftMotor The back left SpeedController object used to drive the robot. - * @param frontLeftMotor The front left SpeedController object used to drive the robot - * @param rearRightMotor The back right SpeedController object used to drive the robot. - * @param frontRightMotor The front right SpeedController object used to drive the robot. + * + * @param rearLeftMotor The back left SpeedController object used to drive the + * robot. + * @param frontLeftMotor The front left SpeedController object used to drive + * the robot + * @param rearRightMotor The back right SpeedController object used to drive + * the robot. + * @param frontRightMotor The front right SpeedController object used to drive + * the robot. */ -RobotDrive::RobotDrive(SpeedController *frontLeftMotor, - SpeedController *rearLeftMotor, - SpeedController *frontRightMotor, - SpeedController *rearRightMotor) { - InitRobotDrive(); - if (frontLeftMotor == nullptr || rearLeftMotor == nullptr || - frontRightMotor == nullptr || rearRightMotor == nullptr) { - wpi_setWPIError(NullParameter); - return; - } - m_frontLeftMotor = make_shared_nodelete(frontLeftMotor); - m_rearLeftMotor = make_shared_nodelete(rearLeftMotor); - m_frontRightMotor = make_shared_nodelete(frontRightMotor); - m_rearRightMotor = make_shared_nodelete(rearRightMotor); +RobotDrive::RobotDrive(SpeedController* frontLeftMotor, + SpeedController* rearLeftMotor, + SpeedController* frontRightMotor, + SpeedController* rearRightMotor) { + InitRobotDrive(); + if (frontLeftMotor == nullptr || rearLeftMotor == nullptr || + frontRightMotor == nullptr || rearRightMotor == nullptr) { + wpi_setWPIError(NullParameter); + return; + } + m_frontLeftMotor = make_shared_nodelete(frontLeftMotor); + m_rearLeftMotor = make_shared_nodelete(rearLeftMotor); + m_frontRightMotor = make_shared_nodelete(frontRightMotor); + m_rearRightMotor = make_shared_nodelete(rearRightMotor); } -RobotDrive::RobotDrive(SpeedController &frontLeftMotor, - SpeedController &rearLeftMotor, - SpeedController &frontRightMotor, - SpeedController &rearRightMotor) { - InitRobotDrive(); - m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor); - m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor); - m_frontRightMotor = make_shared_nodelete(&frontRightMotor); - m_rearRightMotor = make_shared_nodelete(&rearRightMotor); +RobotDrive::RobotDrive(SpeedController& frontLeftMotor, + SpeedController& rearLeftMotor, + SpeedController& frontRightMotor, + SpeedController& rearRightMotor) { + InitRobotDrive(); + m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor); + m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor); + m_frontRightMotor = make_shared_nodelete(&frontRightMotor); + m_rearRightMotor = make_shared_nodelete(&rearRightMotor); } RobotDrive::RobotDrive(std::shared_ptr frontLeftMotor, std::shared_ptr rearLeftMotor, std::shared_ptr frontRightMotor, std::shared_ptr rearRightMotor) { - InitRobotDrive(); - if (frontLeftMotor == nullptr || rearLeftMotor == nullptr || - frontRightMotor == nullptr || rearRightMotor == nullptr) { - wpi_setWPIError(NullParameter); - return; - } - m_frontLeftMotor = frontLeftMotor; - m_rearLeftMotor = rearLeftMotor; - m_frontRightMotor = frontRightMotor; - m_rearRightMotor = rearRightMotor; + InitRobotDrive(); + if (frontLeftMotor == nullptr || rearLeftMotor == nullptr || + frontRightMotor == nullptr || rearRightMotor == nullptr) { + wpi_setWPIError(NullParameter); + return; + } + m_frontLeftMotor = frontLeftMotor; + m_rearLeftMotor = rearLeftMotor; + m_frontRightMotor = frontRightMotor; + m_rearRightMotor = rearRightMotor; } /** @@ -182,375 +199,389 @@ RobotDrive::RobotDrive(std::shared_ptr frontLeftMotor, * This function will most likely be used in an autonomous routine. * * @param outputMagnitude The speed setting for the outside wheel in a turn, - * forward or backwards, +1 to -1. - * @param curve The rate of turn, constant for different forward speeds. Set - * curve < 0 for left turn or curve > 0 for right turn. - * Set curve = e^(-r/w) to get a turn radius r for wheelbase w of your robot. - * Conversely, turn radius r = -ln(curve)*w for a given value of curve and - * wheelbase w. + * forward or backwards, +1 to -1. + * @param curve The rate of turn, constant for different forward + * speeds. Set curve < 0 for left turn or curve > 0 for + * right turn. Set curve = e^(-r/w) to get a turn radius + * r for wheelbase w of your robot. Conversely, turn + * radius r = -ln(curve)*w for a given value of curve and + * wheelbase w. */ -void RobotDrive::Drive(float outputMagnitude, float curve) -{ - float leftOutput, rightOutput; - static bool reported = false; - if (!reported) - { - reported = true; - } +void RobotDrive::Drive(float outputMagnitude, float curve) { + float leftOutput, rightOutput; + static bool reported = false; + if (!reported) { + reported = true; + } - if (curve < 0) - { - float value = log(-curve); - float ratio = (value - m_sensitivity)/(value + m_sensitivity); - if (ratio == 0) ratio =.0000000001; - leftOutput = outputMagnitude / ratio; - rightOutput = outputMagnitude; - } - else if (curve > 0) - { - float value = log(curve); - float ratio = (value - m_sensitivity)/(value + m_sensitivity); - if (ratio == 0) ratio =.0000000001; - leftOutput = outputMagnitude; - rightOutput = outputMagnitude / ratio; - } - else - { - leftOutput = outputMagnitude; - rightOutput = outputMagnitude; - } - SetLeftRightMotorOutputs(leftOutput, rightOutput); + if (curve < 0) { + float value = log(-curve); + float ratio = (value - m_sensitivity) / (value + m_sensitivity); + if (ratio == 0) ratio = .0000000001; + leftOutput = outputMagnitude / ratio; + rightOutput = outputMagnitude; + } else if (curve > 0) { + float value = log(curve); + float ratio = (value - m_sensitivity) / (value + m_sensitivity); + if (ratio == 0) ratio = .0000000001; + leftOutput = outputMagnitude; + rightOutput = outputMagnitude / ratio; + } else { + leftOutput = outputMagnitude; + rightOutput = outputMagnitude; + } + SetLeftRightMotorOutputs(leftOutput, rightOutput); } /** * Provide tank steering using the stored robot configuration. + * * Drive the robot using two joystick inputs. The Y-axis will be selected from * each Joystick object. - * @param leftStick The joystick to control the left side of the robot. + * + * @param leftStick The joystick to control the left side of the robot. * @param rightStick The joystick to control the right side of the robot. */ -void RobotDrive::TankDrive(GenericHID *leftStick, GenericHID *rightStick, bool squaredInputs) -{ - if (leftStick == nullptr || rightStick == nullptr) - { - wpi_setWPIError(NullParameter); - return; - } - TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs); +void RobotDrive::TankDrive(GenericHID* leftStick, GenericHID* rightStick, + bool squaredInputs) { + if (leftStick == nullptr || rightStick == nullptr) { + wpi_setWPIError(NullParameter); + return; + } + TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs); } -void RobotDrive::TankDrive(GenericHID &leftStick, GenericHID &rightStick, bool squaredInputs) -{ - TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs); +void RobotDrive::TankDrive(GenericHID& leftStick, GenericHID& rightStick, + bool squaredInputs) { + TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs); } /** * Provide tank steering using the stored robot configuration. - * This function lets you pick the axis to be used on each Joystick object for the left - * and right sides of the robot. - * @param leftStick The Joystick object to use for the left side of the robot. - * @param leftAxis The axis to select on the left side Joystick object. + * + * This function lets you pick the axis to be used on each Joystick object for + * the left and right sides of the robot. + * + * @param leftStick The Joystick object to use for the left side of the robot. + * @param leftAxis The axis to select on the left side Joystick object. * @param rightStick The Joystick object to use for the right side of the robot. - * @param rightAxis The axis to select on the right side Joystick object. + * @param rightAxis The axis to select on the right side Joystick object. */ -void RobotDrive::TankDrive(GenericHID *leftStick, uint32_t leftAxis, - GenericHID *rightStick, uint32_t rightAxis, bool squaredInputs) -{ - if (leftStick == nullptr || rightStick == nullptr) - { - wpi_setWPIError(NullParameter); - return; - } - TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis), squaredInputs); +void RobotDrive::TankDrive(GenericHID* leftStick, uint32_t leftAxis, + GenericHID* rightStick, uint32_t rightAxis, + bool squaredInputs) { + if (leftStick == nullptr || rightStick == nullptr) { + wpi_setWPIError(NullParameter); + return; + } + TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis), + squaredInputs); } -void RobotDrive::TankDrive(GenericHID &leftStick, uint32_t leftAxis, - GenericHID &rightStick, uint32_t rightAxis, bool squaredInputs) -{ - TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis), squaredInputs); +void RobotDrive::TankDrive(GenericHID& leftStick, uint32_t leftAxis, + GenericHID& rightStick, uint32_t rightAxis, + bool squaredInputs) { + TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis), + squaredInputs); } - /** * Provide tank steering using the stored robot configuration. + * * This function lets you directly provide joystick values from any source. - * @param leftValue The value of the left stick. + * + * @param leftValue The value of the left stick. * @param rightValue The value of the right stick. */ -void RobotDrive::TankDrive(float leftValue, float rightValue, bool squaredInputs) -{ - static bool reported = false; - if (!reported) - { - reported = true; - } +void RobotDrive::TankDrive(float leftValue, float rightValue, + bool squaredInputs) { + static bool reported = false; + if (!reported) { + reported = true; + } - // square the inputs (while preserving the sign) to increase fine control while permitting full power - leftValue = Limit(leftValue); - rightValue = Limit(rightValue); - if(squaredInputs) - { - if (leftValue >= 0.0) - { - leftValue = (leftValue * leftValue); - } - else - { - leftValue = -(leftValue * leftValue); - } - if (rightValue >= 0.0) - { - rightValue = (rightValue * rightValue); - } - else - { - rightValue = -(rightValue * rightValue); - } - } + // square the inputs (while preserving the sign) to increase fine control + // while permitting full power + leftValue = Limit(leftValue); + rightValue = Limit(rightValue); + if (squaredInputs) { + if (leftValue >= 0.0) { + leftValue = (leftValue * leftValue); + } else { + leftValue = -(leftValue * leftValue); + } + if (rightValue >= 0.0) { + rightValue = (rightValue * rightValue); + } else { + rightValue = -(rightValue * rightValue); + } + } - SetLeftRightMotorOutputs(leftValue, rightValue); + SetLeftRightMotorOutputs(leftValue, rightValue); } /** * Arcade drive implements single stick driving. - * Given a single Joystick, the class assumes the Y axis for the move value and the X axis - * for the rotate value. + * + * Given a single Joystick, the class assumes the Y axis for the move value and + * the X axis for the rotate value. * (Should add more information here regarding the way that arcade drive works.) - * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected - * for forwards/backwards and the X-axis will be selected for rotation rate. - * @param squaredInputs If true, the sensitivity will be increased for small values + * + * @param stick The joystick to use for Arcade single-stick driving. + * The Y-axis will be selected for forwards/backwards and + * the X-axis will be selected for rotation rate. + * @param squaredInputs If true, the sensitivity will be increased for small + * values */ -void RobotDrive::ArcadeDrive(GenericHID *stick, bool squaredInputs) -{ - // simply call the full-featured ArcadeDrive with the appropriate values - ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs); +void RobotDrive::ArcadeDrive(GenericHID* stick, bool squaredInputs) { + // simply call the full-featured ArcadeDrive with the appropriate values + ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs); } /** * Arcade drive implements single stick driving. - * Given a single Joystick, the class assumes the Y axis for the move value and the X axis - * for the rotate value. + * + * Given a single Joystick, the class assumes the Y axis for the move value and + * the X axis for the rotate value. * (Should add more information here regarding the way that arcade drive works.) - * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected - * for forwards/backwards and the X-axis will be selected for rotation rate. - * @param squaredInputs If true, the sensitivity will be increased for small values + * + * @param stick The joystick to use for Arcade single-stick driving. + * The Y-axis will be selected for forwards/backwards and + * the X-axis will be selected for rotation rate. + * @param squaredInputs If true, the sensitivity will be increased for small + * values */ -void RobotDrive::ArcadeDrive(GenericHID &stick, bool squaredInputs) -{ - // simply call the full-featured ArcadeDrive with the appropriate values - ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs); +void RobotDrive::ArcadeDrive(GenericHID& stick, bool squaredInputs) { + // simply call the full-featured ArcadeDrive with the appropriate values + ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs); } /** * Arcade drive implements single stick driving. - * Given two joystick instances and two axis, compute the values to send to either two - * or four motors. - * @param moveStick The Joystick object that represents the forward/backward direction - * @param moveAxis The axis on the moveStick object to use for fowards/backwards (typically Y_AXIS) - * @param rotateStick The Joystick object that represents the rotation value - * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS) - * @param squaredInputs Setting this parameter to true increases the sensitivity at lower speeds + * + * Given two joystick instances and two axis, compute the values to send to + * either two or four motors. + * + * @param moveStick The Joystick object that represents the forward/backward + * direction + * @param moveAxis The axis on the moveStick object to use for + * forwards/backwards (typically Y_AXIS) + * @param rotateStick The Joystick object that represents the rotation value + * @param rotateAxis The axis on the rotation object to use for the rotate + * right/left (typically X_AXIS) + * @param squaredInputs Setting this parameter to true increases the sensitivity + * at lower speeds */ void RobotDrive::ArcadeDrive(GenericHID* moveStick, uint32_t moveAxis, - GenericHID* rotateStick, uint32_t rotateAxis, - bool squaredInputs) -{ - float moveValue = moveStick->GetRawAxis(moveAxis); - float rotateValue = rotateStick->GetRawAxis(rotateAxis); + GenericHID* rotateStick, uint32_t rotateAxis, + bool squaredInputs) { + float moveValue = moveStick->GetRawAxis(moveAxis); + float rotateValue = rotateStick->GetRawAxis(rotateAxis); - ArcadeDrive(moveValue, rotateValue, squaredInputs); + ArcadeDrive(moveValue, rotateValue, squaredInputs); } /** * Arcade drive implements single stick driving. - * Given two joystick instances and two axis, compute the values to send to either two - * or four motors. - * @param moveStick The Joystick object that represents the forward/backward direction - * @param moveAxis The axis on the moveStick object to use for fowards/backwards (typically Y_AXIS) - * @param rotateStick The Joystick object that represents the rotation value - * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS) - * @param squaredInputs Setting this parameter to true increases the sensitivity at lower speeds + * + * Given two joystick instances and two axis, compute the values to send to + * either two or four motors. + * + * @param moveStick The Joystick object that represents the forward/backward + * direction + * @param moveAxis The axis on the moveStick object to use for + * forwards/backwards (typically Y_AXIS) + * @param rotateStick The Joystick object that represents the rotation value + * @param rotateAxis The axis on the rotation object to use for the rotate + * right/left (typically X_AXIS) + * @param squaredInputs Setting this parameter to true increases the sensitivity + * at lower speeds */ +void RobotDrive::ArcadeDrive(GenericHID& moveStick, uint32_t moveAxis, + GenericHID& rotateStick, uint32_t rotateAxis, + bool squaredInputs) { + float moveValue = moveStick.GetRawAxis(moveAxis); + float rotateValue = rotateStick.GetRawAxis(rotateAxis); -void RobotDrive::ArcadeDrive(GenericHID &moveStick, uint32_t moveAxis, - GenericHID &rotateStick, uint32_t rotateAxis, - bool squaredInputs) -{ - float moveValue = moveStick.GetRawAxis(moveAxis); - float rotateValue = rotateStick.GetRawAxis(rotateAxis); - - ArcadeDrive(moveValue, rotateValue, squaredInputs); + ArcadeDrive(moveValue, rotateValue, squaredInputs); } /** * Arcade drive implements single stick driving. + * * This function lets you directly provide joystick values from any source. - * @param moveValue The value to use for fowards/backwards - * @param rotateValue The value to use for the rotate right/left + * + * @param moveValue The value to use for fowards/backwards + * @param rotateValue The value to use for the rotate right/left * @param squaredInputs If set, increases the sensitivity at low speeds */ -void RobotDrive::ArcadeDrive(float moveValue, float rotateValue, bool squaredInputs) -{ - static bool reported = false; - if (!reported) - { - reported = true; - } +void RobotDrive::ArcadeDrive(float moveValue, float rotateValue, + bool squaredInputs) { + static bool reported = false; + if (!reported) { + reported = true; + } - // local variables to hold the computed PWM values for the motors - float leftMotorOutput; - float rightMotorOutput; + // local variables to hold the computed PWM values for the motors + float leftMotorOutput; + float rightMotorOutput; - moveValue = Limit(moveValue); - rotateValue = Limit(rotateValue); + moveValue = Limit(moveValue); + rotateValue = Limit(rotateValue); - if (squaredInputs) - { - // square the inputs (while preserving the sign) to increase fine control while permitting full power - if (moveValue >= 0.0) - { - moveValue = (moveValue * moveValue); - } - else - { - moveValue = -(moveValue * moveValue); - } - if (rotateValue >= 0.0) - { - rotateValue = (rotateValue * rotateValue); - } - else - { - rotateValue = -(rotateValue * rotateValue); - } - } + if (squaredInputs) { + // square the inputs (while preserving the sign) to increase fine control + // while permitting full power + if (moveValue >= 0.0) { + moveValue = (moveValue * moveValue); + } else { + moveValue = -(moveValue * moveValue); + } + if (rotateValue >= 0.0) { + rotateValue = (rotateValue * rotateValue); + } else { + rotateValue = -(rotateValue * rotateValue); + } + } - if (moveValue > 0.0) - { - if (rotateValue > 0.0) - { - leftMotorOutput = moveValue - rotateValue; - rightMotorOutput = std::max(moveValue, rotateValue); - } - else - { - leftMotorOutput = std::max(moveValue, -rotateValue); - rightMotorOutput = moveValue + rotateValue; - } - } - else - { - if (rotateValue > 0.0) - { - leftMotorOutput = - std::max(-moveValue, rotateValue); - rightMotorOutput = moveValue + rotateValue; - } - else - { - leftMotorOutput = moveValue - rotateValue; - rightMotorOutput = - std::max(-moveValue, -rotateValue); - } - } - SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput); + if (moveValue > 0.0) { + if (rotateValue > 0.0) { + leftMotorOutput = moveValue - rotateValue; + rightMotorOutput = std::max(moveValue, rotateValue); + } else { + leftMotorOutput = std::max(moveValue, -rotateValue); + rightMotorOutput = moveValue + rotateValue; + } + } else { + if (rotateValue > 0.0) { + leftMotorOutput = -std::max(-moveValue, rotateValue); + rightMotorOutput = moveValue + rotateValue; + } else { + leftMotorOutput = moveValue - rotateValue; + rightMotorOutput = -std::max(-moveValue, -rotateValue); + } + } + SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput); } /** * Drive method for Mecanum wheeled robots. * * A method for driving with Mecanum wheeled robots. There are 4 wheels - * on the robot, arranged so that the front and back wheels are toed in 45 degrees. - * When looking at the wheels from the top, the roller axles should form an X across the robot. + * on the robot, arranged so that the front and back wheels are toed in 45 + * degrees. When looking at the wheels from the top, the roller axles should + * form an X across the robot. * * This is designed to be directly driven by joystick axes. * - * @param x The speed that the robot should drive in the X direction. [-1.0..1.0] - * @param y The speed that the robot should drive in the Y direction. - * This input is inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0] - * @param rotation The rate of rotation for the robot that is completely independent of - * the translation. [-1.0..1.0] - * @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented controls. + * @param x The speed that the robot should drive in the X direction. + * [-1.0..1.0] + * @param y The speed that the robot should drive in the Y direction. + * This input is inverted to match the forward == -1.0 that + * joysticks produce. [-1.0..1.0] + * @param rotation The rate of rotation for the robot that is completely + * independent of the translation. [-1.0..1.0] + * @param gyroAngle The current angle reading from the gyro. Use this to + * implement field-oriented controls. */ -void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float gyroAngle) -{ - static bool reported = false; - if (!reported) - { - reported = true; - } +void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, + float gyroAngle) { + static bool reported = false; + if (!reported) { + reported = true; + } - double xIn = x; - double yIn = y; - // Negate y for the joystick. - yIn = -yIn; - // Compenstate for gyro angle. - RotateVector(xIn, yIn, gyroAngle); + double xIn = x; + double yIn = y; + // Negate y for the joystick. + yIn = -yIn; + // Compenstate for gyro angle. + RotateVector(xIn, yIn, gyroAngle); - double wheelSpeeds[kMaxNumberOfMotors]; - wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation; - wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation; - wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation; - wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation; + double wheelSpeeds[kMaxNumberOfMotors]; + wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation; + wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation; + wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation; + wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation; - Normalize(wheelSpeeds); + Normalize(wheelSpeeds); - uint8_t syncGroup = 0x80; + uint8_t syncGroup = 0x80; - m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup); - m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup); - m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup); - m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup); + m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * + m_invertedMotors[kFrontLeftMotor] * m_maxOutput, + syncGroup); + m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * + m_invertedMotors[kFrontRightMotor] * m_maxOutput, + syncGroup); + m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * + m_invertedMotors[kRearLeftMotor] * m_maxOutput, + syncGroup); + m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * + m_invertedMotors[kRearRightMotor] * m_maxOutput, + syncGroup); - // CANJaguar::UpdateSyncGroup(syncGroup); + // CANJaguar::UpdateSyncGroup(syncGroup); - // FIXME: m_safetyHelper->Feed(); + // FIXME: m_safetyHelper->Feed(); } /** * Drive method for Mecanum wheeled robots. * * A method for driving with Mecanum wheeled robots. There are 4 wheels - * on the robot, arranged so that the front and back wheels are toed in 45 degrees. - * When looking at the wheels from the top, the roller axles should form an X across the robot. + * on the robot, arranged so that the front and back wheels are toed in 45 + * degrees. When looking at the wheels from the top, the roller axles should + * form an X across the robot. * - * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0] - * @param direction The direction the robot should drive in degrees. The direction and maginitute are - * independent of the rotation rate. - * @param rotation The rate of rotation for the robot that is completely independent of - * the magnitute or direction. [-1.0..1.0] + * @param magnitude The speed that the robot should drive in a given direction. + * [-1.0..1.0] + * @param direction The direction the robot should drive in degrees. The + * direction and maginitute are independent of the rotation + * rate. + * @param rotation The rate of rotation for the robot that is completely + * independent of the magnitute or direction. [-1.0..1.0] */ -void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rotation) -{ - static bool reported = false; - if (!reported) - { - reported = true; - } +void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, + float rotation) { + static bool reported = false; + if (!reported) { + reported = true; + } - // Normalized for full power along the Cartesian axes. - magnitude = Limit(magnitude) * sqrt(2.0); - // The rollers are at 45 degree angles. - double dirInRad = (direction + 45.0) * 3.14159 / 180.0; - double cosD = cos(dirInRad); - double sinD = sin(dirInRad); + // Normalized for full power along the Cartesian axes. + magnitude = Limit(magnitude) * sqrt(2.0); + // The rollers are at 45 degree angles. + double dirInRad = (direction + 45.0) * 3.14159 / 180.0; + double cosD = cos(dirInRad); + double sinD = sin(dirInRad); - double wheelSpeeds[kMaxNumberOfMotors]; - wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation; - wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation; - wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation; - wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation; + double wheelSpeeds[kMaxNumberOfMotors]; + wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation; + wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation; + wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation; + wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation; - Normalize(wheelSpeeds); + Normalize(wheelSpeeds); - uint8_t syncGroup = 0x80; + uint8_t syncGroup = 0x80; - m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup); - m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup); - m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup); - m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup); + m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * + m_invertedMotors[kFrontLeftMotor] * m_maxOutput, + syncGroup); + m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * + m_invertedMotors[kFrontRightMotor] * m_maxOutput, + syncGroup); + m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * + m_invertedMotors[kRearLeftMotor] * m_maxOutput, + syncGroup); + m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * + m_invertedMotors[kRearRightMotor] * m_maxOutput, + syncGroup); - // CANJaguar::UpdateSyncGroup(syncGroup); + // CANJaguar::UpdateSyncGroup(syncGroup); - // FIXME: m_safetyHelper->Feed(); + // FIXME: m_safetyHelper->Feed(); } /** @@ -558,167 +589,162 @@ void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rota * * This is an alias to MecanumDrive_Polar() for backward compatability * - * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0] - * @param direction The direction the robot should drive. The direction and maginitute are - * independent of the rotation rate. - * @param rotation The rate of rotation for the robot that is completely independent of - * the magnitute or direction. [-1.0..1.0] + * @param magnitude The speed that the robot should drive in a given direction. + * [-1.0..1.0] + * @param direction The direction the robot should drive. The direction and + * magnitude are independent of the rotation rate. + * @param rotation The rate of rotation for the robot that is completely + * independent of the magnitude or direction. [-1.0..1.0] */ -void RobotDrive::HolonomicDrive(float magnitude, float direction, float rotation) -{ - MecanumDrive_Polar(magnitude, direction, rotation); +void RobotDrive::HolonomicDrive(float magnitude, float direction, + float rotation) { + MecanumDrive_Polar(magnitude, direction, rotation); } -/** Set the speed of the right and left motors. +/** + * Set the speed of the right and left motors. + * * This is used once an appropriate drive setup function is called such as * TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput" * and includes flipping the direction of one side for opposing motors. - * @param leftOutput The speed to send to the left side of the robot. + * + * @param leftOutput The speed to send to the left side of the robot. * @param rightOutput The speed to send to the right side of the robot. */ -void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput) -{ - wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr); +void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput) { + wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr); - uint8_t syncGroup = 0x80; + uint8_t syncGroup = 0x80; - if (m_frontLeftMotor != nullptr) - m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup); - m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup); + if (m_frontLeftMotor != nullptr) + m_frontLeftMotor->Set( + Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, + syncGroup); + m_rearLeftMotor->Set( + Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, + syncGroup); - if (m_frontRightMotor != nullptr) - m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup); - m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup); + if (m_frontRightMotor != nullptr) + m_frontRightMotor->Set( + -Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, + syncGroup); + m_rearRightMotor->Set( + -Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, + syncGroup); - // CANJaguar::UpdateSyncGroup(syncGroup); + // CANJaguar::UpdateSyncGroup(syncGroup); - // FIXME: m_safetyHelper->Feed(); + // FIXME: m_safetyHelper->Feed(); } /** * Limit motor values to the -1.0 to +1.0 range. */ -float RobotDrive::Limit(float num) -{ - if (num > 1.0) - { - return 1.0; - } - if (num < -1.0) - { - return -1.0; - } - return num; +float RobotDrive::Limit(float num) { + if (num > 1.0) { + return 1.0; + } + if (num < -1.0) { + return -1.0; + } + return num; } /** * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0. */ -void RobotDrive::Normalize(double *wheelSpeeds) -{ - double maxMagnitude = fabs(wheelSpeeds[0]); - int32_t i; - for (i=1; i 1.0) - { - for (i=0; i 1.0) { + for (i = 0; i < kMaxNumberOfMotors; i++) { + wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude; + } + } } /** * Rotate a vector in Cartesian space. */ -void RobotDrive::RotateVector(double &x, double &y, double angle) -{ - double cosA = cos(angle * (3.14159 / 180.0)); - double sinA = sin(angle * (3.14159 / 180.0)); - double xOut = x * cosA - y * sinA; - double yOut = x * sinA + y * cosA; - x = xOut; - y = yOut; +void RobotDrive::RotateVector(double& x, double& y, double angle) { + double cosA = cos(angle * (3.14159 / 180.0)); + double sinA = sin(angle * (3.14159 / 180.0)); + double xOut = x * cosA - y * sinA; + double yOut = x * sinA + y * cosA; + x = xOut; + y = yOut; } -/* +/** * Invert a motor direction. + * * This is used when a motor should run in the opposite direction as the drive - * code would normally run it. Motors that are direct drive would be inverted, the - * Drive code assumes that the motors are geared with one reversal. - * @param motor The motor index to invert. + * code would normally run it. Motors that are direct drive would be inverted, + * the Drive code assumes that the motors are geared with one reversal. + * + * @param motor The motor index to invert. * @param isInverted True if the motor should be inverted when operated. */ -void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) -{ - if (motor < 0 || motor > 3) - { - wpi_setWPIError(InvalidMotorIndex); - return; - } - m_invertedMotors[motor] = isInverted ? -1 : 1; +void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) { + if (motor < 0 || motor > 3) { + wpi_setWPIError(InvalidMotorIndex); + return; + } + m_invertedMotors[motor] = isInverted ? -1 : 1; } /** * Set the turning sensitivity. * * This only impacts the Drive() entry-point. - * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value) + * + * @param sensitivity Effectively sets the turning sensitivity (or turn radius + * for a given value) */ -void RobotDrive::SetSensitivity(float sensitivity) -{ - m_sensitivity = sensitivity; +void RobotDrive::SetSensitivity(float sensitivity) { + m_sensitivity = sensitivity; } /** - * Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus. - * @param maxOutput Multiplied with the output percentage computed by the drive functions. + * Configure the scaling factor for using RobotDrive with motor controllers in a + * mode other than PercentVbus. + * + * @param maxOutput Multiplied with the output percentage computed by the drive + * functions. */ -void RobotDrive::SetMaxOutput(double maxOutput) -{ - m_maxOutput = maxOutput; +void RobotDrive::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; } + +void RobotDrive::SetExpiration(float timeout) { + // FIXME: m_safetyHelper->SetExpiration(timeout); } - - -void RobotDrive::SetExpiration(float timeout) -{ - // FIXME: m_safetyHelper->SetExpiration(timeout); +float RobotDrive::GetExpiration() const { + return -1; // FIXME: return m_safetyHelper->GetExpiration(); } -float RobotDrive::GetExpiration() const -{ - return -1; // FIXME: return m_safetyHelper->GetExpiration(); +bool RobotDrive::IsAlive() const { + return true; // FIXME: m_safetyHelper->IsAlive(); } -bool RobotDrive::IsAlive() const -{ - return true; // FIXME: m_safetyHelper->IsAlive(); +bool RobotDrive::IsSafetyEnabled() const { + return false; // FIXME: return m_safetyHelper->IsSafetyEnabled(); } -bool RobotDrive::IsSafetyEnabled() const -{ - return false; // FIXME: return m_safetyHelper->IsSafetyEnabled(); +void RobotDrive::SetSafetyEnabled(bool enabled) { + // FIXME: m_safetyHelper->SetSafetyEnabled(enabled); } -void RobotDrive::SetSafetyEnabled(bool enabled) -{ - // FIXME: m_safetyHelper->SetSafetyEnabled(enabled); +void RobotDrive::GetDescription(std::ostringstream& desc) const { + desc << "RobotDrive"; } -void RobotDrive::GetDescription(std::ostringstream& desc) const -{ - desc << "RobotDrive"; -} - -void RobotDrive::StopMotor() -{ - if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Disable(); - if (m_frontRightMotor != nullptr) m_frontRightMotor->Disable(); - if (m_rearLeftMotor != nullptr) m_rearLeftMotor->Disable(); - if (m_rearRightMotor != nullptr) m_rearRightMotor->Disable(); +void RobotDrive::StopMotor() { + if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Disable(); + if (m_frontRightMotor != nullptr) m_frontRightMotor->Disable(); + if (m_rearLeftMotor != nullptr) m_rearLeftMotor->Disable(); + if (m_rearRightMotor != nullptr) m_rearRightMotor->Disable(); } diff --git a/wpilibc/simulation/src/SafePWM.cpp b/wpilibc/simulation/src/SafePWM.cpp index 57bd729e33..dd3314b151 100644 --- a/wpilibc/simulation/src/SafePWM.cpp +++ b/wpilibc/simulation/src/SafePWM.cpp @@ -6,89 +6,84 @@ /*----------------------------------------------------------------------------*/ #include "SafePWM.h" -#include #include +#include /** * Constructor for a SafePWM object taking a channel number. + * * @param channel The PWM channel number (0..19). */ -SafePWM::SafePWM(uint32_t channel): PWM(channel) -{ - m_safetyHelper = std::make_unique(this); - m_safetyHelper->SetSafetyEnabled(false); +SafePWM::SafePWM(uint32_t channel) : PWM(channel) { + m_safetyHelper = std::make_unique(this); + m_safetyHelper->SetSafetyEnabled(false); } -/* - * Set the expiration time for the PWM object +/** + * Set the expiration time for the PWM object. + * * @param timeout The timeout (in seconds) for this motor object */ -void SafePWM::SetExpiration(float timeout) -{ - m_safetyHelper->SetExpiration(timeout); +void SafePWM::SetExpiration(float timeout) { + m_safetyHelper->SetExpiration(timeout); } /** * Return the expiration time for the PWM object. + * * @returns The expiration time value. */ -float SafePWM::GetExpiration() const -{ - return m_safetyHelper->GetExpiration(); -} +float SafePWM::GetExpiration() const { return m_safetyHelper->GetExpiration(); } /** * Check if the PWM object is currently alive or stopped due to a timeout. - * @returns a bool value that is true if the motor has NOT timed out and should still - * be running. + * + * @return a bool value that is true if the motor has NOT timed out and should + * still be running. */ -bool SafePWM::IsAlive() const -{ - return m_safetyHelper->IsAlive(); -} +bool SafePWM::IsAlive() const { return m_safetyHelper->IsAlive(); } /** * Stop the motor associated with this PWM object. - * This is called by the MotorSafetyHelper object when it has a timeout for this PWM and needs to - * stop it from running. + * + * This is called by the MotorSafetyHelper object when it has a timeout for this + * PWM and needs to stop it from running. */ -void SafePWM::StopMotor() -{ - SetRaw(kPwmDisabled); -} +void SafePWM::StopMotor() { SetRaw(kPwmDisabled); } /** - * Enable/disable motor safety for this device + * Enable/disable motor safety for this device. + * * Turn on and off the motor safety option for this PWM object. + * * @param enabled True if motor safety is enforced for this object */ -void SafePWM::SetSafetyEnabled(bool enabled) -{ - m_safetyHelper->SetSafetyEnabled(enabled); +void SafePWM::SetSafetyEnabled(bool enabled) { + m_safetyHelper->SetSafetyEnabled(enabled); } /** - * Check if motor safety is enabled for this object + * Check if motor safety is enabled for this object. + * * @returns True if motor safety is enforced for this object */ -bool SafePWM::IsSafetyEnabled() const -{ - return m_safetyHelper->IsSafetyEnabled(); +bool SafePWM::IsSafetyEnabled() const { + return m_safetyHelper->IsSafetyEnabled(); } -void SafePWM::GetDescription(std::ostringstream& desc) const -{ - desc << "PWM " << GetChannel(); +void SafePWM::GetDescription(std::ostringstream& desc) const { + desc << "PWM " << GetChannel(); } /** * Feed the MotorSafety timer when setting the speed. - * This method is called by the subclass motor whenever it updates its speed, thereby reseting - * the timeout value. + * + * This method is called by the subclass motor whenever it updates its speed, + * thereby reseting the timeout value. + * * @param speed Value to pass to the PWM class */ -void SafePWM::SetSpeed(float speed) -{ - PWM::SetSpeed(speed); - m_safetyHelper->Feed(); +void SafePWM::SetSpeed(float speed) { + PWM::SetSpeed(speed); + m_safetyHelper->Feed(); } diff --git a/wpilibc/simulation/src/SampleRobot.cpp b/wpilibc/simulation/src/SampleRobot.cpp index 21dcbfa82c..3b282f25d5 100644 --- a/wpilibc/simulation/src/SampleRobot.cpp +++ b/wpilibc/simulation/src/SampleRobot.cpp @@ -8,151 +8,137 @@ #include "SampleRobot.h" #include -#include "Timer.h" -#include "SmartDashboard/SmartDashboard.h" #include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SmartDashboard.h" +#include "Timer.h" #include "networktables/NetworkTable.h" #if defined(_UNIX) - #include +#include #elif defined(_WIN32) - #include - void sleep(unsigned milliseconds) - { - Sleep(milliseconds); - } +#include +void sleep(unsigned milliseconds) { Sleep(milliseconds); } #endif - -SampleRobot::SampleRobot() - : m_robotMainOverridden (true) -{ -} +SampleRobot::SampleRobot() : m_robotMainOverridden(true) {} /** * Robot-wide initialization code should go here. * - * Programmers should override this method for default Robot-wide initialization which will - * be called each time the robot enters the disabled state. + * Programmers should override this method for default Robot-wide initialization + * which will be called each time the robot enters the disabled state. */ -void SampleRobot::RobotInit() -{ - printf("Default %s() method... Override me!\n", __FUNCTION__); +void SampleRobot::RobotInit() { + printf("Default %s() method... Override me!\n", __FUNCTION__); } /** * Disabled should go here. - * Programmers should override this method to run code that should run while the field is - * disabled. + * + * Programmers should override this method to run code that should run while the + * field is disabled. */ -void SampleRobot::Disabled() -{ - printf("Default %s() method... Override me!\n", __FUNCTION__); +void SampleRobot::Disabled() { + printf("Default %s() method... Override me!\n", __FUNCTION__); } /** * Autonomous should go here. - * Programmers should override this method to run code that should run while the field is - * in the autonomous period. This will be called once each time the robot enters the - * autonomous state. + * + * Programmers should override this method to run code that should run while the + * field is in the autonomous period. This will be called once each time the + * robot enters the autonomous state. */ -void SampleRobot::Autonomous() -{ - printf("Default %s() method... Override me!\n", __FUNCTION__); +void SampleRobot::Autonomous() { + printf("Default %s() method... Override me!\n", __FUNCTION__); } /** * Operator control (tele-operated) code should go here. - * Programmers should override this method to run code that should run while the field is - * in the Operator Control (tele-operated) period. This is called once each time the robot - * enters the teleop state. + * + * Programmers should override this method to run code that should run while the + * field is in the Operator Control (tele-operated) period. This is called once + * each time the robot enters the teleop state. */ -void SampleRobot::OperatorControl() -{ - printf("Default %s() method... Override me!\n", __FUNCTION__); +void SampleRobot::OperatorControl() { + printf("Default %s() method... Override me!\n", __FUNCTION__); } /** * Test program should go here. - * Programmers should override this method to run code that executes while the robot is - * in test mode. This will be called once whenever the robot enters test mode + * + * Programmers should override this method to run code that executes while the + * robot is in test mode. This will be called once whenever the robot enters + * test mode. */ -void SampleRobot::Test() -{ - printf("Default %s() method... Override me!\n", __FUNCTION__); +void SampleRobot::Test() { + printf("Default %s() method... Override me!\n", __FUNCTION__); } /** * Robot main program for free-form programs. * - * This should be overridden by user subclasses if the intent is to not use the Autonomous() and - * OperatorControl() methods. In that case, the program is responsible for sensing when to run - * the autonomous and operator control functions in their program. + * This should be overridden by user subclasses if the intent is to not use the + * Autonomous() and OperatorControl() methods. In that case, the program is + * responsible for sensing when to run the autonomous and operator control + * functions in their program. * - * This method will be called immediately after the constructor is called. If it has not been - * overridden by a user subclass (i.e. the default version runs), then the Autonomous() and - * OperatorControl() methods will be called. + * This method will be called immediately after the constructor is called. If it + * has not been overridden by a user subclass (i.e. the default version runs), + * then the Autonomous() and OperatorControl() methods will be called. */ -void SampleRobot::RobotMain() -{ - m_robotMainOverridden = false; -} +void SampleRobot::RobotMain() { m_robotMainOverridden = false; } /** * Start a competition. - * This code needs to track the order of the field starting to ensure that everything happens - * in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl - * or Test when the robot is enabled. After running the correct method, wait for some state to - * change, either the other mode starts or the robot is disabled. Then go back and wait for the + * + * This code needs to track the order of the field starting to ensure that + * everything happens in the right order. Repeatedly run the correct method, + * either Autonomous or OperatorControl or Test when the robot is enabled. + * After running the correct method, wait for some state to change, either the + * other mode starts or the robot is disabled. Then go back and wait for the * robot to be enabled again. */ -void SampleRobot::StartCompetition() -{ - LiveWindow *lw = LiveWindow::GetInstance(); +void SampleRobot::StartCompetition() { + LiveWindow* lw = LiveWindow::GetInstance(); - SmartDashboard::init(); - NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false); + SmartDashboard::init(); + NetworkTable::GetTable("LiveWindow") + ->GetSubTable("~STATUS~") + ->PutBoolean("LW Enabled", false); - RobotMain(); + RobotMain(); - if (!m_robotMainOverridden) - { - // first and one-time initialization - lw->SetEnabled(false); - RobotInit(); + if (!m_robotMainOverridden) { + // first and one-time initialization + lw->SetEnabled(false); + RobotInit(); - while (true) - { - if (IsDisabled()) - { - m_ds.InDisabled(true); - Disabled(); - m_ds.InDisabled(false); - while (IsDisabled()) sleep(1); //m_ds.WaitForData(); - } - else if (IsAutonomous()) - { - m_ds.InAutonomous(true); - Autonomous(); - m_ds.InAutonomous(false); - while (IsAutonomous() && IsEnabled()) sleep(1); //m_ds.WaitForData(); - } - else if (IsTest()) - { - lw->SetEnabled(true); - m_ds.InTest(true); - Test(); - m_ds.InTest(false); - while (IsTest() && IsEnabled()) sleep(1); //m_ds.WaitForData(); - lw->SetEnabled(false); - } - else - { - m_ds.InOperatorControl(true); - OperatorControl(); - m_ds.InOperatorControl(false); - while (IsOperatorControl() && IsEnabled()) sleep(1); //m_ds.WaitForData(); - } - } - } + while (true) { + if (IsDisabled()) { + m_ds.InDisabled(true); + Disabled(); + m_ds.InDisabled(false); + while (IsDisabled()) sleep(1); // m_ds.WaitForData(); + } else if (IsAutonomous()) { + m_ds.InAutonomous(true); + Autonomous(); + m_ds.InAutonomous(false); + while (IsAutonomous() && IsEnabled()) sleep(1); // m_ds.WaitForData(); + } else if (IsTest()) { + lw->SetEnabled(true); + m_ds.InTest(true); + Test(); + m_ds.InTest(false); + while (IsTest() && IsEnabled()) sleep(1); // m_ds.WaitForData(); + lw->SetEnabled(false); + } else { + m_ds.InOperatorControl(true); + OperatorControl(); + m_ds.InOperatorControl(false); + while (IsOperatorControl() && IsEnabled()) + sleep(1); // m_ds.WaitForData(); + } + } + } } diff --git a/wpilibc/simulation/src/SensorBase.cpp b/wpilibc/simulation/src/SensorBase.cpp index de18bcfcc8..b97691a3d4 100644 --- a/wpilibc/simulation/src/SensorBase.cpp +++ b/wpilibc/simulation/src/SensorBase.cpp @@ -17,44 +17,42 @@ const uint32_t SensorBase::kPwmChannels; const uint32_t SensorBase::kRelayChannels; const uint32_t SensorBase::kPDPChannels; const uint32_t SensorBase::kChassisSlots; -SensorBase *SensorBase::m_singletonList = nullptr; +SensorBase* SensorBase::m_singletonList = nullptr; /** * Creates an instance of the sensor base and gets an FPGA handle */ -SensorBase::SensorBase() -{ -} +SensorBase::SensorBase() {} /** * Add sensor to the singleton list. + * * Add this sensor to the list of singletons that need to be deleted when * the robot program exits. Each of the sensors on this list are singletons, * that is they aren't allocated directly with new, but instead are allocated * by the static GetInstance method. As a result, they are never deleted when * the program exits. Consequently these sensors may still be holding onto - * resources and need to have their destructors called at the end of the program. + * resources and need to have their destructors called at the end of the + * program. */ -void SensorBase::AddToSingletonList() -{ - m_nextSingleton = m_singletonList; - m_singletonList = this; +void SensorBase::AddToSingletonList() { + m_nextSingleton = m_singletonList; + m_singletonList = this; } /** * Delete all the singleton classes on the list. + * * All the classes that were allocated as singletons need to be deleted so * their resources can be freed. */ -void SensorBase::DeleteSingletons() -{ - for (SensorBase *next = m_singletonList; next != nullptr;) - { - SensorBase *tmp = next; - next = next->m_nextSingleton; - delete tmp; - } - m_singletonList = nullptr; +void SensorBase::DeleteSingletons() { + for (SensorBase* next = m_singletonList; next != nullptr;) { + SensorBase* tmp = next; + next = next->m_nextSingleton; + delete tmp; + } + m_singletonList = nullptr; } /** @@ -62,91 +60,83 @@ void SensorBase::DeleteSingletons() * * @return Solenoid module is valid and present */ -bool SensorBase::CheckSolenoidModule(uint8_t moduleNumber) -{ - return 1 <= moduleNumber && moduleNumber <= 2; // TODO: Fix for Athena +bool SensorBase::CheckSolenoidModule(uint8_t moduleNumber) { + return 1 <= moduleNumber && moduleNumber <= 2; // TODO: Fix for Athena } /** * Check that the digital channel number is valid. - * Verify that the channel number is one of the legal channel numbers. Channel numbers are - * 1-based. + * + * Verify that the channel number is one of the legal channel numbers. Channel + * numbers are 1-based. * * @return Digital channel is valid */ -bool SensorBase::CheckDigitalChannel(uint32_t channel) -{ - if (channel > 0 && channel <= kDigitalChannels) - return true; - return false; +bool SensorBase::CheckDigitalChannel(uint32_t channel) { + if (channel > 0 && channel <= kDigitalChannels) return true; + return false; } /** * Check that the digital channel number is valid. - * Verify that the channel number is one of the legal channel numbers. Channel numbers are - * 1-based. + * + * Verify that the channel number is one of the legal channel numbers. Channel + * numbers are 1-based. * * @return Relay channel is valid */ -bool SensorBase::CheckRelayChannel(uint32_t channel) -{ - if (channel > 0 && channel <= kRelayChannels) - return true; - return false; +bool SensorBase::CheckRelayChannel(uint32_t channel) { + if (channel > 0 && channel <= kRelayChannels) return true; + return false; } /** * Check that the digital channel number is valid. - * Verify that the channel number is one of the legal channel numbers. Channel numbers are - * 1-based. + * + * Verify that the channel number is one of the legal channel numbers. Channel + * numbers are 1-based. * * @return PWM channel is valid */ -bool SensorBase::CheckPWMChannel(uint32_t channel) -{ - if (channel > 0 && channel <= kPwmChannels) - return true; - return false; +bool SensorBase::CheckPWMChannel(uint32_t channel) { + if (channel > 0 && channel <= kPwmChannels) return true; + return false; } /** * Check that the analog input number is valid. - * Verify that the analog input number is one of the legal channel numbers. Channel numbers - * are 1-based. + * + * Verify that the analog input number is one of the legal channel numbers. + * Channel numbers are 1-based. * * @return Analog channel is valid */ -bool SensorBase::CheckAnalogInput(uint32_t channel) -{ - if (channel > 0 && channel <= kAnalogInputs) - return true; - return false; +bool SensorBase::CheckAnalogInput(uint32_t channel) { + if (channel > 0 && channel <= kAnalogInputs) return true; + return false; } /** * Check that the analog output number is valid. - * Verify that the analog output number is one of the legal channel numbers. Channel numbers - * are 1-based. + * + * Verify that the analog output number is one of the legal channel numbers. + * Channel numbers are 1-based. * * @return Analog channel is valid */ -bool SensorBase::CheckAnalogOutput(uint32_t channel) -{ - if (channel > 0 && channel <= kAnalogOutputs) - return true; - return false; +bool SensorBase::CheckAnalogOutput(uint32_t channel) { + if (channel > 0 && channel <= kAnalogOutputs) return true; + return false; } /** * Verify that the solenoid channel number is within limits. - * + * * @return Solenoid channel is valid */ -bool SensorBase::CheckSolenoidChannel(uint32_t channel) -{ - if (channel > 0 && channel <= kSolenoidChannels) - return true; - return false; +bool SensorBase::CheckSolenoidChannel(uint32_t channel) { + if (channel > 0 && channel <= kSolenoidChannels) return true; + return false; } /** @@ -154,9 +144,7 @@ bool SensorBase::CheckSolenoidChannel(uint32_t channel) * * @return PDP channel is valid */ -bool SensorBase::CheckPDPChannel(uint32_t channel) -{ - if (channel > 0 && channel <= kPDPChannels) - return true; - return false; +bool SensorBase::CheckPDPChannel(uint32_t channel) { + if (channel > 0 && channel <= kPDPChannels) return true; + return false; } diff --git a/wpilibc/simulation/src/Solenoid.cpp b/wpilibc/simulation/src/Solenoid.cpp index a9f4e9b083..9086d18464 100644 --- a/wpilibc/simulation/src/Solenoid.cpp +++ b/wpilibc/simulation/src/Solenoid.cpp @@ -6,8 +6,8 @@ /*----------------------------------------------------------------------------*/ #include "Solenoid.h" -#include "WPIErrors.h" #include "LiveWindow/LiveWindow.h" +#include "WPIErrors.h" #include "simulation/simTime.h" /** @@ -21,20 +21,19 @@ Solenoid::Solenoid(uint32_t channel) : Solenoid(1, channel) {} * Constructor. * * @param moduleNumber The solenoid module (1 or 2). - * @param channel The channel on the solenoid module to control (1..8). + * @param channel The channel on the solenoid module to control (1..8). */ -Solenoid::Solenoid(uint8_t moduleNumber, uint32_t channel) -{ - char buffer[50]; - int n = sprintf(buffer, "pneumatic/%d/%d", moduleNumber, channel); - m_impl = new SimContinuousOutput(buffer); - - LiveWindow::GetInstance()->AddActuator("Solenoid", moduleNumber, channel, - this); +Solenoid::Solenoid(uint8_t moduleNumber, uint32_t channel) { + char buffer[50]; + int n = sprintf(buffer, "pneumatic/%d/%d", moduleNumber, channel); + m_impl = new SimContinuousOutput(buffer); + + LiveWindow::GetInstance()->AddActuator("Solenoid", moduleNumber, channel, + this); } Solenoid::~Solenoid() { - if (m_table != nullptr) m_table->RemoveTableListener(this); + if (m_table != nullptr) m_table->RemoveTableListener(this); } /** @@ -42,10 +41,9 @@ Solenoid::~Solenoid() { * * @param on Turn the solenoid output off or on. */ -void Solenoid::Set(bool on) -{ - m_on = on; - m_impl->Set(on ? 1 : -1); +void Solenoid::Set(bool on) { + m_on = on; + m_impl->Set(on ? 1 : -1); } /** @@ -53,47 +51,39 @@ void Solenoid::Set(bool on) * * @return The current value of the solenoid. */ -bool Solenoid::Get() const -{ - return m_on; -} - +bool Solenoid::Get() const { return m_on; } void Solenoid::ValueChanged(ITable* source, llvm::StringRef key, std::shared_ptr value, bool isNew) { if (!value->IsBoolean()) return; - Set(value->GetBoolean()); + Set(value->GetBoolean()); } void Solenoid::UpdateTable() { - if (m_table != nullptr) { - m_table->PutBoolean("Value", Get()); - } + if (m_table != nullptr) { + m_table->PutBoolean("Value", Get()); + } } void Solenoid::StartLiveWindowMode() { - Set(false); - if (m_table != nullptr) { - m_table->AddTableListener("Value", this, true); - } + Set(false); + if (m_table != nullptr) { + m_table->AddTableListener("Value", this, true); + } } void Solenoid::StopLiveWindowMode() { - Set(false); - if (m_table != nullptr) { - m_table->RemoveTableListener(this); - } + Set(false); + if (m_table != nullptr) { + m_table->RemoveTableListener(this); + } } -std::string Solenoid::GetSmartDashboardType() const { - return "Solenoid"; -} +std::string Solenoid::GetSmartDashboardType() const { return "Solenoid"; } void Solenoid::InitTable(std::shared_ptr subTable) { - m_table = subTable; - UpdateTable(); + m_table = subTable; + UpdateTable(); } -std::shared_ptr Solenoid::GetTable() const { - return m_table; -} +std::shared_ptr Solenoid::GetTable() const { return m_table; } diff --git a/wpilibc/simulation/src/Talon.cpp b/wpilibc/simulation/src/Talon.cpp index 9a6d806672..06b8c2b782 100644 --- a/wpilibc/simulation/src/Talon.cpp +++ b/wpilibc/simulation/src/Talon.cpp @@ -13,8 +13,7 @@ /** * @param channel The PWM channel that the Talon is attached to. */ -Talon::Talon(uint32_t channel) : SafePWM(channel) -{ +Talon::Talon(uint32_t channel) : SafePWM(channel) { /* Note that the Talon uses the following bounds for PWM values. These values * should work reasonably well for most controllers, but if users experience * issues such as asymmetric behavior around the deadband or inability to @@ -41,38 +40,26 @@ Talon::Talon(uint32_t channel) : SafePWM(channel) * The PWM value is set using a range of -1.0 to 1.0, appropriately * scaling the value for the FPGA. * - * @param speed The speed value between -1.0 and 1.0 to set. + * @param speed The speed value between -1.0 and 1.0 to set. * @param syncGroup Unused interface. */ -void Talon::Set(float speed, uint8_t syncGroup) -{ - SetSpeed(speed); -} +void Talon::Set(float speed, uint8_t syncGroup) { SetSpeed(speed); } /** * Get the recently set value of the PWM. * * @return The most recently set value for the PWM between -1.0 and 1.0. */ -float Talon::Get() const -{ - return GetSpeed(); -} +float Talon::Get() const { return GetSpeed(); } /** * Common interface for disabling a motor. */ -void Talon::Disable() -{ - SetRaw(kPwmDisabled); -} +void Talon::Disable() { SetRaw(kPwmDisabled); } /** * Write out the PID value as seen in the PIDOutput base object. * * @param output Write out the PWM value as was found in the PIDController */ -void Talon::PIDWrite(float output) -{ - Set(output); -} +void Talon::PIDWrite(float output) { Set(output); } diff --git a/wpilibc/simulation/src/Timer.cpp b/wpilibc/simulation/src/Timer.cpp index 8bef9f2925..c0ece9700f 100644 --- a/wpilibc/simulation/src/Timer.cpp +++ b/wpilibc/simulation/src/Timer.cpp @@ -9,152 +9,144 @@ #include -#include "simulation/simTime.h" #include "Utility.h" - +#include "simulation/simTime.h" // Internal stuff -#include "simulation/SimFloatInput.h" #include "simulation/MainNode.h" -namespace wpilib { namespace internal { - double simTime = 0; - std::condition_variable time_wait; - std::mutex time_wait_mutex; +#include "simulation/SimFloatInput.h" +namespace wpilib { +namespace internal { +double simTime = 0; +std::condition_variable time_wait; +std::mutex time_wait_mutex; - void time_callback(const msgs::ConstFloat64Ptr &msg) { - simTime = msg->data(); - time_wait.notify_all(); - } -}} +void time_callback(const msgs::ConstFloat64Ptr& msg) { + simTime = msg->data(); + time_wait.notify_all(); +} +} +} /** * Pause the task for a specified time. * - * Pause the execution of the program for a specified period of time given in seconds. - * Motors will continue to run at their last assigned values, and sensors will continue to - * update. Only the task containing the wait will pause until the wait time is expired. + * Pause the execution of the program for a specified period of time given in + * seconds. Motors will continue to run at their last assigned values, and + * sensors will continue to update. Only the task containing the wait will + * pause until the wait time is expired. * * @param seconds Length of time to pause, in seconds. */ -void Wait(double seconds) -{ - if (seconds < 0.0) return; +void Wait(double seconds) { + if (seconds < 0.0) return; - double start = wpilib::internal::simTime; + double start = wpilib::internal::simTime; - std::unique_lock lock(wpilib::internal::time_wait_mutex); - while ((wpilib::internal::simTime - start) < seconds) { - wpilib::internal::time_wait.wait(lock); - } + std::unique_lock lock(wpilib::internal::time_wait_mutex); + while ((wpilib::internal::simTime - start) < seconds) { + wpilib::internal::time_wait.wait(lock); + } } /* * Return the FPGA system clock time in seconds. + * * This is deprecated and just forwards to Timer::GetFPGATimestamp(). + * * @returns Robot running time in seconds. */ -double GetClock() -{ - return Timer::GetFPGATimestamp(); -} +double GetClock() { return Timer::GetFPGATimestamp(); } /** * @brief Gives real-time clock system time with nanosecond resolution - * @return The time, just in case you want the robot to start autonomous at 8pm on Saturday (except in simulation). -*/ -double GetTime() -{ - return Timer::GetFPGATimestamp(); // The epoch starts when Gazebo starts + * @return The time, just in case you want the robot to start autonomous at 8pm + * on Saturday (except in simulation). + */ +double GetTime() { + return Timer::GetFPGATimestamp(); // The epoch starts when Gazebo starts } -//for compatibility with msvc12--see C2864 +// for compatibility with msvc12--see C2864 const double Timer::kRolloverTime = (1ll << 32) / 1e6; /** * Create a new timer object. * - * Create a new timer object and reset the time to zero. The timer is initially not running and - * must be started. + * Create a new timer object and reset the time to zero. The timer is initially + * not running and must be started. */ -Timer::Timer() - : m_startTime (0.0) - , m_accumulatedTime (0.0) - , m_running (false) -{ - //Creates a semaphore to control access to critical regions. - //Initially 'open' - Reset(); +Timer::Timer() : m_startTime(0.0), m_accumulatedTime(0.0), m_running(false) { + // Creates a semaphore to control access to critical regions. + // Initially 'open' + Reset(); } /** - * Get the current time from the timer. If the clock is running it is derived from - * the current system clock the start time stored in the timer class. If the clock - * is not running, then return the time when it was last stopped. + * Get the current time from the timer. + * + * If the clock is running it is derived from the current system clock the + * start time stored in the timer class. If the clock is not running, then + * return the time when it was last stopped. * * @return unsigned Current time value for this timer in seconds */ -double Timer::Get() const -{ - double result; - double currentTime = GetFPGATimestamp(); +double Timer::Get() const { + double result; + double currentTime = GetFPGATimestamp(); - std::lock_guard sync(m_mutex); - if(m_running) - { - // This math won't work if the timer rolled over (71 minutes after boot). - // TODO: Check for it and compensate. - result = (currentTime - m_startTime) + m_accumulatedTime; - } - else - { - result = m_accumulatedTime; - } + std::lock_guard sync(m_mutex); + if (m_running) { + // This math won't work if the timer rolled over (71 minutes after boot). + // TODO: Check for it and compensate. + result = (currentTime - m_startTime) + m_accumulatedTime; + } else { + result = m_accumulatedTime; + } - return result; + return result; } /** * Reset the timer by setting the time to 0. * - * Make the timer startTime the current time so new requests will be relative to now + * Make the timer startTime the current time so new requests will be relative to + * now. */ -void Timer::Reset() -{ - std::lock_guard sync(m_mutex); - m_accumulatedTime = 0; - m_startTime = GetFPGATimestamp(); +void Timer::Reset() { + std::lock_guard sync(m_mutex); + m_accumulatedTime = 0; + m_startTime = GetFPGATimestamp(); } /** * Start the timer running. + * * Just set the running flag to true indicating that all time requests should be * relative to the system clock. */ -void Timer::Start() -{ - std::lock_guard sync(m_mutex); - if (!m_running) - { - m_startTime = GetFPGATimestamp(); - m_running = true; - } +void Timer::Start() { + std::lock_guard sync(m_mutex); + if (!m_running) { + m_startTime = GetFPGATimestamp(); + m_running = true; + } } /** * Stop the timer. + * * This computes the time as of now and clears the running flag, causing all * subsequent time requests to be read from the accumulated time rather than * looking at the system clock. */ -void Timer::Stop() -{ - double temp = Get(); +void Timer::Stop() { + double temp = Get(); - std::lock_guard sync(m_mutex); - if (m_running) - { - m_accumulatedTime = temp; - m_running = false; - } + std::lock_guard sync(m_mutex); + if (m_running) { + m_accumulatedTime = temp; + m_running = false; + } } /** @@ -165,45 +157,38 @@ void Timer::Stop() * @param period The period to check for (in seconds). * @return If the period has passed. */ -bool Timer::HasPeriodPassed(double period) -{ - if (Get() > period) - { - std::lock_guard sync(m_mutex); - // Advance the start time by the period. - // Don't set it to the current time... we want to avoid drift. - m_startTime += period; - return true; - } - return false; +bool Timer::HasPeriodPassed(double period) { + if (Get() > period) { + std::lock_guard sync(m_mutex); + // Advance the start time by the period. + // Don't set it to the current time... we want to avoid drift. + m_startTime += period; + return true; + } + return false; } /* * Return the FPGA system clock time in seconds. * * Return the time from the FPGA hardware clock in seconds since the FPGA - * started. - * Rolls over after 71 minutes. - * @returns Robot running time in seconds. + * started. Rolls over after 71 minutes. + * + * @return Robot running time in seconds. */ -double Timer::GetFPGATimestamp() -{ - // FPGA returns the timestamp in microseconds - // Call the helper GetFPGATime() in Utility.cpp - return wpilib::internal::simTime; +double Timer::GetFPGATimestamp() { + // FPGA returns the timestamp in microseconds + // Call the helper GetFPGATime() in Utility.cpp + return wpilib::internal::simTime; } /* * Not in a match. */ -double Timer::GetMatchTime() -{ - return Timer::GetFPGATimestamp(); -} +double Timer::GetMatchTime() { return Timer::GetFPGATimestamp(); } // Internal function that reads the PPC timestamp counter. -extern "C" -{ - uint32_t niTimestamp32(void); - uint64_t niTimestamp64(void); +extern "C" { +uint32_t niTimestamp32(void); +uint64_t niTimestamp64(void); } diff --git a/wpilibc/simulation/src/Utility.cpp b/wpilibc/simulation/src/Utility.cpp index 4b49725dd4..70c31dadfb 100644 --- a/wpilibc/simulation/src/Utility.cpp +++ b/wpilibc/simulation/src/Utility.cpp @@ -5,26 +5,21 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */ -/*----------------------------------------------------------------------------*/ - #include "Utility.h" -#include "Timer.h" -#include "simulation/simTime.h" #include #include +#include "Timer.h" +#include "simulation/simTime.h" -#include -#include #include #include #include +#include +#include #if not defined(_WIN32) - #include - #include +#include +#include #endif static bool stackTraceEnabled = false; @@ -33,40 +28,37 @@ static bool suspendOnAssertEnabled = false; /** * Enable Stack trace after asserts. */ -void wpi_stackTraceOnAssertEnable(bool enabled) -{ - stackTraceEnabled = enabled; -} +void wpi_stackTraceOnAssertEnable(bool enabled) { stackTraceEnabled = enabled; } /** * Enable suspend on asssert. + * * If enabled, the user task will be suspended whenever an assert fails. This - * will allow the user to attach to the task with the debugger and examine variables - * around the failure. + * will allow the user to attach to the task with the debugger and examine + * variables around the failure. */ -void wpi_suspendOnAssertEnabled(bool enabled) -{ - suspendOnAssertEnabled = enabled; +void wpi_suspendOnAssertEnabled(bool enabled) { + suspendOnAssertEnabled = enabled; } -static void wpi_handleTracing() -{ - // if (stackTraceEnabled) - // { - // printf("\n---------------------------\n"); - // printCurrentStackTrace(); - // } - printf("\n"); +static void wpi_handleTracing() { + // if (stackTraceEnabled) + // { + // printf("\n---------------------------\n"); + // printCurrentStackTrace(); + // } + printf("\n"); } /** * Assert implementation. * This allows breakpoints to be set on an assert. - * The users don't call this, but instead use the wpi_assert macros in Utility.h. + * The users don't call this, but instead use the wpi_assert macros in + * Utility.h. */ -bool wpi_assert_impl(bool conditionValue, const char *conditionText, - const char *message, const char *fileName, - uint32_t lineNumber, const char *funcName) { +bool wpi_assert_impl(bool conditionValue, const char* conditionText, + const char* message, const char* fileName, + uint32_t lineNumber, const char* funcName) { if (!conditionValue) { std::stringstream errorStream; @@ -90,15 +82,15 @@ bool wpi_assert_impl(bool conditionValue, const char *conditionText, /** * Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl - * This should not be called directly; it should only be used by wpi_assertEqual_impl - * and wpi_assertNotEqual_impl. + * This should not be called directly; it should only be used by + * wpi_assertEqual_impl and wpi_assertNotEqual_impl. */ void wpi_assertEqual_common_impl(int valueA, int valueB, - const std::string &equalityType, - const std::string &message, - const std::string &fileName, + const std::string& equalityType, + const std::string& message, + const std::string& fileName, uint32_t lineNumber, - const std::string &funcName) { + const std::string& funcName) { // Error string buffer std::stringstream error; @@ -124,116 +116,97 @@ void wpi_assertEqual_common_impl(int valueA, int valueB, * Assert equal implementation. * This determines whether the two given integers are equal. If not, * the value of each is printed along with an optional message string. - * The users don't call this, but instead use the wpi_assertEqual macros in Utility.h. + * The users don't call this, but instead use the wpi_assertEqual macros in + * Utility.h. */ -bool wpi_assertEqual_impl(int valueA, - int valueB, - const std::string &message, - const std::string &fileName, - uint32_t lineNumber, - const std::string &funcName) -{ - if(!(valueA == valueB)) - { - wpi_assertEqual_common_impl(valueA, valueB, "!=", message, fileName, lineNumber, funcName); - } - return valueA == valueB; +bool wpi_assertEqual_impl(int valueA, int valueB, const std::string& message, + const std::string& fileName, uint32_t lineNumber, + const std::string& funcName) { + if (!(valueA == valueB)) { + wpi_assertEqual_common_impl(valueA, valueB, "!=", message, fileName, + lineNumber, funcName); + } + return valueA == valueB; } /** * Assert not equal implementation. * This determines whether the two given integers are equal. If so, * the value of each is printed along with an optional message string. - * The users don't call this, but instead use the wpi_assertNotEqual macros in Utility.h. + * The users don't call this, but instead use the wpi_assertNotEqual macros in + * Utility.h. */ -bool wpi_assertNotEqual_impl(int valueA, - int valueB, - const std::string &message, - const std::string &fileName, - uint32_t lineNumber, - const std::string &funcName) -{ - if(!(valueA != valueB)) - { - wpi_assertEqual_common_impl(valueA, valueB, "==", message, fileName, lineNumber, funcName); - } - return valueA != valueB; +bool wpi_assertNotEqual_impl(int valueA, int valueB, const std::string& message, + const std::string& fileName, uint32_t lineNumber, + const std::string& funcName) { + if (!(valueA != valueB)) { + wpi_assertEqual_common_impl(valueA, valueB, "==", message, fileName, + lineNumber, funcName); + } + return valueA != valueB; } /** * Read the microsecond-resolution timer on the FPGA. * - * @return The current time in microseconds according to the FPGA (since FPGA reset). + * @return The current time in microseconds according to the FPGA (since FPGA + * reset). */ -uint64_t GetFPGATime() -{ - return wpilib::internal::simTime * 1e6; -} +uint64_t GetFPGATime() { return wpilib::internal::simTime * 1e6; } -//TODO: implement symbol demangling and backtrace on windows +// TODO: implement symbol demangling and backtrace on windows #if not defined(_WIN32) /** * Demangle a C++ symbol, used for printing stack traces. */ -static std::string demangle(char const *mangledSymbol) -{ - char buffer[256]; - size_t length; - int status; +static std::string demangle(char const* mangledSymbol) { + char buffer[256]; + size_t length; + int status; - if(sscanf(mangledSymbol, "%*[^(]%*[^_]%255[^)+]", buffer)) - { - char *symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status); + if (sscanf(mangledSymbol, "%*[^(]%*[^_]%255[^)+]", buffer)) { + char* symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status); - if(status == 0) - { - return symbol; - } - else - { - // If the symbol couldn't be demangled, it's probably a C function, - // so just return it as-is. - return buffer; - } - } + if (status == 0) { + return symbol; + } else { + // If the symbol couldn't be demangled, it's probably a C function, + // so just return it as-is. + return buffer; + } + } - - // If everything else failed, just return the mangled symbol - return mangledSymbol; + // If everything else failed, just return the mangled symbol + return mangledSymbol; } /** * Get a stack trace, ignoring the first "offset" symbols. */ -std::string GetStackTrace(uint32_t offset) -{ - void *stackTrace[128]; - int stackSize = backtrace(stackTrace, 128); - char **mangledSymbols = backtrace_symbols(stackTrace, stackSize); - std::stringstream trace; +std::string GetStackTrace(uint32_t offset) { + void* stackTrace[128]; + int stackSize = backtrace(stackTrace, 128); + char** mangledSymbols = backtrace_symbols(stackTrace, stackSize); + std::stringstream trace; - for(int i = offset; i < stackSize; i++) - { - // Only print recursive functions once in a row. - if(i == 0 ||stackTrace[i] != stackTrace[i - 1]) - { - trace << "\tat " << demangle(mangledSymbols[i]) << std::endl; - } - } + for (int i = offset; i < stackSize; i++) { + // Only print recursive functions once in a row. + if (i == 0 || stackTrace[i] != stackTrace[i - 1]) { + trace << "\tat " << demangle(mangledSymbols[i]) << std::endl; + } + } - free(mangledSymbols); + free(mangledSymbols); - return trace.str(); + return trace.str(); } #else -static std::string demangle(char const *mangledSymbol) -{ - return "no demangling on windows"; +static std::string demangle(char const* mangledSymbol) { + return "no demangling on windows"; } -std::string GetStackTrace(uint32_t offset) -{ - return "no stack trace on windows"; +std::string GetStackTrace(uint32_t offset) { + return "no stack trace on windows"; } #endif diff --git a/wpilibc/simulation/src/Victor.cpp b/wpilibc/simulation/src/Victor.cpp index 72e33bc1da..905c73a3f3 100644 --- a/wpilibc/simulation/src/Victor.cpp +++ b/wpilibc/simulation/src/Victor.cpp @@ -13,8 +13,7 @@ /** * @param channel The PWM channel that the Victor is attached to. */ -Victor::Victor(uint32_t channel) : SafePWM(channel) -{ +Victor::Victor(uint32_t channel) : SafePWM(channel) { /* Note that the Victor uses the following bounds for PWM values. These values * were determined empirically and optimized for the Victor 888. These values * should work reasonably well for Victor 884 controllers as well but if users @@ -43,38 +42,26 @@ Victor::Victor(uint32_t channel) : SafePWM(channel) * The PWM value is set using a range of -1.0 to 1.0, appropriately * scaling the value for the FPGA. * - * @param speed The speed value between -1.0 and 1.0 to set. + * @param speed The speed value between -1.0 and 1.0 to set. * @param syncGroup Unused interface. */ -void Victor::Set(float speed, uint8_t syncGroup) -{ - SetSpeed(speed); -} +void Victor::Set(float speed, uint8_t syncGroup) { SetSpeed(speed); } /** * Get the recently set value of the PWM. * * @return The most recently set value for the PWM between -1.0 and 1.0. */ -float Victor::Get() const -{ - return GetSpeed(); -} +float Victor::Get() const { return GetSpeed(); } /** * Common interface for disabling a motor. */ -void Victor::Disable() -{ - SetRaw(kPwmDisabled); -} +void Victor::Disable() { SetRaw(kPwmDisabled); } /** * Write out the PID value as seen in the PIDOutput base object. * * @param output Write out the PWM value as was found in the PIDController */ -void Victor::PIDWrite(float output) -{ - Set(output); -} +void Victor::PIDWrite(float output) { Set(output); } diff --git a/wpilibc/simulation/src/simulation/SimContinuousOutput.cpp b/wpilibc/simulation/src/simulation/SimContinuousOutput.cpp index 39dbf706b4..675726f3f7 100644 --- a/wpilibc/simulation/src/simulation/SimContinuousOutput.cpp +++ b/wpilibc/simulation/src/simulation/SimContinuousOutput.cpp @@ -9,16 +9,14 @@ #include "simulation/MainNode.h" SimContinuousOutput::SimContinuousOutput(std::string topic) { - pub = MainNode::Advertise("~/simulator/"+topic); - std::cout << "Initialized ~/simulator/"+topic << std::endl; + pub = MainNode::Advertise("~/simulator/" + topic); + std::cout << "Initialized ~/simulator/" + topic << std::endl; } void SimContinuousOutput::Set(float speed) { - msgs::Float64 msg; - msg.set_data(speed); - pub->Publish(msg); + msgs::Float64 msg; + msg.set_data(speed); + pub->Publish(msg); } -float SimContinuousOutput::Get() { - return speed; -} +float SimContinuousOutput::Get() { return speed; } diff --git a/wpilibc/simulation/src/simulation/SimDigitalInput.cpp b/wpilibc/simulation/src/simulation/SimDigitalInput.cpp index b39f2aeb4e..d8f4f05950 100644 --- a/wpilibc/simulation/src/simulation/SimDigitalInput.cpp +++ b/wpilibc/simulation/src/simulation/SimDigitalInput.cpp @@ -9,14 +9,13 @@ #include "simulation/MainNode.h" SimDigitalInput::SimDigitalInput(std::string topic) { - sub = MainNode::Subscribe("~/simulator/"+topic, &SimDigitalInput::callback, this); - std::cout << "Initialized ~/simulator/"+topic << std::endl; + sub = MainNode::Subscribe("~/simulator/" + topic, &SimDigitalInput::callback, + this); + std::cout << "Initialized ~/simulator/" + topic << std::endl; } -bool SimDigitalInput::Get() { - return value; -} +bool SimDigitalInput::Get() { return value; } -void SimDigitalInput::callback(const msgs::ConstBoolPtr &msg) { +void SimDigitalInput::callback(const msgs::ConstBoolPtr& msg) { value = msg->data(); } diff --git a/wpilibc/simulation/src/simulation/SimEncoder.cpp b/wpilibc/simulation/src/simulation/SimEncoder.cpp index 0b5ed20d3c..89d3c7f176 100644 --- a/wpilibc/simulation/src/simulation/SimEncoder.cpp +++ b/wpilibc/simulation/src/simulation/SimEncoder.cpp @@ -9,52 +9,44 @@ #include "simulation/MainNode.h" SimEncoder::SimEncoder(std::string topic) { - commandPub = MainNode::Advertise("~/simulator/"+topic+"/control"); + commandPub = + MainNode::Advertise("~/simulator/" + topic + "/control"); - posSub = MainNode::Subscribe("~/simulator/"+topic+"/position", - &SimEncoder::positionCallback, this); - velSub = MainNode::Subscribe("~/simulator/"+topic+"/velocity", - &SimEncoder::velocityCallback, this); + posSub = MainNode::Subscribe("~/simulator/" + topic + "/position", + &SimEncoder::positionCallback, this); + velSub = MainNode::Subscribe("~/simulator/" + topic + "/velocity", + &SimEncoder::velocityCallback, this); - if (commandPub->WaitForConnection(gazebo::common::Time(5.0))) { // Wait up to five seconds. - std::cout << "Initialized ~/simulator/" + topic << std::endl; - } else { - std::cerr << "Failed to initialize ~/simulator/" + topic + ": does the encoder exist?" << std::endl; - } + if (commandPub->WaitForConnection( + gazebo::common::Time(5.0))) { // Wait up to five seconds. + std::cout << "Initialized ~/simulator/" + topic << std::endl; + } else { + std::cerr << "Failed to initialize ~/simulator/" + topic + + ": does the encoder exist?" + << std::endl; + } } -void SimEncoder::Reset() { - sendCommand("reset"); -} +void SimEncoder::Reset() { sendCommand("reset"); } -void SimEncoder::Start() { - sendCommand("start"); -} +void SimEncoder::Start() { sendCommand("start"); } -void SimEncoder::Stop() { - sendCommand("stop"); -} +void SimEncoder::Stop() { sendCommand("stop"); } -double SimEncoder::GetPosition() { - return position; -} - -double SimEncoder::GetVelocity() { - return velocity; -} +double SimEncoder::GetPosition() { return position; } +double SimEncoder::GetVelocity() { return velocity; } void SimEncoder::sendCommand(std::string cmd) { - msgs::GzString msg; - msg.set_data(cmd); - commandPub->Publish(msg); + msgs::GzString msg; + msg.set_data(cmd); + commandPub->Publish(msg); } - -void SimEncoder::positionCallback(const msgs::ConstFloat64Ptr &msg) { - position = msg->data(); +void SimEncoder::positionCallback(const msgs::ConstFloat64Ptr& msg) { + position = msg->data(); } -void SimEncoder::velocityCallback(const msgs::ConstFloat64Ptr &msg) { - velocity = msg->data(); +void SimEncoder::velocityCallback(const msgs::ConstFloat64Ptr& msg) { + velocity = msg->data(); } diff --git a/wpilibc/simulation/src/simulation/SimFloatInput.cpp b/wpilibc/simulation/src/simulation/SimFloatInput.cpp index 1fa93370f4..77474f7523 100644 --- a/wpilibc/simulation/src/simulation/SimFloatInput.cpp +++ b/wpilibc/simulation/src/simulation/SimFloatInput.cpp @@ -9,14 +9,13 @@ #include "simulation/MainNode.h" SimFloatInput::SimFloatInput(std::string topic) { - sub = MainNode::Subscribe("~/simulator/"+topic, &SimFloatInput::callback, this); - std::cout << "Initialized ~/simulator/"+topic << std::endl; + sub = MainNode::Subscribe("~/simulator/" + topic, &SimFloatInput::callback, + this); + std::cout << "Initialized ~/simulator/" + topic << std::endl; } -double SimFloatInput::Get() { - return value; -} +double SimFloatInput::Get() { return value; } -void SimFloatInput::callback(const msgs::ConstFloat64Ptr &msg) { +void SimFloatInput::callback(const msgs::ConstFloat64Ptr& msg) { value = msg->data(); } diff --git a/wpilibc/simulation/src/simulation/SimGyro.cpp b/wpilibc/simulation/src/simulation/SimGyro.cpp index c93434dfd1..07352dde3c 100644 --- a/wpilibc/simulation/src/simulation/SimGyro.cpp +++ b/wpilibc/simulation/src/simulation/SimGyro.cpp @@ -9,32 +9,29 @@ #include "simulation/MainNode.h" SimGyro::SimGyro(std::string topic) { - commandPub = MainNode::Advertise("~/simulator/"+topic+"/control"); - - posSub = MainNode::Subscribe("~/simulator/"+topic+"/position", - &SimGyro::positionCallback, this); - velSub = MainNode::Subscribe("~/simulator/"+topic+"/velocity", - &SimGyro::velocityCallback, this); + commandPub = + MainNode::Advertise("~/simulator/" + topic + "/control"); - if (commandPub->WaitForConnection(gazebo::common::Time(5.0))) { // Wait up to five seconds. - std::cout << "Initialized ~/simulator/" + topic << std::endl; - } else { - std::cerr << "Failed to initialize ~/simulator/" + topic + ": does the gyro exist?" << std::endl; - } + posSub = MainNode::Subscribe("~/simulator/" + topic + "/position", + &SimGyro::positionCallback, this); + velSub = MainNode::Subscribe("~/simulator/" + topic + "/velocity", + &SimGyro::velocityCallback, this); + + if (commandPub->WaitForConnection( + gazebo::common::Time(5.0))) { // Wait up to five seconds. + std::cout << "Initialized ~/simulator/" + topic << std::endl; + } else { + std::cerr << "Failed to initialize ~/simulator/" + topic + + ": does the gyro exist?" + << std::endl; + } } -void SimGyro::Reset() { - sendCommand("reset"); -} +void SimGyro::Reset() { sendCommand("reset"); } -double SimGyro::GetAngle() { - return position; -} - -double SimGyro::GetVelocity() { - return velocity; -} +double SimGyro::GetAngle() { return position; } +double SimGyro::GetVelocity() { return velocity; } void SimGyro::sendCommand(std::string cmd) { msgs::GzString msg; @@ -42,11 +39,10 @@ void SimGyro::sendCommand(std::string cmd) { commandPub->Publish(msg); } - -void SimGyro::positionCallback(const msgs::ConstFloat64Ptr &msg) { - position = msg->data(); +void SimGyro::positionCallback(const msgs::ConstFloat64Ptr& msg) { + position = msg->data(); } -void SimGyro::velocityCallback(const msgs::ConstFloat64Ptr &msg) { - velocity = msg->data(); +void SimGyro::velocityCallback(const msgs::ConstFloat64Ptr& msg) { + velocity = msg->data(); } diff --git a/wpilibcIntegrationTests/src/AnalogLoopTest.cpp b/wpilibcIntegrationTests/src/AnalogLoopTest.cpp index 8e4e139156..799142a779 100644 --- a/wpilibcIntegrationTests/src/AnalogLoopTest.cpp +++ b/wpilibcIntegrationTests/src/AnalogLoopTest.cpp @@ -10,8 +10,8 @@ #include #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" static const double kDelayTime = 0.01; @@ -20,8 +20,8 @@ static const double kDelayTime = 0.01; */ class AnalogLoopTest : public testing::Test { protected: - AnalogInput *m_input; - AnalogOutput *m_output; + AnalogInput* m_input; + AnalogOutput* m_output; virtual void SetUp() override { m_input = new AnalogInput(TestBench::kFakeAnalogOutputChannel); @@ -102,8 +102,8 @@ TEST_F(AnalogLoopTest, AnalogTriggerCounterWorks) { << "Analog trigger counter did not count 50 ticks"; } -static void InterruptHandler(uint32_t interruptAssertedMask, void *param) { - *(int *)param = 12345; +static void InterruptHandler(uint32_t interruptAssertedMask, void* param) { + *(int*)param = 12345; } TEST_F(AnalogLoopTest, AsynchronusInterruptWorks) { @@ -112,7 +112,8 @@ TEST_F(AnalogLoopTest, AsynchronusInterruptWorks) { trigger.SetLimitsVoltage(2.0f, 3.0f); // Given an interrupt handler that sets an int to 12345 - std::shared_ptr triggerOutput = trigger.CreateOutput(kState); + std::shared_ptr triggerOutput = + trigger.CreateOutput(kState); triggerOutput->RequestInterrupts(InterruptHandler, ¶m); triggerOutput->EnableInterrupts(); diff --git a/wpilibcIntegrationTests/src/AnalogPotentiometerTest.cpp b/wpilibcIntegrationTests/src/AnalogPotentiometerTest.cpp index 07934ae367..3beb874d7a 100644 --- a/wpilibcIntegrationTests/src/AnalogPotentiometerTest.cpp +++ b/wpilibcIntegrationTests/src/AnalogPotentiometerTest.cpp @@ -7,8 +7,8 @@ #include #include -#include #include +#include #include "TestBench.h" #include "gtest/gtest.h" @@ -17,8 +17,8 @@ static const double kAngle = 180.0; class AnalogPotentiometerTest : public testing::Test { protected: - AnalogOutput *m_fakePot; - AnalogPotentiometer *m_pot; + AnalogOutput* m_fakePot; + AnalogPotentiometer* m_pot; virtual void SetUp() override { m_fakePot = new AnalogOutput(TestBench::kAnalogOutputChannel); diff --git a/wpilibcIntegrationTests/src/CANJaguarTest.cpp b/wpilibcIntegrationTests/src/CANJaguarTest.cpp index 6f897ea03b..be581f8116 100644 --- a/wpilibcIntegrationTests/src/CANJaguarTest.cpp +++ b/wpilibcIntegrationTests/src/CANJaguarTest.cpp @@ -6,13 +6,13 @@ /*----------------------------------------------------------------------------*/ #include -#include #include +#include #include #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" static constexpr double kSpikeTime = 0.5; @@ -39,10 +39,10 @@ static constexpr double kMotorPercent = 0.5; static constexpr double kMotorSpeed = 100; class CANJaguarTest : public testing::Test { protected: - CANJaguar *m_jaguar; + CANJaguar* m_jaguar; DigitalOutput *m_fakeForwardLimit, *m_fakeReverseLimit; - AnalogOutput *m_fakePotentiometer; - Relay *m_spike; + AnalogOutput* m_fakePotentiometer; + Relay* m_spike; virtual void SetUp() override { m_spike = new Relay(TestBench::kCANJaguarRelayChannel, Relay::kForwardOnly); @@ -110,7 +110,8 @@ class CANJaguarTest : public testing::Test { finalSpeed = m_jaguar->GetSpeed(); EXPECT_FALSE(SignNum(initialSpeed) == SignNum(finalSpeed)) << "CAN Jaguar did not invert direction negative. Initial displacement " - "was: " << initialSpeed << " Final displacement was: " << finalSpeed + "was: " + << initialSpeed << " Final displacement was: " << finalSpeed << " Sign of initial displacement was: " << SignNum(initialSpeed) << " Sign of final displacement was: " << SignNum(finalSpeed); } @@ -121,8 +122,7 @@ class CANJaguarTest : public testing::Test { * causes a ResourceAlreadyAllocated error. */ TEST_F(CANJaguarTest, AlreadyAllocatedError) { - std::cout << "The following errors are expected." << std::endl - << std::endl; + std::cout << "The following errors are expected." << std::endl << std::endl; CANJaguar jaguar(TestBench::kCANJaguarID); EXPECT_EQ(wpi_error_value_ResourceAlreadyAllocated, @@ -135,8 +135,7 @@ TEST_F(CANJaguarTest, AlreadyAllocatedError) { * out-of-range error. */ TEST_F(CANJaguarTest, 64OutOfRangeError) { - std::cout << "The following errors are expected." << std::endl - << std::endl; + std::cout << "The following errors are expected." << std::endl << std::endl; CANJaguar jaguar(64); EXPECT_EQ(wpi_error_value_ChannelIndexOutOfRange, jaguar.GetError().GetCode()) @@ -148,8 +147,7 @@ TEST_F(CANJaguarTest, 64OutOfRangeError) { * error. */ TEST_F(CANJaguarTest, 0OutOfRangeError) { - std::cout << "The following errors are expected." << std::endl - << std::endl; + std::cout << "The following errors are expected." << std::endl << std::endl; CANJaguar jaguar(0); EXPECT_EQ(wpi_error_value_ChannelIndexOutOfRange, jaguar.GetError().GetCode()) diff --git a/wpilibcIntegrationTests/src/CANTalonTest.cpp b/wpilibcIntegrationTests/src/CANTalonTest.cpp index 515c840242..255f91002a 100644 --- a/wpilibcIntegrationTests/src/CANTalonTest.cpp +++ b/wpilibcIntegrationTests/src/CANTalonTest.cpp @@ -7,8 +7,8 @@ #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" const int deviceId = 0; @@ -71,6 +71,6 @@ TEST(CANTalonTest, DISABLED_PositionModeWorks) { TEST(CANTalonTest, GetFaults) { CANTalon talon(deviceId); - EXPECT_EQ(talon.GetFaults(),0); - EXPECT_EQ(talon.GetStickyFaults(),0); + EXPECT_EQ(talon.GetFaults(), 0); + EXPECT_EQ(talon.GetStickyFaults(), 0); } diff --git a/wpilibcIntegrationTests/src/CircularBufferTest.cpp b/wpilibcIntegrationTests/src/CircularBufferTest.cpp index 5a9cd4c06a..20209d14dc 100644 --- a/wpilibcIntegrationTests/src/CircularBufferTest.cpp +++ b/wpilibcIntegrationTests/src/CircularBufferTest.cpp @@ -6,21 +6,18 @@ /*----------------------------------------------------------------------------*/ #include -#include "gtest/gtest.h" #include +#include "gtest/gtest.h" -static const std::array values = {751.848, 766.366, 342.657, - 234.252, 716.126, 132.344, - 445.697, 22.727, 421.125, - 799.913}; +static const std::array values = { + 751.848, 766.366, 342.657, 234.252, 716.126, + 132.344, 445.697, 22.727, 421.125, 799.913}; -static const std::array pushFrontOut = {799.913, 421.125, 22.727, - 445.697, 132.344, 716.126, - 234.252, 342.657}; +static const std::array pushFrontOut = { + 799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657}; -static const std::array pushBackOut = {342.657, 234.252, 716.126, - 132.344, 445.697, 22.727, - 421.125, 799.913}; +static const std::array pushBackOut = { + 342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913}; TEST(CircularBufferTest, PushFrontTest) { CircularBuffer queue(8); @@ -63,27 +60,27 @@ TEST(CircularBufferTest, PushPopTest) { * front-most elements. */ - queue.PushBack(4.0); // Overwrite 1 with 4 + queue.PushBack(4.0); // Overwrite 1 with 4 // The buffer now contains 2, 3 and 4 EXPECT_EQ(2.0, queue[0]); EXPECT_EQ(3.0, queue[1]); EXPECT_EQ(4.0, queue[2]); - queue.PushBack(5.0); // Overwrite 2 with 5 + queue.PushBack(5.0); // Overwrite 2 with 5 // The buffer now contains 3, 4 and 5 EXPECT_EQ(3.0, queue[0]); EXPECT_EQ(4.0, queue[1]); EXPECT_EQ(5.0, queue[2]); - EXPECT_EQ(5.0, queue.PopBack()); // 5 is removed + EXPECT_EQ(5.0, queue.PopBack()); // 5 is removed // The buffer now contains 3 and 4 EXPECT_EQ(3.0, queue[0]); EXPECT_EQ(4.0, queue[1]); - EXPECT_EQ(3.0, queue.PopFront()); // 3 is removed + EXPECT_EQ(3.0, queue.PopFront()); // 3 is removed // Leaving only one element with value == 4 EXPECT_EQ(4.0, queue[0]); diff --git a/wpilibcIntegrationTests/src/ConditionVariableTest.cpp b/wpilibcIntegrationTests/src/ConditionVariableTest.cpp index c405e94bc6..41c638fa37 100644 --- a/wpilibcIntegrationTests/src/ConditionVariableTest.cpp +++ b/wpilibcIntegrationTests/src/ConditionVariableTest.cpp @@ -10,12 +10,12 @@ #include "HAL/cpp/priority_condition_variable.h" #include "HAL/cpp/priority_mutex.h" -#include "gtest/gtest.h" #include #include #include #include #include +#include "gtest/gtest.h" namespace wpilib { namespace testing { @@ -41,7 +41,7 @@ class ConditionVariableTest : public ::testing::Test { // Information for when running with predicates. std::atomic m_pred_var{false}; - void ShortSleep(unsigned long time=10) { + void ShortSleep(unsigned long time = 10) { std::this_thread::sleep_for(std::chrono::milliseconds(time)); } @@ -58,8 +58,7 @@ class ConditionVariableTest : public ::testing::Test { ShortSleep(); bool locked = m_mutex.try_lock(); if (locked) m_mutex.unlock(); - EXPECT_TRUE(locked) - << "The condition variable failed to unlock the lock."; + EXPECT_TRUE(locked) << "The condition variable failed to unlock the lock."; } void NotifyAll() { m_cond.notify_all(); } @@ -91,13 +90,12 @@ class ConditionVariableTest : public ::testing::Test { // the timeout works properly. void WaitTimeTest(bool wait_for) { std::atomic timed_out{true}; - auto wait_until = [this, &timed_out, wait_for](std::atomic &done) { + auto wait_until = [this, &timed_out, wait_for](std::atomic& done) { priority_lock lock(m_mutex); done = false; if (wait_for) { auto wait_time = std::chrono::milliseconds(100); - timed_out = - m_cond.wait_for(lock, wait_time) == std::cv_status::timeout; + timed_out = m_cond.wait_for(lock, wait_time) == std::cv_status::timeout; } else { auto wait_time = std::chrono::system_clock::now() + std::chrono::milliseconds(100); @@ -138,8 +136,8 @@ class ConditionVariableTest : public ::testing::Test { // false, the return value will be false. std::atomic retval{true}; auto predicate = [this]() -> bool { return m_pred_var; }; - auto wait_until = - [this, &retval, predicate, wait_for](std::atomic &done) { + auto wait_until = [this, &retval, predicate, + wait_for](std::atomic& done) { priority_lock lock(m_mutex); done = false; if (wait_for) { @@ -203,15 +201,19 @@ class ConditionVariableTest : public ::testing::Test { // std::terminate is called and all threads are terminated. // Detaching is non-optimal, but should allow the rest of the tests to run // before anything drastic occurs. - if (m_done1) m_watcher1.join(); - else m_watcher1.detach(); - if (m_done2) m_watcher2.join(); - else m_watcher2.detach(); + if (m_done1) + m_watcher1.join(); + else + m_watcher1.detach(); + if (m_done2) + m_watcher2.join(); + else + m_watcher2.detach(); } }; TEST_F(ConditionVariableTest, NotifyAll) { - auto wait = [this](std::atomic &done) { + auto wait = [this](std::atomic& done) { priority_lock lock(m_mutex); done = false; m_cond.wait(lock); @@ -226,7 +228,7 @@ TEST_F(ConditionVariableTest, NotifyAll) { } TEST_F(ConditionVariableTest, NotifyOne) { - auto wait = [this](std::atomic &done) { + auto wait = [this](std::atomic& done) { priority_lock lock(m_mutex); done = false; m_cond.wait(lock); @@ -248,7 +250,7 @@ TEST_F(ConditionVariableTest, NotifyOne) { TEST_F(ConditionVariableTest, WaitWithPredicate) { auto predicate = [this]() -> bool { return m_pred_var; }; - auto wait_predicate = [this, predicate](std::atomic &done) { + auto wait_predicate = [this, predicate](std::atomic& done) { priority_lock lock(m_mutex); done = false; m_cond.wait(lock, predicate); @@ -262,24 +264,20 @@ TEST_F(ConditionVariableTest, WaitWithPredicate) { PredicateTest(); } -TEST_F(ConditionVariableTest, WaitUntil) { - WaitTimeTest(false); -} +TEST_F(ConditionVariableTest, WaitUntil) { WaitTimeTest(false); } TEST_F(ConditionVariableTest, WaitUntilWithPredicate) { WaitTimePredicateTest(false); } -TEST_F(ConditionVariableTest, WaitFor) { - WaitTimeTest(true); -} +TEST_F(ConditionVariableTest, WaitFor) { WaitTimeTest(true); } TEST_F(ConditionVariableTest, WaitForWithPredicate) { WaitTimePredicateTest(true); } TEST_F(ConditionVariableTest, NativeHandle) { - auto wait = [this](std::atomic &done) { + auto wait = [this](std::atomic& done) { priority_lock lock(m_mutex); done = false; m_cond.wait(lock); diff --git a/wpilibcIntegrationTests/src/CounterTest.cpp b/wpilibcIntegrationTests/src/CounterTest.cpp index cbf9dc5647..3d65483f37 100644 --- a/wpilibcIntegrationTests/src/CounterTest.cpp +++ b/wpilibcIntegrationTests/src/CounterTest.cpp @@ -10,8 +10,8 @@ #include #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" static const double kMotorDelay = 2.5; @@ -19,12 +19,12 @@ static const double kMaxPeriod = 2.0; class CounterTest : public testing::Test { protected: - Counter *m_talonCounter; - Counter *m_victorCounter; - Counter *m_jaguarCounter; - Talon *m_talon; - Victor *m_victor; - Jaguar *m_jaguar; + Counter* m_talonCounter; + Counter* m_victorCounter; + Counter* m_jaguarCounter; + Talon* m_talon; + Victor* m_victor; + Jaguar* m_jaguar; virtual void SetUp() override { m_talonCounter = new Counter(TestBench::kTalonEncoderChannelA); diff --git a/wpilibcIntegrationTests/src/DIOLoopTest.cpp b/wpilibcIntegrationTests/src/DIOLoopTest.cpp index 480e4b7962..284aed1df7 100644 --- a/wpilibcIntegrationTests/src/DIOLoopTest.cpp +++ b/wpilibcIntegrationTests/src/DIOLoopTest.cpp @@ -10,8 +10,8 @@ #include #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" static const double kCounterTime = 0.001; @@ -26,8 +26,8 @@ static const double kSynchronousInterruptTimeTolerance = 0.01; */ class DIOLoopTest : public testing::Test { protected: - DigitalInput *m_input; - DigitalOutput *m_output; + DigitalInput* m_input; + DigitalOutput* m_output; virtual void SetUp() override { m_input = new DigitalInput(TestBench::kLoop1InputChannel); @@ -70,15 +70,16 @@ TEST_F(DIOLoopTest, DIOPWM) { EXPECT_FALSE(m_input->Get()) << "The digital output was turned off, but " << "the digital input is on."; - //Set frequency to 2.0 Hz + // Set frequency to 2.0 Hz m_output->SetPWMRate(2.0); - //Enable PWM, but leave it off + // Enable PWM, but leave it off m_output->EnablePWM(0.0); Wait(0.5); m_output->UpdateDutyCycle(0.5); m_input->RequestInterrupts(); m_input->SetUpSourceEdge(false, true); - InterruptableSensorBase::WaitResult result = m_input->WaitForInterrupt(3.0, true); + InterruptableSensorBase::WaitResult result = + m_input->WaitForInterrupt(3.0, true); Wait(0.5); bool firstCycle = m_input->Get(); @@ -101,7 +102,7 @@ TEST_F(DIOLoopTest, DIOPWM) { bool secondAfterStop = m_input->Get(); EXPECT_EQ(InterruptableSensorBase::WaitResult::kFallingEdge, result) - << "WaitForInterrupt was not falling."; + << "WaitForInterrupt was not falling."; EXPECT_FALSE(firstCycle) << "Input not low after first delay"; EXPECT_TRUE(secondCycle) << "Input not high after second delay"; @@ -136,8 +137,8 @@ TEST_F(DIOLoopTest, FakeCounter) { EXPECT_EQ(100, counter.Get()) << "Counter did not count up to 100."; } -static void InterruptHandler(uint32_t interruptAssertedMask, void *param) { - *(int *)param = 12345; +static void InterruptHandler(uint32_t interruptAssertedMask, void* param) { + *(int*)param = 12345; } TEST_F(DIOLoopTest, AsynchronousInterruptWorks) { @@ -157,8 +158,8 @@ TEST_F(DIOLoopTest, AsynchronousInterruptWorks) { EXPECT_EQ(12345, param) << "The interrupt did not run."; } -static void *InterruptTriggerer(void *data) { - DigitalOutput *output = static_cast(data); +static void* InterruptTriggerer(void* data) { + DigitalOutput* output = static_cast(data); output->Set(false); Wait(kSynchronousInterruptTime); output->Set(true); @@ -171,7 +172,8 @@ TEST_F(DIOLoopTest, SynchronousInterruptWorks) { // If we have another thread trigger the interrupt in a few seconds pthread_t interruptTriggererLoop; - pthread_create(&interruptTriggererLoop, nullptr, InterruptTriggerer, m_output); + pthread_create(&interruptTriggererLoop, nullptr, InterruptTriggerer, + m_output); // Then this thread should pause and resume after that number of seconds Timer timer; diff --git a/wpilibcIntegrationTests/src/DigitalGlitchFilterTest.cpp b/wpilibcIntegrationTests/src/DigitalGlitchFilterTest.cpp index c30deb8be1..46dc6ebff2 100644 --- a/wpilibcIntegrationTests/src/DigitalGlitchFilterTest.cpp +++ b/wpilibcIntegrationTests/src/DigitalGlitchFilterTest.cpp @@ -6,9 +6,9 @@ /*----------------------------------------------------------------------------*/ #include +#include #include #include -#include #include "gtest/gtest.h" /** diff --git a/wpilibcIntegrationTests/src/FakeEncoderTest.cpp b/wpilibcIntegrationTests/src/FakeEncoderTest.cpp index 601430b8a4..79919266e7 100644 --- a/wpilibcIntegrationTests/src/FakeEncoderTest.cpp +++ b/wpilibcIntegrationTests/src/FakeEncoderTest.cpp @@ -10,19 +10,19 @@ #include #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" static const double kDelayTime = 0.001; class FakeEncoderTest : public testing::Test { protected: - DigitalOutput *m_outputA; - DigitalOutput *m_outputB; - AnalogOutput *m_indexOutput; + DigitalOutput* m_outputA; + DigitalOutput* m_outputB; + AnalogOutput* m_indexOutput; - Encoder *m_encoder; - AnalogTrigger *m_indexAnalogTrigger; + Encoder* m_encoder; + AnalogTrigger* m_indexAnalogTrigger; std::shared_ptr m_indexAnalogTriggerOutput; virtual void SetUp() override { diff --git a/wpilibcIntegrationTests/src/FilterNoiseTest.cpp b/wpilibcIntegrationTests/src/FilterNoiseTest.cpp index cad0b174f9..f3ff0c4f9c 100644 --- a/wpilibcIntegrationTests/src/FilterNoiseTest.cpp +++ b/wpilibcIntegrationTests/src/FilterNoiseTest.cpp @@ -5,17 +5,17 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +#include #include #include #include #include -#include #include -#include "gtest/gtest.h" -#include "TestBench.h" #include "Base.h" +#include "TestBench.h" +#include "gtest/gtest.h" enum FilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG }; @@ -42,16 +42,14 @@ constexpr double kStdDev = 10.0; */ class NoiseGenerator : public PIDSource { public: - NoiseGenerator(double (*dataFunc)(double), double stdDev) : - m_distr(0.0, stdDev) { + NoiseGenerator(double (*dataFunc)(double), double stdDev) + : m_distr(0.0, stdDev) { m_dataFunc = dataFunc; } void SetPIDSourceType(PIDSourceType pidSource) override {} - double Get() { - return m_dataFunc(m_count) + m_noise; - } + double Get() { return m_dataFunc(m_count) + m_noise; } double PIDGet() override { m_noise = m_distr(m_gen); @@ -59,9 +57,7 @@ class NoiseGenerator : public PIDSource { return m_dataFunc(m_count) + m_noise; } - void Reset() { - m_count = -TestBench::kFilterStep; - } + void Reset() { m_count = -TestBench::kFilterStep; } private: std::function m_dataFunc; @@ -83,24 +79,24 @@ class FilterNoiseTest : public testing::TestWithParam { std::unique_ptr m_filter; std::shared_ptr m_noise; - static double GetData(double t) { - return 100.0 * std::sin(2.0 * M_PI * t); - } + static double GetData(double t) { return 100.0 * std::sin(2.0 * M_PI * t); } void SetUp() override { m_noise = std::make_shared(GetData, kStdDev); switch (GetParam()) { case TEST_SINGLE_POLE_IIR: { - m_filter = std::make_unique(LinearDigitalFilter::SinglePoleIIR(m_noise, - TestBench::kSinglePoleIIRTimeConstant, - TestBench::kFilterStep)); + m_filter = std::make_unique( + LinearDigitalFilter::SinglePoleIIR( + m_noise, TestBench::kSinglePoleIIRTimeConstant, + TestBench::kFilterStep)); break; } case TEST_MOVAVG: { - m_filter = std::make_unique(LinearDigitalFilter::MovingAverage(m_noise, - TestBench::kMovAvgTaps)); + m_filter = std::make_unique( + LinearDigitalFilter::MovingAverage(m_noise, + TestBench::kMovAvgTaps)); break; } } diff --git a/wpilibcIntegrationTests/src/FilterOutputTest.cpp b/wpilibcIntegrationTests/src/FilterOutputTest.cpp index 8a64ca4517..1cbb4a6d44 100644 --- a/wpilibcIntegrationTests/src/FilterOutputTest.cpp +++ b/wpilibcIntegrationTests/src/FilterOutputTest.cpp @@ -5,17 +5,17 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +#include #include #include #include #include -#include #include -#include "gtest/gtest.h" -#include "TestBench.h" #include "Base.h" +#include "TestBench.h" +#include "gtest/gtest.h" enum FilterOutputTestType { TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS, TEST_MOVAVG }; @@ -37,9 +37,7 @@ std::ostream& operator<<(std::ostream& os, const FilterOutputTestType& type) { class DataWrapper : public PIDSource { public: - DataWrapper(double (*dataFunc)(double)) { - m_dataFunc = dataFunc; - } + DataWrapper(double (*dataFunc)(double)) { m_dataFunc = dataFunc; } virtual void SetPIDSourceType(PIDSourceType pidSource) {} @@ -48,9 +46,7 @@ class DataWrapper : public PIDSource { return m_dataFunc(m_count); } - void Reset() { - m_count = -TestBench::kFilterStep; - } + void Reset() { m_count = -TestBench::kFilterStep; } private: std::function m_dataFunc; @@ -77,24 +73,26 @@ class FilterOutputTest : public testing::TestWithParam { switch (GetParam()) { case TEST_SINGLE_POLE_IIR: { - m_filter = std::make_unique(LinearDigitalFilter::SinglePoleIIR(m_data, - TestBench::kSinglePoleIIRTimeConstant, - TestBench::kFilterStep)); + m_filter = std::make_unique( + LinearDigitalFilter::SinglePoleIIR( + m_data, TestBench::kSinglePoleIIRTimeConstant, + TestBench::kFilterStep)); m_expectedOutput = TestBench::kSinglePoleIIRExpectedOutput; break; } case TEST_HIGH_PASS: { - m_filter = std::make_unique(LinearDigitalFilter::HighPass(m_data, - TestBench::kHighPassTimeConstant, - TestBench::kFilterStep)); + m_filter = + std::make_unique(LinearDigitalFilter::HighPass( + m_data, TestBench::kHighPassTimeConstant, + TestBench::kFilterStep)); m_expectedOutput = TestBench::kHighPassExpectedOutput; break; } case TEST_MOVAVG: { - m_filter = std::make_unique(LinearDigitalFilter::MovingAverage(m_data, - TestBench::kMovAvgTaps)); + m_filter = std::make_unique( + LinearDigitalFilter::MovingAverage(m_data, TestBench::kMovAvgTaps)); m_expectedOutput = TestBench::kMovAvgExpectedOutput; break; } @@ -109,7 +107,8 @@ TEST_P(FilterOutputTest, FilterOutput) { m_data->Reset(); double filterOutput = 0.0; - for (double t = 0.0; t < TestBench::kFilterTime; t += TestBench::kFilterStep) { + for (double t = 0.0; t < TestBench::kFilterTime; + t += TestBench::kFilterStep) { filterOutput = m_filter->PIDGet(); } diff --git a/wpilibcIntegrationTests/src/MotorEncoderTest.cpp b/wpilibcIntegrationTests/src/MotorEncoderTest.cpp index 9b3c609979..55fdeb7c02 100644 --- a/wpilibcIntegrationTests/src/MotorEncoderTest.cpp +++ b/wpilibcIntegrationTests/src/MotorEncoderTest.cpp @@ -11,12 +11,12 @@ #include #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" enum MotorEncoderTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON }; -std::ostream &operator<<(std::ostream &os, MotorEncoderTestType const &type) { +std::ostream& operator<<(std::ostream& os, MotorEncoderTestType const& type) { switch (type) { case TEST_VICTOR: os << "Victor"; @@ -41,8 +41,8 @@ static constexpr double kMotorTime = 0.5; */ class MotorEncoderTest : public testing::TestWithParam { protected: - SpeedController *m_speedController; - Encoder *m_encoder; + SpeedController* m_speedController; + Encoder* m_encoder; virtual void SetUp() override { switch (GetParam()) { @@ -145,7 +145,9 @@ TEST_P(MotorEncoderTest, PositionPIDController) { RecordProperty("PIDError", pid.GetError()); - EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: "< #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" enum MotorInvertingTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON }; static const double motorSpeed = 0.15; static const double delayTime = 0.5; -std::ostream &operator<<(std::ostream &os, MotorInvertingTestType const &type) { +std::ostream& operator<<(std::ostream& os, MotorInvertingTestType const& type) { switch (type) { case TEST_VICTOR: os << "Victor"; @@ -34,8 +34,8 @@ std::ostream &operator<<(std::ostream &os, MotorInvertingTestType const &type) { class MotorInvertingTest : public testing::TestWithParam { protected: - SpeedController *m_speedController; - Encoder *m_encoder; + SpeedController* m_speedController; + Encoder* m_encoder; virtual void SetUp() override { switch (GetParam()) { case TEST_VICTOR: diff --git a/wpilibcIntegrationTests/src/MutexTest.cpp b/wpilibcIntegrationTests/src/MutexTest.cpp index dcd373c299..a87602ab97 100644 --- a/wpilibcIntegrationTests/src/MutexTest.cpp +++ b/wpilibcIntegrationTests/src/MutexTest.cpp @@ -8,10 +8,10 @@ #include "HAL/cpp/priority_mutex.h" #include "TestBench.h" -#include "gtest/gtest.h" #include #include #include +#include "gtest/gtest.h" namespace wpilib { namespace testing { @@ -67,7 +67,7 @@ void SetThreadRealtimePriorityOrDie(int priority) { template class LowPriorityThread { public: - LowPriorityThread(MutexType *mutex) + LowPriorityThread(MutexType* mutex) : m_mutex(mutex), m_hold_mutex(1), m_success(0) {} void operator()() { @@ -75,7 +75,8 @@ class LowPriorityThread { SetThreadRealtimePriorityOrDie(20); m_mutex->lock(); m_started.Notify(); - while (m_hold_mutex.test_and_set()) {} + while (m_hold_mutex.test_and_set()) { + } m_mutex->unlock(); m_success.store(1); } @@ -86,7 +87,7 @@ class LowPriorityThread { private: // priority_mutex to grab and release. - MutexType *m_mutex; + MutexType* m_mutex; Notification m_started; // Atomic type used to signal when the thread should unlock the mutex. // Using a mutex to protect something could cause other issues, and a delay @@ -134,7 +135,7 @@ class BusyWaitingThread { template class HighPriorityThread { public: - HighPriorityThread(MutexType *mutex) : m_mutex(mutex), m_success(0) {} + HighPriorityThread(MutexType* mutex) : m_mutex(mutex), m_success(0) {} void operator()() { SetProcessorAffinity(0); @@ -149,7 +150,7 @@ class HighPriorityThread { private: Notification m_started; - MutexType *m_mutex; + MutexType* m_mutex; std::atomic m_success; }; @@ -220,7 +221,7 @@ class InversionTestRunner { bool m_success = false; }; -//TODO: Fix roborio permissions to run as root. +// TODO: Fix roborio permissions to run as root. // Priority inversion test. TEST(MutexTest, DISABLED_PriorityInversionTest) { diff --git a/wpilibcIntegrationTests/src/NotifierTest.cpp b/wpilibcIntegrationTests/src/NotifierTest.cpp index 7d51256c3a..02926aa1df 100644 --- a/wpilibcIntegrationTests/src/NotifierTest.cpp +++ b/wpilibcIntegrationTests/src/NotifierTest.cpp @@ -7,12 +7,12 @@ #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" unsigned notifierCounter; -void notifierHandler(void *) { notifierCounter++; } +void notifierHandler(void*) { notifierCounter++; } /** * Test if the Wait function works diff --git a/wpilibcIntegrationTests/src/PCMTest.cpp b/wpilibcIntegrationTests/src/PCMTest.cpp index 75597dd57b..20b659b1e1 100644 --- a/wpilibcIntegrationTests/src/PCMTest.cpp +++ b/wpilibcIntegrationTests/src/PCMTest.cpp @@ -12,8 +12,8 @@ #include #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" /* The PCM switches the compressor up to a couple seconds after the pressure switch changes. */ @@ -29,11 +29,11 @@ static const double kCompressorOffVoltage = 1.68; class PCMTest : public testing::Test { protected: - Compressor *m_compressor; + Compressor* m_compressor; - DigitalOutput *m_fakePressureSwitch; - AnalogInput *m_fakeCompressor; - DoubleSolenoid *m_doubleSolenoid; + DigitalOutput* m_fakePressureSwitch; + AnalogInput* m_fakeCompressor; + DoubleSolenoid* m_doubleSolenoid; DigitalInput *m_fakeSolenoid1, *m_fakeSolenoid2; virtual void SetUp() override { @@ -138,17 +138,20 @@ TEST_F(PCMTest, DoubleSolenoid) { Wait(kSolenoidDelayTime); EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off"; EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off"; - EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kOff) << "Solenoid does not read off"; + EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kOff) + << "Solenoid does not read off"; solenoid.Set(DoubleSolenoid::kForward); Wait(kSolenoidDelayTime); EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on"; EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off"; - EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kForward) << "Solenoid does not read forward"; + EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kForward) + << "Solenoid does not read forward"; solenoid.Set(DoubleSolenoid::kReverse); Wait(kSolenoidDelayTime); EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off"; EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on"; - EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kReverse) << "Solenoid does not read reverse"; + EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kReverse) + << "Solenoid does not read reverse"; } diff --git a/wpilibcIntegrationTests/src/PIDToleranceTest.cpp b/wpilibcIntegrationTests/src/PIDToleranceTest.cpp index 35e60ae583..83586f8f47 100644 --- a/wpilibcIntegrationTests/src/PIDToleranceTest.cpp +++ b/wpilibcIntegrationTests/src/PIDToleranceTest.cpp @@ -6,74 +6,81 @@ /*----------------------------------------------------------------------------*/ #include -#include "gtest/gtest.h" -#include "TestBench.h" -#include "PIDSource.h" #include "PIDController.h" #include "PIDOutput.h" +#include "PIDSource.h" +#include "TestBench.h" +#include "gtest/gtest.h" -class PIDToleranceTest : public testing::Test{ -protected: - const double setpoint = 50.0; - const double range = 200; - const double tolerance = 10.0; - class fakeInput : public PIDSource{ - public: - double val = 0; - void SetPIDSourceType(PIDSourceType pidSource){ - } - PIDSourceType GetPIDSourceType(){ - return PIDSourceType::kDisplacement; - } - double PIDGet(){; - return val; - } - }; - class fakeOutput : public PIDOutput{ - void PIDWrite(float output){ - - } - }; - fakeInput inp; - fakeOutput out; - PIDController *pid; - virtual void SetUp() override { - pid = new PIDController(0.5,0.0,0.0,&inp,&out); - pid->SetInputRange(-range/2,range/2); - } - virtual void TearDown() override { - delete pid; - } - virtual void reset(){ - inp.val = 0; - } +class PIDToleranceTest : public testing::Test { + protected: + const double setpoint = 50.0; + const double range = 200; + const double tolerance = 10.0; + class fakeInput : public PIDSource { + public: + double val = 0; + void SetPIDSourceType(PIDSourceType pidSource) {} + PIDSourceType GetPIDSourceType() { return PIDSourceType::kDisplacement; } + double PIDGet() { + ; + return val; + } + }; + class fakeOutput : public PIDOutput { + void PIDWrite(float output) {} + }; + fakeInput inp; + fakeOutput out; + PIDController* pid; + virtual void SetUp() override { + pid = new PIDController(0.5, 0.0, 0.0, &inp, &out); + pid->SetInputRange(-range / 2, range / 2); + } + virtual void TearDown() override { delete pid; } + virtual void reset() { inp.val = 0; } }; -TEST_F(PIDToleranceTest, Absolute){ - reset(); - pid->SetAbsoluteTolerance(tolerance); - pid->SetSetpoint(setpoint); - pid->Enable(); - EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError(); - inp.val = setpoint+tolerance/2; - Wait(1.0); - EXPECT_TRUE(pid->OnTarget())<<"Error was not in tolerance when it should have been. Error was " << pid->GetAvgError(); - inp.val = setpoint+10*tolerance; - Wait(1.0); - EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError(); +TEST_F(PIDToleranceTest, Absolute) { + reset(); + pid->SetAbsoluteTolerance(tolerance); + pid->SetSetpoint(setpoint); + pid->Enable(); + EXPECT_FALSE(pid->OnTarget()) + << "Error was in tolerance when it should not have been. Error was " + << pid->GetAvgError(); + inp.val = setpoint + tolerance / 2; + Wait(1.0); + EXPECT_TRUE(pid->OnTarget()) + << "Error was not in tolerance when it should have been. Error was " + << pid->GetAvgError(); + inp.val = setpoint + 10 * tolerance; + Wait(1.0); + EXPECT_FALSE(pid->OnTarget()) + << "Error was in tolerance when it should not have been. Error was " + << pid->GetAvgError(); } -TEST_F(PIDToleranceTest, Percent){ - reset(); - pid->SetPercentTolerance(tolerance); - pid->SetSetpoint(setpoint); - pid->Enable(); - EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError(); - inp.val = setpoint+(tolerance)/200*range;//half of percent tolerance away from setpoint - Wait(1.0); - EXPECT_TRUE(pid->OnTarget())<<"Error was not in tolerance when it should have been. Error was " << pid->GetAvgError(); - inp.val = setpoint+(tolerance)/50*range;//double percent tolerance away from setPoint - Wait(1.0); - EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError(); - +TEST_F(PIDToleranceTest, Percent) { + reset(); + pid->SetPercentTolerance(tolerance); + pid->SetSetpoint(setpoint); + pid->Enable(); + EXPECT_FALSE(pid->OnTarget()) + << "Error was in tolerance when it should not have been. Error was " + << pid->GetAvgError(); + inp.val = setpoint + + (tolerance) / 200 * + range; // half of percent tolerance away from setpoint + Wait(1.0); + EXPECT_TRUE(pid->OnTarget()) + << "Error was not in tolerance when it should have been. Error was " + << pid->GetAvgError(); + inp.val = + setpoint + + (tolerance) / 50 * range; // double percent tolerance away from setPoint + Wait(1.0); + EXPECT_FALSE(pid->OnTarget()) + << "Error was in tolerance when it should not have been. Error was " + << pid->GetAvgError(); } diff --git a/wpilibcIntegrationTests/src/PowerDistributionPanelTest.cpp b/wpilibcIntegrationTests/src/PowerDistributionPanelTest.cpp index ccfb796419..1a71f571f7 100644 --- a/wpilibcIntegrationTests/src/PowerDistributionPanelTest.cpp +++ b/wpilibcIntegrationTests/src/PowerDistributionPanelTest.cpp @@ -10,17 +10,17 @@ #include #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" static const double kMotorTime = 0.25; class PowerDistributionPanelTest : public testing::Test { protected: - PowerDistributionPanel *m_pdp; - Talon *m_talon; - Victor *m_victor; - Jaguar *m_jaguar; + PowerDistributionPanel* m_pdp; + Talon* m_talon; + Victor* m_victor; + Jaguar* m_jaguar; virtual void SetUp() override { m_pdp = new PowerDistributionPanel(); @@ -55,4 +55,3 @@ TEST_F(PowerDistributionPanelTest, CheckCurrentTalon) { ASSERT_GT(m_pdp->GetCurrent(TestBench::kTalonPDPChannel), 0) << "The Talon current was not positive"; } - diff --git a/wpilibcIntegrationTests/src/PreferencesTest.cpp b/wpilibcIntegrationTests/src/PreferencesTest.cpp index 31e2fc2ddf..8e6f342550 100644 --- a/wpilibcIntegrationTests/src/PreferencesTest.cpp +++ b/wpilibcIntegrationTests/src/PreferencesTest.cpp @@ -7,11 +7,11 @@ #include #include -#include "gtest/gtest.h" #include #include +#include "gtest/gtest.h" -static const char *kFileName = "networktables.ini"; +static const char* kFileName = "networktables.ini"; static const double kSaveTime = 1.2; /** @@ -23,17 +23,23 @@ TEST(PreferencesTest, ReadPreferencesFromFile) { std::remove(kFileName); std::ofstream preferencesFile(kFileName); preferencesFile << "[NetworkTables Storage 3.0]" << std::endl; - preferencesFile << "string \"/Preferences/testFileGetString\"=\"Hello, preferences file\"" - << std::endl; + preferencesFile + << "string \"/Preferences/testFileGetString\"=\"Hello, preferences file\"" + << std::endl; preferencesFile << "double \"/Preferences/testFileGetInt\"=1" << std::endl; - preferencesFile << "double \"/Preferences/testFileGetDouble\"=0.5" << std::endl; - preferencesFile << "double \"/Preferences/testFileGetFloat\"=0.25" << std::endl; - preferencesFile << "boolean \"/Preferences/testFileGetBoolean\"=true" << std::endl; - preferencesFile << "double \"/Preferences/testFileGetLong\"=1000000000000000000" << std::endl; + preferencesFile << "double \"/Preferences/testFileGetDouble\"=0.5" + << std::endl; + preferencesFile << "double \"/Preferences/testFileGetFloat\"=0.25" + << std::endl; + preferencesFile << "boolean \"/Preferences/testFileGetBoolean\"=true" + << std::endl; + preferencesFile + << "double \"/Preferences/testFileGetLong\"=1000000000000000000" + << std::endl; preferencesFile.close(); NetworkTable::Initialize(); - Preferences *preferences = Preferences::GetInstance(); + Preferences* preferences = Preferences::GetInstance(); EXPECT_EQ("Hello, preferences file", preferences->GetString("testFileGetString")); EXPECT_EQ(1, preferences->GetInt("testFileGetInt")); @@ -52,7 +58,7 @@ TEST(PreferencesTest, WritePreferencesToFile) { NetworkTable::GlobalDeleteAll(); std::remove(kFileName); NetworkTable::Initialize(); - Preferences *preferences = Preferences::GetInstance(); + Preferences* preferences = Preferences::GetInstance(); preferences->PutString("testFilePutString", "Hello, preferences file"); preferences->PutInt("testFilePutInt", 1); preferences->PutDouble("testFilePutDouble", 0.5); @@ -63,7 +69,7 @@ TEST(PreferencesTest, WritePreferencesToFile) { Wait(kSaveTime); - static char const *kExpectedFileContents[] = { + static char const* kExpectedFileContents[] = { "[NetworkTables Storage 3.0]", "boolean \"/Preferences/testFilePutBoolean\"=true", "double \"/Preferences/testFilePutDouble\"=0.5", diff --git a/wpilibcIntegrationTests/src/RelayTest.cpp b/wpilibcIntegrationTests/src/RelayTest.cpp index e18c88489c..8f4448cbc4 100644 --- a/wpilibcIntegrationTests/src/RelayTest.cpp +++ b/wpilibcIntegrationTests/src/RelayTest.cpp @@ -5,20 +5,20 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +#include "Relay.h" #include #include #include -#include "gtest/gtest.h" #include "TestBench.h" -#include "Relay.h" +#include "gtest/gtest.h" static const double kDelayTime = 0.01; class RelayTest : public testing::Test { protected: - Relay *m_relay; - DigitalInput *m_forward; - DigitalInput *m_reverse; + Relay* m_relay; + DigitalInput* m_forward; + DigitalInput* m_reverse; virtual void SetUp() override { m_relay = new Relay(TestBench::kRelayChannel); diff --git a/wpilibcIntegrationTests/src/TestEnvironment.cpp b/wpilibcIntegrationTests/src/TestEnvironment.cpp index b3f55002a5..c1ba3cc50e 100644 --- a/wpilibcIntegrationTests/src/TestEnvironment.cpp +++ b/wpilibcIntegrationTests/src/TestEnvironment.cpp @@ -42,5 +42,5 @@ class TestEnvironment : public testing::Environment { virtual void TearDown() override {} }; -testing::Environment *const environment = +testing::Environment* const environment = testing::AddGlobalTestEnvironment(new TestEnvironment); diff --git a/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp b/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp index 5b42543ed0..d21b54e6e4 100644 --- a/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp +++ b/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp @@ -9,8 +9,8 @@ #include #include #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" static constexpr double kServoResetTime = 2.0; @@ -29,9 +29,9 @@ static constexpr double kSensitivity = 0.013; */ class TiltPanCameraTest : public testing::Test { protected: - static AnalogGyro *m_gyro; + static AnalogGyro* m_gyro; Servo *m_tilt, *m_pan; - Accelerometer *m_spiAccel; + Accelerometer* m_spiAccel; static void SetUpTestCase() { // The gyro object blocks for 5 seconds in the constructor, so only @@ -66,7 +66,7 @@ class TiltPanCameraTest : public testing::Test { } }; -AnalogGyro *TiltPanCameraTest::m_gyro = nullptr; +AnalogGyro* TiltPanCameraTest::m_gyro = nullptr; /** * Test if the gyro angle defaults to 0 immediately after being reset. diff --git a/wpilibcIntegrationTests/src/TimerTest.cpp b/wpilibcIntegrationTests/src/TimerTest.cpp index 5543938a8a..a2b0ba97bc 100644 --- a/wpilibcIntegrationTests/src/TimerTest.cpp +++ b/wpilibcIntegrationTests/src/TimerTest.cpp @@ -6,14 +6,14 @@ /*----------------------------------------------------------------------------*/ #include -#include "gtest/gtest.h" #include "TestBench.h" +#include "gtest/gtest.h" static const double kWaitTime = 0.5; class TimerTest : public testing::Test { protected: - Timer *m_timer; + Timer* m_timer; virtual void SetUp() override { m_timer = new Timer; } diff --git a/wpilibcIntegrationTests/src/command/CommandTest.cpp b/wpilibcIntegrationTests/src/command/CommandTest.cpp index 9b3f7d0cc9..d6de4102d3 100644 --- a/wpilibcIntegrationTests/src/command/CommandTest.cpp +++ b/wpilibcIntegrationTests/src/command/CommandTest.cpp @@ -32,7 +32,7 @@ class CommandTest : public testing::Test { */ void TeardownScheduler() { Scheduler::GetInstance()->ResetAll(); } - void AssertCommandState(MockCommand &command, int initialize, int execute, + void AssertCommandState(MockCommand& command, int initialize, int execute, int isFinished, int end, int interrupted) { EXPECT_EQ(initialize, command.GetInitializeCount()); EXPECT_EQ(execute, command.GetExecuteCount()); @@ -44,10 +44,10 @@ class CommandTest : public testing::Test { class ASubsystem : public Subsystem { private: - Command *m_command = nullptr; + Command* m_command = nullptr; public: - ASubsystem(const std::string &name) : Subsystem(name) {} + ASubsystem(const std::string& name) : Subsystem(name) {} virtual void InitDefaultCommand() override { if (m_command != nullptr) { @@ -55,7 +55,7 @@ class ASubsystem : public Subsystem { } } - void Init(Command *command) { m_command = command; } + void Init(Command* command) { m_command = command; } }; // CommandParallelGroupTest ported from CommandParallelGroupTest.java diff --git a/wpilibj/src/athena/cpp/lib/AccelerometerJNI.cpp b/wpilibj/src/athena/cpp/lib/AccelerometerJNI.cpp index 6de5934b40..c84e1216af 100644 --- a/wpilibj/src/athena/cpp/lib/AccelerometerJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/AccelerometerJNI.cpp @@ -5,9 +5,9 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +#include "HAL/Accelerometer.hpp" #include #include "edu_wpi_first_wpilibj_hal_AccelerometerJNI.h" -#include "HAL/Accelerometer.hpp" extern "C" { @@ -16,10 +16,10 @@ extern "C" { * Method: setAccelerometerActive * Signature: (Z)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_setAccelerometerActive - (JNIEnv *, jclass, jboolean active) -{ - setAccelerometerActive(active); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_setAccelerometerActive( + JNIEnv *, jclass, jboolean active) { + setAccelerometerActive(active); } /* @@ -27,10 +27,10 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_setAccele * Method: setAccelerometerRange * Signature: (I)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_setAccelerometerRange - (JNIEnv *, jclass, jint range) -{ - setAccelerometerRange((AccelerometerRange)range); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_setAccelerometerRange( + JNIEnv *, jclass, jint range) { + setAccelerometerRange((AccelerometerRange)range); } /* @@ -38,10 +38,10 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_setAccele * Method: getAccelerometerX * Signature: ()D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_getAccelerometerX - (JNIEnv *, jclass) -{ - return getAccelerometerX(); +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_getAccelerometerX( + JNIEnv *, jclass) { + return getAccelerometerX(); } /* @@ -49,10 +49,10 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_getAcc * Method: getAccelerometerY * Signature: ()D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_getAccelerometerY - (JNIEnv *, jclass) -{ - return getAccelerometerY(); +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_getAccelerometerY( + JNIEnv *, jclass) { + return getAccelerometerY(); } /* @@ -60,10 +60,10 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_getAcc * Method: getAccelerometerZ * Signature: ()D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_getAccelerometerZ - (JNIEnv *, jclass) -{ - return getAccelerometerZ(); +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_AccelerometerJNI_getAccelerometerZ( + JNIEnv *, jclass) { + return getAccelerometerZ(); } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/AnalogJNI.cpp b/wpilibj/src/athena/cpp/lib/AnalogJNI.cpp index 2144ce763f..7dfc01d38c 100644 --- a/wpilibj/src/athena/cpp/lib/AnalogJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/AnalogJNI.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_AnalogJNI.h" @@ -17,9 +17,11 @@ // set the logging level TLogLevel analogJNILogLevel = logWARNING; -#define ANALOGJNI_LOG(level) \ - if (level > analogJNILogLevel) ; \ - else Log().Get(level) +#define ANALOGJNI_LOG(level) \ + if (level > analogJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -28,16 +30,16 @@ extern "C" { * Method: initializeAnalogInputPort * Signature: (J)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initializeAnalogInputPort - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - void* analog = initializeAnalogInputPort((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << analog; - CheckStatus(env, status); - return (jlong)analog; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initializeAnalogInputPort( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)id; + int32_t status = 0; + void *analog = initializeAnalogInputPort((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << analog; + CheckStatus(env, status); + return (jlong)analog; } /* @@ -45,11 +47,11 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initializeAnalo * Method: freeAnalogInputPort * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_freeAnalogInputPort - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - freeAnalogInputPort((void*)id); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_freeAnalogInputPort( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)id; + freeAnalogInputPort((void *)id); } /* @@ -57,16 +59,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_freeAnalogInputP * Method: initializeAnalogOutputPort * Signature: (J)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initializeAnalogOutputPort - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - void* analog = initializeAnalogOutputPort((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << analog; - CheckStatus(env, status); - return (jlong)analog; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initializeAnalogOutputPort( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)id; + int32_t status = 0; + void *analog = initializeAnalogOutputPort((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << analog; + CheckStatus(env, status); + return (jlong)analog; } /* @@ -74,11 +76,11 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initializeAnalo * Method: freeAnalogOutputPort * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_freeAnalogOutputPort - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - freeAnalogOutputPort((void*)id); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_freeAnalogOutputPort( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)id; + freeAnalogOutputPort((void *)id); } /* @@ -86,13 +88,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_freeAnalogOutput * Method: checkAnalogModule * Signature: (B)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_checkAnalogModule - (JNIEnv *, jclass, jbyte value) -{ - //ANALOGJNI_LOG(logDEBUG) << "Module = " << (jint)value; - jboolean returnValue = checkAnalogModule( value ); - //ANALOGJNI_LOG(logDEBUG) << "checkAnalogModuleResult = " << (jint)returnValue; - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_checkAnalogModule( + JNIEnv *, jclass, jbyte value) { + // ANALOGJNI_LOG(logDEBUG) << "Module = " << (jint)value; + jboolean returnValue = checkAnalogModule(value); + // ANALOGJNI_LOG(logDEBUG) << "checkAnalogModuleResult = " << + // (jint)returnValue; + return returnValue; } /* @@ -100,13 +103,14 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_checkAnalogM * Method: checkAnalogInputChannel * Signature: (I)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_checkAnalogInputChannel - (JNIEnv *, jclass, jint value) -{ - //ANALOGJNI_LOG(logDEBUG) << "Channel = " << value; - jboolean returnValue = checkAnalogInputChannel( value ); - //ANALOGJNI_LOG(logDEBUG) << "checkAnalogChannelResult = " << (jint)returnValue; - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_checkAnalogInputChannel( + JNIEnv *, jclass, jint value) { + // ANALOGJNI_LOG(logDEBUG) << "Channel = " << value; + jboolean returnValue = checkAnalogInputChannel(value); + // ANALOGJNI_LOG(logDEBUG) << "checkAnalogChannelResult = " << + // (jint)returnValue; + return returnValue; } /* @@ -114,13 +118,14 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_checkAnalogI * Method: checkAnalogOutputChannel * Signature: (I)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_checkAnalogOutputChannel - (JNIEnv *, jclass, jint value) -{ - //ANALOGJNI_LOG(logDEBUG) << "Channel = " << value; - jboolean returnValue = checkAnalogOutputChannel( value ); - //ANALOGJNI_LOG(logDEBUG) << "checkAnalogChannelResult = " << (jint)returnValue; - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_checkAnalogOutputChannel( + JNIEnv *, jclass, jint value) { + // ANALOGJNI_LOG(logDEBUG) << "Channel = " << value; + jboolean returnValue = checkAnalogOutputChannel(value); + // ANALOGJNI_LOG(logDEBUG) << "checkAnalogChannelResult = " << + // (jint)returnValue; + return returnValue; } /* @@ -128,15 +133,14 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_checkAnalogO * Method: setAnalogOutput * Signature: (JD)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogOutput - (JNIEnv * env, jclass, jlong id, jdouble voltage) -{ - ANALOGJNI_LOG(logDEBUG) << "Calling setAnalogOutput"; - ANALOGJNI_LOG(logDEBUG) << "Voltage = " << voltage; - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; - setAnalogOutput((void*)id, voltage, &status); - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogOutput( + JNIEnv *env, jclass, jlong id, jdouble voltage) { + ANALOGJNI_LOG(logDEBUG) << "Calling setAnalogOutput"; + ANALOGJNI_LOG(logDEBUG) << "Voltage = " << voltage; + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; + setAnalogOutput((void *)id, voltage, &status); + CheckStatus(env, status); } /* @@ -144,13 +148,13 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogOutput * Method: getAnalogOutput * Signature: (J)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogOutput - (JNIEnv * env, jclass, jlong id) -{ - int32_t status = 0; - double val = getAnalogOutput((void*)id, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogOutput( + JNIEnv *env, jclass, jlong id) { + int32_t status = 0; + double val = getAnalogOutput((void *)id, &status); + CheckStatus(env, status); + return val; } /* @@ -158,14 +162,14 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogOutp * Method: setAnalogSampleRate * Signature: (D)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogSampleRate - (JNIEnv * env, jclass, jdouble value) -{ - ANALOGJNI_LOG(logDEBUG) << "SampleRate = " << value; - int32_t status = 0; - setAnalogSampleRate( value, &status ); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogSampleRate( + JNIEnv *env, jclass, jdouble value) { + ANALOGJNI_LOG(logDEBUG) << "SampleRate = " << value; + int32_t status = 0; + setAnalogSampleRate(value, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -173,15 +177,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogSampleR * Method: getAnalogSampleRate * Signature: ()D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogSampleRate - (JNIEnv * env, jclass) -{ - int32_t status = 0; - double returnValue = getAnalogSampleRate( &status ); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "SampleRate = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogSampleRate( + JNIEnv *env, jclass) { + int32_t status = 0; + double returnValue = getAnalogSampleRate(&status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "SampleRate = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -189,15 +193,15 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogSamp * Method: setAnalogAverageBits * Signature: (JI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogAverageBits - (JNIEnv * env, jclass, jlong id, jint value) -{ - ANALOGJNI_LOG(logDEBUG) << "AverageBits = " << value; - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; - setAnalogAverageBits((void*)id, value, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogAverageBits( + JNIEnv *env, jclass, jlong id, jint value) { + ANALOGJNI_LOG(logDEBUG) << "AverageBits = " << value; + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; + setAnalogAverageBits((void *)id, value, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -205,16 +209,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogAverage * Method: getAnalogAverageBits * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogAverageBits - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; - jint returnValue = getAnalogAverageBits((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "AverageBits = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogAverageBits( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; + jint returnValue = getAnalogAverageBits((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "AverageBits = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -222,15 +226,15 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogAverage * Method: setAnalogOversampleBits * Signature: (JI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogOversampleBits - (JNIEnv * env, jclass, jlong id, jint value) -{ - ANALOGJNI_LOG(logDEBUG) << "OversampleBits = " << value; - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; - setAnalogOversampleBits((void*)id, value, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogOversampleBits( + JNIEnv *env, jclass, jlong id, jint value) { + ANALOGJNI_LOG(logDEBUG) << "OversampleBits = " << value; + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; + setAnalogOversampleBits((void *)id, value, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -238,16 +242,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogOversam * Method: getAnalogOversampleBits * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogOversampleBits - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; - jint returnValue = getAnalogOversampleBits((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "OversampleBits = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogOversampleBits( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; + jint returnValue = getAnalogOversampleBits((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "OversampleBits = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -255,16 +259,16 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogOversam * Method: getAnalogValue * Signature: (J)S */ -JNIEXPORT jshort JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogValue - (JNIEnv * env, jclass, jlong id) -{ - //ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; - jshort returnValue = getAnalogValue((void*)id, &status); - //ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - //ANALOGJNI_LOG(logDEBUG) << "Value = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jshort JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogValue( + JNIEnv *env, jclass, jlong id) { + // ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; + int32_t status = 0; + jshort returnValue = getAnalogValue((void *)id, &status); + // ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + // ANALOGJNI_LOG(logDEBUG) << "Value = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -272,16 +276,16 @@ JNIEXPORT jshort JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogValue * Method: getAnalogAverageValue * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogAverageValue - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; - jint returnValue = getAnalogAverageValue((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "AverageValue = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogAverageValue( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; + jint returnValue = getAnalogAverageValue((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "AverageValue = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -289,17 +293,17 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogAverage * Method: getAnalogVoltsToValue * Signature: (JD)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogVoltsToValue - (JNIEnv * env, jclass, jlong id, jdouble voltageValue) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - ANALOGJNI_LOG(logDEBUG) << "VoltageValue = " << voltageValue; - int32_t status = 0; - jint returnValue = getAnalogVoltsToValue((void*)id, voltageValue, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "Value = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogVoltsToValue( + JNIEnv *env, jclass, jlong id, jdouble voltageValue) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + ANALOGJNI_LOG(logDEBUG) << "VoltageValue = " << voltageValue; + int32_t status = 0; + jint returnValue = getAnalogVoltsToValue((void *)id, voltageValue, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "Value = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -307,16 +311,16 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogVoltsTo * Method: getAnalogVoltage * Signature: (J)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogVoltage - (JNIEnv * env, jclass, jlong id) -{ - //ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; - jdouble returnValue = getAnalogVoltage((void*)id, &status); - //ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - //ANALOGJNI_LOG(logDEBUG) << "Voltage = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogVoltage( + JNIEnv *env, jclass, jlong id) { + // ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; + int32_t status = 0; + jdouble returnValue = getAnalogVoltage((void *)id, &status); + // ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + // ANALOGJNI_LOG(logDEBUG) << "Voltage = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -324,16 +328,16 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogVolt * Method: getAnalogAverageVoltage * Signature: (J)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogAverageVoltage - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; - jdouble returnValue = getAnalogAverageVoltage((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "AverageVoltage = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogAverageVoltage( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; + jdouble returnValue = getAnalogAverageVoltage((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "AverageVoltage = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -341,17 +345,17 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogAver * Method: getAnalogLSBWeight * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogLSBWeight - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogLSBWeight( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; - jint returnValue = getAnalogLSBWeight((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "AnalogLSBWeight = " << returnValue; - CheckStatus(env, status); - return returnValue; + jint returnValue = getAnalogLSBWeight((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "AnalogLSBWeight = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -359,17 +363,16 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogLSBWeig * Method: getAnalogOffset * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogOffset - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogOffset( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; - jint returnValue = getAnalogOffset((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "AnalogOffset = " << returnValue; - CheckStatus(env, status); - return returnValue; + jint returnValue = getAnalogOffset((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "AnalogOffset = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -377,18 +380,18 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogOffset * Method: isAccumulatorChannel * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_isAccumulatorChannel - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "isAccumulatorChannel"; - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_isAccumulatorChannel( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "isAccumulatorChannel"; + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; - jboolean returnValue = isAccumulatorChannel((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "AnalogOffset = " << returnValue; - CheckStatus(env, status); - return returnValue; + jboolean returnValue = isAccumulatorChannel((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "AnalogOffset = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -396,14 +399,13 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_isAccumulato * Method: initAccumulator * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initAccumulator - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; - initAccumulator((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initAccumulator( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; + initAccumulator((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -411,15 +413,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initAccumulator * Method: resetAccumulator * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_resetAccumulator - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_resetAccumulator( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; - resetAccumulator((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); + resetAccumulator((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -427,15 +429,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_resetAccumulator * Method: setAccumulatorCenter * Signature: (JI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAccumulatorCenter - (JNIEnv * env, jclass, jlong id, jint center) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAccumulatorCenter( + JNIEnv *env, jclass, jlong id, jint center) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; - setAccumulatorCenter((void*)id, center, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); + setAccumulatorCenter((void *)id, center, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -443,15 +445,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAccumulatorCe * Method: setAccumulatorDeadband * Signature: (JI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAccumulatorDeadband - (JNIEnv * env, jclass, jlong id, jint deadband) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAccumulatorDeadband( + JNIEnv *env, jclass, jlong id, jint deadband) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; - setAccumulatorDeadband((void*)id, deadband, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); + setAccumulatorDeadband((void *)id, deadband, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -459,18 +461,18 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAccumulatorDe * Method: getAccumulatorValue * Signature: (J)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAccumulatorValue - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAccumulatorValue( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; - jlong returnValue = getAccumulatorValue((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "AccumulatorValue = " << returnValue; - CheckStatus(env, status); + jlong returnValue = getAccumulatorValue((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "AccumulatorValue = " << returnValue; + CheckStatus(env, status); - return returnValue; + return returnValue; } /* @@ -478,17 +480,17 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAccumulatorV * Method: getAccumulatorCount * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAccumulatorCount - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAccumulatorCount( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; - jint returnValue = getAccumulatorCount((void*)id, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "AccumulatorCount = " << returnValue; - CheckStatus(env, status); - return returnValue; + jint returnValue = getAccumulatorCount((void *)id, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "AccumulatorCount = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -496,21 +498,21 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAccumulatorCo * Method: getAccumulatorOutput * Signature: (JLjava/nio/LongBuffer;Ljava/nio/IntBuffer;)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAccumulatorOutput - (JNIEnv * env, jclass, jlong id, jobject value, jobject count) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void*)id; - int32_t status = 0; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAccumulatorOutput( + JNIEnv *env, jclass, jlong id, jobject value, jobject count) { + ANALOGJNI_LOG(logDEBUG) << "Analog Ptr = " << (void *)id; + int32_t status = 0; - jlong * valuePtr = (jlong*)env->GetDirectBufferAddress(value); - uint32_t * countPtr = (uint32_t*)env->GetDirectBufferAddress(count); + jlong *valuePtr = (jlong *)env->GetDirectBufferAddress(value); + uint32_t *countPtr = (uint32_t *)env->GetDirectBufferAddress(count); - getAccumulatorOutput((void*)id, valuePtr, countPtr, &status); + getAccumulatorOutput((void *)id, valuePtr, countPtr, &status); - ANALOGJNI_LOG(logDEBUG) << "Value = " << *valuePtr; - ANALOGJNI_LOG(logDEBUG) << "Count = " << *countPtr; - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); + ANALOGJNI_LOG(logDEBUG) << "Value = " << *valuePtr; + ANALOGJNI_LOG(logDEBUG) << "Count = " << *countPtr; + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -518,20 +520,21 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAccumulatorOu * Method: initializeAnalogTrigger * Signature: (JLjava/nio/IntBuffer;)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initializeAnalogTrigger - (JNIEnv * env, jclass, jlong id, jobject index) -{ - ANALOGJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initializeAnalogTrigger( + JNIEnv *env, jclass, jlong id, jobject index) { + ANALOGJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)id; - jint * indexPtr = (jint*)env->GetDirectBufferAddress(index); - ANALOGJNI_LOG(logDEBUG) << "Index Ptr = " << indexPtr; + jint *indexPtr = (jint *)env->GetDirectBufferAddress(index); + ANALOGJNI_LOG(logDEBUG) << "Index Ptr = " << indexPtr; - int32_t status = 0; - void* analogTrigger = initializeAnalogTrigger((void*)id, (uint32_t *)indexPtr, &status); - ANALOGJNI_LOG(logDEBUG) << "Status = " << status; - ANALOGJNI_LOG(logDEBUG) << "AnalogTrigger Ptr = " << analogTrigger; - CheckStatus(env, status); - return (jlong)analogTrigger; + int32_t status = 0; + void *analogTrigger = + initializeAnalogTrigger((void *)id, (uint32_t *)indexPtr, &status); + ANALOGJNI_LOG(logDEBUG) << "Status = " << status; + ANALOGJNI_LOG(logDEBUG) << "AnalogTrigger Ptr = " << analogTrigger; + CheckStatus(env, status); + return (jlong)analogTrigger; } /* @@ -539,14 +542,14 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_initializeAnalo * Method: cleanAnalogTrigger * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_cleanAnalogTrigger - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void*)id; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_cleanAnalogTrigger( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void *)id; - int32_t status = 0; - cleanAnalogTrigger((void*)id, &status); - CheckStatus(env, status); + int32_t status = 0; + cleanAnalogTrigger((void *)id, &status); + CheckStatus(env, status); } /* @@ -554,14 +557,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_cleanAnalogTrigg * Method: setAnalogTriggerLimitsRaw * Signature: (JII)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTriggerLimitsRaw - (JNIEnv * env, jclass, jlong id, jint lower, jint upper) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void*)id; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTriggerLimitsRaw( + JNIEnv *env, jclass, jlong id, jint lower, jint upper) { + ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void *)id; - int32_t status = 0; - setAnalogTriggerLimitsRaw((void*)id, lower, upper, &status); - CheckStatus(env, status); + int32_t status = 0; + setAnalogTriggerLimitsRaw((void *)id, lower, upper, &status); + CheckStatus(env, status); } /* @@ -569,14 +572,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTrigger * Method: setAnalogTriggerLimitsVoltage * Signature: (JDD)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTriggerLimitsVoltage - (JNIEnv * env, jclass, jlong id, jdouble lower, jdouble upper) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void*)id; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTriggerLimitsVoltage( + JNIEnv *env, jclass, jlong id, jdouble lower, jdouble upper) { + ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void *)id; - int32_t status = 0; - setAnalogTriggerLimitsVoltage((void*)id, lower, upper, &status); - CheckStatus(env, status); + int32_t status = 0; + setAnalogTriggerLimitsVoltage((void *)id, lower, upper, &status); + CheckStatus(env, status); } /* @@ -584,14 +587,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTrigger * Method: setAnalogTriggerAveraged * Signature: (JZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTriggerAveraged - (JNIEnv * env, jclass, jlong id, jboolean averaged) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void*)id; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTriggerAveraged( + JNIEnv *env, jclass, jlong id, jboolean averaged) { + ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void *)id; - int32_t status = 0; - setAnalogTriggerAveraged((void*)id, averaged, &status); - CheckStatus(env, status); + int32_t status = 0; + setAnalogTriggerAveraged((void *)id, averaged, &status); + CheckStatus(env, status); } /* @@ -599,14 +602,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTrigger * Method: setAnalogTriggerFiltered * Signature: (JZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTriggerFiltered - (JNIEnv * env, jclass, jlong id, jboolean filtered) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void*)id; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTriggerFiltered( + JNIEnv *env, jclass, jlong id, jboolean filtered) { + ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void *)id; - int32_t status = 0; - setAnalogTriggerFiltered((void*)id, filtered, &status); - CheckStatus(env, status); + int32_t status = 0; + setAnalogTriggerFiltered((void *)id, filtered, &status); + CheckStatus(env, status); } /* @@ -614,15 +617,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_setAnalogTrigger * Method: getAnalogTriggerInWindow * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogTriggerInWindow - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void*)id; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogTriggerInWindow( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void *)id; - int32_t status = 0; - jboolean val = getAnalogTriggerInWindow((void*)id, &status); - CheckStatus(env, status); - return val; + int32_t status = 0; + jboolean val = getAnalogTriggerInWindow((void *)id, &status); + CheckStatus(env, status); + return val; } /* @@ -630,15 +633,15 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogTri * Method: getAnalogTriggerTriggerState * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogTriggerTriggerState - (JNIEnv * env, jclass, jlong id) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void*)id; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogTriggerTriggerState( + JNIEnv *env, jclass, jlong id) { + ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void *)id; - int32_t status = 0; - jboolean val = getAnalogTriggerTriggerState((void*)id, &status); - CheckStatus(env, status); - return val; + int32_t status = 0; + jboolean val = getAnalogTriggerTriggerState((void *)id, &status); + CheckStatus(env, status); + return val; } /* @@ -646,15 +649,16 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogTri * Method: getAnalogTriggerOutput * Signature: (JI)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogTriggerOutput - (JNIEnv * env, jclass, jlong id, jint type) -{ - ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void*)id; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_AnalogJNI_getAnalogTriggerOutput( + JNIEnv *env, jclass, jlong id, jint type) { + ANALOGJNI_LOG(logDEBUG) << "Analog Trigger Ptr = " << (void *)id; - int32_t status = 0; - jboolean val = getAnalogTriggerOutput((void*)id, (AnalogTriggerType)type, &status); - CheckStatus(env, status); - return val; + int32_t status = 0; + jboolean val = + getAnalogTriggerOutput((void *)id, (AnalogTriggerType)type, &status); + CheckStatus(env, status); + return val; } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/CANJNI.cpp b/wpilibj/src/athena/cpp/lib/CANJNI.cpp index 935fae964b..1cb73d171c 100644 --- a/wpilibj/src/athena/cpp/lib/CANJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/CANJNI.cpp @@ -5,23 +5,25 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_can_CANJNI.h" -#include "HAL/CAN.hpp" #include "FRC_NetworkCommunication/CANSessionMux.h" +#include "HAL/CAN.hpp" #include "HALUtil.h" // set the logging level -//TLogLevel canJNILogLevel = logDEBUG; +// TLogLevel canJNILogLevel = logDEBUG; TLogLevel canJNILogLevel = logERROR; -#define CANJNI_LOG(level) \ - if (level > canJNILogLevel) ; \ - else Log().Get(level) +#define CANJNI_LOG(level) \ + if (level > canJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -30,42 +32,40 @@ extern "C" { * Method: FRCNetworkCommunicationCANSessionMuxSendMessage * Signature: (ILjava/nio/ByteBuffer;I)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_can_CANJNI_FRCNetworkCommunicationCANSessionMuxSendMessage - (JNIEnv * env, jclass, jint messageID, jobject data, jint periodMs) -{ - CANJNI_LOG(logDEBUG) << "Calling CANJNI FRCNetworkCommunicationCANSessionMuxSendMessage"; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_can_CANJNI_FRCNetworkCommunicationCANSessionMuxSendMessage( + JNIEnv *env, jclass, jint messageID, jobject data, jint periodMs) { + CANJNI_LOG(logDEBUG) + << "Calling CANJNI FRCNetworkCommunicationCANSessionMuxSendMessage"; - uint8_t *dataBuffer = (uint8_t *)(data? env->GetDirectBufferAddress(data) : 0); - uint8_t dataSize = (uint8_t)(data? env->GetDirectBufferCapacity(data) : 0); + uint8_t *dataBuffer = + (uint8_t *)(data ? env->GetDirectBufferAddress(data) : 0); + uint8_t dataSize = (uint8_t)(data ? env->GetDirectBufferCapacity(data) : 0); - CANJNI_LOG(logDEBUG) << "Message ID " << std::hex << messageID; + CANJNI_LOG(logDEBUG) << "Message ID " << std::hex << messageID; - if(logDEBUG <= canJNILogLevel) - { - if(dataBuffer) - { - std::ostringstream str; - str << std::setfill('0') << std::hex; - for(int i = 0; i < dataSize; i++) - { - str << std::setw(2) << (int)dataBuffer[i] << ' '; - } + if (logDEBUG <= canJNILogLevel) { + if (dataBuffer) { + std::ostringstream str; + str << std::setfill('0') << std::hex; + for (int i = 0; i < dataSize; i++) { + str << std::setw(2) << (int)dataBuffer[i] << ' '; + } - Log().Get(logDEBUG) << "Data: " << str.str(); - } - else - { - CANJNI_LOG(logDEBUG) << "Data: null"; - } + Log().Get(logDEBUG) << "Data: " << str.str(); + } else { + CANJNI_LOG(logDEBUG) << "Data: null"; } + } - CANJNI_LOG(logDEBUG) << "Period: " << periodMs; + CANJNI_LOG(logDEBUG) << "Period: " << periodMs; - int32_t status = 0; - FRC_NetworkCommunication_CANSessionMux_sendMessage(messageID, dataBuffer, dataSize, periodMs, &status); + int32_t status = 0; + FRC_NetworkCommunication_CANSessionMux_sendMessage( + messageID, dataBuffer, dataSize, periodMs, &status); - CANJNI_LOG(logDEBUG) << "Status: " << status; - CheckCANStatus(env, status, messageID); + CANJNI_LOG(logDEBUG) << "Status: " << status; + CheckCANStatus(env, status, messageID); } static uint8_t buffer[8]; @@ -75,38 +75,39 @@ static uint8_t buffer[8]; * Method: FRCNetworkCommunicationCANSessionMuxReceiveMessage * Signature: (Ljava/nio/IntBuffer;ILjava/nio/ByteBuffer;)Ljava/nio/ByteBuffer; */ -JNIEXPORT jobject JNICALL Java_edu_wpi_first_wpilibj_can_CANJNI_FRCNetworkCommunicationCANSessionMuxReceiveMessage - (JNIEnv * env, jclass, jobject messageID, jint messageIDMask, jobject timeStamp) -{ - CANJNI_LOG(logDEBUG) << "Calling CANJNI FRCNetworkCommunicationCANSessionMuxReceiveMessage"; +JNIEXPORT jobject JNICALL +Java_edu_wpi_first_wpilibj_can_CANJNI_FRCNetworkCommunicationCANSessionMuxReceiveMessage( + JNIEnv *env, jclass, jobject messageID, jint messageIDMask, + jobject timeStamp) { + CANJNI_LOG(logDEBUG) + << "Calling CANJNI FRCNetworkCommunicationCANSessionMuxReceiveMessage"; - uint32_t *messageIDPtr = (uint32_t *)env->GetDirectBufferAddress(messageID); - uint32_t *timeStampPtr = (uint32_t *)env->GetDirectBufferAddress(timeStamp); + uint32_t *messageIDPtr = (uint32_t *)env->GetDirectBufferAddress(messageID); + uint32_t *timeStampPtr = (uint32_t *)env->GetDirectBufferAddress(timeStamp); - uint8_t dataSize = 0; + uint8_t dataSize = 0; - int32_t status = 0; - FRC_NetworkCommunication_CANSessionMux_receiveMessage(messageIDPtr, messageIDMask, buffer, &dataSize, timeStampPtr, &status); + int32_t status = 0; + FRC_NetworkCommunication_CANSessionMux_receiveMessage( + messageIDPtr, messageIDMask, buffer, &dataSize, timeStampPtr, &status); - CANJNI_LOG(logDEBUG) << "Message ID " << std::hex << *messageIDPtr; + CANJNI_LOG(logDEBUG) << "Message ID " << std::hex << *messageIDPtr; - if(logDEBUG <= canJNILogLevel) - { - std::ostringstream str; - str << std::setfill('0') << std::hex; - for(int i = 0; i < dataSize; i++) - { - str << std::setw(2) << (int)buffer[i] << ' '; - } - - Log().Get(logDEBUG) << "Data: " << str.str(); + if (logDEBUG <= canJNILogLevel) { + std::ostringstream str; + str << std::setfill('0') << std::hex; + for (int i = 0; i < dataSize; i++) { + str << std::setw(2) << (int)buffer[i] << ' '; } - CANJNI_LOG(logDEBUG) << "Timestamp: " << *timeStampPtr; - CANJNI_LOG(logDEBUG) << "Status: " << status; + Log().Get(logDEBUG) << "Data: " << str.str(); + } - if (!CheckCANStatus(env, status, *messageIDPtr)) return nullptr; - return env->NewDirectByteBuffer(buffer, dataSize); + CANJNI_LOG(logDEBUG) << "Timestamp: " << *timeStampPtr; + CANJNI_LOG(logDEBUG) << "Status: " << status; + + if (!CheckCANStatus(env, status, *messageIDPtr)) return nullptr; + return env->NewDirectByteBuffer(buffer, dataSize); } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/CompressorJNI.cpp b/wpilibj/src/athena/cpp/lib/CompressorJNI.cpp index 583cad0889..2df574fb26 100644 --- a/wpilibj/src/athena/cpp/lib/CompressorJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/CompressorJNI.cpp @@ -1,14 +1,14 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#include "Log.hpp" -#include "edu_wpi_first_wpilibj_hal_CompressorJNI.h" +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + #include "HAL/HAL.hpp" #include "HALUtil.h" +#include "Log.hpp" +#include "edu_wpi_first_wpilibj_hal_CompressorJNI.h" extern "C" { @@ -17,11 +17,11 @@ extern "C" { * Method: initializeCompressor * Signature: (B)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_initializeCompressor - (JNIEnv *env, jclass, jbyte module) -{ - void* pcm = initializeCompressor(module); - return (jlong)pcm; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_initializeCompressor( + JNIEnv *env, jclass, jbyte module) { + void *pcm = initializeCompressor(module); + return (jlong)pcm; } /* @@ -29,25 +29,24 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_initializeC * Method: checkCompressorModule * Signature: (B)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_checkCompressorModule - (JNIEnv *env, jclass, jbyte module) -{ - return checkCompressorModule(module); +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_checkCompressorModule( + JNIEnv *env, jclass, jbyte module) { + return checkCompressorModule(module); } - /* * Class: edu_wpi_first_wpilibj_hal_CompressorJNI * Method: getCompressor * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressor - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - bool val = getCompressor((void*)pcm_pointer, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressor( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + bool val = getCompressor((void *)pcm_pointer, &status); + CheckStatus(env, status); + return val; } /* @@ -55,12 +54,12 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompr * Method: setClosedLoopControl * Signature: (JZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_setClosedLoopControl - (JNIEnv *env, jclass, jlong pcm_pointer, jboolean value) -{ - int32_t status = 0; - setClosedLoopControl((void*)pcm_pointer, value, &status); - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_setClosedLoopControl( + JNIEnv *env, jclass, jlong pcm_pointer, jboolean value) { + int32_t status = 0; + setClosedLoopControl((void *)pcm_pointer, value, &status); + CheckStatus(env, status); } /* @@ -68,13 +67,13 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_setClosedLoo * Method: getClosedLoopControl * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getClosedLoopControl - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - bool val = getClosedLoopControl((void*)pcm_pointer, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getClosedLoopControl( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + bool val = getClosedLoopControl((void *)pcm_pointer, &status); + CheckStatus(env, status); + return val; } /* @@ -82,13 +81,13 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getClose * Method: getPressureSwitch * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getPressureSwitch - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - bool val = getPressureSwitch((void*)pcm_pointer, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getPressureSwitch( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + bool val = getPressureSwitch((void *)pcm_pointer, &status); + CheckStatus(env, status); + return val; } /* @@ -96,13 +95,13 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getPress * Method: getCompressorCurrent * Signature: (J)F */ -JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorCurrent - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - float val = getCompressorCurrent((void*)pcm_pointer, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorCurrent( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + float val = getCompressorCurrent((void *)pcm_pointer, &status); + CheckStatus(env, status); + return val; } /* @@ -110,13 +109,13 @@ JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompres * Method: getCompressorCurrentTooHighFault * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorCurrentTooHighFault - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - bool val = getCompressorCurrentTooHighFault((void*)pcm_pointer, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorCurrentTooHighFault( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + bool val = getCompressorCurrentTooHighFault((void *)pcm_pointer, &status); + CheckStatus(env, status); + return val; } /* @@ -124,13 +123,14 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompr * Method: getCompressorCurrentTooHighStickyFault * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorCurrentTooHighStickyFault - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - bool val = getCompressorCurrentTooHighStickyFault((void*)pcm_pointer, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorCurrentTooHighStickyFault( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + bool val = + getCompressorCurrentTooHighStickyFault((void *)pcm_pointer, &status); + CheckStatus(env, status); + return val; } /* @@ -138,13 +138,13 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompr * Method: getCompressorShortedStickyFault * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorShortedStickyFault - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - bool val = getCompressorShortedStickyFault((void*)pcm_pointer, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorShortedStickyFault( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + bool val = getCompressorShortedStickyFault((void *)pcm_pointer, &status); + CheckStatus(env, status); + return val; } /* @@ -152,13 +152,13 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompr * Method: getCompressorShortedFault * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorShortedFault - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - bool val = getCompressorShortedFault((void*)pcm_pointer, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorShortedFault( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + bool val = getCompressorShortedFault((void *)pcm_pointer, &status); + CheckStatus(env, status); + return val; } /* @@ -166,13 +166,13 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompr * Method: getCompressorNotConnectedStickyFault * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorNotConnectedStickyFault - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - bool val = getCompressorNotConnectedStickyFault((void*)pcm_pointer, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorNotConnectedStickyFault( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + bool val = getCompressorNotConnectedStickyFault((void *)pcm_pointer, &status); + CheckStatus(env, status); + return val; } /* @@ -180,25 +180,25 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompr * Method: getCompressorNotConnectedFault * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorNotConnectedFault - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - bool val = getCompressorNotConnectedFault((void*)pcm_pointer, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_getCompressorNotConnectedFault( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + bool val = getCompressorNotConnectedFault((void *)pcm_pointer, &status); + CheckStatus(env, status); + return val; } /* * Class: edu_wpi_first_wpilibj_hal_CompressorJNI * Method: clearAllPCMStickyFaults * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CompressorJNI_clearAllPCMStickyFaults - (JNIEnv *env, jclass, jlong pcm_pointer) -{ - int32_t status = 0; - clearAllPCMStickyFaults((void*)pcm_pointer, &status); - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CompressorJNI_clearAllPCMStickyFaults( + JNIEnv *env, jclass, jlong pcm_pointer) { + int32_t status = 0; + clearAllPCMStickyFaults((void *)pcm_pointer, &status); + CheckStatus(env, status); } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/CounterJNI.cpp b/wpilibj/src/athena/cpp/lib/CounterJNI.cpp index 965a3d328e..ff278fb5c2 100644 --- a/wpilibj/src/athena/cpp/lib/CounterJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/CounterJNI.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_CounterJNI.h" @@ -18,9 +18,11 @@ // set the logging level TLogLevel counterJNILogLevel = logWARNING; -#define COUNTERJNI_LOG(level) \ - if (level > counterJNILogLevel) ; \ - else Log().Get(level) +#define COUNTERJNI_LOG(level) \ + if (level > counterJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -29,20 +31,20 @@ extern "C" { * Method: initializeCounter * Signature: (ILjava/nio/IntBuffer;)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_initializeCounter - (JNIEnv * env, jclass, jint mode, jobject index) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI initializeCounter"; - COUNTERJNI_LOG(logDEBUG) << "Mode = " << mode; - jint * indexPtr = (jint*)env->GetDirectBufferAddress(index); - COUNTERJNI_LOG(logDEBUG) << "Index Ptr = " << (uint32_t*)indexPtr; - int32_t status = 0; - void* counter = initializeCounter((Mode)mode, (uint32_t*)indexPtr, &status); - COUNTERJNI_LOG(logDEBUG) << "Index = " << *indexPtr; - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - COUNTERJNI_LOG(logDEBUG) << "COUNTER Ptr = " << counter; - CheckStatus(env, status); - return (jlong)counter; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_initializeCounter( + JNIEnv* env, jclass, jint mode, jobject index) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI initializeCounter"; + COUNTERJNI_LOG(logDEBUG) << "Mode = " << mode; + jint* indexPtr = (jint*)env->GetDirectBufferAddress(index); + COUNTERJNI_LOG(logDEBUG) << "Index Ptr = " << (uint32_t*)indexPtr; + int32_t status = 0; + void* counter = initializeCounter((Mode)mode, (uint32_t*)indexPtr, &status); + COUNTERJNI_LOG(logDEBUG) << "Index = " << *indexPtr; + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + COUNTERJNI_LOG(logDEBUG) << "COUNTER Ptr = " << counter; + CheckStatus(env, status); + return (jlong)counter; } /* @@ -50,15 +52,14 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_initializeCoun * Method: freeCounter * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_freeCounter - (JNIEnv * env, jclass, jlong id) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI freeCounter"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - freeCounter((void*)id, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_freeCounter( + JNIEnv* env, jclass, jlong id) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI freeCounter"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + freeCounter((void*)id, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -66,16 +67,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_freeCounter * Method: setCounterAverageSize * Signature: (JI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterAverageSize - (JNIEnv * env, jclass, jlong id, jint value) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterAverageSize"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "AverageSize = " << value; - int32_t status = 0; - setCounterAverageSize((void*)id, value, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterAverageSize( + JNIEnv* env, jclass, jlong id, jint value) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterAverageSize"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "AverageSize = " << value; + int32_t status = 0; + setCounterAverageSize((void*)id, value, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -83,17 +84,17 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterAvera * Method: setCounterUpSource * Signature: (JIZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpSource - (JNIEnv * env, jclass, jlong id, jint pin, jboolean analogTrigger) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterUpSource"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "Pin = " << pin; - COUNTERJNI_LOG(logDEBUG) << "AnalogTrigger = " << (jint)analogTrigger; - int32_t status = 0; - setCounterUpSource((void*)id, pin, analogTrigger, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpSource( + JNIEnv* env, jclass, jlong id, jint pin, jboolean analogTrigger) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterUpSource"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "Pin = " << pin; + COUNTERJNI_LOG(logDEBUG) << "AnalogTrigger = " << (jint)analogTrigger; + int32_t status = 0; + setCounterUpSource((void*)id, pin, analogTrigger, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -101,17 +102,17 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpSou * Method: setCounterUpSourceEdge * Signature: (JZZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpSourceEdge - (JNIEnv * env, jclass, jlong id, jboolean valueRise, jboolean valueFall) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterUpSourceEdge"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "Rise = " << (jint)valueRise; - COUNTERJNI_LOG(logDEBUG) << "Fall = " << (jint)valueFall; - int32_t status = 0; - setCounterUpSourceEdge((void*)id, valueRise, valueFall, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpSourceEdge( + JNIEnv* env, jclass, jlong id, jboolean valueRise, jboolean valueFall) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterUpSourceEdge"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "Rise = " << (jint)valueRise; + COUNTERJNI_LOG(logDEBUG) << "Fall = " << (jint)valueFall; + int32_t status = 0; + setCounterUpSourceEdge((void*)id, valueRise, valueFall, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -119,15 +120,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpSou * Method: clearCounterUpSource * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_clearCounterUpSource - (JNIEnv * env, jclass, jlong id) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI clearCounterUpSource"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - clearCounterUpSource((void*)id, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_clearCounterUpSource( + JNIEnv* env, jclass, jlong id) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI clearCounterUpSource"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + clearCounterUpSource((void*)id, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -135,21 +136,23 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_clearCounterUpS * Method: setCounterDownSource * Signature: (JIZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterDownSource - (JNIEnv * env, jclass, jlong id, jint pin, jboolean analogTrigger) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterDownSource"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "Pin = " << pin; - COUNTERJNI_LOG(logDEBUG) << "AnalogTrigger = " << (jint)analogTrigger; - int32_t status = 0; - setCounterDownSource((void*)id, pin, analogTrigger, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - if (status == PARAMETER_OUT_OF_RANGE) { - ThrowIllegalArgumentException(env, "Counter only supports DownSource in TwoPulse and ExternalDirection modes."); - return; - } - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterDownSource( + JNIEnv* env, jclass, jlong id, jint pin, jboolean analogTrigger) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterDownSource"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "Pin = " << pin; + COUNTERJNI_LOG(logDEBUG) << "AnalogTrigger = " << (jint)analogTrigger; + int32_t status = 0; + setCounterDownSource((void*)id, pin, analogTrigger, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + if (status == PARAMETER_OUT_OF_RANGE) { + ThrowIllegalArgumentException(env, + "Counter only supports DownSource in " + "TwoPulse and ExternalDirection modes."); + return; + } + CheckStatus(env, status); } /* @@ -157,17 +160,17 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterDownS * Method: setCounterDownSourceEdge * Signature: (JZZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterDownSourceEdge - (JNIEnv * env, jclass, jlong id, jboolean valueRise, jboolean valueFall) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterDownSourceEdge"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "Rise = " << (jint)valueRise; - COUNTERJNI_LOG(logDEBUG) << "Fall = " << (jint)valueFall; - int32_t status = 0; - setCounterDownSourceEdge((void*)id, valueRise, valueFall, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterDownSourceEdge( + JNIEnv* env, jclass, jlong id, jboolean valueRise, jboolean valueFall) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterDownSourceEdge"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "Rise = " << (jint)valueRise; + COUNTERJNI_LOG(logDEBUG) << "Fall = " << (jint)valueFall; + int32_t status = 0; + setCounterDownSourceEdge((void*)id, valueRise, valueFall, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -175,15 +178,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterDownS * Method: clearCounterDownSource * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_clearCounterDownSource - (JNIEnv * env, jclass, jlong id) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI clearCounterDownSource"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - clearCounterDownSource((void*)id, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_clearCounterDownSource( + JNIEnv* env, jclass, jlong id) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI clearCounterDownSource"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + clearCounterDownSource((void*)id, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -191,15 +194,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_clearCounterDow * Method: setCounterUpDownMode * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpDownMode - (JNIEnv * env, jclass, jlong id) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterUpDownMode"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - setCounterUpDownMode((void*)id, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpDownMode( + JNIEnv* env, jclass, jlong id) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterUpDownMode"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + setCounterUpDownMode((void*)id, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -207,15 +210,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpDow * Method: setCounterExternalDirectionMode * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterExternalDirectionMode - (JNIEnv * env, jclass, jlong id) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterExternalDirectionMode"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - setCounterExternalDirectionMode((void*)id, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterExternalDirectionMode( + JNIEnv* env, jclass, jlong id) { + COUNTERJNI_LOG(logDEBUG) + << "Calling COUNTERJNI setCounterExternalDirectionMode"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + setCounterExternalDirectionMode((void*)id, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -223,16 +227,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterExter * Method: setCounterSemiPeriodMode * Signature: (JZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterSemiPeriodMode - (JNIEnv * env, jclass, jlong id, jboolean value) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterSemiPeriodMode"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "SemiPeriodMode = " << (jint)value; - int32_t status = 0; - setCounterSemiPeriodMode((void*)id, value, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterSemiPeriodMode( + JNIEnv* env, jclass, jlong id, jboolean value) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterSemiPeriodMode"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "SemiPeriodMode = " << (jint)value; + int32_t status = 0; + setCounterSemiPeriodMode((void*)id, value, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -240,16 +244,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterSemiP * Method: setCounterPulseLengthMode * Signature: (JD)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterPulseLengthMode - (JNIEnv * env, jclass, jlong id, jdouble value) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterPulseLengthMode"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "PulseLengthMode = " << value; - int32_t status = 0; - setCounterPulseLengthMode((void*)id, value, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterPulseLengthMode( + JNIEnv* env, jclass, jlong id, jdouble value) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterPulseLengthMode"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "PulseLengthMode = " << value; + int32_t status = 0; + setCounterPulseLengthMode((void*)id, value, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -257,17 +261,18 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterPulse * Method: getCounterSamplesToAverage * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterSamplesToAverage - (JNIEnv * env, jclass, jlong id) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI getCounterSamplesToAverage"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - jint returnValue = getCounterSamplesToAverage((void*)id, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - COUNTERJNI_LOG(logDEBUG) << "getCounterSamplesToAverageResult = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterSamplesToAverage( + JNIEnv* env, jclass, jlong id) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI getCounterSamplesToAverage"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + jint returnValue = getCounterSamplesToAverage((void*)id, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + COUNTERJNI_LOG(logDEBUG) << "getCounterSamplesToAverageResult = " + << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -275,20 +280,20 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterSampl * Method: setCounterSamplesToAverage * Signature: (JI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterSamplesToAverage - (JNIEnv * env, jclass, jlong id, jint value) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterSamplesToAverage"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "SamplesToAverage = " << value; - int32_t status = 0; - setCounterSamplesToAverage((void*)id, value, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - if (status == PARAMETER_OUT_OF_RANGE) { - ThrowBoundaryException(env, value, 1, 127); - return; - } - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterSamplesToAverage( + JNIEnv* env, jclass, jlong id, jint value) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterSamplesToAverage"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "SamplesToAverage = " << value; + int32_t status = 0; + setCounterSamplesToAverage((void*)id, value, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + if (status == PARAMETER_OUT_OF_RANGE) { + ThrowBoundaryException(env, value, 1, 127); + return; + } + CheckStatus(env, status); } /* @@ -296,15 +301,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterSampl * Method: resetCounter * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_resetCounter - (JNIEnv * env, jclass, jlong id) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI resetCounter"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - resetCounter((void*)id, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_resetCounter( + JNIEnv* env, jclass, jlong id) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI resetCounter"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + resetCounter((void*)id, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -312,17 +316,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_resetCounter * Method: getCounter * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounter - (JNIEnv * env, jclass, jlong id) -{ - //COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI getCounter"; - //COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - jint returnValue = getCounter((void*)id, &status); - //COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - //COUNTERJNI_LOG(logDEBUG) << "getCounterResult = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounter( + JNIEnv* env, jclass, jlong id) { + // COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI getCounter"; + // COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + jint returnValue = getCounter((void*)id, &status); + // COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + // COUNTERJNI_LOG(logDEBUG) << "getCounterResult = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -330,17 +333,17 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounter * Method: getCounterPeriod * Signature: (J)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterPeriod - (JNIEnv *env, jclass, jlong id) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI getCounterPeriod"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - jdouble returnValue = getCounterPeriod((void*)id, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - COUNTERJNI_LOG(logDEBUG) << "getCounterPeriodResult = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterPeriod( + JNIEnv* env, jclass, jlong id) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI getCounterPeriod"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + jdouble returnValue = getCounterPeriod((void*)id, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + COUNTERJNI_LOG(logDEBUG) << "getCounterPeriodResult = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -348,16 +351,16 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterPe * Method: setCounterMaxPeriod * Signature: (JD)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterMaxPeriod - (JNIEnv * env, jclass, jlong id, jdouble value) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterMaxPeriod"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "MaxPeriod = " << value; - int32_t status = 0; - setCounterMaxPeriod((void*)id, value, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterMaxPeriod( + JNIEnv* env, jclass, jlong id, jdouble value) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterMaxPeriod"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "MaxPeriod = " << value; + int32_t status = 0; + setCounterMaxPeriod((void*)id, value, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -365,16 +368,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterMaxPe * Method: setCounterUpdateWhenEmpty * Signature: (JZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpdateWhenEmpty - (JNIEnv * env, jclass, jlong id, jboolean value) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterMaxPeriod"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "UpdateWhenEmpty = " << (jint)value; - int32_t status = 0; - setCounterUpdateWhenEmpty((void*)id, value, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpdateWhenEmpty( + JNIEnv* env, jclass, jlong id, jboolean value) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterMaxPeriod"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "UpdateWhenEmpty = " << (jint)value; + int32_t status = 0; + setCounterUpdateWhenEmpty((void*)id, value, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -382,17 +385,17 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterUpdat * Method: getCounterStopped * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterStopped - (JNIEnv * env, jclass, jlong id) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI getCounterStopped"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = getCounterStopped((void*)id, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - COUNTERJNI_LOG(logDEBUG) << "getCounterStoppedResult = " << (jint)returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterStopped( + JNIEnv* env, jclass, jlong id) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI getCounterStopped"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + jboolean returnValue = getCounterStopped((void*)id, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + COUNTERJNI_LOG(logDEBUG) << "getCounterStoppedResult = " << (jint)returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -400,17 +403,18 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterS * Method: getCounterDirection * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterDirection - (JNIEnv * env, jclass, jlong id) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI getCounterDirection"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = getCounterDirection((void*)id, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - COUNTERJNI_LOG(logDEBUG) << "getCounterDirectionResult = " << (jint)returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterDirection( + JNIEnv* env, jclass, jlong id) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI getCounterDirection"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + int32_t status = 0; + jboolean returnValue = getCounterDirection((void*)id, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + COUNTERJNI_LOG(logDEBUG) << "getCounterDirectionResult = " + << (jint)returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -418,16 +422,16 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_getCounterD * Method: setCounterReverseDirection * Signature: (JZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterReverseDirection - (JNIEnv * env, jclass, jlong id, jboolean value) -{ - COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterReverseDirection"; - COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; - COUNTERJNI_LOG(logDEBUG) << "ReverseDirection = " << (jint)value; - int32_t status = 0; - setCounterReverseDirection((void*)id, value, &status); - COUNTERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CounterJNI_setCounterReverseDirection( + JNIEnv* env, jclass, jlong id, jboolean value) { + COUNTERJNI_LOG(logDEBUG) << "Calling COUNTERJNI setCounterReverseDirection"; + COUNTERJNI_LOG(logDEBUG) << "Counter Ptr = " << (void*)id; + COUNTERJNI_LOG(logDEBUG) << "ReverseDirection = " << (jint)value; + int32_t status = 0; + setCounterReverseDirection((void*)id, value, &status); + COUNTERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/DIOJNI.cpp b/wpilibj/src/athena/cpp/lib/DIOJNI.cpp index c3996983cd..53674c4a1b 100644 --- a/wpilibj/src/athena/cpp/lib/DIOJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/DIOJNI.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_DIOJNI.h" @@ -17,9 +17,11 @@ // set the logging level TLogLevel dioJNILogLevel = logWARNING; -#define DIOJNI_LOG(level) \ - if (level > dioJNILogLevel) ; \ - else Log().Get(level) +#define DIOJNI_LOG(level) \ + if (level > dioJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -28,17 +30,17 @@ extern "C" { * Method: initializeDigitalPort * Signature: (J)J; */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_initializeDigitalPort - (JNIEnv * env, jclass, jlong id) -{ - DIOJNI_LOG(logDEBUG) << "Calling DIOJNI initializeDigitalPort"; - DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - void* dio = initializeDigitalPort((void*)id, &status); - DIOJNI_LOG(logDEBUG) << "Status = " << status; - DIOJNI_LOG(logDEBUG) << "DIO Ptr = " << dio; - CheckStatus(env, status); - return (jlong)dio; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_DIOJNI_initializeDigitalPort( + JNIEnv *env, jclass, jlong id) { + DIOJNI_LOG(logDEBUG) << "Calling DIOJNI initializeDigitalPort"; + DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)id; + int32_t status = 0; + void *dio = initializeDigitalPort((void *)id, &status); + DIOJNI_LOG(logDEBUG) << "Status = " << status; + DIOJNI_LOG(logDEBUG) << "DIO Ptr = " << dio; + CheckStatus(env, status); + return (jlong)dio; } /* @@ -46,12 +48,11 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_initializeDigitalP * Method: freeDigitalPort * Signature: (J)V; */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_freeDigitalPort -(JNIEnv * env, jclass, jlong id) -{ - DIOJNI_LOG(logDEBUG) << "Calling DIOJNI freeDigitalPort"; - DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - freeDigitalPort((void*)id); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_freeDigitalPort( + JNIEnv *env, jclass, jlong id) { + DIOJNI_LOG(logDEBUG) << "Calling DIOJNI freeDigitalPort"; + DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)id; + freeDigitalPort((void *)id); } /* @@ -59,17 +60,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_freeDigitalPort * Method: allocateDIO * Signature: (JZ)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_allocateDIO - (JNIEnv * env, jclass, jlong id, jboolean value) -{ - DIOJNI_LOG(logDEBUG) << "Calling DIOJNI allocateDIO"; - DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = allocateDIO((void*)id, value, &status); - DIOJNI_LOG(logDEBUG) << "Status = " << status; - DIOJNI_LOG(logDEBUG) << "allocateDIOResult = " << (jint)returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_allocateDIO( + JNIEnv *env, jclass, jlong id, jboolean value) { + DIOJNI_LOG(logDEBUG) << "Calling DIOJNI allocateDIO"; + DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)id; + int32_t status = 0; + jboolean returnValue = allocateDIO((void *)id, value, &status); + DIOJNI_LOG(logDEBUG) << "Status = " << status; + DIOJNI_LOG(logDEBUG) << "allocateDIOResult = " << (jint)returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -77,15 +77,14 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_allocateDIO * Method: freeDIO * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_freeDIO - (JNIEnv * env, jclass, jlong id) -{ - DIOJNI_LOG(logDEBUG) << "Calling DIOJNI freeDIO"; - DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - freeDIO((void*)id, &status); - DIOJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_DIOJNI_freeDIO(JNIEnv *env, jclass, jlong id) { + DIOJNI_LOG(logDEBUG) << "Calling DIOJNI freeDIO"; + DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)id; + int32_t status = 0; + freeDIO((void *)id, &status); + DIOJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -93,16 +92,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_freeDIO * Method: setDIO * Signature: (JS)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_setDIO - (JNIEnv *env, jclass, jlong id, jshort value) -{ - //DIOJNI_LOG(logDEBUG) << "Calling DIOJNI setDIO"; - //DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - //DIOJNI_LOG(logDEBUG) << "Value = " << value; - int32_t status = 0; - setDIO((void*)id, value, &status); - //DIOJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_setDIO( + JNIEnv *env, jclass, jlong id, jshort value) { + // DIOJNI_LOG(logDEBUG) << "Calling DIOJNI setDIO"; + // DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + // DIOJNI_LOG(logDEBUG) << "Value = " << value; + int32_t status = 0; + setDIO((void *)id, value, &status); + // DIOJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -110,18 +108,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_setDIO * Method: getDIO * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_getDIO - (JNIEnv * env, jclass, jlong id) -{ - //DIOJNI_LOG(logDEBUG) << "Calling DIOJNI getDIO"; - //DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = getDIO((void*)id, &status); - //DIOJNI_LOG(logDEBUG) << "Status = " << status; - //DIOJNI_LOG(logDEBUG) << "getDIOResult = " << (jint)returnValue; - CheckStatus(env, status); - return returnValue; - +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_DIOJNI_getDIO(JNIEnv *env, jclass, jlong id) { + // DIOJNI_LOG(logDEBUG) << "Calling DIOJNI getDIO"; + // DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + int32_t status = 0; + jboolean returnValue = getDIO((void *)id, &status); + // DIOJNI_LOG(logDEBUG) << "Status = " << status; + // DIOJNI_LOG(logDEBUG) << "getDIOResult = " << (jint)returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -129,17 +125,17 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_getDIO * Method: getDIODirection * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_getDIODirection - (JNIEnv *env, jclass, jlong id) -{ - DIOJNI_LOG(logDEBUG) << "Calling DIOJNI getDIODirection (RR upd)"; - //DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = getDIODirection((void*)id, &status); - //DIOJNI_LOG(logDEBUG) << "Status = " << status; - DIOJNI_LOG(logDEBUG) << "getDIODirectionResult = " << (jint)returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_DIOJNI_getDIODirection( + JNIEnv *env, jclass, jlong id) { + DIOJNI_LOG(logDEBUG) << "Calling DIOJNI getDIODirection (RR upd)"; + // DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + int32_t status = 0; + jboolean returnValue = getDIODirection((void *)id, &status); + // DIOJNI_LOG(logDEBUG) << "Status = " << status; + DIOJNI_LOG(logDEBUG) << "getDIODirectionResult = " << (jint)returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -147,16 +143,15 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_getDIODirection * Method: pulse * Signature: (JD)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_pulse - (JNIEnv *env, jclass, jlong id, jdouble value) -{ - DIOJNI_LOG(logDEBUG) << "Calling DIOJNI pulse (RR upd)"; - //DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - //DIOJNI_LOG(logDEBUG) << "Value = " << value; - int32_t status = 0; - pulse((void*)id, value, &status); - DIOJNI_LOG(logDEBUG) << "Did it work? Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_pulse( + JNIEnv *env, jclass, jlong id, jdouble value) { + DIOJNI_LOG(logDEBUG) << "Calling DIOJNI pulse (RR upd)"; + // DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + // DIOJNI_LOG(logDEBUG) << "Value = " << value; + int32_t status = 0; + pulse((void *)id, value, &status); + DIOJNI_LOG(logDEBUG) << "Did it work? Status = " << status; + CheckStatus(env, status); } /* @@ -164,19 +159,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_pulse * Method: isPulsing * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_isPulsing - (JNIEnv *env, jclass, jlong id) -{ - DIOJNI_LOG(logDEBUG) << "Calling DIOJNI isPulsing (RR upd)"; - //DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = isPulsing((void*)id, &status); - //DIOJNI_LOG(logDEBUG) << "Status = " << status; - DIOJNI_LOG(logDEBUG) << "isPulsingResult = " << (jint)returnValue; - CheckStatus(env, status); - return returnValue; - - +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_DIOJNI_isPulsing(JNIEnv *env, jclass, jlong id) { + DIOJNI_LOG(logDEBUG) << "Calling DIOJNI isPulsing (RR upd)"; + // DIOJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + int32_t status = 0; + jboolean returnValue = isPulsing((void *)id, &status); + // DIOJNI_LOG(logDEBUG) << "Status = " << status; + DIOJNI_LOG(logDEBUG) << "isPulsingResult = " << (jint)returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -184,16 +176,15 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_isPulsing * Method: isAnyPulsing * Signature: ()Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_isAnyPulsing - (JNIEnv *env, jclass) -{ - DIOJNI_LOG(logDEBUG) << "Calling DIOJNI isAnyPulsing (RR upd)"; - int32_t status = 0; - jboolean returnValue = isAnyPulsing( &status ); - //DIOJNI_LOG(logDEBUG) << "Status = " << status; - DIOJNI_LOG(logDEBUG) << "isAnyPulsingResult = " << (jint)returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_DIOJNI_isAnyPulsing(JNIEnv *env, jclass) { + DIOJNI_LOG(logDEBUG) << "Calling DIOJNI isAnyPulsing (RR upd)"; + int32_t status = 0; + jboolean returnValue = isAnyPulsing(&status); + // DIOJNI_LOG(logDEBUG) << "Status = " << status; + DIOJNI_LOG(logDEBUG) << "isAnyPulsingResult = " << (jint)returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -201,17 +192,15 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_isAnyPulsing * Method: getLoopTiming * Signature: ()S */ -JNIEXPORT jshort JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_getLoopTiming - (JNIEnv * env, jclass) -{ - DIOJNI_LOG(logDEBUG) << "Calling DIOJNI getLoopTimeing"; - int32_t status = 0; - jshort returnValue = getLoopTiming( &status ); - DIOJNI_LOG(logDEBUG) << "Status = " << status; - DIOJNI_LOG(logDEBUG) << "LoopTiming = " << returnValue; - CheckStatus(env, status); - return returnValue; - +JNIEXPORT jshort JNICALL +Java_edu_wpi_first_wpilibj_hal_DIOJNI_getLoopTiming(JNIEnv *env, jclass) { + DIOJNI_LOG(logDEBUG) << "Calling DIOJNI getLoopTimeing"; + int32_t status = 0; + jshort returnValue = getLoopTiming(&status); + DIOJNI_LOG(logDEBUG) << "Status = " << status; + DIOJNI_LOG(logDEBUG) << "LoopTiming = " << returnValue; + CheckStatus(env, status); + return returnValue; } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/DigitalGlitchFilterJNI.cpp b/wpilibj/src/athena/cpp/lib/DigitalGlitchFilterJNI.cpp index b1bd50d493..a6e2f9232c 100644 --- a/wpilibj/src/athena/cpp/lib/DigitalGlitchFilterJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/DigitalGlitchFilterJNI.cpp @@ -15,9 +15,9 @@ * Class: edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI * Method: setFilterSelect */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_setFilterSelect - (JNIEnv* env, jclass, jlong port_pointer, jint filter_index) -{ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_setFilterSelect( + JNIEnv* env, jclass, jlong port_pointer, jint filter_index) { int32_t status = 0; void* digital_port_pointer = reinterpret_cast(port_pointer); @@ -29,9 +29,9 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_set * Class: edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI * Method: getFilterSelect */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_getFilterSelect - (JNIEnv *env, jclass, jlong port_pointer) -{ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_getFilterSelect( + JNIEnv* env, jclass, jlong port_pointer) { int32_t status = 0; void* digital_port_pointer = reinterpret_cast(port_pointer); @@ -44,9 +44,9 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_get * Class: edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI * Method: setFilterPeriod */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_setFilterPeriod - (JNIEnv *env, jclass, jint filter_index, jint fpga_cycles) -{ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_setFilterPeriod( + JNIEnv* env, jclass, jint filter_index, jint fpga_cycles) { int32_t status = 0; setFilterPeriod(filter_index, fpga_cycles, &status); @@ -57,9 +57,9 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_set * Class: edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI * Method: getFilterPeriod */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_getFilterPeriod - (JNIEnv *env, jclass, jint filter_index) -{ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_DigitalGlitchFilterJNI_getFilterPeriod( + JNIEnv* env, jclass, jint filter_index) { int32_t status = 0; jint result = getFilterPeriod(filter_index, &status); diff --git a/wpilibj/src/athena/cpp/lib/EncoderJNI.cpp b/wpilibj/src/athena/cpp/lib/EncoderJNI.cpp index 0970a9409b..5cd5d58da7 100644 --- a/wpilibj/src/athena/cpp/lib/EncoderJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/EncoderJNI.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_EncoderJNI.h" @@ -18,9 +18,11 @@ // set the logging level TLogLevel encoderJNILogLevel = logWARNING; -#define ENCODERJNI_LOG(level) \ - if (level > encoderJNILogLevel) ; \ - else Log().Get(level) +#define ENCODERJNI_LOG(level) \ + if (level > encoderJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -29,29 +31,33 @@ extern "C" { * Method: initializeEncoder * Signature: (BIZBIZZLjava/nio/IntBuffer;)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_initializeEncoder - (JNIEnv * env, jclass, jbyte port_a_module, jint port_a_pin, jboolean port_a_analog_trigger, jbyte port_b_module, jint port_b_pin, jboolean port_b_analog_trigger, jboolean reverseDirection, jobject index) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI initializeEncoder"; - ENCODERJNI_LOG(logDEBUG) << "Module A = " << (jint)port_a_module; - ENCODERJNI_LOG(logDEBUG) << "Pin A = " << port_a_pin; - ENCODERJNI_LOG(logDEBUG) << "Analog Trigger A = " << (jint)port_a_analog_trigger; - ENCODERJNI_LOG(logDEBUG) << "Module B = " << (jint)port_b_module; - ENCODERJNI_LOG(logDEBUG) << "Pin B = " << port_b_pin; - ENCODERJNI_LOG(logDEBUG) << "Analog Trigger B = " << (jint)port_b_analog_trigger; - ENCODERJNI_LOG(logDEBUG) << "Reverse direction = " << (jint)reverseDirection; - jint * indexPtr = (jint*)env->GetDirectBufferAddress(index); - ENCODERJNI_LOG(logDEBUG) << "Index Ptr = " << indexPtr; - int32_t status = 0; - void* encoder = initializeEncoder(port_a_module, port_a_pin, port_a_analog_trigger, - port_b_module, port_b_pin, port_b_analog_trigger, - reverseDirection, indexPtr, &status); +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_EncoderJNI_initializeEncoder( + JNIEnv* env, jclass, jbyte port_a_module, jint port_a_pin, + jboolean port_a_analog_trigger, jbyte port_b_module, jint port_b_pin, + jboolean port_b_analog_trigger, jboolean reverseDirection, jobject index) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI initializeEncoder"; + ENCODERJNI_LOG(logDEBUG) << "Module A = " << (jint)port_a_module; + ENCODERJNI_LOG(logDEBUG) << "Pin A = " << port_a_pin; + ENCODERJNI_LOG(logDEBUG) << "Analog Trigger A = " + << (jint)port_a_analog_trigger; + ENCODERJNI_LOG(logDEBUG) << "Module B = " << (jint)port_b_module; + ENCODERJNI_LOG(logDEBUG) << "Pin B = " << port_b_pin; + ENCODERJNI_LOG(logDEBUG) << "Analog Trigger B = " + << (jint)port_b_analog_trigger; + ENCODERJNI_LOG(logDEBUG) << "Reverse direction = " << (jint)reverseDirection; + jint* indexPtr = (jint*)env->GetDirectBufferAddress(index); + ENCODERJNI_LOG(logDEBUG) << "Index Ptr = " << indexPtr; + int32_t status = 0; + void* encoder = initializeEncoder( + port_a_module, port_a_pin, port_a_analog_trigger, port_b_module, + port_b_pin, port_b_analog_trigger, reverseDirection, indexPtr, &status); - ENCODERJNI_LOG(logDEBUG) << "Index = " << *indexPtr; - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - ENCODERJNI_LOG(logDEBUG) << "ENCODER Ptr = " << encoder; - CheckStatus(env, status); - return (jlong)encoder; + ENCODERJNI_LOG(logDEBUG) << "Index = " << *indexPtr; + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + ENCODERJNI_LOG(logDEBUG) << "ENCODER Ptr = " << encoder; + CheckStatus(env, status); + return (jlong)encoder; } /* @@ -59,15 +65,14 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_initializeEnco * Method: freeEncoder * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_freeEncoder - (JNIEnv * env, jclass, jlong id) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI freeEncoder"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - int32_t status = 0; - freeEncoder((void*)id, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_freeEncoder( + JNIEnv* env, jclass, jlong id) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI freeEncoder"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + int32_t status = 0; + freeEncoder((void*)id, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -75,15 +80,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_freeEncoder * Method: resetEncoder * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_resetEncoder - (JNIEnv * env, jclass, jlong id) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI resetEncoder"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - int32_t status = 0; - resetEncoder((void*)id, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_resetEncoder( + JNIEnv* env, jclass, jlong id) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI resetEncoder"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + int32_t status = 0; + resetEncoder((void*)id, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -91,17 +95,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_resetEncoder * Method: getEncoder * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoder - (JNIEnv * env, jclass, jlong id) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoder"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - int32_t status = 0; - jint returnValue = getEncoder((void*)id, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - ENCODERJNI_LOG(logDEBUG) << "getEncoderResult = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoder( + JNIEnv* env, jclass, jlong id) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoder"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + int32_t status = 0; + jint returnValue = getEncoder((void*)id, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + ENCODERJNI_LOG(logDEBUG) << "getEncoderResult = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -109,17 +112,17 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoder * Method: getEncoderPeriod * Signature: (J)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderPeriod - (JNIEnv * env, jclass, jlong id) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderPeriod"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - int32_t status = 0; - double returnValue = getEncoderPeriod((void*)id, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - ENCODERJNI_LOG(logDEBUG) << "getEncoderPeriodResult = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderPeriod( + JNIEnv* env, jclass, jlong id) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderPeriod"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + int32_t status = 0; + double returnValue = getEncoderPeriod((void*)id, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + ENCODERJNI_LOG(logDEBUG) << "getEncoderPeriodResult = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -127,15 +130,15 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderPe * Method: setEncoderMaxPeriod * Signature: (JD)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderMaxPeriod - (JNIEnv * env, jclass, jlong id, jdouble value) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderMaxPeriod"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - int32_t status = 0; - setEncoderMaxPeriod((void*)id, value, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderMaxPeriod( + JNIEnv* env, jclass, jlong id, jdouble value) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderMaxPeriod"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + int32_t status = 0; + setEncoderMaxPeriod((void*)id, value, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -143,17 +146,17 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderMaxPe * Method: getEncoderStopped * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderStopped - (JNIEnv * env, jclass, jlong id) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderStopped"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = getEncoderStopped((void*)id, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - ENCODERJNI_LOG(logDEBUG) << "getEncoderStoppedResult = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderStopped( + JNIEnv* env, jclass, jlong id) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderStopped"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + int32_t status = 0; + jboolean returnValue = getEncoderStopped((void*)id, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + ENCODERJNI_LOG(logDEBUG) << "getEncoderStoppedResult = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -161,17 +164,17 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderS * Method: getEncoderDirection * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderDirection - (JNIEnv * env, jclass, jlong id) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderDirection"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = getEncoderDirection((void*)id, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - ENCODERJNI_LOG(logDEBUG) << "getEncoderDirectionResult = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderDirection( + JNIEnv* env, jclass, jlong id) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderDirection"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + int32_t status = 0; + jboolean returnValue = getEncoderDirection((void*)id, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + ENCODERJNI_LOG(logDEBUG) << "getEncoderDirectionResult = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -179,15 +182,15 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderD * Method: setEncoderReverseDirection * Signature: (JZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderReverseDirection - (JNIEnv * env, jclass, jlong id, jboolean value) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderReverseDirection"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - int32_t status = 0; - setEncoderReverseDirection((void*)id, value, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderReverseDirection( + JNIEnv* env, jclass, jlong id, jboolean value) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderReverseDirection"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + int32_t status = 0; + setEncoderReverseDirection((void*)id, value, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -195,19 +198,19 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderRever * Method: setEncoderSamplesToAverage * Signature: (JI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderSamplesToAverage - (JNIEnv * env, jclass, jlong id, jint value) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderSamplesToAverage"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - int32_t status = 0; - setEncoderSamplesToAverage((void*)id, value, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - if (status == PARAMETER_OUT_OF_RANGE) { - ThrowBoundaryException(env, value, 1, 127); - return; - } - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderSamplesToAverage( + JNIEnv* env, jclass, jlong id, jint value) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderSamplesToAverage"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + int32_t status = 0; + setEncoderSamplesToAverage((void*)id, value, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + if (status == PARAMETER_OUT_OF_RANGE) { + ThrowBoundaryException(env, value, 1, 127); + return; + } + CheckStatus(env, status); } /* @@ -215,17 +218,18 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderSampl * Method: getEncoderSamplesToAverage * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderSamplesToAverage - (JNIEnv * env, jclass, jlong id) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderSamplesToAverage"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - int32_t status = 0; - jint returnValue = getEncoderSamplesToAverage((void*)id, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - ENCODERJNI_LOG(logDEBUG) << "getEncoderSamplesToAverageResult = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderSamplesToAverage( + JNIEnv* env, jclass, jlong id) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderSamplesToAverage"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + int32_t status = 0; + jint returnValue = getEncoderSamplesToAverage((void*)id, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + ENCODERJNI_LOG(logDEBUG) << "getEncoderSamplesToAverageResult = " + << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -233,19 +237,24 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderSampl * Method: setEncoderIndexSource * Signature: (JIZZZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderIndexSource - (JNIEnv * env, jclass, jlong id, jint pin, jboolean analogTrigger, jboolean activeHigh, jboolean edgeSensitive) -{ - ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderIndexSource"; - ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; - ENCODERJNI_LOG(logDEBUG) << "Pin = " << pin; - ENCODERJNI_LOG(logDEBUG) << "Analog Trigger = " << (analogTrigger?"true":"false"); - ENCODERJNI_LOG(logDEBUG) << "Active High = " << (activeHigh?"true":"false"); - ENCODERJNI_LOG(logDEBUG) << "Edge Sensitive = " << (edgeSensitive?"true":"false"); - int32_t status = 0; - setEncoderIndexSource((void*)id, pin, analogTrigger, activeHigh, edgeSensitive, &status); - ENCODERJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderIndexSource( + JNIEnv* env, jclass, jlong id, jint pin, jboolean analogTrigger, + jboolean activeHigh, jboolean edgeSensitive) { + ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderIndexSource"; + ENCODERJNI_LOG(logDEBUG) << "Encoder Ptr = " << (void*)id; + ENCODERJNI_LOG(logDEBUG) << "Pin = " << pin; + ENCODERJNI_LOG(logDEBUG) << "Analog Trigger = " + << (analogTrigger ? "true" : "false"); + ENCODERJNI_LOG(logDEBUG) << "Active High = " + << (activeHigh ? "true" : "false"); + ENCODERJNI_LOG(logDEBUG) << "Edge Sensitive = " + << (edgeSensitive ? "true" : "false"); + int32_t status = 0; + setEncoderIndexSource((void*)id, pin, analogTrigger, activeHigh, + edgeSensitive, &status); + ENCODERJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/FRCNetworkCommunicationsLibrary.cpp b/wpilibj/src/athena/cpp/lib/FRCNetworkCommunicationsLibrary.cpp index 3219254c75..6e3bb0cf6f 100644 --- a/wpilibj/src/athena/cpp/lib/FRCNetworkCommunicationsLibrary.cpp +++ b/wpilibj/src/athena/cpp/lib/FRCNetworkCommunicationsLibrary.cpp @@ -5,12 +5,12 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" -#include "edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary.h" #include "HAL/HAL.hpp" +#include "edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary.h" //#include "NetworkCommunication/FRCComm.h" //#include "NetworkCommunication/UsageReporting.h" #include "HALUtil.h" @@ -18,330 +18,342 @@ // set the logging level TLogLevel netCommLogLevel = logWARNING; -#define NETCOMM_LOG(level) \ - if (level > netCommLogLevel) ; \ - else Log().Get(level) +#define NETCOMM_LOG(level) \ + if (level > netCommLogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { /* - * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary * Method: FRC_NetworkCommunication_nUsageReporting_report * Signature: (BBBLjava/lang/String;)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationUsageReportingReport - (JNIEnv * paramEnv, jclass, jbyte paramResource, jbyte paramInstanceNumber, jbyte paramContext, jstring paramFeature) -{ - const char * featureStr = paramEnv->GetStringUTFChars(paramFeature, NULL); - NETCOMM_LOG(logDEBUG) << "Calling FRCNetworkCommunicationsLibrary report " << "res:"<< (unsigned int)paramResource << " instance:" << (unsigned int)paramInstanceNumber << " context:" << (unsigned int)paramContext << " feature:" << featureStr; - jint returnValue = HALReport(paramResource, paramInstanceNumber, paramContext, featureStr); - paramEnv->ReleaseStringUTFChars(paramFeature,featureStr); - return returnValue; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationUsageReportingReport( + JNIEnv *paramEnv, jclass, jbyte paramResource, jbyte paramInstanceNumber, + jbyte paramContext, jstring paramFeature) { + const char *featureStr = paramEnv->GetStringUTFChars(paramFeature, NULL); + NETCOMM_LOG(logDEBUG) << "Calling FRCNetworkCommunicationsLibrary report " + << "res:" << (unsigned int)paramResource + << " instance:" << (unsigned int)paramInstanceNumber + << " context:" << (unsigned int)paramContext + << " feature:" << featureStr; + jint returnValue = + HALReport(paramResource, paramInstanceNumber, paramContext, featureStr); + paramEnv->ReleaseStringUTFChars(paramFeature, featureStr); + return returnValue; } /* - * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary * Method: FRC_NetworkCommunication_observeUserProgramStarting * Signature: ()V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationObserveUserProgramStarting - (JNIEnv *, jclass) -{ - HALNetworkCommunicationObserveUserProgramStarting(); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationObserveUserProgramStarting( + JNIEnv *, jclass) { + HALNetworkCommunicationObserveUserProgramStarting(); } /* - * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary * Method: FRC_NetworkCommunication_observeUserProgramDisabled * Signature: ()V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationObserveUserProgramDisabled - (JNIEnv *, jclass) -{ - HALNetworkCommunicationObserveUserProgramDisabled(); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationObserveUserProgramDisabled( + JNIEnv *, jclass) { + HALNetworkCommunicationObserveUserProgramDisabled(); } /* - * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary * Method: FRC_NetworkCommunication_observeUserProgramAutonomous * Signature: ()V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationObserveUserProgramAutonomous - (JNIEnv *, jclass) -{ - HALNetworkCommunicationObserveUserProgramAutonomous(); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationObserveUserProgramAutonomous( + JNIEnv *, jclass) { + HALNetworkCommunicationObserveUserProgramAutonomous(); } /* - * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary * Method: FRC_NetworkCommunication_observeUserProgramTeleop * Signature: ()V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationObserveUserProgramTeleop - (JNIEnv *, jclass) -{ - HALNetworkCommunicationObserveUserProgramTeleop(); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationObserveUserProgramTeleop( + JNIEnv *, jclass) { + HALNetworkCommunicationObserveUserProgramTeleop(); } /* - * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRC_NetworkCommunicationsLibrary * Method: FRC_NetworkCommunication_observeUserProgramTest * Signature: ()V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationObserveUserProgramTest - (JNIEnv *, jclass) -{ - HALNetworkCommunicationObserveUserProgramTest(); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationObserveUserProgramTest( + JNIEnv *, jclass) { + HALNetworkCommunicationObserveUserProgramTest(); } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: FRCNetworkCommunicationReserve * Signature: ()V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationReserve - (JNIEnv *, jclass) -{ - int rv = HALInitialize(0); - assert(1 == rv); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_FRCNetworkCommunicationReserve( + JNIEnv *, jclass) { + int rv = HALInitialize(0); + assert(1 == rv); } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: NativeHALGetControlWord * Signature: ()I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_NativeHALGetControlWord - (JNIEnv *, jclass) -{ - NETCOMM_LOG(logDEBUG) << "Calling HAL Control Word"; - jint controlWord; - HALGetControlWord((HALControlWord*)&controlWord); - return controlWord; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_NativeHALGetControlWord( + JNIEnv *, jclass) { + NETCOMM_LOG(logDEBUG) << "Calling HAL Control Word"; + jint controlWord; + HALGetControlWord((HALControlWord *)&controlWord); + return controlWord; } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: NativeHALGetAllianceStation * Signature: ()I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_NativeHALGetAllianceStation - (JNIEnv *, jclass) -{ - NETCOMM_LOG(logDEBUG) << "Calling HAL Alliance Station"; - jint allianceStation; - HALGetAllianceStation((HALAllianceStationID*)&allianceStation); - return allianceStation; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_NativeHALGetAllianceStation( + JNIEnv *, jclass) { + NETCOMM_LOG(logDEBUG) << "Calling HAL Alliance Station"; + jint allianceStation; + HALGetAllianceStation((HALAllianceStationID *)&allianceStation); + return allianceStation; } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALGetJoystickAxes * Signature: (B)[S */ -JNIEXPORT jshortArray JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickAxes - (JNIEnv * env, jclass, jbyte joystickNum) -{ - NETCOMM_LOG(logDEBUG) << "Calling HALJoystickAxes"; - HALJoystickAxes axes; - HALGetJoystickAxes(joystickNum, &axes); +JNIEXPORT jshortArray JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickAxes( + JNIEnv *env, jclass, jbyte joystickNum) { + NETCOMM_LOG(logDEBUG) << "Calling HALJoystickAxes"; + HALJoystickAxes axes; + HALGetJoystickAxes(joystickNum, &axes); - jshortArray axesArray = env->NewShortArray(axes.count); - env->SetShortArrayRegion(axesArray, 0, axes.count, axes.axes); + jshortArray axesArray = env->NewShortArray(axes.count); + env->SetShortArrayRegion(axesArray, 0, axes.count, axes.axes); - return axesArray; + return axesArray; } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALGetJoystickPOVs * Signature: (B)[S */ -JNIEXPORT jshortArray JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickPOVs - (JNIEnv * env, jclass, jbyte joystickNum) -{ - NETCOMM_LOG(logDEBUG) << "Calling HALJoystickPOVs"; - HALJoystickPOVs povs; - HALGetJoystickPOVs(joystickNum, &povs); +JNIEXPORT jshortArray JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickPOVs( + JNIEnv *env, jclass, jbyte joystickNum) { + NETCOMM_LOG(logDEBUG) << "Calling HALJoystickPOVs"; + HALJoystickPOVs povs; + HALGetJoystickPOVs(joystickNum, &povs); - jshortArray povsArray = env->NewShortArray(povs.count); - env->SetShortArrayRegion(povsArray, 0, povs.count, povs.povs); + jshortArray povsArray = env->NewShortArray(povs.count); + env->SetShortArrayRegion(povsArray, 0, povs.count, povs.povs); - return povsArray; + return povsArray; } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALGetJoystickButtons * Signature: (B)S */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickButtons - (JNIEnv * env, jclass, jbyte joystickNum, jobject count) -{ - NETCOMM_LOG(logDEBUG) << "Calling HALJoystickButtons"; - HALJoystickButtons joystickButtons; - HALGetJoystickButtons(joystickNum, &joystickButtons); - jbyte *countPtr = (jbyte*)env->GetDirectBufferAddress(count); - NETCOMM_LOG(logDEBUG) << "Buttons = " << joystickButtons.buttons; - NETCOMM_LOG(logDEBUG) << "Count = " << (jint)joystickButtons.count; - *countPtr = joystickButtons.count; - NETCOMM_LOG(logDEBUG) << "CountBuffer = " << (jint)*countPtr; - return joystickButtons.buttons; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickButtons( + JNIEnv *env, jclass, jbyte joystickNum, jobject count) { + NETCOMM_LOG(logDEBUG) << "Calling HALJoystickButtons"; + HALJoystickButtons joystickButtons; + HALGetJoystickButtons(joystickNum, &joystickButtons); + jbyte *countPtr = (jbyte *)env->GetDirectBufferAddress(count); + NETCOMM_LOG(logDEBUG) << "Buttons = " << joystickButtons.buttons; + NETCOMM_LOG(logDEBUG) << "Count = " << (jint)joystickButtons.count; + *countPtr = joystickButtons.count; + NETCOMM_LOG(logDEBUG) << "CountBuffer = " << (jint)*countPtr; + return joystickButtons.buttons; } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALSetJoystickOutputs * Signature: (BISS)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALSetJoystickOutputs - (JNIEnv *, jclass, jbyte port, jint outputs, jshort leftRumble, jshort rightRumble) -{ - NETCOMM_LOG(logDEBUG) << "Calling HALSetJoystickOutputs on port " << port; - NETCOMM_LOG(logDEBUG) << "Outputs: " << outputs; - NETCOMM_LOG(logDEBUG) << "Left Rumble: " << leftRumble << " Right Rumble: " << rightRumble; - return HALSetJoystickOutputs(port, outputs, leftRumble, rightRumble); +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALSetJoystickOutputs( + JNIEnv *, jclass, jbyte port, jint outputs, jshort leftRumble, + jshort rightRumble) { + NETCOMM_LOG(logDEBUG) << "Calling HALSetJoystickOutputs on port " << port; + NETCOMM_LOG(logDEBUG) << "Outputs: " << outputs; + NETCOMM_LOG(logDEBUG) << "Left Rumble: " << leftRumble + << " Right Rumble: " << rightRumble; + return HALSetJoystickOutputs(port, outputs, leftRumble, rightRumble); } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALGetJoystickIsXbox * Signature: (B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickIsXbox - (JNIEnv *, jclass, jbyte port) - { - NETCOMM_LOG(logDEBUG) << "Calling HALGetJoystickIsXbox"; - return HALGetJoystickIsXbox(port); - } +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickIsXbox( + JNIEnv *, jclass, jbyte port) { + NETCOMM_LOG(logDEBUG) << "Calling HALGetJoystickIsXbox"; + return HALGetJoystickIsXbox(port); +} /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALGetJoystickType * Signature: (B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickType - (JNIEnv *, jclass, jbyte port) - { - NETCOMM_LOG(logDEBUG) << "Calling HALGetJoystickType"; - return HALGetJoystickType(port); - } +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickType( + JNIEnv *, jclass, jbyte port) { + NETCOMM_LOG(logDEBUG) << "Calling HALGetJoystickType"; + return HALGetJoystickType(port); +} /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALGetJoystickName * Signature: (B)Ljava/lang/String; */ -JNIEXPORT jstring JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickName - (JNIEnv * env, jclass, jbyte port) - { - NETCOMM_LOG(logDEBUG) << "Calling HALGetJoystickName"; - char* joystickName = HALGetJoystickName(port); - jstring str = env->NewStringUTF(joystickName); - std::free(joystickName); - return str; - } +JNIEXPORT jstring JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickName( + JNIEnv *env, jclass, jbyte port) { + NETCOMM_LOG(logDEBUG) << "Calling HALGetJoystickName"; + char *joystickName = HALGetJoystickName(port); + jstring str = env->NewStringUTF(joystickName); + std::free(joystickName); + return str; +} /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALGetJoystickAxisType * Signature: (BB)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickAxisType - (JNIEnv *, jclass, jbyte joystickNum, jbyte axis) -{ - NETCOMM_LOG(logDEBUG) << "Calling HALGetJoystickAxisType"; - return HALGetJoystickAxisType(joystickNum, axis); +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickAxisType( + JNIEnv *, jclass, jbyte joystickNum, jbyte axis) { + NETCOMM_LOG(logDEBUG) << "Calling HALGetJoystickAxisType"; + return HALGetJoystickAxisType(joystickNum, axis); } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: setNewDataSem * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_setNewDataSem - (JNIEnv * env, jclass, jlong id ) -{ - NETCOMM_LOG(logDEBUG) << "Mutex Ptr = " << (void*)id; - HALSetNewDataSem((MULTIWAIT_ID)id); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_setNewDataSem( + JNIEnv *env, jclass, jlong id) { + NETCOMM_LOG(logDEBUG) << "Mutex Ptr = " << (void *)id; + HALSetNewDataSem((MULTIWAIT_ID)id); } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALGetMatchTime * Signature: ()F */ -JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetMatchTime - (JNIEnv * env, jclass) -{ - jfloat matchTime; - HALGetMatchTime((float*)&matchTime); - return matchTime; +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetMatchTime( + JNIEnv *env, jclass) { + jfloat matchTime; + HALGetMatchTime((float *)&matchTime); + return matchTime; } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALGetSystemActive * Signature: ()Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetSystemActive - (JNIEnv * env, jclass) -{ - int32_t status = 0; - bool val = HALGetSystemActive(&status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetSystemActive( + JNIEnv *env, jclass) { + int32_t status = 0; + bool val = HALGetSystemActive(&status); + CheckStatus(env, status); + return val; } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALGetBrownedOut * Signature: ()Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetBrownedOut - (JNIEnv * env, jclass) -{ - int32_t status = 0; - bool val = HALGetBrownedOut(&status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetBrownedOut( + JNIEnv *env, jclass) { + int32_t status = 0; + bool val = HALGetBrownedOut(&status); + CheckStatus(env, status); + return val; } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALSetErrorData * Signature: (Ljava/lang/String;)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALSetErrorData - (JNIEnv * env, jclass, jstring error) -{ - const char * errorStr = env->GetStringUTFChars(error, NULL); +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALSetErrorData( + JNIEnv *env, jclass, jstring error) { + const char *errorStr = env->GetStringUTFChars(error, NULL); jsize length = env->GetStringUTFLength(error); NETCOMM_LOG(logDEBUG) << "Set Error: " << errorStr; NETCOMM_LOG(logDEBUG) << "Length: " << length; - jint returnValue = HALSetErrorData(errorStr, (jint) length, 0); - env->ReleaseStringUTFChars(error,errorStr); + jint returnValue = HALSetErrorData(errorStr, (jint)length, 0); + env->ReleaseStringUTFChars(error, errorStr); return returnValue; } /* - * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary + * Class: edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary * Method: HALSendError * Signature: (ZIZLjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALSendError - (JNIEnv * env, jclass, jboolean isError, jint errorCode, jboolean isLVCode, jstring details, jstring location, jstring callStack, jboolean printMsg) -{ - const char * detailsStr = env->GetStringUTFChars(details, NULL); - const char * locationStr = env->GetStringUTFChars(location, NULL); - const char * callStackStr = env->GetStringUTFChars(callStack, NULL); +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALSendError( + JNIEnv *env, jclass, jboolean isError, jint errorCode, jboolean isLVCode, + jstring details, jstring location, jstring callStack, jboolean printMsg) { + const char *detailsStr = env->GetStringUTFChars(details, NULL); + const char *locationStr = env->GetStringUTFChars(location, NULL); + const char *callStackStr = env->GetStringUTFChars(callStack, NULL); NETCOMM_LOG(logDEBUG) << "Send Error: " << detailsStr; NETCOMM_LOG(logDEBUG) << "Location: " << locationStr; NETCOMM_LOG(logDEBUG) << "Call Stack: " << callStackStr; - jint returnValue = HALSendError(isError, errorCode, isLVCode, detailsStr, locationStr, callStackStr, printMsg); - env->ReleaseStringUTFChars(details,detailsStr); - env->ReleaseStringUTFChars(location,locationStr); - env->ReleaseStringUTFChars(callStack,callStackStr); + jint returnValue = HALSendError(isError, errorCode, isLVCode, detailsStr, + locationStr, callStackStr, printMsg); + env->ReleaseStringUTFChars(details, detailsStr); + env->ReleaseStringUTFChars(location, locationStr); + env->ReleaseStringUTFChars(callStack, callStackStr); return returnValue; } diff --git a/wpilibj/src/athena/cpp/lib/HALUtil.cpp b/wpilibj/src/athena/cpp/lib/HALUtil.cpp index 276b6b6875..dd2732b9bd 100644 --- a/wpilibj/src/athena/cpp/lib/HALUtil.cpp +++ b/wpilibj/src/athena/cpp/lib/HALUtil.cpp @@ -6,23 +6,25 @@ /*----------------------------------------------------------------------------*/ #include "HALUtil.h" -#include #include -#include "Log.hpp" -#include "edu_wpi_first_wpilibj_hal_HALUtil.h" -#include "FRC_NetworkCommunication/CANSessionMux.h" -#include "HAL/HAL.hpp" -#include "errno.h" +#include #include #include #include +#include "FRC_NetworkCommunication/CANSessionMux.h" +#include "HAL/HAL.hpp" +#include "Log.hpp" +#include "edu_wpi_first_wpilibj_hal_HALUtil.h" +#include "errno.h" // set the logging level TLogLevel halUtilLogLevel = logWARNING; -#define HALUTIL_LOG(level) \ - if (level > halUtilLogLevel) ; \ - else Log().Get(level) +#define HALUTIL_LOG(level) \ + if (level > halUtilLogLevel) \ + ; \ + else \ + Log().Get(level) #define kRioStatusOffset -63000 #define kRioStatusSuccess 0 @@ -43,7 +45,7 @@ static jclass canMessageNotAllowedExCls = nullptr; static jclass canNotInitializedExCls = nullptr; static jclass uncleanStatusExCls = nullptr; -static void GetStackTrace(JNIEnv *env, std::string& res, std::string& func) { +static void GetStackTrace(JNIEnv *env, std::string &res, std::string &func) { // create a throwable static jmethodID constructorId = nullptr; if (!constructorId) @@ -89,7 +91,7 @@ static void GetStackTrace(JNIEnv *env, std::string& res, std::string& func) { } // add a line to res - //res += " at "; + // res += " at "; const char *tmp = env->GetStringUTFChars(stackElementString, nullptr); res += tmp; res += '\n'; @@ -212,8 +214,7 @@ extern "C" { // // indicate JNI version support desired and load classes // -JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) -{ +JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) { jvm = vm; // set our logging level @@ -292,8 +293,7 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) return JNI_VERSION_1_6; } -JNIEXPORT void JNICALL JNI_OnUnload(JavaVM *vm, void *reserved) -{ +JNIEXPORT void JNICALL JNI_OnUnload(JavaVM *vm, void *reserved) { JNIEnv *env; if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) return; @@ -305,7 +305,8 @@ JNIEXPORT void JNICALL JNI_OnUnload(JavaVM *vm, void *reserved) if (boundaryExCls) env->DeleteGlobalRef(boundaryExCls); if (canInvalidBufferExCls) env->DeleteGlobalRef(canInvalidBufferExCls); if (canMessageNotFoundExCls) env->DeleteGlobalRef(canMessageNotFoundExCls); - if (canMessageNotAllowedExCls) env->DeleteGlobalRef(canMessageNotAllowedExCls); + if (canMessageNotAllowedExCls) + env->DeleteGlobalRef(canMessageNotAllowedExCls); if (canNotInitializedExCls) env->DeleteGlobalRef(canNotInitializedExCls); if (uncleanStatusExCls) env->DeleteGlobalRef(uncleanStatusExCls); jvm = nullptr; @@ -316,9 +317,9 @@ JNIEXPORT void JNICALL JNI_OnUnload(JavaVM *vm, void *reserved) * Method: initializeMutex * Signature: (I)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_initializeMutexNormal - (JNIEnv * env, jclass) -{ +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_HALUtil_initializeMutexNormal( + JNIEnv *env, jclass) { HALUTIL_LOG(logDEBUG) << "Calling HALUtil initializeMutex"; MUTEX_ID mutex = initializeMutexNormal(); HALUTIL_LOG(logDEBUG) << "Mutex Ptr = " << mutex; @@ -330,9 +331,8 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_initializeMutexNo * Method: deleteMutex * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_deleteMutex - (JNIEnv * env, jclass, jlong id ) -{ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_deleteMutex( + JNIEnv *env, jclass, jlong id) { HALUTIL_LOG(logDEBUG) << "Calling HALUtil deleteMutex"; HALUTIL_LOG(logDEBUG) << "Mutex Ptr = " << (MUTEX_ID)id; deleteMutex((MUTEX_ID)id); @@ -343,11 +343,10 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_deleteMutex * Method: takeMutex * Signature: (JI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_takeMutex - (JNIEnv * env, jclass, jlong id) -{ - //HALUTIL_LOG(logDEBUG) << "Calling HALUtil takeMutex"; - //HALUTIL_LOG(logDEBUG) << "Mutex Ptr = " << (MUTEX_ID)id; +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_takeMutex( + JNIEnv *env, jclass, jlong id) { + // HALUTIL_LOG(logDEBUG) << "Calling HALUtil takeMutex"; + // HALUTIL_LOG(logDEBUG) << "Mutex Ptr = " << (MUTEX_ID)id; takeMutex((MUTEX_ID)id); } @@ -356,9 +355,9 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_takeMutex * Method: initializeMultiWait * Signature: ()J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_initializeMultiWait - (JNIEnv * env, jclass) -{ +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_HALUtil_initializeMultiWait( + JNIEnv *env, jclass) { HALUTIL_LOG(logDEBUG) << "Calling HALUtil initializeMultiWait"; MULTIWAIT_ID multiWait = initializeMultiWait(); HALUTIL_LOG(logDEBUG) << "MultiWait Ptr = " << multiWait; @@ -370,9 +369,8 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_initializeMultiWa * Method: deleteMultiWait * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_deleteMultiWait - (JNIEnv * env, jclass, jlong id) -{ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_deleteMultiWait( + JNIEnv *env, jclass, jlong id) { HALUTIL_LOG(logDEBUG) << "Calling HALUtil deleteMultiWait"; HALUTIL_LOG(logDEBUG) << "MultiWait Ptr = " << (MULTIWAIT_ID)id; deleteMultiWait((MULTIWAIT_ID)id); @@ -383,9 +381,8 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_deleteMultiWait * Method: takeMultiWait * Signature: (JJ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_takeMultiWait - (JNIEnv * env, jclass, jlong multiWaitId, jlong mutexId) -{ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_takeMultiWait( + JNIEnv *env, jclass, jlong multiWaitId, jlong mutexId) { takeMultiWait((MULTIWAIT_ID)multiWaitId, (MUTEX_ID)mutexId); } @@ -394,9 +391,8 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_takeMultiWait * Method: getFPGAVersion * Signature: ()S */ -JNIEXPORT jshort JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGAVersion - (JNIEnv * env, jclass) -{ +JNIEXPORT jshort JNICALL +Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGAVersion(JNIEnv *env, jclass) { HALUTIL_LOG(logDEBUG) << "Calling HALUtil getFPGAVersion"; int32_t status = 0; jshort returnValue = getFPGAVersion(&status); @@ -411,9 +407,8 @@ JNIEXPORT jshort JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGAVersion * Method: getFPGARevision * Signature: ()I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGARevision - (JNIEnv * env, jclass) -{ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGARevision(JNIEnv *env, jclass) { HALUTIL_LOG(logDEBUG) << "Calling HALUtil getFPGARevision"; int32_t status = 0; jint returnValue = getFPGARevision(&status); @@ -428,14 +423,13 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGARevision * Method: getFPGATime * Signature: ()J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGATime - (JNIEnv * env, jclass) -{ - //HALUTIL_LOG(logDEBUG) << "Calling HALUtil getFPGATime"; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGATime(JNIEnv *env, jclass) { + // HALUTIL_LOG(logDEBUG) << "Calling HALUtil getFPGATime"; int32_t status = 0; jlong returnValue = getFPGATime(&status); - //HALUTIL_LOG(logDEBUG) << "Status = " << status; - //HALUTIL_LOG(logDEBUG) << "FPGATime = " << returnValue; + // HALUTIL_LOG(logDEBUG) << "Status = " << status; + // HALUTIL_LOG(logDEBUG) << "FPGATime = " << returnValue; CheckStatus(env, status); return returnValue; } @@ -445,14 +439,13 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGATime * Method: getFPGAButton * Signature: ()I */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGAButton - (JNIEnv * env, jclass) -{ - //HALUTIL_LOG(logDEBUG) << "Calling HALUtil getFPGATime"; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGAButton(JNIEnv *env, jclass) { + // HALUTIL_LOG(logDEBUG) << "Calling HALUtil getFPGATime"; int32_t status = 0; jboolean returnValue = getFPGAButton(&status); - //HALUTIL_LOG(logDEBUG) << "Status = " << status; - //HALUTIL_LOG(logDEBUG) << "FPGATime = " << returnValue; + // HALUTIL_LOG(logDEBUG) << "Status = " << status; + // HALUTIL_LOG(logDEBUG) << "FPGATime = " << returnValue; CheckStatus(env, status); return returnValue; } @@ -462,11 +455,12 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getFPGAButton * Method: getHALErrorMessage * Signature: (I)Ljava/lang/String; */ -JNIEXPORT jstring JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getHALErrorMessage - (JNIEnv * paramEnv, jclass, jint paramId) -{ - const char * msg = getHALErrorMessage(paramId); - HALUTIL_LOG(logDEBUG) << "Calling HALUtil getHALErrorMessage id=" << paramId << " msg=" << msg; +JNIEXPORT jstring JNICALL +Java_edu_wpi_first_wpilibj_hal_HALUtil_getHALErrorMessage( + JNIEnv *paramEnv, jclass, jint paramId) { + const char *msg = getHALErrorMessage(paramId); + HALUTIL_LOG(logDEBUG) << "Calling HALUtil getHALErrorMessage id=" << paramId + << " msg=" << msg; return paramEnv->NewStringUTF(msg); } @@ -475,9 +469,8 @@ JNIEXPORT jstring JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getHALErrorMess * Method: getHALErrno * Signature: ()I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getHALErrno - (JNIEnv *, jclass) -{ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_HALUtil_getHALErrno(JNIEnv *, jclass) { return errno; } @@ -486,11 +479,11 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getHALErrno * Method: getHALstrerror * Signature: (I)Ljava/lang/String; */ -JNIEXPORT jstring JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getHALstrerror - (JNIEnv * env, jclass, jint errorCode) -{ - const char * msg = strerror(errno); - HALUTIL_LOG(logDEBUG) << "Calling HALUtil getHALstrerror errorCode=" << errorCode << " msg=" << msg; +JNIEXPORT jstring JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_getHALstrerror( + JNIEnv *env, jclass, jint errorCode) { + const char *msg = strerror(errno); + HALUTIL_LOG(logDEBUG) << "Calling HALUtil getHALstrerror errorCode=" + << errorCode << " msg=" << msg; return env->NewStringUTF(msg); } diff --git a/wpilibj/src/athena/cpp/lib/I2CJNI.cpp b/wpilibj/src/athena/cpp/lib/I2CJNI.cpp index d6eddff436..ffd688bf52 100644 --- a/wpilibj/src/athena/cpp/lib/I2CJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/I2CJNI.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_I2CJNI.h" @@ -17,9 +17,11 @@ // set the logging level TLogLevel i2cJNILogLevel = logWARNING; -#define I2CJNI_LOG(level) \ - if (level > i2cJNILogLevel) ; \ - else Log().Get(level) +#define I2CJNI_LOG(level) \ + if (level > i2cJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -28,15 +30,14 @@ extern "C" { * Method: i2cInitialize * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CInitialize - (JNIEnv * env, jclass, jbyte value) -{ - I2CJNI_LOG(logDEBUG) << "Calling I2CJNI i2CInititalize"; - I2CJNI_LOG(logDEBUG) << "Port: " << (jint) value; - int32_t status = 0; - i2CInitialize(value, &status); - I2CJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CInitialize( + JNIEnv* env, jclass, jbyte value) { + I2CJNI_LOG(logDEBUG) << "Calling I2CJNI i2CInititalize"; + I2CJNI_LOG(logDEBUG) << "Port: " << (jint)value; + int32_t status = 0; + i2CInitialize(value, &status); + I2CJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -44,24 +45,26 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CInitialize * Method: i2CTransaction * Signature: (BBLjava/nio/ByteBuffer;BLjava/nio/ByteBuffer;B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CTransaction - (JNIEnv * env, jclass, jbyte port, jbyte address, jobject dataToSend, jbyte sendSize, jobject dataReceived, jbyte receiveSize) -{ - I2CJNI_LOG(logDEBUG) << "Calling I2CJNI i2CTransaction"; - I2CJNI_LOG(logDEBUG) << "Port = " << (jint)port; - I2CJNI_LOG(logDEBUG) << "Address = " << (jint)address; - uint8_t* dataToSendPtr = nullptr; - if (dataToSend != 0) { - dataToSendPtr = (uint8_t*)env->GetDirectBufferAddress(dataToSend); - } - I2CJNI_LOG(logDEBUG) << "DataToSendPtr = " << (jint*)dataToSendPtr; - I2CJNI_LOG(logDEBUG) << "SendSize = " << (jint)sendSize; - uint8_t* dataReceivedPtr = (uint8_t*)env->GetDirectBufferAddress(dataReceived); - I2CJNI_LOG(logDEBUG) << "DataReceivedPtr = " << (jint*)dataReceivedPtr; - I2CJNI_LOG(logDEBUG) << "ReceiveSize = " << (jint)receiveSize; - jint returnValue = i2CTransaction(port, address, dataToSendPtr, sendSize, dataReceivedPtr, receiveSize); - I2CJNI_LOG(logDEBUG) << "ReturnValue = " << returnValue; - return returnValue; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CTransaction( + JNIEnv* env, jclass, jbyte port, jbyte address, jobject dataToSend, + jbyte sendSize, jobject dataReceived, jbyte receiveSize) { + I2CJNI_LOG(logDEBUG) << "Calling I2CJNI i2CTransaction"; + I2CJNI_LOG(logDEBUG) << "Port = " << (jint)port; + I2CJNI_LOG(logDEBUG) << "Address = " << (jint)address; + uint8_t* dataToSendPtr = nullptr; + if (dataToSend != 0) { + dataToSendPtr = (uint8_t*)env->GetDirectBufferAddress(dataToSend); + } + I2CJNI_LOG(logDEBUG) << "DataToSendPtr = " << (jint*)dataToSendPtr; + I2CJNI_LOG(logDEBUG) << "SendSize = " << (jint)sendSize; + uint8_t* dataReceivedPtr = + (uint8_t*)env->GetDirectBufferAddress(dataReceived); + I2CJNI_LOG(logDEBUG) << "DataReceivedPtr = " << (jint*)dataReceivedPtr; + I2CJNI_LOG(logDEBUG) << "ReceiveSize = " << (jint)receiveSize; + jint returnValue = i2CTransaction(port, address, dataToSendPtr, sendSize, + dataReceivedPtr, receiveSize); + I2CJNI_LOG(logDEBUG) << "ReturnValue = " << returnValue; + return returnValue; } /* @@ -69,23 +72,22 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CTransaction * Method: i2CWrite * Signature: (BBLjava/nio/ByteBuffer;B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CWrite - (JNIEnv * env, jclass, jbyte port, jbyte address, jobject dataToSend, jbyte sendSize) -{ - - I2CJNI_LOG(logDEBUG) << "Calling I2CJNI i2CWrite"; - I2CJNI_LOG(logDEBUG) << "Port = " << (jint)port; - I2CJNI_LOG(logDEBUG) << "Address = " << (jint)address; - uint8_t* dataToSendPtr = nullptr; - - if (dataToSend != 0) { - dataToSendPtr = (uint8_t*)env->GetDirectBufferAddress(dataToSend); - } - I2CJNI_LOG(logDEBUG) << "DataToSendPtr = " << dataToSendPtr; - I2CJNI_LOG(logDEBUG) << "SendSize = " << (jint)dataToSend; - jint returnValue = i2CWrite(port, address, dataToSendPtr, sendSize); - I2CJNI_LOG(logDEBUG) << "ReturnValue = " << (jint)returnValue; - return returnValue; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CWrite( + JNIEnv* env, jclass, jbyte port, jbyte address, jobject dataToSend, + jbyte sendSize) { + I2CJNI_LOG(logDEBUG) << "Calling I2CJNI i2CWrite"; + I2CJNI_LOG(logDEBUG) << "Port = " << (jint)port; + I2CJNI_LOG(logDEBUG) << "Address = " << (jint)address; + uint8_t* dataToSendPtr = nullptr; + + if (dataToSend != 0) { + dataToSendPtr = (uint8_t*)env->GetDirectBufferAddress(dataToSend); + } + I2CJNI_LOG(logDEBUG) << "DataToSendPtr = " << dataToSendPtr; + I2CJNI_LOG(logDEBUG) << "SendSize = " << (jint)dataToSend; + jint returnValue = i2CWrite(port, address, dataToSendPtr, sendSize); + I2CJNI_LOG(logDEBUG) << "ReturnValue = " << (jint)returnValue; + return returnValue; } /* @@ -93,18 +95,19 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CWrite * Method: i2CRead * Signature: (BBLjava/nio/ByteBuffer;B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CRead - (JNIEnv * env, jclass, jbyte port, jbyte address, jobject dataReceived, jbyte receiveSize) -{ - I2CJNI_LOG(logDEBUG) << "Calling I2CJNI i2CRead"; - I2CJNI_LOG(logDEBUG) << "Port = " << port; - I2CJNI_LOG(logDEBUG) << "Address = " << address; - uint8_t* dataReceivedPtr = (uint8_t*)env->GetDirectBufferAddress(dataReceived); - I2CJNI_LOG(logDEBUG) << "DataReceivedPtr = " << dataReceivedPtr; - I2CJNI_LOG(logDEBUG) << "ReceiveSize = " << receiveSize; - jint returnValue = i2CRead(port, address, dataReceivedPtr, receiveSize); - I2CJNI_LOG(logDEBUG) << "ReturnValue = " << returnValue; - return returnValue; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CRead( + JNIEnv* env, jclass, jbyte port, jbyte address, jobject dataReceived, + jbyte receiveSize) { + I2CJNI_LOG(logDEBUG) << "Calling I2CJNI i2CRead"; + I2CJNI_LOG(logDEBUG) << "Port = " << port; + I2CJNI_LOG(logDEBUG) << "Address = " << address; + uint8_t* dataReceivedPtr = + (uint8_t*)env->GetDirectBufferAddress(dataReceived); + I2CJNI_LOG(logDEBUG) << "DataReceivedPtr = " << dataReceivedPtr; + I2CJNI_LOG(logDEBUG) << "ReceiveSize = " << receiveSize; + jint returnValue = i2CRead(port, address, dataReceivedPtr, receiveSize); + I2CJNI_LOG(logDEBUG) << "ReturnValue = " << returnValue; + return returnValue; } /* @@ -112,11 +115,10 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CRead * Method: i2CClose * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CClose - (JNIEnv *, jclass, jbyte value) -{ - I2CJNI_LOG(logDEBUG) << "Calling I2CJNI i2CClose"; - i2CClose(value); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_I2CJNI_i2CClose(JNIEnv*, jclass, jbyte value) { + I2CJNI_LOG(logDEBUG) << "Calling I2CJNI i2CClose"; + i2CClose(value); } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/InterruptJNI.cpp b/wpilibj/src/athena/cpp/lib/InterruptJNI.cpp index 1425799359..1929cee40c 100644 --- a/wpilibj/src/athena/cpp/lib/InterruptJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/InterruptJNI.cpp @@ -5,24 +5,26 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include #include #include #include #include "Log.hpp" -#include "edu_wpi_first_wpilibj_hal_InterruptJNI.h" #include "HAL/Interrupts.hpp" #include "HALUtil.h" #include "SafeThread.h" +#include "edu_wpi_first_wpilibj_hal_InterruptJNI.h" TLogLevel interruptJNILogLevel = logERROR; -#define INTERRUPTJNI_LOG(level) \ - if (level > interruptJNILogLevel) ; \ - else Log().Get(level) +#define INTERRUPTJNI_LOG(level) \ + if (level > interruptJNILogLevel) \ + ; \ + else \ + Log().Get(level) // Thread where callbacks are actually performed. // @@ -74,7 +76,7 @@ void InterruptJNI::SetFunc(JNIEnv* env, jobject func, jmethodID mid, } void InterruptThreadJNI::Main() { - JNIEnv *env; + JNIEnv* env; JavaVMAttachArgs args; args.version = JNI_VERSION_1_2; args.name = const_cast("Interrupt"); @@ -119,9 +121,9 @@ extern "C" { * Method: initializeInterrupts * Signature: (IZ)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_initializeInterrupts - (JNIEnv * env, jclass, jint interruptIndex, jboolean watcher) -{ +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_InterruptJNI_initializeInterrupts( + JNIEnv* env, jclass, jint interruptIndex, jboolean watcher) { INTERRUPTJNI_LOG(logDEBUG) << "Calling INTERRUPTJNI initializeInterrupts"; INTERRUPTJNI_LOG(logDEBUG) << "interruptIndex = " << interruptIndex; INTERRUPTJNI_LOG(logDEBUG) << "watcher = " << (bool)watcher; @@ -136,15 +138,14 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_initializeIn return (jlong)interrupt; } - /* * Class: edu_wpi_first_wpilibj_hal_InterruptJNI * Method: cleanInterrupts * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_cleanInterrupts - (JNIEnv * env, jclass, jlong interrupt_pointer) -{ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_InterruptJNI_cleanInterrupts( + JNIEnv* env, jclass, jlong interrupt_pointer) { INTERRUPTJNI_LOG(logDEBUG) << "Calling INTERRUPTJNI cleanInterrupts"; INTERRUPTJNI_LOG(logDEBUG) << "Interrupt Ptr = " << (void*)interrupt_pointer; @@ -161,9 +162,10 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_cleanInterrup * Method: waitForInterrupt * Signature: (JD)V */ -JNIEXPORT int JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_waitForInterrupt - (JNIEnv * env, jclass, jlong interrupt_pointer, jdouble timeout, jboolean ignorePrevious) -{ +JNIEXPORT int JNICALL +Java_edu_wpi_first_wpilibj_hal_InterruptJNI_waitForInterrupt( + JNIEnv* env, jclass, jlong interrupt_pointer, jdouble timeout, + jboolean ignorePrevious) { INTERRUPTJNI_LOG(logDEBUG) << "Calling INTERRUPTJNI waitForInterrupt"; INTERRUPTJNI_LOG(logDEBUG) << "Interrupt Ptr = " << (void*)interrupt_pointer; @@ -182,9 +184,9 @@ JNIEXPORT int JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_waitForInterru * Method: enableInterrupts * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_enableInterrupts - (JNIEnv * env, jclass, jlong interrupt_pointer) -{ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_InterruptJNI_enableInterrupts( + JNIEnv* env, jclass, jlong interrupt_pointer) { INTERRUPTJNI_LOG(logDEBUG) << "Calling INTERRUPTJNI enableInterrupts"; INTERRUPTJNI_LOG(logDEBUG) << "Interrupt Ptr = " << (void*)interrupt_pointer; @@ -201,9 +203,9 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_enableInterru * Method: disableInterrupts * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_disableInterrupts - (JNIEnv * env, jclass, jlong interrupt_pointer) -{ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_InterruptJNI_disableInterrupts( + JNIEnv* env, jclass, jlong interrupt_pointer) { INTERRUPTJNI_LOG(logDEBUG) << "Calling INTERRUPTJNI disableInterrupts"; INTERRUPTJNI_LOG(logDEBUG) << "Interrupt Ptr = " << (void*)interrupt_pointer; @@ -220,9 +222,9 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_disableInterr * Method: readRisingTimestamp * Signature: (J)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_readRisingTimestamp - (JNIEnv * env, jclass, jlong interrupt_pointer) -{ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_InterruptJNI_readRisingTimestamp( + JNIEnv* env, jclass, jlong interrupt_pointer) { INTERRUPTJNI_LOG(logDEBUG) << "Calling INTERRUPTJNI readRisingTimestamp"; INTERRUPTJNI_LOG(logDEBUG) << "Interrupt Ptr = " << (void*)interrupt_pointer; @@ -239,9 +241,9 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_readRising * Method: readFallingTimestamp * Signature: (J)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_readFallingTimestamp - (JNIEnv * env, jclass, jlong interrupt_pointer) -{ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_InterruptJNI_readFallingTimestamp( + JNIEnv* env, jclass, jlong interrupt_pointer) { INTERRUPTJNI_LOG(logDEBUG) << "Calling INTERRUPTJNI readFallingTimestamp"; INTERRUPTJNI_LOG(logDEBUG) << "Interrupt Ptr = " << (void*)interrupt_pointer; @@ -258,14 +260,16 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_readFallin * Method: requestInterrupts * Signature: (JBIZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_requestInterrupts - (JNIEnv * env, jclass, jlong interrupt_pointer, jbyte routing_module, jint routing_pin, jboolean routing_analog_trigger) -{ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_InterruptJNI_requestInterrupts( + JNIEnv* env, jclass, jlong interrupt_pointer, jbyte routing_module, + jint routing_pin, jboolean routing_analog_trigger) { INTERRUPTJNI_LOG(logDEBUG) << "Calling INTERRUPTJNI requestInterrupts"; INTERRUPTJNI_LOG(logDEBUG) << "Interrupt Ptr = " << (void*)interrupt_pointer; INTERRUPTJNI_LOG(logDEBUG) << "routing module = " << (jint)routing_module; INTERRUPTJNI_LOG(logDEBUG) << "routing pin = " << routing_pin; - INTERRUPTJNI_LOG(logDEBUG) << "routing analog trigger = " << (jint)routing_analog_trigger; + INTERRUPTJNI_LOG(logDEBUG) << "routing analog trigger = " + << (jint)routing_analog_trigger; int32_t status = 0; requestInterrupts((void*)interrupt_pointer, (uint8_t)routing_module, @@ -278,11 +282,13 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_requestInterr /* * Class: edu_wpi_first_wpilibj_hal_InterruptJNI * Method: attachInterruptHandler - * Signature: (JLedu/wpi/first/wpilibj/hal/InterruptJNI/InterruptHandlerFunction;Ljava/nio/ByteBuffer;)V + * Signature: + * (JLedu/wpi/first/wpilibj/hal/InterruptJNI/InterruptHandlerFunction;Ljava/nio/ByteBuffer;)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_attachInterruptHandler - (JNIEnv * env, jclass, jlong interrupt_pointer, jobject handler, jobject param) -{ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_InterruptJNI_attachInterruptHandler( + JNIEnv* env, jclass, jlong interrupt_pointer, jobject handler, + jobject param) { INTERRUPTJNI_LOG(logDEBUG) << "Calling INTERRUPTJNI attachInterruptHandler"; INTERRUPTJNI_LOG(logDEBUG) << "Interrupt Ptr = " << (void*)interrupt_pointer; @@ -320,9 +326,10 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_attachInterru * Method: setInterruptUpSourceEdge * Signature: (JZZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_InterruptJNI_setInterruptUpSourceEdge - (JNIEnv * env, jclass, jlong interrupt_pointer, jboolean risingEdge, jboolean fallingEdge) -{ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_InterruptJNI_setInterruptUpSourceEdge( + JNIEnv* env, jclass, jlong interrupt_pointer, jboolean risingEdge, + jboolean fallingEdge) { INTERRUPTJNI_LOG(logDEBUG) << "Calling INTERRUPTJNI setInterruptUpSourceEdge"; INTERRUPTJNI_LOG(logDEBUG) << "Interrupt Ptr = " << (void*)interrupt_pointer; INTERRUPTJNI_LOG(logDEBUG) << "Rising Edge = " << (bool)risingEdge; diff --git a/wpilibj/src/athena/cpp/lib/JNIWrapper.cpp b/wpilibj/src/athena/cpp/lib/JNIWrapper.cpp index ab3a6082d5..b1bf41affe 100644 --- a/wpilibj/src/athena/cpp/lib/JNIWrapper.cpp +++ b/wpilibj/src/athena/cpp/lib/JNIWrapper.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_JNIWrapper.h" @@ -20,15 +20,15 @@ extern "C" { * Method: getPortWithModule * Signature: (BB)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_JNIWrapper_getPortWithModule - (JNIEnv * env, jclass, jbyte module, jbyte pin) -{ - //FILE_LOG(logDEBUG) << "Calling JNIWrapper getPortWithModlue"; - //FILE_LOG(logDEBUG) << "Module = " << (jint)module; - //FILE_LOG(logDEBUG) << "Pin = " << (jint)pin; - void* port = getPortWithModule(module, pin); - //FILE_LOG(logDEBUG) << "Port Ptr = " << port; - return (jlong)port; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_JNIWrapper_getPortWithModule( + JNIEnv* env, jclass, jbyte module, jbyte pin) { + // FILE_LOG(logDEBUG) << "Calling JNIWrapper getPortWithModlue"; + // FILE_LOG(logDEBUG) << "Module = " << (jint)module; + // FILE_LOG(logDEBUG) << "Pin = " << (jint)pin; + void* port = getPortWithModule(module, pin); + // FILE_LOG(logDEBUG) << "Port Ptr = " << port; + return (jlong)port; } /* @@ -36,15 +36,14 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_JNIWrapper_getPortWithMod * Method: getPort * Signature: (BB)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_JNIWrapper_getPort - (JNIEnv * env, jclass, jbyte pin) -{ - //FILE_LOG(logDEBUG) << "Calling JNIWrapper getPortWithModlue"; - //FILE_LOG(logDEBUG) << "Module = " << (jint)module; - //FILE_LOG(logDEBUG) << "Pin = " << (jint)pin; - void* port = getPort(pin); - //FILE_LOG(logDEBUG) << "Port Ptr = " << port; - return (jlong)port; +JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_JNIWrapper_getPort( + JNIEnv* env, jclass, jbyte pin) { + // FILE_LOG(logDEBUG) << "Calling JNIWrapper getPortWithModlue"; + // FILE_LOG(logDEBUG) << "Module = " << (jint)module; + // FILE_LOG(logDEBUG) << "Pin = " << (jint)pin; + void* port = getPort(pin); + // FILE_LOG(logDEBUG) << "Port Ptr = " << port; + return (jlong)port; } /* @@ -52,14 +51,13 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_JNIWrapper_getPort * Method: freePort * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_JNIWrapper_freePort - (JNIEnv * env, jclass, jlong id) -{ - //FILE_LOG(logDEBUG) << "Calling JNIWrapper getPortWithModlue"; - //FILE_LOG(logDEBUG) << "Module = " << (jint)module; - //FILE_LOG(logDEBUG) << "Pin = " << (jint)pin; - freePort((void*)id); - //FILE_LOG(logDEBUG) << "Port Ptr = " << port; +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_JNIWrapper_freePort( + JNIEnv* env, jclass, jlong id) { + // FILE_LOG(logDEBUG) << "Calling JNIWrapper getPortWithModlue"; + // FILE_LOG(logDEBUG) << "Module = " << (jint)module; + // FILE_LOG(logDEBUG) << "Pin = " << (jint)pin; + freePort((void*)id); + // FILE_LOG(logDEBUG) << "Port Ptr = " << port; } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/NotifierJNI.cpp b/wpilibj/src/athena/cpp/lib/NotifierJNI.cpp index 9224547705..4e45533ff3 100644 --- a/wpilibj/src/athena/cpp/lib/NotifierJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/NotifierJNI.cpp @@ -5,26 +5,28 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include +#include "HAL/Notifier.hpp" #include +#include +#include #include #include #include #include #include -#include -#include "Log.hpp" -#include "edu_wpi_first_wpilibj_hal_NotifierJNI.h" -#include "HAL/Notifier.hpp" #include "HALUtil.h" +#include "Log.hpp" #include "SafeThread.h" +#include "edu_wpi_first_wpilibj_hal_NotifierJNI.h" // set the logging level TLogLevel notifierJNILogLevel = logWARNING; -#define NOTIFIERJNI_LOG(level) \ - if (level > notifierJNILogLevel) ; \ - else Log().Get(level) +#define NOTIFIERJNI_LOG(level) \ + if (level > notifierJNILogLevel) \ + ; \ + else \ + Log().Get(level) // Thread where callbacks are actually performed. // @@ -50,7 +52,7 @@ class NotifierThreadJNI : public SafeThread { class NotifierJNI : public SafeThreadOwner { public: - void SetFunc(JNIEnv* env, jobject func, jmethodID mid); + void SetFunc(JNIEnv *env, jobject func, jmethodID mid); void Notify(uint64_t currentTime) { auto thr = GetThread(); @@ -61,7 +63,7 @@ class NotifierJNI : public SafeThreadOwner { } }; -void NotifierJNI::SetFunc(JNIEnv* env, jobject func, jmethodID mid) { +void NotifierJNI::SetFunc(JNIEnv *env, jobject func, jmethodID mid) { auto thr = GetThread(); if (!thr) return; // free global reference @@ -75,9 +77,9 @@ void NotifierThreadJNI::Main() { JNIEnv *env; JavaVMAttachArgs args; args.version = JNI_VERSION_1_2; - args.name = const_cast("Notifier"); + args.name = const_cast("Notifier"); args.group = nullptr; - jint rs = jvm->AttachCurrentThreadAsDaemon((void**)&env, &args); + jint rs = jvm->AttachCurrentThreadAsDaemon((void **)&env, &args); if (rs != JNI_OK) return; std::unique_lock lock(m_mutex); @@ -104,8 +106,8 @@ void NotifierThreadJNI::Main() { jvm->DetachCurrentThread(); } -void notifierHandler(uint64_t currentTimeInt, void* param) { - ((NotifierJNI*)param)->Notify(currentTimeInt); +void notifierHandler(uint64_t currentTimeInt, void *param) { + ((NotifierJNI *)param)->Notify(currentTimeInt); } extern "C" { @@ -115,9 +117,9 @@ extern "C" { * Method: initializeNotifier * Signature: (Ljava/lang/Runnable;)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_initializeNotifier - (JNIEnv *env, jclass, jobject func) -{ +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_NotifierJNI_initializeNotifier( + JNIEnv *env, jclass, jobject func) { NOTIFIERJNI_LOG(logDEBUG) << "Calling NOTIFIERJNI initializeNotifier"; jclass cls = env->GetObjectClass(func); @@ -135,7 +137,7 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_initializeNot // each notifier runs in its own thread; this is so if one takes too long // to execute, it doesn't keep the others from running - NotifierJNI* notify = new NotifierJNI; + NotifierJNI *notify = new NotifierJNI; notify->Start(); notify->SetFunc(env, func, mid); int32_t status = 0; @@ -157,17 +159,16 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_initializeNot * Method: cleanNotifier * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_cleanNotifier - (JNIEnv *env, jclass, jlong notifierPtr) -{ +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_cleanNotifier( + JNIEnv *env, jclass, jlong notifierPtr) { NOTIFIERJNI_LOG(logDEBUG) << "Calling NOTIFIERJNI cleanNotifier"; NOTIFIERJNI_LOG(logDEBUG) << "Notifier Ptr = " << (void *)notifierPtr; int32_t status = 0; - NotifierJNI* notify = - (NotifierJNI*)getNotifierParam((void*)notifierPtr, &status); - cleanNotifier((void*)notifierPtr, &status); + NotifierJNI *notify = + (NotifierJNI *)getNotifierParam((void *)notifierPtr, &status); + cleanNotifier((void *)notifierPtr, &status); NOTIFIERJNI_LOG(logDEBUG) << "Status = " << status; CheckStatus(env, status); delete notify; @@ -178,9 +179,9 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_cleanNotifier * Method: updateNotifierAlarm * Signature: (JJ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_updateNotifierAlarm - (JNIEnv *env, jclass cls, jlong notifierPtr, jlong triggerTime) -{ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_NotifierJNI_updateNotifierAlarm( + JNIEnv *env, jclass cls, jlong notifierPtr, jlong triggerTime) { NOTIFIERJNI_LOG(logDEBUG) << "Calling NOTIFIERJNI updateNotifierAlarm"; NOTIFIERJNI_LOG(logDEBUG) << "Notifier Ptr = " << (void *)notifierPtr; @@ -188,7 +189,7 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_updateNotifier NOTIFIERJNI_LOG(logDEBUG) << "triggerTime = " << triggerTime; int32_t status = 0; - updateNotifierAlarm((void*)notifierPtr, (uint64_t)triggerTime, &status); + updateNotifierAlarm((void *)notifierPtr, (uint64_t)triggerTime, &status); NOTIFIERJNI_LOG(logDEBUG) << "Status = " << status; CheckStatus(env, status); } @@ -198,15 +199,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_updateNotifier * Method: stopNotifierAlarm * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_NotifierJNI_stopNotifierAlarm - (JNIEnv *env, jclass cls, jlong notifierPtr) -{ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_NotifierJNI_stopNotifierAlarm( + JNIEnv *env, jclass cls, jlong notifierPtr) { NOTIFIERJNI_LOG(logDEBUG) << "Calling NOTIFIERJNI stopNotifierAlarm"; NOTIFIERJNI_LOG(logDEBUG) << "Notifier Ptr = " << (void *)notifierPtr; int32_t status = 0; - stopNotifierAlarm((void*)notifierPtr, &status); + stopNotifierAlarm((void *)notifierPtr, &status); NOTIFIERJNI_LOG(logDEBUG) << "Status = " << status; CheckStatus(env, status); } diff --git a/wpilibj/src/athena/cpp/lib/PDPJNI.cpp b/wpilibj/src/athena/cpp/lib/PDPJNI.cpp index 1b360db174..8b92625f31 100644 --- a/wpilibj/src/athena/cpp/lib/PDPJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/PDPJNI.cpp @@ -5,9 +5,9 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include "edu_wpi_first_wpilibj_hal_PDPJNI.h" #include "HAL/PDP.hpp" #include "HALUtil.h" +#include "edu_wpi_first_wpilibj_hal_PDPJNI.h" extern "C" { @@ -16,22 +16,21 @@ extern "C" { * Method: getPDPTemperature * Signature: (I)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_initializePDP - (JNIEnv *, jclass, jint module) -{ - initializePDP(module); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_initializePDP( + JNIEnv *, jclass, jint module) { + initializePDP(module); } - + /* * Class: edu_wpi_first_wpilibj_hal_PDPJNI * Method: getPDPTemperature * Signature: (I)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTemperature - (JNIEnv *env, jclass, jint module) -{ - int32_t status = 0; - double temperature = getPDPTemperature(module, &status); +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTemperature( + JNIEnv *env, jclass, jint module) { + int32_t status = 0; + double temperature = getPDPTemperature(module, &status); CheckStatus(env, status, false); return temperature; } @@ -41,11 +40,10 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTemperatur * Method: getPDPVoltage * Signature: (I)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPVoltage - (JNIEnv *env, jclass, jint module) -{ - int32_t status = 0; - double voltage = getPDPVoltage(module, &status); +JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPVoltage( + JNIEnv *env, jclass, jint module) { + int32_t status = 0; + double voltage = getPDPVoltage(module, &status); CheckStatus(env, status, false); return voltage; } @@ -55,11 +53,11 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPVoltage * Method: getPDPChannelCurrent * Signature: (BI)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPChannelCurrent - (JNIEnv *env, jclass, jbyte channel, jint module) -{ - int32_t status = 0; - double current = getPDPChannelCurrent(module, channel, &status); +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPChannelCurrent( + JNIEnv *env, jclass, jbyte channel, jint module) { + int32_t status = 0; + double current = getPDPChannelCurrent(module, channel, &status); CheckStatus(env, status, false); return current; } @@ -69,11 +67,11 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPChannelCur * Method: getPDPTotalCurrent * Signature: (I)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTotalCurrent - (JNIEnv *env, jclass, jint module) -{ - int32_t status = 0; - double current = getPDPTotalCurrent(module, &status); +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTotalCurrent( + JNIEnv *env, jclass, jint module) { + int32_t status = 0; + double current = getPDPTotalCurrent(module, &status); CheckStatus(env, status, false); return current; } @@ -83,11 +81,11 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTotalCurre * Method: getPDPTotalPower * Signature: (I)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTotalPower - (JNIEnv *env, jclass, jint module) -{ - int32_t status = 0; - double power = getPDPTotalPower(module, &status); +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTotalPower( + JNIEnv *env, jclass, jint module) { + int32_t status = 0; + double power = getPDPTotalPower(module, &status); CheckStatus(env, status, false); return power; } @@ -97,26 +95,25 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTotalPower * Method: resetPDPTotalEnergy * Signature: (I)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTotalEnergy - (JNIEnv *env, jclass, jint module) -{ - int32_t status = 0; - double energy = getPDPTotalEnergy(module, &status); +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_PDPJNI_getPDPTotalEnergy( + JNIEnv *env, jclass, jint module) { + int32_t status = 0; + double energy = getPDPTotalEnergy(module, &status); CheckStatus(env, status, false); return energy; } - /* * Class: edu_wpi_first_wpilibj_hal_PDPJNI * Method: resetPDPTotalEnergy * Signature: (I)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_resetPDPTotalEnergy - (JNIEnv *env, jclass, jint module) -{ - int32_t status = 0; - resetPDPTotalEnergy(module, &status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_PDPJNI_resetPDPTotalEnergy( + JNIEnv *env, jclass, jint module) { + int32_t status = 0; + resetPDPTotalEnergy(module, &status); CheckStatus(env, status, false); } @@ -125,11 +122,11 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_resetPDPTotalEnergy * Method: clearStickyFaults * Signature: (I)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PDPJNI_clearPDPStickyFaults - (JNIEnv *env, jclass, jint module) -{ - int32_t status = 0; - clearPDPStickyFaults(module, &status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_PDPJNI_clearPDPStickyFaults( + JNIEnv *env, jclass, jint module) { + int32_t status = 0; + clearPDPStickyFaults(module, &status); CheckStatus(env, status, false); } diff --git a/wpilibj/src/athena/cpp/lib/PWMJNI.cpp b/wpilibj/src/athena/cpp/lib/PWMJNI.cpp index 917c94eeaf..004f62dcf1 100644 --- a/wpilibj/src/athena/cpp/lib/PWMJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/PWMJNI.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_PWMJNI.h" @@ -17,9 +17,11 @@ // set the logging level TLogLevel pwmJNILogLevel = logWARNING; -#define PWMJNI_LOG(level) \ - if (level > pwmJNILogLevel) ; \ - else Log().Get(level) +#define PWMJNI_LOG(level) \ + if (level > pwmJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -28,17 +30,17 @@ extern "C" { * Method: allocatePWMChannel * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_allocatePWMChannel - (JNIEnv * env, jclass, jlong id) -{ - PWMJNI_LOG(logDEBUG) << "Calling DIOJNI allocatePWMChannel"; - PWMJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = allocatePWMChannel((void*)id, &status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - PWMJNI_LOG(logDEBUG) << "allocatePWMChannelResult = " << (jint)returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_PWMJNI_allocatePWMChannel( + JNIEnv* env, jclass, jlong id) { + PWMJNI_LOG(logDEBUG) << "Calling DIOJNI allocatePWMChannel"; + PWMJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + int32_t status = 0; + jboolean returnValue = allocatePWMChannel((void*)id, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + PWMJNI_LOG(logDEBUG) << "allocatePWMChannelResult = " << (jint)returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -46,15 +48,14 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_allocatePWMChan * Method: freePWMChannel * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_freePWMChannel - (JNIEnv * env, jclass, jlong id) -{ - PWMJNI_LOG(logDEBUG) << "Calling DIOJNI freePWMChannel"; - PWMJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - freePWMChannel((void*)id, &status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_freePWMChannel( + JNIEnv* env, jclass, jlong id) { + PWMJNI_LOG(logDEBUG) << "Calling DIOJNI freePWMChannel"; + PWMJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + int32_t status = 0; + freePWMChannel((void*)id, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -62,15 +63,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_freePWMChannel * Method: setPWM * Signature: (JS)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWM - (JNIEnv * env, jclass, jlong id, jshort value) -{ - PWMJNI_LOG(logDEBUG) << "DigitalPort Ptr = " << (void*)id; - PWMJNI_LOG(logDEBUG) << "PWM Value = " << value; - int32_t status = 0; - setPWM((void*)id, value, &status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWM( + JNIEnv* env, jclass, jlong id, jshort value) { + PWMJNI_LOG(logDEBUG) << "DigitalPort Ptr = " << (void*)id; + PWMJNI_LOG(logDEBUG) << "PWM Value = " << value; + int32_t status = 0; + setPWM((void*)id, value, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -78,16 +78,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWM * Method: getPWM * Signature: (J)S */ -JNIEXPORT jshort JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_getPWM - (JNIEnv * env, jclass, jlong id) -{ - PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << (void*)id; - int32_t status = 0; - jshort returnValue = getPWM((void*)id, &status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - PWMJNI_LOG(logDEBUG) << "Value = " << returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jshort JNICALL +Java_edu_wpi_first_wpilibj_hal_PWMJNI_getPWM(JNIEnv* env, jclass, jlong id) { + PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << (void*)id; + int32_t status = 0; + jshort returnValue = getPWM((void*)id, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + PWMJNI_LOG(logDEBUG) << "Value = " << returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -95,14 +94,13 @@ JNIEXPORT jshort JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_getPWM * Method: latchPWMZero * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_latchPWMZero - (JNIEnv * env, jclass, jlong id) -{ - PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << (void*)id; - int32_t status = 0; - latchPWMZero((void*)id, &status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_latchPWMZero( + JNIEnv* env, jclass, jlong id) { + PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << (void*)id; + int32_t status = 0; + latchPWMZero((void*)id, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -110,15 +108,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_latchPWMZero * Method: setPWMPeriodScale * Signature: (JI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMPeriodScale - (JNIEnv * env, jclass, jlong id, jint value) -{ - PWMJNI_LOG(logDEBUG) << "DigitalPort Ptr = " << (void*)id; - PWMJNI_LOG(logDEBUG) << "PeriodScale Value = " << value; - int32_t status = 0; - setPWMPeriodScale((void*)id, value, &status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMPeriodScale( + JNIEnv* env, jclass, jlong id, jint value) { + PWMJNI_LOG(logDEBUG) << "DigitalPort Ptr = " << (void*)id; + PWMJNI_LOG(logDEBUG) << "PeriodScale Value = " << value; + int32_t status = 0; + setPWMPeriodScale((void*)id, value, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -126,16 +123,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMPeriodScale * Method: allocatePWM * Signature: ()J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_allocatePWM - (JNIEnv * env, jclass) -{ - PWMJNI_LOG(logDEBUG) << "Calling PWMJNI allocatePWM"; - int32_t status = 0; - void* pwm = allocatePWM(&status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << pwm; - CheckStatus(env, status); - return (jlong)pwm; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_PWMJNI_allocatePWM(JNIEnv* env, jclass) { + PWMJNI_LOG(logDEBUG) << "Calling PWMJNI allocatePWM"; + int32_t status = 0; + void* pwm = allocatePWM(&status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << pwm; + CheckStatus(env, status); + return (jlong)pwm; } /* @@ -143,15 +139,14 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_allocatePWM * Method: freePWM * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_freePWM - (JNIEnv * env, jclass, jlong id) -{ - PWMJNI_LOG(logDEBUG) << "Calling PWMJNI freePWM"; - PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << (void*)id; - int32_t status = 0; - freePWM((void*)id, &status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_PWMJNI_freePWM(JNIEnv* env, jclass, jlong id) { + PWMJNI_LOG(logDEBUG) << "Calling PWMJNI freePWM"; + PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << (void*)id; + int32_t status = 0; + freePWM((void*)id, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -159,15 +154,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_freePWM * Method: setPWMRate * Signature: (D)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMRate - (JNIEnv * env, jclass, jdouble value) -{ - PWMJNI_LOG(logDEBUG) << "Calling PWMJNI setPWMRate"; - PWMJNI_LOG(logDEBUG) << "Rate= " << value; - int32_t status = 0; - setPWMRate(value, &status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMRate( + JNIEnv* env, jclass, jdouble value) { + PWMJNI_LOG(logDEBUG) << "Calling PWMJNI setPWMRate"; + PWMJNI_LOG(logDEBUG) << "Rate= " << value; + int32_t status = 0; + setPWMRate(value, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -175,16 +169,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMRate * Method: setPWMDutyCycle * Signature: (JD)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMDutyCycle - (JNIEnv * env, jclass, jlong id, jdouble value) -{ - PWMJNI_LOG(logDEBUG) << "Calling PWMJNI setPWMDutyCycle"; - PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << (void*)id; - PWMJNI_LOG(logDEBUG) << "DutyCycle= " << value; - int32_t status = 0; - setPWMDutyCycle((void*)id, value, &status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMDutyCycle( + JNIEnv* env, jclass, jlong id, jdouble value) { + PWMJNI_LOG(logDEBUG) << "Calling PWMJNI setPWMDutyCycle"; + PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << (void*)id; + PWMJNI_LOG(logDEBUG) << "DutyCycle= " << value; + int32_t status = 0; + setPWMDutyCycle((void*)id, value, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -192,16 +185,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMDutyCycle * Method: setPWMOutputChannel * Signature: (JI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMOutputChannel - (JNIEnv * env, jclass, jlong id, jint value) -{ - PWMJNI_LOG(logDEBUG) << "Calling PWMJNI setPWMOutputChannel"; - PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << (void*)id; - PWMJNI_LOG(logDEBUG) << "Pin= " << value; - int32_t status = 0; - setPWMOutputChannel((void*)id, (uint32_t) value, &status); - PWMJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_PWMJNI_setPWMOutputChannel( + JNIEnv* env, jclass, jlong id, jint value) { + PWMJNI_LOG(logDEBUG) << "Calling PWMJNI setPWMOutputChannel"; + PWMJNI_LOG(logDEBUG) << "PWM Ptr = " << (void*)id; + PWMJNI_LOG(logDEBUG) << "Pin= " << value; + int32_t status = 0; + setPWMOutputChannel((void*)id, (uint32_t)value, &status); + PWMJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/PowerJNI.cpp b/wpilibj/src/athena/cpp/lib/PowerJNI.cpp index 170daeba6e..faafb9a4e8 100644 --- a/wpilibj/src/athena/cpp/lib/PowerJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/PowerJNI.cpp @@ -5,10 +5,10 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include -#include "edu_wpi_first_wpilibj_hal_PowerJNI.h" #include "HAL/Power.hpp" +#include #include "HALUtil.h" +#include "edu_wpi_first_wpilibj_hal_PowerJNI.h" extern "C" { @@ -17,11 +17,10 @@ extern "C" { * Method: getVinVoltage * Signature: ()F */ -JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getVinVoltage - (JNIEnv * env, jclass) -{ - int32_t status = 0; - float val = getVinVoltage(&status); +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getVinVoltage(JNIEnv* env, jclass) { + int32_t status = 0; + float val = getVinVoltage(&status); CheckStatus(env, status); return val; } @@ -31,11 +30,10 @@ JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getVinVoltage * Method: getVinCurrent * Signature: ()F */ -JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getVinCurrent - (JNIEnv * env, jclass) -{ - int32_t status = 0; - float val = getVinCurrent(&status); +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getVinCurrent(JNIEnv* env, jclass) { + int32_t status = 0; + float val = getVinCurrent(&status); CheckStatus(env, status); return val; } @@ -45,11 +43,10 @@ JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getVinCurrent * Method: getUserVoltage6V * Signature: ()F */ -JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserVoltage6V - (JNIEnv * env, jclass) -{ - int32_t status = 0; - float val = getUserVoltage6V(&status); +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserVoltage6V(JNIEnv* env, jclass) { + int32_t status = 0; + float val = getUserVoltage6V(&status); CheckStatus(env, status); return val; } @@ -59,11 +56,10 @@ JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserVoltage6 * Method: getUserCurrent6V * Signature: ()F */ -JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrent6V - (JNIEnv * env, jclass) -{ - int32_t status = 0; - float val = getUserCurrent6V(&status); +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrent6V(JNIEnv* env, jclass) { + int32_t status = 0; + float val = getUserCurrent6V(&status); CheckStatus(env, status); return val; } @@ -73,11 +69,10 @@ JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrent6 * Method: getUserActive6V * Signature: ()Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserActive6V - (JNIEnv * env, jclass) -{ - int32_t status = 0; - bool val = getUserActive6V(&status); +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserActive6V(JNIEnv* env, jclass) { + int32_t status = 0; + bool val = getUserActive6V(&status); CheckStatus(env, status); return val; } @@ -87,11 +82,11 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserActive * Method: getUserCurrentFaults6V * Signature: ()I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrentFaults6V - (JNIEnv * env, jclass) -{ - int32_t status = 0; - int val = getUserCurrentFaults6V(&status); +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrentFaults6V( + JNIEnv* env, jclass) { + int32_t status = 0; + int val = getUserCurrentFaults6V(&status); CheckStatus(env, status); return val; } @@ -101,11 +96,10 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrentFau * Method: getUserVoltage5V * Signature: ()F */ -JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserVoltage5V - (JNIEnv * env, jclass) -{ - int32_t status = 0; - float val = getUserVoltage5V(&status); +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserVoltage5V(JNIEnv* env, jclass) { + int32_t status = 0; + float val = getUserVoltage5V(&status); CheckStatus(env, status); return val; } @@ -115,11 +109,10 @@ JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserVoltage5 * Method: getUserCurrent5V * Signature: ()F */ -JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrent5V - (JNIEnv * env, jclass) -{ - int32_t status = 0; - float val = getUserCurrent5V(&status); +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrent5V(JNIEnv* env, jclass) { + int32_t status = 0; + float val = getUserCurrent5V(&status); CheckStatus(env, status); return val; } @@ -129,11 +122,10 @@ JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrent5 * Method: getUserActive5V * Signature: ()Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserActive5V - (JNIEnv * env, jclass) -{ - int32_t status = 0; - bool val = getUserActive5V(&status); +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserActive5V(JNIEnv* env, jclass) { + int32_t status = 0; + bool val = getUserActive5V(&status); CheckStatus(env, status); return val; } @@ -143,11 +135,11 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserActive * Method: getUserCurrentFaults5V * Signature: ()I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrentFaults5V - (JNIEnv * env, jclass) -{ - int32_t status = 0; - int val = getUserCurrentFaults5V(&status); +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrentFaults5V( + JNIEnv* env, jclass) { + int32_t status = 0; + int val = getUserCurrentFaults5V(&status); CheckStatus(env, status); return val; } @@ -157,11 +149,10 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrentFau * Method: getUserVoltage3V3 * Signature: ()F */ -JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserVoltage3V3 - (JNIEnv * env, jclass) -{ - int32_t status = 0; - float val = getUserVoltage3V3(&status); +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserVoltage3V3(JNIEnv* env, jclass) { + int32_t status = 0; + float val = getUserVoltage3V3(&status); CheckStatus(env, status); return val; } @@ -171,11 +162,10 @@ JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserVoltage3 * Method: getUserCurrent3V3 * Signature: ()F */ -JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrent3V3 - (JNIEnv * env, jclass) -{ - int32_t status = 0; - float val = getUserCurrent3V3(&status); +JNIEXPORT jfloat JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrent3V3(JNIEnv* env, jclass) { + int32_t status = 0; + float val = getUserCurrent3V3(&status); CheckStatus(env, status); return val; } @@ -185,11 +175,10 @@ JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrent3 * Method: getUserActive3V3 * Signature: ()Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserActive3V3 - (JNIEnv * env, jclass) -{ - int32_t status = 0; - bool val = getUserActive3V3(&status); +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserActive3V3(JNIEnv* env, jclass) { + int32_t status = 0; + bool val = getUserActive3V3(&status); CheckStatus(env, status); return val; } @@ -199,11 +188,11 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserActive * Method: getUserCurrentFaults3V3 * Signature: ()I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrentFaults3V3 - (JNIEnv * env, jclass) -{ - int32_t status = 0; - int val = getUserCurrentFaults3V3(&status); +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_PowerJNI_getUserCurrentFaults3V3( + JNIEnv* env, jclass) { + int32_t status = 0; + int val = getUserCurrentFaults3V3(&status); CheckStatus(env, status); return val; } diff --git a/wpilibj/src/athena/cpp/lib/RelayJNI.cpp b/wpilibj/src/athena/cpp/lib/RelayJNI.cpp index 62046bdf65..05fe1026af 100644 --- a/wpilibj/src/athena/cpp/lib/RelayJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/RelayJNI.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_RelayJNI.h" @@ -17,9 +17,11 @@ // set the logging level TLogLevel relayJNILogLevel = logWARNING; -#define RELAYJNI_LOG(level) \ - if (level > relayJNILogLevel) ; \ - else Log().Get(level) +#define RELAYJNI_LOG(level) \ + if (level > relayJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -28,16 +30,15 @@ extern "C" { * Method: setRelayForward * Signature: (JZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_RelayJNI_setRelayForward - (JNIEnv * env, jclass, jlong id, jboolean value) -{ - RELAYJNI_LOG(logDEBUG) << "Calling RELAYJNI setRelayForward"; - RELAYJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - RELAYJNI_LOG(logDEBUG) << "Flag = " << (jint)value; - int32_t status = 0; - setRelayForward((void*)id, value, &status); - RELAYJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_RelayJNI_setRelayForward( + JNIEnv* env, jclass, jlong id, jboolean value) { + RELAYJNI_LOG(logDEBUG) << "Calling RELAYJNI setRelayForward"; + RELAYJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + RELAYJNI_LOG(logDEBUG) << "Flag = " << (jint)value; + int32_t status = 0; + setRelayForward((void*)id, value, &status); + RELAYJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -45,16 +46,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_RelayJNI_setRelayForward * Method: setRelayReverse * Signature: (JZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_RelayJNI_setRelayReverse - (JNIEnv * env, jclass, jlong id, jboolean value) -{ - RELAYJNI_LOG(logDEBUG) << "Calling RELAYJNI setRelayReverse"; - RELAYJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - RELAYJNI_LOG(logDEBUG) << "Flag = " << (jint)value; - int32_t status = 0; - setRelayReverse((void*)id, value, &status); - RELAYJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_RelayJNI_setRelayReverse( + JNIEnv* env, jclass, jlong id, jboolean value) { + RELAYJNI_LOG(logDEBUG) << "Calling RELAYJNI setRelayReverse"; + RELAYJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + RELAYJNI_LOG(logDEBUG) << "Flag = " << (jint)value; + int32_t status = 0; + setRelayReverse((void*)id, value, &status); + RELAYJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -62,17 +62,17 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_RelayJNI_setRelayReverse * Method: getRelayForward * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_RelayJNI_getRelayForward - (JNIEnv * env, jclass, jlong id) -{ - RELAYJNI_LOG(logDEBUG) << "Calling RELAYJNI getRelayForward"; - RELAYJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = getRelayForward((void*)id, &status); - RELAYJNI_LOG(logDEBUG) << "Status = " << status; - RELAYJNI_LOG(logDEBUG) << "getRelayForwardResult = " << (jint)returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_RelayJNI_getRelayForward( + JNIEnv* env, jclass, jlong id) { + RELAYJNI_LOG(logDEBUG) << "Calling RELAYJNI getRelayForward"; + RELAYJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + int32_t status = 0; + jboolean returnValue = getRelayForward((void*)id, &status); + RELAYJNI_LOG(logDEBUG) << "Status = " << status; + RELAYJNI_LOG(logDEBUG) << "getRelayForwardResult = " << (jint)returnValue; + CheckStatus(env, status); + return returnValue; } /* @@ -80,17 +80,17 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_RelayJNI_getRelayForwa * Method: getRelayReverse * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_RelayJNI_getRelayReverse - (JNIEnv * env, jclass, jlong id) -{ - RELAYJNI_LOG(logDEBUG) << "Calling RELAYJNI getRelayReverse"; - RELAYJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - int32_t status = 0; - jboolean returnValue = getRelayReverse((void*)id, &status); - RELAYJNI_LOG(logDEBUG) << "Status = " << status; - RELAYJNI_LOG(logDEBUG) << "getRelayReverseResult = " << (jint)returnValue; - CheckStatus(env, status); - return returnValue; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_RelayJNI_getRelayReverse( + JNIEnv* env, jclass, jlong id) { + RELAYJNI_LOG(logDEBUG) << "Calling RELAYJNI getRelayReverse"; + RELAYJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; + int32_t status = 0; + jboolean returnValue = getRelayReverse((void*)id, &status); + RELAYJNI_LOG(logDEBUG) << "Status = " << status; + RELAYJNI_LOG(logDEBUG) << "getRelayReverseResult = " << (jint)returnValue; + CheckStatus(env, status); + return returnValue; } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/SPIJNI.cpp b/wpilibj/src/athena/cpp/lib/SPIJNI.cpp index f5aca21c7a..83a0c38f97 100644 --- a/wpilibj/src/athena/cpp/lib/SPIJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/SPIJNI.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_SPIJNI.h" @@ -17,9 +17,11 @@ // set the logging level TLogLevel spiJNILogLevel = logWARNING; -#define SPIJNI_LOG(level) \ - if (level > spiJNILogLevel) ; \ - else Log().Get(level) +#define SPIJNI_LOG(level) \ + if (level > spiJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -28,15 +30,14 @@ extern "C" { * Method: spiInitialize * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiInitialize - (JNIEnv * env, jclass, jbyte port) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiInitialize"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; - spiInitialize(port, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiInitialize( + JNIEnv *env, jclass, jbyte port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiInitialize"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; + spiInitialize(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -44,22 +45,23 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiInitialize * Method: spiTransaction * Signature: (BLjava/nio/ByteBuffer;Ljava/nio/ByteBuffer;B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiTransaction - (JNIEnv * env, jclass, jbyte port, jobject dataToSend, jobject dataReceived, jbyte size) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiTransaction"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - uint8_t* dataToSendPtr = nullptr; - if (dataToSend != 0) { - dataToSendPtr = (uint8_t*)env->GetDirectBufferAddress(dataToSend); - } - uint8_t* dataReceivedPtr = (uint8_t*)env->GetDirectBufferAddress(dataReceived); - SPIJNI_LOG(logDEBUG) << "Size = " << (jint)size; - SPIJNI_LOG(logDEBUG) << "DataToSendPtr = " << dataToSendPtr; - SPIJNI_LOG(logDEBUG) << "DataReceivedPtr = " << dataReceivedPtr; - jint retVal = spiTransaction(port, dataToSendPtr, dataReceivedPtr, size); - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << (jint)retVal; - return retVal; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiTransaction( + JNIEnv *env, jclass, jbyte port, jobject dataToSend, jobject dataReceived, + jbyte size) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiTransaction"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + uint8_t *dataToSendPtr = nullptr; + if (dataToSend != 0) { + dataToSendPtr = (uint8_t *)env->GetDirectBufferAddress(dataToSend); + } + uint8_t *dataReceivedPtr = + (uint8_t *)env->GetDirectBufferAddress(dataReceived); + SPIJNI_LOG(logDEBUG) << "Size = " << (jint)size; + SPIJNI_LOG(logDEBUG) << "DataToSendPtr = " << dataToSendPtr; + SPIJNI_LOG(logDEBUG) << "DataReceivedPtr = " << dataReceivedPtr; + jint retVal = spiTransaction(port, dataToSendPtr, dataReceivedPtr, size); + SPIJNI_LOG(logDEBUG) << "ReturnValue = " << (jint)retVal; + return retVal; } /* @@ -67,53 +69,49 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiTransaction * Method: spiWrite * Signature: (BLjava/nio/ByteBuffer;B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiWrite - (JNIEnv * env, jclass, jbyte port, jobject dataToSend, jbyte size) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiWrite"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - uint8_t* dataToSendPtr = nullptr; - if (dataToSend != 0) { - dataToSendPtr = (uint8_t*)env->GetDirectBufferAddress(dataToSend); - } - SPIJNI_LOG(logDEBUG) << "Size = " << (jint)size; - SPIJNI_LOG(logDEBUG) << "DataToSendPtr = " << dataToSendPtr; - jint retVal = spiWrite(port, dataToSendPtr, size); - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << (jint)retVal; - return retVal; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiWrite( + JNIEnv *env, jclass, jbyte port, jobject dataToSend, jbyte size) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiWrite"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + uint8_t *dataToSendPtr = nullptr; + if (dataToSend != 0) { + dataToSendPtr = (uint8_t *)env->GetDirectBufferAddress(dataToSend); + } + SPIJNI_LOG(logDEBUG) << "Size = " << (jint)size; + SPIJNI_LOG(logDEBUG) << "DataToSendPtr = " << dataToSendPtr; + jint retVal = spiWrite(port, dataToSendPtr, size); + SPIJNI_LOG(logDEBUG) << "ReturnValue = " << (jint)retVal; + return retVal; } - /* * Class: edu_wpi_first_wpilibj_hal_SPIJNI * Method: spiRead * Signature: (BLjava/nio/ByteBuffer;B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiRead - (JNIEnv * env, jclass, jbyte port, jobject dataReceived, jbyte size) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiRead"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - uint8_t* dataReceivedPtr = (uint8_t*)env->GetDirectBufferAddress(dataReceived); - SPIJNI_LOG(logDEBUG) << "Size = " << (jint)size; - SPIJNI_LOG(logDEBUG) << "DataReceivedPtr = " << dataReceivedPtr; - jint retVal = spiRead(port, (uint8_t*)dataReceivedPtr, size); - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << (jint)retVal; - return retVal; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiRead( + JNIEnv *env, jclass, jbyte port, jobject dataReceived, jbyte size) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiRead"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + uint8_t *dataReceivedPtr = + (uint8_t *)env->GetDirectBufferAddress(dataReceived); + SPIJNI_LOG(logDEBUG) << "Size = " << (jint)size; + SPIJNI_LOG(logDEBUG) << "DataReceivedPtr = " << dataReceivedPtr; + jint retVal = spiRead(port, (uint8_t *)dataReceivedPtr, size); + SPIJNI_LOG(logDEBUG) << "ReturnValue = " << (jint)retVal; + return retVal; } - /* * Class: edu_wpi_first_wpilibj_hal_SPIJNI * Method: spiClose * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiClose - (JNIEnv *, jclass, jbyte port) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiClose"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - spiClose(port); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiClose(JNIEnv *, jclass, jbyte port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiClose"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + spiClose(port); } /* @@ -121,13 +119,12 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiClose * Method: spiSetSpeed * Signature: (BI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetSpeed - (JNIEnv *, jclass, jbyte port, jint speed) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetSpeed"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - SPIJNI_LOG(logDEBUG) << "Speed = " << (jint) speed; - spiSetSpeed(port, speed); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetSpeed( + JNIEnv *, jclass, jbyte port, jint speed) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetSpeed"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + SPIJNI_LOG(logDEBUG) << "Speed = " << (jint)speed; + spiSetSpeed(port, speed); } /* @@ -135,15 +132,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetSpeed * Method: spiSetOpts * Signature: (BIII)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetOpts - (JNIEnv *, jclass, jbyte port, jint msb_first, jint sample_on_trailing, jint clk_idle_high) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetOpts"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - SPIJNI_LOG(logDEBUG) << "msb_first = " << msb_first; - SPIJNI_LOG(logDEBUG) << "sample_on_trailing = " << sample_on_trailing; - SPIJNI_LOG(logDEBUG) << "clk_idle_high = " << clk_idle_high; - spiSetOpts(port, msb_first, sample_on_trailing, clk_idle_high); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetOpts( + JNIEnv *, jclass, jbyte port, jint msb_first, jint sample_on_trailing, + jint clk_idle_high) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetOpts"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + SPIJNI_LOG(logDEBUG) << "msb_first = " << msb_first; + SPIJNI_LOG(logDEBUG) << "sample_on_trailing = " << sample_on_trailing; + SPIJNI_LOG(logDEBUG) << "clk_idle_high = " << clk_idle_high; + spiSetOpts(port, msb_first, sample_on_trailing, clk_idle_high); } /* @@ -151,15 +148,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetOpts * Method: spiSetChipSelectActiveHigh * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetChipSelectActiveHigh - (JNIEnv * env, jclass, jbyte port) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetCSActiveHigh"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; - spiSetChipSelectActiveHigh(port, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetChipSelectActiveHigh( + JNIEnv *env, jclass, jbyte port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetCSActiveHigh"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; + spiSetChipSelectActiveHigh(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -167,15 +164,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetChipSelectAct * Method: spiSetChipSelectActiveLow * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetChipSelectActiveLow - (JNIEnv * env, jclass, jbyte port) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetCSActiveLow"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; - spiSetChipSelectActiveLow(port, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetChipSelectActiveLow( + JNIEnv *env, jclass, jbyte port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetCSActiveLow"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; + spiSetChipSelectActiveLow(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -183,25 +180,26 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetChipSelectAct * Method: spiInitAccumulator * Signature: (BIIBIIBBZZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiInitAccumulator - (JNIEnv *env, jclass, jbyte port, jint period, jint cmd, jbyte xferSize, jint validMask, jint validValue, jbyte dataShift, jbyte dataSize, jboolean isSigned, jboolean bigEndian) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiInitAccumulator"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - SPIJNI_LOG(logDEBUG) << "Period = " << period; - SPIJNI_LOG(logDEBUG) << "Cmd = " << cmd; - SPIJNI_LOG(logDEBUG) << "XferSize = " << (jint) xferSize; - SPIJNI_LOG(logDEBUG) << "ValidMask = " << validMask; - SPIJNI_LOG(logDEBUG) << "ValidValue = " << validValue; - SPIJNI_LOG(logDEBUG) << "DataShift = " << (jint) dataShift; - SPIJNI_LOG(logDEBUG) << "DataSize = " << (jint) dataSize; - SPIJNI_LOG(logDEBUG) << "IsSigned = " << (jint) isSigned; - SPIJNI_LOG(logDEBUG) << "BigEndian = " << (jint) bigEndian; - int32_t status = 0; - spiInitAccumulator(port, period, cmd, xferSize, validMask, validValue, - dataShift, dataSize, isSigned, bigEndian, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiInitAccumulator( + JNIEnv *env, jclass, jbyte port, jint period, jint cmd, jbyte xferSize, + jint validMask, jint validValue, jbyte dataShift, jbyte dataSize, + jboolean isSigned, jboolean bigEndian) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiInitAccumulator"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + SPIJNI_LOG(logDEBUG) << "Period = " << period; + SPIJNI_LOG(logDEBUG) << "Cmd = " << cmd; + SPIJNI_LOG(logDEBUG) << "XferSize = " << (jint)xferSize; + SPIJNI_LOG(logDEBUG) << "ValidMask = " << validMask; + SPIJNI_LOG(logDEBUG) << "ValidValue = " << validValue; + SPIJNI_LOG(logDEBUG) << "DataShift = " << (jint)dataShift; + SPIJNI_LOG(logDEBUG) << "DataSize = " << (jint)dataSize; + SPIJNI_LOG(logDEBUG) << "IsSigned = " << (jint)isSigned; + SPIJNI_LOG(logDEBUG) << "BigEndian = " << (jint)bigEndian; + int32_t status = 0; + spiInitAccumulator(port, period, cmd, xferSize, validMask, validValue, + dataShift, dataSize, isSigned, bigEndian, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -209,15 +207,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiInitAccumulator * Method: spiFreeAccumulator * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiFreeAccumulator - (JNIEnv *env, jclass, jbyte port) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiFreeAccumulator"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; - spiFreeAccumulator(port, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiFreeAccumulator( + JNIEnv *env, jclass, jbyte port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiFreeAccumulator"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; + spiFreeAccumulator(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -225,15 +222,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiFreeAccumulator * Method: spiResetAccumulator * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiResetAccumulator - (JNIEnv *env, jclass, jbyte port) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiResetAccumulator"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; - spiResetAccumulator(port, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiResetAccumulator( + JNIEnv *env, jclass, jbyte port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiResetAccumulator"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; + spiResetAccumulator(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -241,16 +238,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiResetAccumulator * Method: spiSetAccumulatorCenter * Signature: (BI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAccumulatorCenter - (JNIEnv *env, jclass, jbyte port, jint center) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetAccumulatorCenter"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - SPIJNI_LOG(logDEBUG) << "Center = " << center; - int32_t status = 0; - spiSetAccumulatorCenter(port, center, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAccumulatorCenter( + JNIEnv *env, jclass, jbyte port, jint center) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetAccumulatorCenter"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + SPIJNI_LOG(logDEBUG) << "Center = " << center; + int32_t status = 0; + spiSetAccumulatorCenter(port, center, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -258,16 +255,16 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAccumulatorCe * Method: spiSetAccumulatorDeadband * Signature: (BI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAccumulatorDeadband - (JNIEnv *env, jclass, jbyte port, jint deadband) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetAccumulatorDeadband"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - SPIJNI_LOG(logDEBUG) << "Deadband = " << deadband; - int32_t status = 0; - spiSetAccumulatorDeadband(port, deadband, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAccumulatorDeadband( + JNIEnv *env, jclass, jbyte port, jint deadband) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiSetAccumulatorDeadband"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + SPIJNI_LOG(logDEBUG) << "Deadband = " << deadband; + int32_t status = 0; + spiSetAccumulatorDeadband(port, deadband, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -275,17 +272,17 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiSetAccumulatorDe * Method: spiGetAccumulatorLastValue * Signature: (B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorLastValue - (JNIEnv *env, jclass, jbyte port) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorLastValue"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; - jint retVal = spiGetAccumulatorLastValue(port, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; - CheckStatus(env, status); - return retVal; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorLastValue( + JNIEnv *env, jclass, jbyte port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorLastValue"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; + jint retVal = spiGetAccumulatorLastValue(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; + CheckStatus(env, status); + return retVal; } /* @@ -293,17 +290,17 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorLa * Method: spiGetAccumulatorValue * Signature: (B)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorValue - (JNIEnv *env, jclass, jbyte port) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorValue"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; - jlong retVal = spiGetAccumulatorValue(port, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; - CheckStatus(env, status); - return retVal; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorValue( + JNIEnv *env, jclass, jbyte port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorValue"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; + jlong retVal = spiGetAccumulatorValue(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; + CheckStatus(env, status); + return retVal; } /* @@ -311,17 +308,17 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorV * Method: spiGetAccumulatorCount * Signature: (B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorCount - (JNIEnv *env, jclass, jbyte port) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorCount"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; - jint retVal = spiGetAccumulatorCount(port, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; - CheckStatus(env, status); - return retVal; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorCount( + JNIEnv *env, jclass, jbyte port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorCount"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; + jint retVal = spiGetAccumulatorCount(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; + CheckStatus(env, status); + return retVal; } /* @@ -329,17 +326,17 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorCo * Method: spiGetAccumulatorAverage * Signature: (B)D */ -JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorAverage - (JNIEnv *env, jclass, jbyte port) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorAverage"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; - jdouble retVal = spiGetAccumulatorAverage(port, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; - CheckStatus(env, status); - return retVal; +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorAverage( + JNIEnv *env, jclass, jbyte port) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorAverage"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; + jdouble retVal = spiGetAccumulatorAverage(port, &status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + SPIJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; + CheckStatus(env, status); + return retVal; } /* @@ -347,22 +344,22 @@ JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulato * Method: spiGetAccumulatorOutput * Signature: (BLjava/nio/LongBuffer;Ljava/nio/IntBuffer;)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorOutput - (JNIEnv *env, jclass, jbyte port, jobject value, jobject count) -{ - SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorOutput"; - SPIJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SPIJNI_spiGetAccumulatorOutput( + JNIEnv *env, jclass, jbyte port, jobject value, jobject count) { + SPIJNI_LOG(logDEBUG) << "Calling SPIJNI spiGetAccumulatorOutput"; + SPIJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; - jlong * valuePtr = (jlong*)env->GetDirectBufferAddress(value); - uint32_t * countPtr = (uint32_t*)env->GetDirectBufferAddress(count); + jlong *valuePtr = (jlong *)env->GetDirectBufferAddress(value); + uint32_t *countPtr = (uint32_t *)env->GetDirectBufferAddress(count); - spiGetAccumulatorOutput(port, valuePtr, countPtr, &status); + spiGetAccumulatorOutput(port, valuePtr, countPtr, &status); - SPIJNI_LOG(logDEBUG) << "Status = " << status; - SPIJNI_LOG(logDEBUG) << "Value = " << *valuePtr; - SPIJNI_LOG(logDEBUG) << "Count = " << *countPtr; - CheckStatus(env, status); + SPIJNI_LOG(logDEBUG) << "Status = " << status; + SPIJNI_LOG(logDEBUG) << "Value = " << *valuePtr; + SPIJNI_LOG(logDEBUG) << "Count = " << *countPtr; + CheckStatus(env, status); } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/SafeThread.cpp b/wpilibj/src/athena/cpp/lib/SafeThread.cpp index 24d94405fc..ca931a6db6 100644 --- a/wpilibj/src/athena/cpp/lib/SafeThread.cpp +++ b/wpilibj/src/athena/cpp/lib/SafeThread.cpp @@ -7,7 +7,7 @@ #include "SafeThread.h" -//using namespace nt; +// using namespace nt; void detail::SafeThreadOwnerBase::Start(SafeThread* thr) { SafeThread* curthr = nullptr; diff --git a/wpilibj/src/athena/cpp/lib/SafeThread.h b/wpilibj/src/athena/cpp/lib/SafeThread.h index bd5c2ffc1a..b20c83bc76 100644 --- a/wpilibj/src/athena/cpp/lib/SafeThread.h +++ b/wpilibj/src/athena/cpp/lib/SafeThread.h @@ -13,7 +13,7 @@ #include #include -//namespace nt { +// namespace nt { // Base class for SafeThreadOwner threads. class SafeThread { diff --git a/wpilibj/src/athena/cpp/lib/SerialPortJNI.cpp b/wpilibj/src/athena/cpp/lib/SerialPortJNI.cpp index 794298563d..55281d297a 100644 --- a/wpilibj/src/athena/cpp/lib/SerialPortJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/SerialPortJNI.cpp @@ -5,8 +5,8 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -#include #include +#include #include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_SerialPortJNI.h" @@ -17,9 +17,11 @@ // set the logging level TLogLevel serialJNILogLevel = logWARNING; -#define SERIALJNI_LOG(level) \ - if (level > serialJNILogLevel) ; \ - else Log().Get(level) +#define SERIALJNI_LOG(level) \ + if (level > serialJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -28,15 +30,15 @@ extern "C" { * Method: serialInitializePort * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialInitializePort - (JNIEnv * env, jclass, jbyte port) -{ - SERIALJNI_LOG(logDEBUG) << "Calling Serial Initialize"; - SERIALJNI_LOG(logDEBUG) << "Port = " << (jint) port; - int32_t status = 0; - serialInitializePort(port, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialInitializePort( + JNIEnv* env, jclass, jbyte port) { + SERIALJNI_LOG(logDEBUG) << "Calling Serial Initialize"; + SERIALJNI_LOG(logDEBUG) << "Port = " << (jint)port; + int32_t status = 0; + serialInitializePort(port, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -44,15 +46,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialInitia * Method: serialSetBaudRate * Signature: (BI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetBaudRate - (JNIEnv * env, jclass, jbyte port, jint rate) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Baud Rate"; - SERIALJNI_LOG(logDEBUG) << "Baud: " << rate; - int32_t status = 0; - serialSetBaudRate(port, rate, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetBaudRate( + JNIEnv* env, jclass, jbyte port, jint rate) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Baud Rate"; + SERIALJNI_LOG(logDEBUG) << "Baud: " << rate; + int32_t status = 0; + serialSetBaudRate(port, rate, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -60,15 +62,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetBau * Method: serialSetDataBits * Signature: (BB)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetDataBits - (JNIEnv * env, jclass, jbyte port, jbyte bits) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Data Bits"; - SERIALJNI_LOG(logDEBUG) << "Data Bits: " << bits; - int32_t status = 0; - serialSetDataBits(port, bits, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetDataBits( + JNIEnv* env, jclass, jbyte port, jbyte bits) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Data Bits"; + SERIALJNI_LOG(logDEBUG) << "Data Bits: " << bits; + int32_t status = 0; + serialSetDataBits(port, bits, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -76,15 +78,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetDat * Method: serialSetParity * Signature: (BB)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetParity - (JNIEnv * env, jclass, jbyte port, jbyte parity) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Parity"; - SERIALJNI_LOG(logDEBUG) << "Parity: " << parity; - int32_t status = 0; - serialSetParity(port, parity, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetParity( + JNIEnv* env, jclass, jbyte port, jbyte parity) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Parity"; + SERIALJNI_LOG(logDEBUG) << "Parity: " << parity; + int32_t status = 0; + serialSetParity(port, parity, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -92,15 +94,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetPar * Method: serialSetStopBits * Signature: (BB)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetStopBits - (JNIEnv * env, jclass, jbyte port, jbyte bits) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Stop Bits"; - SERIALJNI_LOG(logDEBUG) << "Stop Bits: " << bits; - int32_t status = 0; - serialSetStopBits(port, bits, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetStopBits( + JNIEnv* env, jclass, jbyte port, jbyte bits) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Stop Bits"; + SERIALJNI_LOG(logDEBUG) << "Stop Bits: " << bits; + int32_t status = 0; + serialSetStopBits(port, bits, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -108,15 +110,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetSto * Method: serialSetWriteMode * Signature: (BB)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetWriteMode - (JNIEnv * env, jclass, jbyte port, jbyte mode) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Write Mode"; - SERIALJNI_LOG(logDEBUG) << "Write mode: " << mode; - int32_t status = 0; - serialSetWriteMode(port, mode, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetWriteMode( + JNIEnv* env, jclass, jbyte port, jbyte mode) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Write Mode"; + SERIALJNI_LOG(logDEBUG) << "Write mode: " << mode; + int32_t status = 0; + serialSetWriteMode(port, mode, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -124,15 +126,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetWri * Method: serialSetFlowControl * Signature: (BB)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetFlowControl - (JNIEnv * env, jclass, jbyte port, jbyte flow) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Flow Control"; - SERIALJNI_LOG(logDEBUG) << "Flow Control: " << flow; - int32_t status = 0; - serialSetFlowControl(port, flow, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetFlowControl( + JNIEnv* env, jclass, jbyte port, jbyte flow) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Flow Control"; + SERIALJNI_LOG(logDEBUG) << "Flow Control: " << flow; + int32_t status = 0; + serialSetFlowControl(port, flow, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -140,15 +142,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetFlo * Method: serialSetTimeout * Signature: (BF)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetTimeout - (JNIEnv * env, jclass, jbyte port, jfloat timeout) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Timeout"; - SERIALJNI_LOG(logDEBUG) << "Timeout: " << timeout; - int32_t status = 0; - serialSetTimeout(port, timeout, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetTimeout( + JNIEnv* env, jclass, jbyte port, jfloat timeout) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Timeout"; + SERIALJNI_LOG(logDEBUG) << "Timeout: " << timeout; + int32_t status = 0; + serialSetTimeout(port, timeout, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -156,15 +158,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetTim * Method: serialEnableTermination * Signature: (BC)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialEnableTermination - (JNIEnv * env, jclass, jbyte port, jchar terminator) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Enable Termination"; - SERIALJNI_LOG(logDEBUG) << "Terminator: " << terminator; - int32_t status = 0; - serialEnableTermination(port, terminator, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialEnableTermination( + JNIEnv* env, jclass, jbyte port, jchar terminator) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Enable Termination"; + SERIALJNI_LOG(logDEBUG) << "Terminator: " << terminator; + int32_t status = 0; + serialEnableTermination(port, terminator, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -172,14 +174,14 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialEnable * Method: serialDisableTermination * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialDisableTermination - (JNIEnv * env, jclass, jbyte port) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Disable termination"; - int32_t status = 0; - serialDisableTermination(port, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialDisableTermination( + JNIEnv* env, jclass, jbyte port) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Disable termination"; + int32_t status = 0; + serialDisableTermination(port, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -187,15 +189,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialDisabl * Method: serialSetReadBufferSize * Signature: (BI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetReadBufferSize - (JNIEnv * env, jclass, jbyte port, jint size) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Read Buffer Size"; - SERIALJNI_LOG(logDEBUG) << "Size: " << size; - int32_t status = 0; - serialSetReadBufferSize(port, size, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetReadBufferSize( + JNIEnv* env, jclass, jbyte port, jint size) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Read Buffer Size"; + SERIALJNI_LOG(logDEBUG) << "Size: " << size; + int32_t status = 0; + serialSetReadBufferSize(port, size, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -203,15 +205,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetRea * Method: serialSetWriteBufferSize * Signature: (BI)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetWriteBufferSize - (JNIEnv * env, jclass, jbyte port, jint size) -{ - SERIALJNI_LOG(logDEBUG) << "Setting Serial Write Buffer Size"; - SERIALJNI_LOG(logDEBUG) << "Size: " << size; - int32_t status = 0; - serialSetWriteBufferSize(port, size, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetWriteBufferSize( + JNIEnv* env, jclass, jbyte port, jint size) { + SERIALJNI_LOG(logDEBUG) << "Setting Serial Write Buffer Size"; + SERIALJNI_LOG(logDEBUG) << "Size: " << size; + int32_t status = 0; + serialSetWriteBufferSize(port, size, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -219,15 +221,15 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialSetWri * Method: serialGetBytesRecieved * Signature: (B)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialGetBytesRecieved - (JNIEnv * env, jclass, jbyte port) -{ - SERIALJNI_LOG(logDEBUG) << "Serial Get Bytes Received"; - int32_t status = 0; - jint retVal = serialGetBytesReceived(port, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); - return retVal; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialGetBytesRecieved( + JNIEnv* env, jclass, jbyte port) { + SERIALJNI_LOG(logDEBUG) << "Serial Get Bytes Received"; + int32_t status = 0; + jint retVal = serialGetBytesReceived(port, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); + return retVal; } /* @@ -235,18 +237,17 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialGetByt * Method: serialRead * Signature: (BLjava/nio/ByteBuffer;I)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialRead - (JNIEnv * env, jclass, jbyte port, jobject dataReceived, jint size) -{ - SERIALJNI_LOG(logDEBUG) << "Serial Read"; - jbyte * dataReceivedPtr = NULL; - dataReceivedPtr = (jbyte*)env->GetDirectBufferAddress(dataReceived); - int32_t status = 0; - jint retVal = serialRead(port, (char*)dataReceivedPtr, size, &status); - SERIALJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); - return retVal; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialRead( + JNIEnv* env, jclass, jbyte port, jobject dataReceived, jint size) { + SERIALJNI_LOG(logDEBUG) << "Serial Read"; + jbyte* dataReceivedPtr = NULL; + dataReceivedPtr = (jbyte*)env->GetDirectBufferAddress(dataReceived); + int32_t status = 0; + jint retVal = serialRead(port, (char*)dataReceivedPtr, size, &status); + SERIALJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); + return retVal; } /* @@ -254,35 +255,33 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialRead * Method: serialWrite * Signature: (BLjava/nio/ByteBuffer;I)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialWrite - (JNIEnv * env, jclass, jbyte port, jobject dataToSend, jint size) -{ - SERIALJNI_LOG(logDEBUG) << "Serial Write"; - jbyte * dataToSendPtr = NULL; - if(dataToSend != 0){ - dataToSendPtr = (jbyte*)env->GetDirectBufferAddress(dataToSend); - } - int32_t status = 0; - jint retVal = serialWrite(port, (const char*)dataToSendPtr, size, &status); - SERIALJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); - return retVal; +JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialWrite( + JNIEnv* env, jclass, jbyte port, jobject dataToSend, jint size) { + SERIALJNI_LOG(logDEBUG) << "Serial Write"; + jbyte* dataToSendPtr = NULL; + if (dataToSend != 0) { + dataToSendPtr = (jbyte*)env->GetDirectBufferAddress(dataToSend); + } + int32_t status = 0; + jint retVal = serialWrite(port, (const char*)dataToSendPtr, size, &status); + SERIALJNI_LOG(logDEBUG) << "ReturnValue = " << retVal; + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); + return retVal; } - + /* * Class: edu_wpi_first_wpilibj_hal_SerialPortJNI * Method: serialFlush * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialFlush - (JNIEnv * env, jclass, jbyte port) -{ - SERIALJNI_LOG(logDEBUG) << "Serial Flush"; - int32_t status = 0; - serialFlush(port, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialFlush( + JNIEnv* env, jclass, jbyte port) { + SERIALJNI_LOG(logDEBUG) << "Serial Flush"; + int32_t status = 0; + serialFlush(port, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -290,14 +289,13 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialFlush * Method: serialClear * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialClear - (JNIEnv * env, jclass, jbyte port) -{ - SERIALJNI_LOG(logDEBUG) << "Serial Clear"; - int32_t status = 0; - serialClear(port, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialClear( + JNIEnv* env, jclass, jbyte port) { + SERIALJNI_LOG(logDEBUG) << "Serial Clear"; + int32_t status = 0; + serialClear(port, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } /* @@ -305,14 +303,13 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialClear * Method: serialClose * Signature: (B)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialClose - (JNIEnv * env, jclass, jbyte port) -{ - SERIALJNI_LOG(logDEBUG) << "Serial Close"; - int32_t status = 0; - serialClose(port, &status); - SERIALJNI_LOG(logDEBUG) << "Status = " << status; - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialClose( + JNIEnv* env, jclass, jbyte port) { + SERIALJNI_LOG(logDEBUG) << "Serial Close"; + int32_t status = 0; + serialClose(port, &status); + SERIALJNI_LOG(logDEBUG) << "Status = " << status; + CheckStatus(env, status); } } // extern "C" diff --git a/wpilibj/src/athena/cpp/lib/SolenoidJNI.cpp b/wpilibj/src/athena/cpp/lib/SolenoidJNI.cpp index 3ee13c84a2..c8a70775f9 100644 --- a/wpilibj/src/athena/cpp/lib/SolenoidJNI.cpp +++ b/wpilibj/src/athena/cpp/lib/SolenoidJNI.cpp @@ -6,8 +6,8 @@ /*----------------------------------------------------------------------------*/ #include -#include "Log.hpp" #include "HAL/HAL.hpp" +#include "Log.hpp" #include "edu_wpi_first_wpilibj_hal_SolenoidJNI.h" @@ -15,9 +15,11 @@ TLogLevel solenoidJNILogLevel = logERROR; -#define SOLENOIDJNI_LOG(level) \ - if (level > solenoidJNILogLevel) ; \ - else Log().Get(level) +#define SOLENOIDJNI_LOG(level) \ + if (level > solenoidJNILogLevel) \ + ; \ + else \ + Log().Get(level) extern "C" { @@ -26,26 +28,30 @@ extern "C" { * Method: initializeSolenoidPort * Signature: (J)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_initializeSolenoidPort - (JNIEnv *env, jclass, jlong port_pointer) -{ - SOLENOIDJNI_LOG(logDEBUG) << "Calling SolenoidJNI initializeSolenoidPort"; - - SOLENOIDJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)port_pointer; - char *aschars = (char *)port_pointer; - SOLENOIDJNI_LOG(logDEBUG) << '\t' << (int)aschars[0] << '\t' << (int)aschars[1] << std::endl; - - int32_t status = 0; - void* solenoid_port_pointer = initializeSolenoidPort((void*)port_pointer, &status); - - SOLENOIDJNI_LOG(logDEBUG) << "Status = " << status; - SOLENOIDJNI_LOG(logDEBUG) << "Solenoid Port Pointer = " << solenoid_port_pointer; - - int *asints = (int *)solenoid_port_pointer; - SOLENOIDJNI_LOG(logDEBUG) << '\t' << asints[0] << '\t' << asints[1] << std::endl; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_initializeSolenoidPort( + JNIEnv *env, jclass, jlong port_pointer) { + SOLENOIDJNI_LOG(logDEBUG) << "Calling SolenoidJNI initializeSolenoidPort"; - CheckStatus(env, status); - return (jlong)solenoid_port_pointer; + SOLENOIDJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)port_pointer; + char *aschars = (char *)port_pointer; + SOLENOIDJNI_LOG(logDEBUG) << '\t' << (int)aschars[0] << '\t' + << (int)aschars[1] << std::endl; + + int32_t status = 0; + void *solenoid_port_pointer = + initializeSolenoidPort((void *)port_pointer, &status); + + SOLENOIDJNI_LOG(logDEBUG) << "Status = " << status; + SOLENOIDJNI_LOG(logDEBUG) << "Solenoid Port Pointer = " + << solenoid_port_pointer; + + int *asints = (int *)solenoid_port_pointer; + SOLENOIDJNI_LOG(logDEBUG) << '\t' << asints[0] << '\t' << asints[1] + << std::endl; + + CheckStatus(env, status); + return (jlong)solenoid_port_pointer; } /* @@ -53,13 +59,13 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_initializeSol * Method: freeSolenoidPort * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_freeSolenoidPort - (JNIEnv *env, jclass, jlong id) -{ - SOLENOIDJNI_LOG(logDEBUG) << "Calling SolenoidJNI initializeSolenoidPort"; +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_freeSolenoidPort( + JNIEnv *env, jclass, jlong id) { + SOLENOIDJNI_LOG(logDEBUG) << "Calling SolenoidJNI initializeSolenoidPort"; - SOLENOIDJNI_LOG(logDEBUG) << "Port Ptr = " << (void*)id; - freeSolenoidPort((void*)id); + SOLENOIDJNI_LOG(logDEBUG) << "Port Ptr = " << (void *)id; + freeSolenoidPort((void *)id); } /* @@ -67,12 +73,12 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_freeSolenoidPo * Method: getPortWithModule * Signature: (BB)J */ -JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getPortWithModule - (JNIEnv *env, jclass, jbyte module, jbyte channel) -{ - void* port_pointer = getPortWithModule(module, channel); - - return (jlong)port_pointer; +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getPortWithModule( + JNIEnv *env, jclass, jbyte module, jbyte channel) { + void *port_pointer = getPortWithModule(module, channel); + + return (jlong)port_pointer; } /* @@ -80,16 +86,16 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getPortWithMo * Method: setSolenoid * Signature: (JZ)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_setSolenoid - (JNIEnv *env, jclass, jlong solenoid_port, jboolean value) -{ - SOLENOIDJNI_LOG(logDEBUG) << "Calling SolenoidJNI SetSolenoid"; - - SOLENOIDJNI_LOG(logDEBUG) << "Solenoid Port Pointer = " << (void*)solenoid_port; - - int32_t status = 0; - setSolenoid((void*)solenoid_port, value, &status); - CheckStatus(env, status); +JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_setSolenoid( + JNIEnv *env, jclass, jlong solenoid_port, jboolean value) { + SOLENOIDJNI_LOG(logDEBUG) << "Calling SolenoidJNI SetSolenoid"; + + SOLENOIDJNI_LOG(logDEBUG) << "Solenoid Port Pointer = " + << (void *)solenoid_port; + + int32_t status = 0; + setSolenoid((void *)solenoid_port, value, &status); + CheckStatus(env, status); } /* @@ -97,13 +103,13 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_setSolenoid * Method: getSolenoid * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getSolenoid - (JNIEnv *env, jclass, jlong solenoid_port) -{ - int32_t status = 0; - jboolean val = getSolenoid((void*)solenoid_port, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getSolenoid( + JNIEnv *env, jclass, jlong solenoid_port) { + int32_t status = 0; + jboolean val = getSolenoid((void *)solenoid_port, &status); + CheckStatus(env, status); + return val; } /* @@ -111,13 +117,13 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getSolenoi * Method: getAllSolenoids * Signature: (J)Z */ -JNIEXPORT jbyte JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getAllSolenoids - (JNIEnv *env, jclass, jlong solenoid_port) -{ - int32_t status = 0; - jbyte val = getAllSolenoids((void*)solenoid_port, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jbyte JNICALL +Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getAllSolenoids( + JNIEnv *env, jclass, jlong solenoid_port) { + int32_t status = 0; + jbyte val = getAllSolenoids((void *)solenoid_port, &status); + CheckStatus(env, status); + return val; } /* @@ -125,51 +131,51 @@ JNIEXPORT jbyte JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getAllSolenoi * Method: getPCMSolenoidBlackList * Signature: (J)I */ -JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getPCMSolenoidBlackList - (JNIEnv *env, jclass, jlong solenoid_port) -{ - int32_t status = 0; - jint val = getPCMSolenoidBlackList((void*)solenoid_port, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getPCMSolenoidBlackList( + JNIEnv *env, jclass, jlong solenoid_port) { + int32_t status = 0; + jint val = getPCMSolenoidBlackList((void *)solenoid_port, &status); + CheckStatus(env, status); + return val; } /* * Class: edu_wpi_first_wpilibj_hal_SolenoidJNI * Method: getPCMSolenoidVoltageStickyFault * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getPCMSolenoidVoltageStickyFault - (JNIEnv *env, jclass, jlong solenoid_port) -{ - int32_t status = 0; - bool val = getPCMSolenoidVoltageStickyFault((void*)solenoid_port, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getPCMSolenoidVoltageStickyFault( + JNIEnv *env, jclass, jlong solenoid_port) { + int32_t status = 0; + bool val = getPCMSolenoidVoltageStickyFault((void *)solenoid_port, &status); + CheckStatus(env, status); + return val; } /* * Class: edu_wpi_first_wpilibj_hal_SolenoidJNI * Method: getPCMSolenoidVoltageFault * Signature: (J)Z */ -JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getPCMSolenoidVoltageFault - (JNIEnv *env, jclass, jlong solenoid_port) -{ - int32_t status = 0; - bool val = getPCMSolenoidVoltageFault((void*)solenoid_port, &status); - CheckStatus(env, status); - return val; +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getPCMSolenoidVoltageFault( + JNIEnv *env, jclass, jlong solenoid_port) { + int32_t status = 0; + bool val = getPCMSolenoidVoltageFault((void *)solenoid_port, &status); + CheckStatus(env, status); + return val; } /* * Class: edu_wpi_first_wpilibj_hal_SolenoidJNI * Method: clearAllPCMStickyFaults * Signature: (J)V */ -JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_clearAllPCMStickyFaults - (JNIEnv *env, jclass, jlong solenoid_port) -{ - int32_t status = 0; - clearAllPCMStickyFaults_sol((void*)solenoid_port, &status); - CheckStatus(env, status); +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_clearAllPCMStickyFaults( + JNIEnv *env, jclass, jlong solenoid_port) { + int32_t status = 0; + clearAllPCMStickyFaults_sol((void *)solenoid_port, &status); + CheckStatus(env, status); } } // extern "C" diff --git a/wpilibj/src/athena/cpp/nivision/dxattr.h b/wpilibj/src/athena/cpp/nivision/dxattr.h index 88444fbb8e..e03a97df55 100644 --- a/wpilibj/src/athena/cpp/nivision/dxattr.h +++ b/wpilibj/src/athena/cpp/nivision/dxattr.h @@ -5,29 +5,59 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -IMAQdxError NI_FUNC IMAQdxGetAttributeU32(IMAQdxSession id, const char* name, uInt32* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeI64(IMAQdxSession id, const char* name, Int64* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeF64(IMAQdxSession id, const char* name, float64* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeString(IMAQdxSession id, const char* name, char value[IMAQDX_MAX_API_STRING_LENGTH]); -IMAQdxError NI_FUNC IMAQdxGetAttributeEnum(IMAQdxSession id, const char* name, IMAQdxEnumItem* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeBool(IMAQdxSession id, const char* name, bool32* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeU32(IMAQdxSession id, const char* name, + uInt32* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeI64(IMAQdxSession id, const char* name, + Int64* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeF64(IMAQdxSession id, const char* name, + float64* value); +IMAQdxError NI_FUNC +IMAQdxGetAttributeString(IMAQdxSession id, const char* name, + char value[IMAQDX_MAX_API_STRING_LENGTH]); +IMAQdxError NI_FUNC IMAQdxGetAttributeEnum(IMAQdxSession id, const char* name, + IMAQdxEnumItem* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeBool(IMAQdxSession id, const char* name, + bool32* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumU32(IMAQdxSession id, const char* name, uInt32* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumI64(IMAQdxSession id, const char* name, Int64* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumF64(IMAQdxSession id, const char* name, float64* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumU32(IMAQdxSession id, + const char* name, + uInt32* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumI64(IMAQdxSession id, + const char* name, + Int64* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeMinimumF64(IMAQdxSession id, + const char* name, + float64* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumU32(IMAQdxSession id, const char* name, uInt32* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumI64(IMAQdxSession id, const char* name, Int64* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumF64(IMAQdxSession id, const char* name, float64* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumU32(IMAQdxSession id, + const char* name, + uInt32* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumI64(IMAQdxSession id, + const char* name, + Int64* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeMaximumF64(IMAQdxSession id, + const char* name, + float64* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementU32(IMAQdxSession id, const char* name, uInt32* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementI64(IMAQdxSession id, const char* name, Int64* value); -IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementF64(IMAQdxSession id, const char* name, float64* value); - -IMAQdxError NI_FUNC IMAQdxSetAttributeU32(IMAQdxSession id, const char* name, uInt32 value); -IMAQdxError NI_FUNC IMAQdxSetAttributeI64(IMAQdxSession id, const char* name, Int64 value); -IMAQdxError NI_FUNC IMAQdxSetAttributeF64(IMAQdxSession id, const char* name, float64 value); -IMAQdxError NI_FUNC IMAQdxSetAttributeString(IMAQdxSession id, const char* name, const char* value); -IMAQdxError NI_FUNC IMAQdxSetAttributeEnum(IMAQdxSession id, const char* name, const IMAQdxEnumItem* value); -IMAQdxError NI_FUNC IMAQdxSetAttributeBool(IMAQdxSession id, const char* name, bool32 value); +IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementU32(IMAQdxSession id, + const char* name, + uInt32* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementI64(IMAQdxSession id, + const char* name, + Int64* value); +IMAQdxError NI_FUNC IMAQdxGetAttributeIncrementF64(IMAQdxSession id, + const char* name, + float64* value); +IMAQdxError NI_FUNC IMAQdxSetAttributeU32(IMAQdxSession id, const char* name, + uInt32 value); +IMAQdxError NI_FUNC IMAQdxSetAttributeI64(IMAQdxSession id, const char* name, + Int64 value); +IMAQdxError NI_FUNC IMAQdxSetAttributeF64(IMAQdxSession id, const char* name, + float64 value); +IMAQdxError NI_FUNC IMAQdxSetAttributeString(IMAQdxSession id, const char* name, + const char* value); +IMAQdxError NI_FUNC IMAQdxSetAttributeEnum(IMAQdxSession id, const char* name, + const IMAQdxEnumItem* value); +IMAQdxError NI_FUNC IMAQdxSetAttributeBool(IMAQdxSession id, const char* name, + bool32 value);