From ff597e6ac45824a431caff21e86a54735285b2c9 Mon Sep 17 00:00:00 2001 From: Colby Skeggs Date: Tue, 24 Jun 2014 10:37:02 -0700 Subject: [PATCH] Fixed C++ side of artf2604 in FRCSim - synchronized C++ codebases, updated examples. Change-Id: I2fdc9deb4c8e249448dcbda4214fd900c2bc4ea8 --- .../GearsBot/src/Subsystems/DriveTrain.cpp | 2 +- .../GearsBot/src/Subsystems/DriveTrain.h | 2 +- .../PacGoat/src/Subsystems/Collector.cpp | 8 +- .../PacGoat/src/Subsystems/DriveTrain.cpp | 14 +- .../PacGoat/src/Subsystems/Pneumatics.cpp | 2 +- .../PacGoat/src/Subsystems/Pneumatics.h | 2 +- .../PacGoat/src/Subsystems/Shooter.cpp | 10 +- .../wpilibC++/include/AnalogPotentiometer.h | 1 - wpilibc/wpilibC++/include/Base.h | 3 +- wpilibc/wpilibC++/include/Controller.h | 3 +- wpilibc/wpilibC++/include/Joystick.h | 154 +++++++++--------- wpilibc/wpilibC++/include/SensorBase.h | 4 +- wpilibc/wpilibC++/lib/AnalogInput.cpp | 6 +- wpilibc/wpilibC++/lib/AnalogPotentiometer.cpp | 1 - wpilibc/wpilibC++/lib/SensorBase.cpp | 4 +- .../{AnalogChannel.h => AnalogInput.h} | 34 ++-- .../include/AnalogPotentiometer.h | 65 ++++++-- wpilibc/wpilibC++Sim/include/Base.h | 8 +- wpilibc/wpilibC++Sim/include/Controller.h | 8 +- wpilibc/wpilibC++Sim/include/Counter.h | 28 ++-- wpilibc/wpilibC++Sim/include/CounterBase.h | 19 ++- wpilibc/wpilibC++Sim/include/DigitalInput.h | 14 +- wpilibc/wpilibC++Sim/include/DriverStation.h | 66 +++++--- wpilibc/wpilibC++Sim/include/Encoder.h | 34 ++-- wpilibc/wpilibC++Sim/include/Error.h | 24 +-- wpilibc/wpilibC++Sim/include/ErrorBase.h | 26 ++- wpilibc/wpilibC++Sim/include/GenericHID.h | 16 +- wpilibc/wpilibC++Sim/include/Gyro.h | 19 +-- wpilibc/wpilibC++Sim/include/IterativeRobot.h | 26 ++- wpilibc/wpilibC++Sim/include/Jaguar.h | 13 +- .../include/LiveWindow/LiveWindow.h | 14 +- wpilibc/wpilibC++Sim/include/Notifier.h | 14 +- wpilibc/wpilibC++Sim/include/PIDController.h | 42 +++-- wpilibc/wpilibC++Sim/include/PIDOutput.h | 6 +- wpilibc/wpilibC++Sim/include/PIDSource.h | 8 +- wpilibc/wpilibC++Sim/include/Resource.h | 6 +- wpilibc/wpilibC++Sim/include/RobotBase.h | 28 ++-- wpilibc/wpilibC++Sim/include/RobotDrive.h | 47 +++--- wpilibc/wpilibC++Sim/include/SensorBase.h | 37 ++--- wpilibc/wpilibC++Sim/include/SimpleRobot.h | 13 +- .../include/SmartDashboard/SmartDashboard.h | 2 +- .../wpilibC++Sim/include/SpeedController.h | 11 +- wpilibc/wpilibC++Sim/include/Talon.h | 14 +- wpilibc/wpilibC++Sim/include/Task.h | 13 +- wpilibc/wpilibC++Sim/include/Timer.h | 7 +- wpilibc/wpilibC++Sim/include/Utility.h | 6 +- wpilibc/wpilibC++Sim/include/Victor.h | 14 +- wpilibc/wpilibC++Sim/include/WPIErrors.h | 7 +- wpilibc/wpilibC++Sim/include/WPILib.h | 10 +- .../include/interfaces/Potentiometer.h | 1 - .../{AnalogChannel.cpp => AnalogInput.cpp} | 72 +++----- .../wpilibC++Sim/src/AnalogPotentiometer.cpp | 57 +++++-- wpilibc/wpilibC++Sim/src/Commands/Command.cpp | 4 +- .../src/Commands/WaitUntilCommand.cpp | 2 +- wpilibc/wpilibC++Sim/src/DigitalInput.cpp | 30 +--- wpilibc/wpilibC++Sim/src/DriverStation.cpp | 64 ++++---- wpilibc/wpilibC++Sim/src/Encoder.cpp | 118 ++++++++------ wpilibc/wpilibC++Sim/src/Error.cpp | 1 - wpilibc/wpilibC++Sim/src/ErrorBase.cpp | 2 +- wpilibc/wpilibC++Sim/src/Gyro.cpp | 41 ++--- wpilibc/wpilibC++Sim/src/IterativeRobot.cpp | 5 +- wpilibc/wpilibC++Sim/src/Jaguar.cpp | 37 ++--- .../src/LiveWindow/LiveWindow.cpp | 23 ++- wpilibc/wpilibC++Sim/src/Notifier.cpp | 18 +- wpilibc/wpilibC++Sim/src/RobotBase.cpp | 11 +- wpilibc/wpilibC++Sim/src/RobotDrive.cpp | 18 +- wpilibc/wpilibC++Sim/src/SensorBase.cpp | 90 +++++----- wpilibc/wpilibC++Sim/src/SimpleRobot.cpp | 30 ++-- .../src/SmartDashboard/SmartDashboard.cpp | 4 +- wpilibc/wpilibC++Sim/src/Talon.cpp | 40 ++--- wpilibc/wpilibC++Sim/src/Utility.cpp | 1 - wpilibc/wpilibC++Sim/src/Victor.cpp | 40 ++--- 72 files changed, 763 insertions(+), 861 deletions(-) rename wpilibc/wpilibC++Sim/include/{AnalogChannel.h => AnalogInput.h} (70%) rename wpilibc/wpilibC++Sim/src/{AnalogChannel.cpp => AnalogInput.cpp} (51%) diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/GearsBot/src/Subsystems/DriveTrain.cpp b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/GearsBot/src/Subsystems/DriveTrain.cpp index 344807da40..635308b72a 100644 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/GearsBot/src/Subsystems/DriveTrain.cpp +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/GearsBot/src/Subsystems/DriveTrain.cpp @@ -29,7 +29,7 @@ DriveTrain::DriveTrain() : Subsystem("DriveTrain") { left_encoder->Start(); right_encoder->Start(); - rangefinder = new AnalogChannel(6); + rangefinder = new AnalogInput(6); gyro = new Gyro(1); // Let's show everything on the LiveWindow diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/GearsBot/src/Subsystems/DriveTrain.h b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/GearsBot/src/Subsystems/DriveTrain.h index 6bf77e653b..1467471646 100644 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/GearsBot/src/Subsystems/DriveTrain.h +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/GearsBot/src/Subsystems/DriveTrain.h @@ -15,7 +15,7 @@ private: *front_right_motor, *back_right_motor; RobotDrive* drive; Encoder *left_encoder, *right_encoder; - AnalogChannel* rangefinder; + AnalogInput* rangefinder; Gyro* gyro; public: diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Collector.cpp b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Collector.cpp index 5f9a346663..245090ff23 100644 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Collector.cpp +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Collector.cpp @@ -4,10 +4,10 @@ Collector::Collector() : Subsystem("Collector") { // Configure devices - rollerMotor = new Victor(1, 6); - ballDetector = new DigitalInput(1, 10); - openDetector = new DigitalInput(1, 6); - piston = new Solenoid(1, 1); + rollerMotor = new Victor(6); + ballDetector = new DigitalInput(10); + openDetector = new DigitalInput(6); + piston = new Solenoid(1); // Put everything to the LiveWindow for testing. // XXX: LiveWindow::GetInstance()->AddActuator("Collector", "Roller Motor", (Victor) rollerMotor); diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/DriveTrain.cpp b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/DriveTrain.cpp index cbf9da094d..761e0cc920 100644 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/DriveTrain.cpp +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/DriveTrain.cpp @@ -6,10 +6,10 @@ DriveTrain::DriveTrain() : Subsystem("DriveTrain") { // Configure drive motors - frontLeftCIM = new Victor(1, 1); - frontRightCIM = new Victor(1, 2); - backLeftCIM = new Victor(1, 3); - backRightCIM = new Victor(1, 4); + frontLeftCIM = new Victor(1); + frontRightCIM = new Victor(2); + backLeftCIM = new Victor(3); + backRightCIM = new Victor(4); // XXX: LiveWindow::GetInstance()->AddActuator("DriveTrain", "Front Left CIM", (Victor) frontLeftCIM); // XXX: LiveWindow::GetInstance()->AddActuator("DriveTrain", "Front Right CIM", (Victor) frontRightCIM); // XXX: LiveWindow::GetInstance()->AddActuator("DriveTrain", "Back Left CIM", (Victor) backLeftCIM); @@ -28,8 +28,8 @@ DriveTrain::DriveTrain() : drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true); // Configure encoders - rightEncoder = new Encoder(1, 1, 1, 2, true, Encoder::k4X); - leftEncoder = new Encoder(2, 5, 2, 6, false, Encoder::k4X); + rightEncoder = new Encoder(1, 2, true, Encoder::k4X); + leftEncoder = new Encoder(5, 6, false, Encoder::k4X); // TODO: Correct encoder module. rightEncoder->SetPIDSourceParameter(PIDSource::kDistance); leftEncoder->SetPIDSourceParameter(PIDSource::kDistance); @@ -49,7 +49,7 @@ DriveTrain::DriveTrain() : LiveWindow::GetInstance()->AddSensor("DriveTrain", "Left Encoder", leftEncoder); // Configure gyro - gyro = new Gyro(1, 2); + gyro = new Gyro(2); #ifdef REAL gyro->SetSensitivity(0.007); // TODO: Handle more gracefully? #endif diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Pneumatics.cpp b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Pneumatics.cpp index fcaf9b61c6..3177d4f9c5 100644 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Pneumatics.cpp +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Pneumatics.cpp @@ -3,7 +3,7 @@ Pneumatics::Pneumatics() : Subsystem("Pneumatics") { - pressureSensor = new AnalogChannel(3); + pressureSensor = new AnalogInput(3); #ifdef REAL compressor = new Compressor(uint8_t(1)); // TODO: (1, 14, 1, 8); #endif diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Pneumatics.h b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Pneumatics.h index 12dc394df7..bac0c16f21 100644 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Pneumatics.h +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Pneumatics.h @@ -12,7 +12,7 @@ class Pneumatics: public Subsystem { private: - AnalogChannel* pressureSensor; + AnalogInput* pressureSensor; #ifdef REAL Compressor* compressor; #endif diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Shooter.cpp b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Shooter.cpp index 2db03fe897..9bb58a90e3 100644 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Shooter.cpp +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/PacGoat/src/Subsystems/Shooter.cpp @@ -4,12 +4,12 @@ Shooter::Shooter() : Subsystem("Shooter") { // Configure Devices - hotGoalSensor = new DigitalInput(1, 3); - piston1 = new DoubleSolenoid(1, 3, 4); - piston2 = new DoubleSolenoid(1, 5, 6); + hotGoalSensor = new DigitalInput(3); + piston1 = new DoubleSolenoid(3, 4); + piston2 = new DoubleSolenoid(5, 6); latchPiston = new Solenoid(1, 2); - piston1ReedSwitchFront = new DigitalInput(1, 9); - piston1ReedSwitchBack = new DigitalInput(1, 11); + piston1ReedSwitchFront = new DigitalInput(9); + piston1ReedSwitchBack = new DigitalInput(11); // Put everything to the LiveWindow for testing. LiveWindow::GetInstance()->AddSensor("Shooter", "Hot Goal Sensor", hotGoalSensor); diff --git a/wpilibc/wpilibC++/include/AnalogPotentiometer.h b/wpilibc/wpilibC++/include/AnalogPotentiometer.h index 1a2658be80..274bcf38af 100644 --- a/wpilibc/wpilibC++/include/AnalogPotentiometer.h +++ b/wpilibc/wpilibC++/include/AnalogPotentiometer.h @@ -70,7 +70,6 @@ public: virtual void StopLiveWindowMode() {} private: - int m_module, m_channel; double m_scale, m_offset; AnalogInput* m_analog_input; ITable* m_table; diff --git a/wpilibc/wpilibC++/include/Base.h b/wpilibc/wpilibC++/include/Base.h index 8e2350473d..2aaa4ee223 100644 --- a/wpilibc/wpilibC++/include/Base.h +++ b/wpilibc/wpilibC++/include/Base.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* 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. */ /*----------------------------------------------------------------------------*/ @@ -16,3 +16,4 @@ #define DISALLOW_COPY_AND_ASSIGN(TypeName) \ TypeName(const TypeName&); \ void operator=(const TypeName&) + diff --git a/wpilibc/wpilibC++/include/Controller.h b/wpilibc/wpilibC++/include/Controller.h index a08c0dbc79..94aaa05177 100644 --- a/wpilibc/wpilibC++/include/Controller.h +++ b/wpilibc/wpilibC++/include/Controller.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* 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. */ /*----------------------------------------------------------------------------*/ @@ -28,3 +28,4 @@ public: */ virtual void Disable() = 0; }; + diff --git a/wpilibc/wpilibC++/include/Joystick.h b/wpilibc/wpilibC++/include/Joystick.h index 3647b788a7..e21e070bfa 100644 --- a/wpilibc/wpilibC++/include/Joystick.h +++ b/wpilibc/wpilibC++/include/Joystick.h @@ -1,77 +1,77 @@ -/*----------------------------------------------------------------------------*/ -/* 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. */ -/*----------------------------------------------------------------------------*/ - -#ifndef JOYSTICK_H_ -#define JOYSTICK_H_ - -#include "GenericHID.h" -#include "ErrorBase.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. - */ -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(); - - uint32_t GetAxisChannel(AxisType axis); - void SetAxisChannel(AxisType axis, uint32_t channel); - - virtual float GetX(JoystickHand hand = kRightHand); - virtual float GetY(JoystickHand hand = kRightHand); - virtual float GetZ(); - virtual float GetTwist(); - virtual float GetThrottle(); - virtual float GetAxis(AxisType axis); - float GetRawAxis(uint32_t axis); - - virtual bool GetTrigger(JoystickHand hand = kRightHand); - virtual bool GetTop(JoystickHand hand = kRightHand); - virtual bool GetBumper(JoystickHand hand = kRightHand); - virtual bool GetButton(ButtonType button); - bool GetRawButton(uint32_t button); - static Joystick* GetStickForPort(uint32_t port); - - virtual float GetMagnitude(); - virtual float GetDirectionRadians(); - virtual float GetDirectionDegrees(); - -private: - DISALLOW_COPY_AND_ASSIGN(Joystick); - void InitJoystick(uint32_t numAxisTypes, uint32_t numButtonTypes); - - DriverStation *m_ds; - uint32_t m_port; - uint32_t *m_axes; - uint32_t *m_buttons; -}; - -#endif - +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#ifndef JOYSTICK_H_ +#define JOYSTICK_H_ + +#include "GenericHID.h" +#include "ErrorBase.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. + */ +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(); + + uint32_t GetAxisChannel(AxisType axis); + void SetAxisChannel(AxisType axis, uint32_t channel); + + virtual float GetX(JoystickHand hand = kRightHand); + virtual float GetY(JoystickHand hand = kRightHand); + virtual float GetZ(); + virtual float GetTwist(); + virtual float GetThrottle(); + virtual float GetAxis(AxisType axis); + float GetRawAxis(uint32_t axis); + + virtual bool GetTrigger(JoystickHand hand = kRightHand); + virtual bool GetTop(JoystickHand hand = kRightHand); + virtual bool GetBumper(JoystickHand hand = kRightHand); + virtual bool GetButton(ButtonType button); + bool GetRawButton(uint32_t button); + static Joystick* GetStickForPort(uint32_t port); + + virtual float GetMagnitude(); + virtual float GetDirectionRadians(); + virtual float GetDirectionDegrees(); + +private: + DISALLOW_COPY_AND_ASSIGN(Joystick); + void InitJoystick(uint32_t numAxisTypes, uint32_t numButtonTypes); + + DriverStation *m_ds; + uint32_t m_port; + uint32_t *m_axes; + uint32_t *m_buttons; +}; + +#endif + diff --git a/wpilibc/wpilibC++/include/SensorBase.h b/wpilibc/wpilibC++/include/SensorBase.h index eaecc44b15..ef4e296ce9 100644 --- a/wpilibc/wpilibC++/include/SensorBase.h +++ b/wpilibc/wpilibC++/include/SensorBase.h @@ -31,13 +31,13 @@ public: static bool CheckRelayChannel(uint32_t channel); static bool CheckPWMChannel(uint32_t channel); static bool CheckAnalogInput(uint32_t channel); - static bool CheckAnalogOutput(uint32_t channel); + static bool CheckAnalogOutput(uint32_t channel); static bool CheckSolenoidChannel(uint32_t channel); static bool CheckPDPChannel(uint32_t channel); static const uint32_t kDigitalChannels = 20; static const uint32_t kAnalogInputs = 8; - static const uint32_t kAnalogOutputs = 2; + static const uint32_t kAnalogOutputs = 2; static const uint32_t kSolenoidChannels = 8; static const uint32_t kSolenoidModules = 2; static const uint32_t kPwmChannels = 20; diff --git a/wpilibc/wpilibC++/lib/AnalogInput.cpp b/wpilibc/wpilibC++/lib/AnalogInput.cpp index 968825f4a8..9b8ed4b87c 100644 --- a/wpilibc/wpilibC++/lib/AnalogInput.cpp +++ b/wpilibc/wpilibC++/lib/AnalogInput.cpp @@ -25,7 +25,7 @@ void AnalogInput::InitAnalogInput(uint32_t channel) char buf[64]; Resource::CreateResourceObject(&inputs, kAnalogInputs); - if (!checkAnalogInputChannel(channel)) + if (!checkAnalogInputChannel(channel)) { snprintf(buf, 64, "analog input %d", channel); wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf); @@ -39,14 +39,14 @@ void AnalogInput::InitAnalogInput(uint32_t channel) return; } - m_channel = channel; + m_channel = channel; void* port = getPort(channel); int32_t status = 0; m_port = initializeAnalogInputPort(port, &status); wpi_setErrorWithContext(status, getHALErrorMessage(status)); - LiveWindow::GetInstance()->AddSensor("AnalogInput",channel, this); + LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this); HALReport(HALUsageReporting::kResourceType_AnalogChannel, channel); } diff --git a/wpilibc/wpilibC++/lib/AnalogPotentiometer.cpp b/wpilibc/wpilibC++/lib/AnalogPotentiometer.cpp index 65e315e627..47e0361bd4 100644 --- a/wpilibc/wpilibC++/lib/AnalogPotentiometer.cpp +++ b/wpilibc/wpilibC++/lib/AnalogPotentiometer.cpp @@ -5,7 +5,6 @@ * Common initialization code called by all constructors. */ void AnalogPotentiometer::initPot(AnalogInput *input, double scale, double offset) { -// m_channel = channel; m_scale = scale; m_offset = offset; m_analog_input = input; diff --git a/wpilibc/wpilibC++/lib/SensorBase.cpp b/wpilibc/wpilibC++/lib/SensorBase.cpp index bc0fe2ceec..909fcd70fa 100644 --- a/wpilibc/wpilibC++/lib/SensorBase.cpp +++ b/wpilibc/wpilibC++/lib/SensorBase.cpp @@ -133,7 +133,7 @@ bool SensorBase::CheckAnalogInput(uint32_t channel) } /** - * Check that the analog output number is value. + * 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. * @@ -161,7 +161,7 @@ bool SensorBase::CheckSolenoidChannel(uint32_t channel) /** * Verify that the power distribution channel number is within limits. * - * @return Solenoid channel is valid + * @return PDP channel is valid */ bool SensorBase::CheckPDPChannel(uint32_t channel) { diff --git a/wpilibc/wpilibC++Sim/include/AnalogChannel.h b/wpilibc/wpilibC++Sim/include/AnalogInput.h similarity index 70% rename from wpilibc/wpilibC++Sim/include/AnalogChannel.h rename to wpilibc/wpilibC++Sim/include/AnalogInput.h index dd75f30d66..fd711797cc 100644 --- a/wpilibc/wpilibC++Sim/include/AnalogChannel.h +++ b/wpilibc/wpilibC++Sim/include/AnalogInput.h @@ -1,11 +1,9 @@ /*----------------------------------------------------------------------------*/ -/* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef ANALOG_CHANNEL_H_ -#define ANALOG_CHANNEL_H_ +#pragma once #include "simulation/SimFloatInput.h" #include "SensorBase.h" @@ -13,10 +11,8 @@ #include "LiveWindow/LiveWindowSendable.h" /** - * Analog channel class. - * - * Each analog channel is read from hardware as a 12-bit number representing -10V to 10V. - * + * 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 @@ -24,21 +20,23 @@ * resolution, while the averaged samples are divided by the number of samples to retain the resolution, * but get more stable values. */ -class AnalogChannel : public SensorBase, public PIDSource, public LiveWindowSendable +class AnalogInput : public SensorBase, public PIDSource, public LiveWindowSendable { public: - AnalogChannel(uint8_t moduleNumber, uint32_t channel); - explicit AnalogChannel(uint32_t channel); - virtual ~AnalogChannel(); + 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(); float GetVoltage(); float GetAverageVoltage(); - uint8_t GetModuleNumber(); uint32_t GetChannel(); double PIDGet(); - + void UpdateTable(); void StartLiveWindowMode(); void StopLiveWindowMode(); @@ -47,12 +45,10 @@ public: ITable * GetTable(); private: - void InitChannel(uint8_t moduleNumber, uint32_t channel); - uint32_t m_channel, m_module; + void InitAnalogInput(uint32_t channel); + uint32_t m_channel; SimFloatInput* m_impl; int64_t m_accumulatorOffset; - + ITable *m_table; }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/AnalogPotentiometer.h b/wpilibc/wpilibC++Sim/include/AnalogPotentiometer.h index 2922c4724d..274bcf38af 100644 --- a/wpilibc/wpilibC++Sim/include/AnalogPotentiometer.h +++ b/wpilibc/wpilibC++Sim/include/AnalogPotentiometer.h @@ -1,24 +1,56 @@ -#include "simulation/SimFloatInput.h" +#include "AnalogInput.h" #include "interfaces/Potentiometer.h" -#include "SensorBase.h" -#include "LiveWindow/LiveWindowSendable.h" +#include "LiveWindow/LiveWindowSendable.h" /** - * A class for reading analog potentiometers. - * + * Class for reading analog potentiometers. Analog potentiometers read + * in an analog voltage that corresponds to a position. Usually the + * position is either degrees or meters. However, if no conversion is + * given it remains volts. + * * @author Alex Henning */ -class AnalogPotentiometer : public SensorBase, public Potentiometer, public LiveWindowSendable { +class AnalogPotentiometer : public Potentiometer, public LiveWindowSendable { public: - AnalogPotentiometer(int slot, int channel, double scale, double offset); - AnalogPotentiometer(int channel, double scale, double offset); - AnalogPotentiometer(int channel, double scale); - AnalogPotentiometer(int channel); + /** + * 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); + + virtual ~AnalogPotentiometer(); + + /** + * Get the current reading of the potentiomere. + * + * @return The current position of the potentiometer. + */ virtual double Get(); + + + /** + * Implement the PIDSource interface. + * + * @return The current reading. + */ virtual double PIDGet(); - + + /* * Live Window code, only does anything if live window is activated. */ @@ -31,19 +63,20 @@ public: * AnalogPotentiometers don't have to do anything special when entering the LiveWindow. */ virtual void StartLiveWindowMode() {} - + /** * AnalogPotentiometers don't have to do anything special when exiting the LiveWindow. */ virtual void StopLiveWindowMode() {} - + private: - int module, channel; - SimFloatInput* impl; + double m_scale, m_offset; + AnalogInput* m_analog_input; ITable* m_table; + bool m_init_analog_input; /** * Common initialization code called by all constructors. */ - void initPot(int slot, int channel, double scale, double offset); + void initPot(AnalogInput *input, double scale, double offset); }; diff --git a/wpilibc/wpilibC++Sim/include/Base.h b/wpilibc/wpilibC++Sim/include/Base.h index eb11f96b8e..2aaa4ee223 100644 --- a/wpilibc/wpilibC++Sim/include/Base.h +++ b/wpilibc/wpilibC++Sim/include/Base.h @@ -1,15 +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. */ /*----------------------------------------------------------------------------*/ - -#ifndef _BASE_H -#define _BASE_H +#pragma once // If don't have C++11, define constexpr as const for WindRiver #if __cplusplus < 201103L #define constexpr const +#define nullptr NULL #endif // A macro to disallow the copy constructor and operator= functions @@ -18,4 +17,3 @@ TypeName(const TypeName&); \ void operator=(const TypeName&) -#endif diff --git a/wpilibc/wpilibC++Sim/include/Controller.h b/wpilibc/wpilibC++Sim/include/Controller.h index 8a1a86b7f7..99eba88055 100644 --- a/wpilibc/wpilibC++Sim/include/Controller.h +++ b/wpilibc/wpilibC++Sim/include/Controller.h @@ -1,11 +1,9 @@ /*----------------------------------------------------------------------------*/ -/* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef CONTROLLER_H -#define CONTROLLER_H +#pragma once #include #include @@ -34,5 +32,3 @@ public: virtual void Disable() = 0; }; -#endif - diff --git a/wpilibc/wpilibC++Sim/include/Counter.h b/wpilibc/wpilibC++Sim/include/Counter.h index 57dcb6f02b..545cd78995 100644 --- a/wpilibc/wpilibC++Sim/include/Counter.h +++ b/wpilibc/wpilibC++Sim/include/Counter.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef CPPCOUNTER_H_ -#define CPPCOUNTER_H_ +#pragma once #include "CounterBase.h" #include "SensorBase.h" @@ -20,25 +18,27 @@ class Counter : public SensorBase, public CounterBase, public LiveWindowSendable { public: - /* typedef enum {kTwoPulse=0, kSemiperiod=1, kPulseLength=2, kExternalDirection=3} Mode; */ Counter(); explicit Counter(uint32_t channel); - Counter(uint8_t moduleNumber, uint32_t channel); - // TODO: [Not Supported] explicit Counter(DigitalSource *source); + // 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); - void SetUpSource(uint8_t moduleNumber, 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); - void SetDownSource(uint8_t moduleNumber, 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); @@ -63,9 +63,11 @@ public: bool GetDirection(); void SetSamplesToAverage(int samplesToAverage); int GetSamplesToAverage(); - uint32_t GetIndex() {return m_index;} - - + uint32_t GetIndex() + { + return m_index; + } + void UpdateTable(); void StartLiveWindowMode(); void StopLiveWindowMode(); @@ -82,8 +84,6 @@ 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. - + ITable *m_table; }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/CounterBase.h b/wpilibc/wpilibC++Sim/include/CounterBase.h index 537e1b2dcc..a5fbae318e 100644 --- a/wpilibc/wpilibC++Sim/include/CounterBase.h +++ b/wpilibc/wpilibC++Sim/include/CounterBase.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef CPPCOUNTER_BASE_H_ -#define CPPCOUNTER_BASE_H_ +#pragma once /** * Interface for counting the number of ticks on a digital input channel. @@ -15,17 +13,20 @@ class CounterBase { public: - typedef enum {k1X, k2X, k4X} EncodingType; + enum EncodingType + { + k1X, + k2X, + k4X + }; virtual ~CounterBase() {} virtual void Start() = 0; - // virtual int32_t Get() = 0; + virtual int32_t Get() = 0; virtual void Reset() = 0; virtual void Stop() = 0; - // virtual double GetPeriod() = 0; - // virtual void SetMaxPeriod(double maxPeriod) = 0; + virtual double GetPeriod() = 0; + virtual void SetMaxPeriod(double maxPeriod) = 0; virtual bool GetStopped() = 0; virtual bool GetDirection() = 0; }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/DigitalInput.h b/wpilibc/wpilibC++Sim/include/DigitalInput.h index 9b71162ebb..0c4ec0d2b8 100644 --- a/wpilibc/wpilibC++Sim/include/DigitalInput.h +++ b/wpilibc/wpilibC++Sim/include/DigitalInput.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef DIGITAL_INPUT_H_ -#define DIGITAL_INPUT_H_ +#pragma once #include "simulation/SimDigitalInput.h" #include "LiveWindow/LiveWindowSendable.h" @@ -20,7 +18,6 @@ class DigitalInput : public LiveWindowSendable { public: explicit DigitalInput(uint32_t channel); - DigitalInput(uint8_t moduleNumber, uint32_t channel); virtual ~DigitalInput(); uint32_t Get(); uint32_t GetChannel(); @@ -33,13 +30,10 @@ public: ITable * GetTable(); private: - void InitDigitalInput(uint8_t moduleNumber, uint32_t channel); + void InitDigitalInput(uint32_t channel); uint32_t m_channel; bool m_lastValue; - SimDigitalInput *m_impl; - + SimDigitalInput *m_impl; + ITable *m_table; }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/DriverStation.h b/wpilibc/wpilibC++Sim/include/DriverStation.h index 5f770b6ff1..92f9e1d4aa 100644 --- a/wpilibc/wpilibC++Sim/include/DriverStation.h +++ b/wpilibc/wpilibC++Sim/include/DriverStation.h @@ -3,19 +3,15 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef __DRIVER_STATION_H__ -#define __DRIVER_STATION_H__ +#pragma once #include "simulation/msgs/msgs.h" #include -//#include "Dashboard.h" -//#include "DriverStationEnhancedIO.h" #include "SensorBase.h" #include "Task.h" struct HALCommonControlData; -class AnalogChannel; +class AnalogInput; using namespace gazebo; @@ -25,13 +21,17 @@ using namespace gazebo; class DriverStation : public SensorBase { public: - enum Alliance {kRed, kBlue, kInvalid}; + enum Alliance + { + kRed, + kBlue, + kInvalid + }; virtual ~DriverStation(); static DriverStation *GetInstance(); - static const uint32_t kBatteryModuleNumber = 1; - static const uint32_t kBatteryChannel = 8; + static const uint32_t kBatteryChannel = 7; static const uint32_t kJoystickPorts = 4; static const uint32_t kJoystickAxes = 6; @@ -46,9 +46,9 @@ public: bool IsEnabled(); bool IsDisabled(); - bool IsAutonomous(); + bool IsAutonomous(); bool IsOperatorControl(); - bool IsTest(); + bool IsTest(); bool IsFMSAttached(); uint32_t GetPacketNumber(); @@ -59,24 +59,39 @@ public: float GetBatteryVoltage(); uint16_t GetTeamNumber(); - void IncrementUpdateNumber() { m_updateNumber++; } + 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;} + 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 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; + } protected: DriverStation(); @@ -102,14 +117,11 @@ private: double m_approxMatchTimeOffset; bool m_userInDisabled; bool m_userInAutonomous; - bool m_userInTeleop; - bool m_userInTest; + bool m_userInTeleop; + bool m_userInTest; transport::SubscriberPtr stateSub; transport::SubscriberPtr joysticksSub[4]; msgs::DriverStationPtr state; msgs::JoystickPtr joysticks[4]; }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/Encoder.h b/wpilibc/wpilibC++Sim/include/Encoder.h index 48aa83d140..af52aa2dad 100644 --- a/wpilibc/wpilibC++Sim/include/Encoder.h +++ b/wpilibc/wpilibC++Sim/include/Encoder.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef QUAD_ENCODER_H_ -#define QUAD_ENCODER_H_ +#pragma once #include "simulation/SimEncoder.h" #include "CounterBase.h" @@ -21,22 +19,26 @@ * 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. + * that are out of phase with each other to allow the FPGA to do direction sensing. */ -class Encoder: public SensorBase, public CounterBase, public PIDSource, public LiveWindowSendable +class Encoder : public SensorBase, public CounterBase, public PIDSource, public LiveWindowSendable { public: - Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection=false, EncodingType encodingType = k4X); - Encoder(uint8_t aModuleNumber, uint32_t aChannel, uint8_t bModuleNumber, uint32_t _bChannel, bool reverseDirection=false, EncodingType encodingType = k4X); + 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(); // CounterBase interface void Start(); + int32_t Get(); + int32_t GetRaw(); void Reset(); void Stop(); + double GetPeriod(); + void SetMaxPeriod(double maxPeriod); bool GetStopped(); bool GetDirection(); double GetDistance(); @@ -48,7 +50,7 @@ public: int GetSamplesToAverage(); void SetPIDSourceParameter(PIDSourceParameter pidSource); double PIDGet(); - + void UpdateTable(); void StartLiveWindowMode(); void StopLiveWindowMode(); @@ -57,23 +59,19 @@ public: ITable * GetTable(); private: - void InitEncoder(int slotA, int channelA, int slotB, int channelB, - bool _reverseDirection, EncodingType encodingType); + void InitEncoder(int channelA, int channelB, bool _reverseDirection, EncodingType encodingType); double DecodingScaleFactor(); // 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 slotA, channelA, slotB, channelB; + int channelA, channelB; double m_distancePerPulse; // distance of travel for each encoder tick EncodingType m_encodingType; // Encoding type - PIDSourceParameter m_pidSource; // Encoder parameter that sources a PID controller - bool reversed; - SimEncoder* impl; - + PIDSourceParameter m_pidSource; // Encoder parameter that sources a PID controller + bool reversed; + SimEncoder* impl; + ITable *m_table; }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/Error.h b/wpilibc/wpilibC++Sim/include/Error.h index 65e6ba0c46..f73ae23af8 100644 --- a/wpilibc/wpilibC++Sim/include/Error.h +++ b/wpilibc/wpilibC++Sim/include/Error.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef _ERROR_H -#define _ERROR_H +#pragma once #include "Base.h" #include @@ -33,10 +31,16 @@ public: const ErrorBase* GetOriginatingObject() const; double GetTime() const; void Clear(); - void Set(Code code, const char* contextMessage, const char* filename, - const char *function, uint32_t lineNumber, const ErrorBase* originatingObject); - static void EnableStackTrace(bool enable) { m_stackTraceEnabled=enable; } - static void EnableSuspendOnError(bool enable) { m_suspendOnErrorEnabled=enable; } + void Set(Code code, const char* contextMessage, const char* filename, const char *function, + uint32_t lineNumber, const ErrorBase* originatingObject); + static void EnableStackTrace(bool enable) + { + m_stackTraceEnabled = enable; + } + static void EnableSuspendOnError(bool enable) + { + m_suspendOnErrorEnabled = enable; + } private: void Report(); @@ -50,9 +54,5 @@ private: double m_timestamp; static bool m_stackTraceEnabled; - static bool m_suspendOnErrorEnabled; - DISALLOW_COPY_AND_ASSIGN(Error); + static bool m_suspendOnErrorEnabled;DISALLOW_COPY_AND_ASSIGN(Error); }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/ErrorBase.h b/wpilibc/wpilibC++Sim/include/ErrorBase.h index 18ea098fae..ca3933c28f 100644 --- a/wpilibc/wpilibC++Sim/include/ErrorBase.h +++ b/wpilibc/wpilibC++Sim/include/ErrorBase.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef _ERROR_BASE_H -#define _ERROR_BASE_H +#pragma once #include "Base.h" #include "Error.h" @@ -39,21 +37,21 @@ public: virtual ~ErrorBase(); virtual Error& GetError(); virtual const Error& GetError() const; - virtual void SetErrnoError(const char *contextMessage, - const char* filename, const char* function, uint32_t lineNumber) const; - virtual void SetImaqError(int success, const char *contextMessage, - const char* filename, const char* function, uint32_t lineNumber) const; - virtual void SetError(Error::Code code, const char *contextMessage, - const char* filename, const char* function, uint32_t lineNumber) const; + virtual void SetErrnoError(const char *contextMessage, const char* filename, + const char* function, uint32_t lineNumber) const; + virtual void SetImaqError(int success, const char *contextMessage, const char* filename, + const char* function, uint32_t lineNumber) const; + virtual void SetError(Error::Code code, const char *contextMessage, const char* filename, + const char* function, uint32_t lineNumber) const; virtual void SetWPIError(const char *errorMessage, const char *contextMessage, - const char* filename, const char* function, uint32_t lineNumber) const; + const char* filename, const char* function, uint32_t lineNumber) const; virtual void CloneError(ErrorBase *rhs) const; virtual void ClearError() const; virtual bool StatusIsFatal() const; - static void SetGlobalError(Error::Code code, const char *contextMessage, - const char* filename, const char* function, uint32_t lineNumber); + static void SetGlobalError(Error::Code code, const char *contextMessage, const char* filename, + const char* function, uint32_t lineNumber); static void SetGlobalWPIError(const char *errorMessage, const char *contextMessage, - const char* filename, const char* function, uint32_t lineNumber); + const char* filename, const char* function, uint32_t lineNumber); static Error& GetGlobalError(); protected: mutable Error m_error; @@ -64,5 +62,3 @@ protected: private: DISALLOW_COPY_AND_ASSIGN(ErrorBase); }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/GenericHID.h b/wpilibc/wpilibC++Sim/include/GenericHID.h index 522ba23769..ac3518942f 100644 --- a/wpilibc/wpilibC++Sim/include/GenericHID.h +++ b/wpilibc/wpilibC++Sim/include/GenericHID.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef GENERIC_HID_H -#define GENERIC_HID_H +#pragma once #include @@ -14,12 +12,15 @@ class GenericHID { public: - typedef enum { + enum JoystickHand + { kLeftHand = 0, kRightHand = 1 - } JoystickHand; + }; - virtual ~GenericHID() {} + virtual ~GenericHID() + { + } virtual float GetX(JoystickHand hand = kRightHand) = 0; virtual float GetY(JoystickHand hand = kRightHand) = 0; @@ -33,6 +34,3 @@ public: virtual bool GetBumper(JoystickHand hand = kRightHand) = 0; virtual bool GetRawButton(uint32_t button) = 0; }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/Gyro.h b/wpilibc/wpilibC++Sim/include/Gyro.h index b17d6f3213..dda0c19028 100644 --- a/wpilibc/wpilibC++Sim/include/Gyro.h +++ b/wpilibc/wpilibC++Sim/include/Gyro.h @@ -3,16 +3,14 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef GYRO_H_ -#define GYRO_H_ +#pragma once #include "SensorBase.h" #include "PIDSource.h" #include "LiveWindow/LiveWindowSendable.h" #include "simulation/SimGyro.h" -class AnalogChannel; +class AnalogInput; class AnalogModule; /** @@ -21,9 +19,9 @@ class AnalogModule; * 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 + * 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 - * AnalogChannel for the current accumulator assignments. + * AnalogInput for the current accumulator assignments. */ class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable { @@ -34,7 +32,6 @@ public: static constexpr float kCalibrationSampleTime = 5.0; static constexpr float kDefaultVoltsPerDegreePerSecond = 0.007; - Gyro(uint8_t moduleNumber, uint32_t channel); explicit Gyro(uint32_t channel); virtual ~Gyro(); virtual float GetAngle(); @@ -44,7 +41,7 @@ public: // PIDSource interface double PIDGet(); - + void UpdateTable(); void StartLiveWindowMode(); void StopLiveWindowMode(); @@ -53,10 +50,10 @@ public: ITable * GetTable(); private: - void InitGyro(int slot, int channel); + void InitGyro(int channel); - SimGyro* impl; + SimGyro* impl; PIDSourceParameter m_pidSource; + ITable *m_table; }; -#endif diff --git a/wpilibc/wpilibC++Sim/include/IterativeRobot.h b/wpilibc/wpilibC++Sim/include/IterativeRobot.h index 1332818f06..b99a0e44b1 100644 --- a/wpilibc/wpilibC++Sim/include/IterativeRobot.h +++ b/wpilibc/wpilibC++Sim/include/IterativeRobot.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef ROBOT_ITERATIVE_H_ -#define ROBOT_ITERATIVE_H_ +#pragma once #include "Timer.h" #include "RobotBase.h" @@ -38,27 +36,28 @@ * */ -class IterativeRobot : public RobotBase { +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 = 0.0; + static constexpr double kDefaultPeriod = 0.0; virtual void StartCompetition(); virtual void RobotInit(); virtual void DisabledInit(); virtual void AutonomousInit(); - virtual void TeleopInit(); - virtual void TestInit(); + virtual void TeleopInit(); + virtual void TestInit(); virtual void DisabledPeriodic(); virtual void AutonomousPeriodic(); - virtual void TeleopPeriodic(); - virtual void TestPeriodic(); + virtual void TeleopPeriodic(); + virtual void TestPeriodic(); void SetPeriod(double period); double GetPeriod(); @@ -73,11 +72,8 @@ private: bool m_disabledInitialized; bool m_autonomousInitialized; - bool m_teleopInitialized; - bool m_testInitialized; + bool m_teleopInitialized; + bool m_testInitialized; double m_period; - Timer m_mainLoopTimer; + Timer m_mainLoopTimer; }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/Jaguar.h b/wpilibc/wpilibC++Sim/include/Jaguar.h index a0e6c736e8..d30c13dc52 100644 --- a/wpilibc/wpilibC++Sim/include/Jaguar.h +++ b/wpilibc/wpilibC++Sim/include/Jaguar.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef JAGUAR_H -#define JAGUAR_H +#pragma once #include "simulation/SimContinuousOutput.h" #include "SpeedController.h" @@ -18,17 +16,14 @@ class Jaguar : public SpeedController { public: explicit Jaguar(uint32_t channel); - Jaguar(uint8_t moduleNumber, uint32_t channel); virtual ~Jaguar(); - virtual void Set(float value, uint8_t syncGroup=0); + virtual void Set(float value, uint8_t syncGroup = 0); virtual float Get(); virtual void Disable(); virtual void PIDWrite(float output); private: - void InitJaguar(int slot, int channel); - SimContinuousOutput* impl; + void InitJaguar(int channel); + SimContinuousOutput* impl; }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/LiveWindow/LiveWindow.h b/wpilibc/wpilibC++Sim/include/LiveWindow/LiveWindow.h index ca05c1b964..93f9cca4cd 100644 --- a/wpilibc/wpilibC++Sim/include/LiveWindow/LiveWindow.h +++ b/wpilibc/wpilibC++Sim/include/LiveWindow/LiveWindow.h @@ -35,9 +35,10 @@ public: void Run(); void AddSensor(const char *subsystem, const char *name, LiveWindowSendable *component); void AddActuator(const char *subsystem, const char *name, LiveWindowSendable *component); - void AddSensor(std::string type, int module, int channel, LiveWindowSendable *component); + void AddSensor(std::string type, int channel, LiveWindowSendable *component); + void AddActuator(std::string type, int channel, LiveWindowSendable *component); void AddActuator(std::string type, int module, int channel, LiveWindowSendable *component); - + bool IsEnabled() { return m_enabled; } void SetEnabled(bool enabled); @@ -49,19 +50,18 @@ private: void UpdateValues(); void Initialize(); void InitializeLiveWindowComponents(); - + std::vector m_sensors; std::map m_components; - + static LiveWindow *m_instance; ITable *m_liveWindowTable; ITable *m_statusTable; - + Scheduler *m_scheduler; - + bool m_enabled; bool m_firstTime; }; #endif - diff --git a/wpilibc/wpilibC++Sim/include/Notifier.h b/wpilibc/wpilibC++Sim/include/Notifier.h index 33ed669900..4bd89879f3 100644 --- a/wpilibc/wpilibC++Sim/include/Notifier.h +++ b/wpilibc/wpilibC++Sim/include/Notifier.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef NOTIFIER_H -#define NOTIFIER_H +#pragma once #include "ErrorBase.h" #include "Task.h" @@ -37,12 +35,10 @@ private: Notifier *m_nextEvent; // next Nofifier event bool m_periodic; // true if this is a periodic event bool m_queued; // indicates if this entry is queued - SEMAPHORE_ID m_handlerSemaphore; // held by interrupt manager task while handler call is in progress + SEMAPHORE_ID m_handlerSemaphore; // held by interrupt manager task while handler call is in progress + DISALLOW_COPY_AND_ASSIGN(Notifier); -private: - static Task *task; - static void Run(); + static Task *task; + static void Run(); }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/PIDController.h b/wpilibc/wpilibC++Sim/include/PIDController.h index c26400a001..1790d9b174 100644 --- a/wpilibc/wpilibC++Sim/include/PIDController.h +++ b/wpilibc/wpilibC++Sim/include/PIDController.h @@ -3,14 +3,12 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef PIDCONTROLLER_H_ -#define PIDCONTROLLER_H_ +#pragma once #include "Base.h" -#include "HAL/Semaphore.hpp" #include "Controller.h" #include "LiveWindow/LiveWindow.h" +#include class PIDOutput; class PIDSource; @@ -26,12 +24,10 @@ class Notifier; class PIDController : public LiveWindowSendable, public Controller, public ITableListener { public: - 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, 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(); virtual float Get(); virtual void SetContinuous(bool continuous = true); @@ -43,23 +39,23 @@ public: virtual float GetI(); virtual float GetD(); virtual float GetF(); - + virtual void SetSetpoint(float setpoint); virtual float GetSetpoint(); virtual float GetError(); - + virtual void SetTolerance(float percent); virtual void SetAbsoluteTolerance(float absValue); virtual void SetPercentTolerance(float percentValue); virtual bool OnTarget(); - + virtual void Enable(); virtual void Disable(); virtual bool IsEnabled(); - + virtual void Reset(); - + virtual void InitTable(ITable* table); private: @@ -75,7 +71,12 @@ private: bool m_enabled; //is the pid controller enabled float m_prevError; // the prior sensor input (used to compute velocity) double m_totalError; //the sum of the errors for use in the integral calc - enum {kAbsoluteTolerance, kPercentTolerance, kNoTolerance} m_toleranceType; + enum + { + kAbsoluteTolerance, + kPercentTolerance, + kNoTolerance + } m_toleranceType; float m_tolerance; //the percetage or absolute error that is considered on target float m_setpoint; float m_error; @@ -88,11 +89,10 @@ private: PIDOutput *m_pidOutput; Notifier *m_controlLoop; - 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); static void CallCalculate(void *controller); - + virtual ITable* GetTable(); virtual std::string GetSmartDashboardType(); virtual void ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew); @@ -105,5 +105,3 @@ protected: DISALLOW_COPY_AND_ASSIGN(PIDController); }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/PIDOutput.h b/wpilibc/wpilibC++Sim/include/PIDOutput.h index 81483a6807..a2e2c95de6 100644 --- a/wpilibc/wpilibC++Sim/include/PIDOutput.h +++ b/wpilibc/wpilibC++Sim/include/PIDOutput.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef PID_OUTPUT_H -#define PID_OUTPUT_H +#pragma once #include "Base.h" @@ -20,5 +18,3 @@ class PIDOutput public: virtual void PIDWrite(float output) = 0; }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/PIDSource.h b/wpilibc/wpilibC++Sim/include/PIDSource.h index a2ed1c296b..e81bd286df 100644 --- a/wpilibc/wpilibC++Sim/include/PIDSource.h +++ b/wpilibc/wpilibC++Sim/include/PIDSource.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef PID_SOURCE_H -#define PID_SOURCE_H +#pragma once /** * PIDSource interface is a generic sensor source for the PID class. @@ -15,8 +13,6 @@ class PIDSource { public: - typedef enum {kDistance, kRate, kAngle} PIDSourceParameter; + enum PIDSourceParameter {kDistance, kRate, kAngle}; virtual double PIDGet() = 0; }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/Resource.h b/wpilibc/wpilibC++Sim/include/Resource.h index 226e64a536..797bd669ef 100644 --- a/wpilibc/wpilibC++Sim/include/Resource.h +++ b/wpilibc/wpilibC++Sim/include/Resource.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef RESOURCE_H_ -#define RESOURCE_H_ +#pragma once #include "ErrorBase.h" #include "HAL/cpp/Synchronized.hpp" @@ -39,5 +37,3 @@ private: DISALLOW_COPY_AND_ASSIGN(Resource); }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/RobotBase.h b/wpilibc/wpilibC++Sim/include/RobotBase.h index 8435affc59..5a545f6c87 100644 --- a/wpilibc/wpilibC++Sim/include/RobotBase.h +++ b/wpilibc/wpilibC++Sim/include/RobotBase.h @@ -3,20 +3,17 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef ROBOT_H_ -#define ROBOT_H_ +#pragma once #include "Base.h" #include "DriverStation.h" #define START_ROBOT_CLASS(_ClassName_) \ - int main() \ - { \ - RobotBase* robot = new _ClassName_(); \ - robot->StartCompetition(); \ - return 0; \ - } + int main() \ + { \ + (new _ClassName_())->StartCompetition(); \ + return 0; \ + } /** * Implement a Robot Program framework. @@ -26,7 +23,8 @@ * 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 { +class RobotBase +{ friend class RobotDeleter; public: static RobotBase &getInstance(); @@ -36,21 +34,17 @@ public: bool IsDisabled(); bool IsAutonomous(); bool IsOperatorControl(); - bool IsTest(); - // bool IsNewDataAvailable(); - // static void startRobotTask(FUNCPTR factory); - // static void robotTask(FUNCPTR factory, Task *task); + bool IsTest(); virtual void StartCompetition() = 0; protected: virtual ~RobotBase(); RobotBase(); + DriverStation *m_ds; private: static RobotBase *m_instance; + DISALLOW_COPY_AND_ASSIGN(RobotBase); }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/RobotDrive.h b/wpilibc/wpilibC++Sim/include/RobotDrive.h index 2165c6cfa0..79fff7f15d 100644 --- a/wpilibc/wpilibC++Sim/include/RobotDrive.h +++ b/wpilibc/wpilibC++Sim/include/RobotDrive.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef ROBOTDRIVE_H_ -#define ROBOTDRIVE_H_ +#pragma once #include "ErrorBase.h" #include @@ -21,38 +19,42 @@ class GenericHID; * 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 ErrorBase +class RobotDrive : public ErrorBase { public: - typedef enum + enum MotorType { kFrontLeftMotor = 0, kFrontRightMotor = 1, kRearLeftMotor = 2, kRearRightMotor = 3 - } MotorType; + }; RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel); RobotDrive(uint32_t frontLeftMotorChannel, uint32_t rearLeftMotorChannel, - uint32_t frontRightMotorChannel, uint32_t rearRightMotorChannel); + uint32_t frontRightMotorChannel, uint32_t rearRightMotorChannel); RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor); RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor); RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor, - SpeedController *frontRightMotor, SpeedController *rearRightMotor); + SpeedController *frontRightMotor, SpeedController *rearRightMotor); RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor, - SpeedController &frontRightMotor, SpeedController &rearRightMotor); + SpeedController &frontRightMotor, SpeedController &rearRightMotor); virtual ~RobotDrive(); 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(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(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); @@ -86,20 +88,21 @@ protected: SpeedController *m_frontRightMotor; SpeedController *m_rearLeftMotor; SpeedController *m_rearRightMotor; - // FIXME: MotorSafetyHelper *m_safetyHelper; - + // 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++; + if (m_frontLeftMotor) + motors++; + if (m_frontRightMotor) + motors++; + if (m_rearLeftMotor) + motors++; + if (m_rearRightMotor) + motors++; return motors; } DISALLOW_COPY_AND_ASSIGN(RobotDrive); }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/SensorBase.h b/wpilibc/wpilibC++Sim/include/SensorBase.h index 31f7d18273..ef4e296ce9 100644 --- a/wpilibc/wpilibC++Sim/include/SensorBase.h +++ b/wpilibc/wpilibC++Sim/include/SensorBase.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef SENSORBASE_H_ -#define SENSORBASE_H_ +#pragma once #include "ErrorBase.h" #include @@ -16,33 +14,35 @@ * Stores most recent status information as well as containing utility functions for checking * channels and error processing. */ -class SensorBase: public ErrorBase { +class SensorBase : public ErrorBase +{ public: SensorBase(); virtual ~SensorBase(); static void DeleteSingletons(); - static uint32_t GetDefaultAnalogModule() { return 1; } - static uint32_t GetDefaultDigitalModule() { return 1; } - static uint32_t GetDefaultSolenoidModule() { return 1; } - static bool CheckAnalogModule(uint8_t moduleNumber); - static bool CheckDigitalModule(uint8_t moduleNumber); - static bool CheckPWMModule(uint8_t moduleNumber); - static bool CheckRelayModule(uint8_t moduleNumber); + + static uint32_t GetDefaultSolenoidModule() + { + return 1; + } + static bool CheckSolenoidModule(uint8_t moduleNumber); static bool CheckDigitalChannel(uint32_t channel); static bool CheckRelayChannel(uint32_t channel); static bool CheckPWMChannel(uint32_t channel); - static bool CheckAnalogChannel(uint32_t channel); + static bool CheckAnalogInput(uint32_t channel); + static bool CheckAnalogOutput(uint32_t channel); static bool CheckSolenoidChannel(uint32_t channel); + static bool CheckPDPChannel(uint32_t channel); - static const uint32_t kDigitalChannels = 14; - static const uint32_t kAnalogChannels = 8; - static const uint32_t kAnalogModules = 2; - static const uint32_t kDigitalModules = 2; + static const uint32_t kDigitalChannels = 20; + static const uint32_t kAnalogInputs = 8; + static const uint32_t kAnalogOutputs = 2; static const uint32_t kSolenoidChannels = 8; static const uint32_t kSolenoidModules = 2; - static const uint32_t kPwmChannels = 10; + static const uint32_t kPwmChannels = 20; static const uint32_t kRelayChannels = 8; + static const uint32_t kPDPChannels = 16; static const uint32_t kChassisSlots = 8; protected: void AddToSingletonList(); @@ -52,6 +52,3 @@ private: static SensorBase *m_singletonList; SensorBase *m_nextSingleton; }; - - -#endif diff --git a/wpilibc/wpilibC++Sim/include/SimpleRobot.h b/wpilibc/wpilibC++Sim/include/SimpleRobot.h index bf580bcf0c..6b96523cb4 100644 --- a/wpilibc/wpilibC++Sim/include/SimpleRobot.h +++ b/wpilibc/wpilibC++Sim/include/SimpleRobot.h @@ -3,29 +3,26 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef SIMPLE_ROBOT_H -#define SIMPLE_ROBOT_H +#pragma once #include "RobotBase.h" /** * @todo If this is going to last until release, it needs a better name. */ -class SimpleRobot : public RobotBase { +class SimpleRobot : public RobotBase +{ public: SimpleRobot(); virtual ~SimpleRobot() {} virtual void RobotInit(); virtual void Disabled(); virtual void Autonomous(); - virtual void OperatorControl(); - virtual void Test(); + virtual void OperatorControl(); + virtual void Test(); virtual void RobotMain(); void StartCompetition(); private: bool m_robotMainOverridden; }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/SmartDashboard/SmartDashboard.h b/wpilibc/wpilibC++Sim/include/SmartDashboard/SmartDashboard.h index c6c86086d3..5c6d3cda04 100644 --- a/wpilibc/wpilibC++Sim/include/SmartDashboard/SmartDashboard.h +++ b/wpilibc/wpilibC++Sim/include/SmartDashboard/SmartDashboard.h @@ -23,7 +23,7 @@ public: static void PutData(std::string key, Sendable *data); static void PutData(NamedSendable *value); - static Sendable* GetData(std::string keyName); + //static Sendable* GetData(std::string keyName); static void PutBoolean(std::string keyName, bool value); static bool GetBoolean(std::string keyName); diff --git a/wpilibc/wpilibC++Sim/include/SpeedController.h b/wpilibc/wpilibC++Sim/include/SpeedController.h index 17bc81300c..c571aa7eac 100644 --- a/wpilibc/wpilibC++Sim/include/SpeedController.h +++ b/wpilibc/wpilibC++Sim/include/SpeedController.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef SPEED_CONTROLLER_H -#define SPEED_CONTROLLER_H +#pragma once #include "PIDOutput.h" @@ -15,14 +13,14 @@ class SpeedController : public PIDOutput { public: - virtual ~SpeedController() {}; + virtual ~SpeedController() {} /** * 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; + virtual void Set(float speed, uint8_t syncGroup = 0) = 0; /** * Common interface for getting the current set speed of a speed controller. * @@ -34,6 +32,3 @@ public: */ virtual void Disable() = 0; }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/Talon.h b/wpilibc/wpilibC++Sim/include/Talon.h index 522adfc7fe..c51cc82253 100644 --- a/wpilibc/wpilibC++Sim/include/Talon.h +++ b/wpilibc/wpilibC++Sim/include/Talon.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef Talon_H -#define Talon_H +#pragma once #include "simulation/SimContinuousOutput.h" #include "SpeedController.h" @@ -18,18 +16,14 @@ class Talon : public SpeedController { public: explicit Talon(uint32_t channel); - Talon(uint8_t moduleNumber, uint32_t channel); virtual ~Talon(); - virtual void Set(float value, uint8_t syncGroup=0); + virtual void Set(float value, uint8_t syncGroup = 0); virtual float Get(); virtual void Disable(); virtual void PIDWrite(float output); private: - void InitTalon(int slot, int channel); - SimContinuousOutput* impl; + void InitTalon(int channel); + SimContinuousOutput* impl; }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/Task.h b/wpilibc/wpilibC++Sim/include/Task.h index c7ea9ee70c..ca1754a50d 100644 --- a/wpilibc/wpilibC++Sim/include/Task.h +++ b/wpilibc/wpilibC++Sim/include/Task.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef __TASK_H__ -#define __TASK_H__ +#pragma once #include "ErrorBase.h" #include "HAL/Task.hpp" @@ -19,11 +17,13 @@ class Task : public ErrorBase public: static const uint32_t kDefaultPriority = 101; - Task(const char* name, FUNCPTR function, int32_t priority = kDefaultPriority, uint32_t stackSize = 20000); + Task(const char* name, FUNCPTR function, int32_t priority = kDefaultPriority, + uint32_t stackSize = 20000); virtual ~Task(); - bool Start(uint32_t arg0 = 0, uint32_t arg1 = 0, uint32_t arg2 = 0, uint32_t arg3 = 0, uint32_t arg4 = 0, - uint32_t arg5 = 0, uint32_t arg6 = 0, uint32_t arg7 = 0, uint32_t arg8 = 0, uint32_t arg9 = 0); + bool Start(uint32_t arg0 = 0, uint32_t arg1 = 0, uint32_t arg2 = 0, uint32_t arg3 = 0, + uint32_t arg4 = 0, uint32_t arg5 = 0, uint32_t arg6 = 0, uint32_t arg7 = 0, + uint32_t arg8 = 0, uint32_t arg9 = 0); bool Restart(); bool Stop(); @@ -50,4 +50,3 @@ private: DISALLOW_COPY_AND_ASSIGN(Task); }; -#endif // __TASK_H__ diff --git a/wpilibc/wpilibC++Sim/include/Timer.h b/wpilibc/wpilibC++Sim/include/Timer.h index 8a03ab757c..67d574f2d3 100644 --- a/wpilibc/wpilibC++Sim/include/Timer.h +++ b/wpilibc/wpilibC++Sim/include/Timer.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef TIMER_H_ -#define TIMER_H_ +#pragma once #include "HAL/Semaphore.hpp" #include "Base.h" @@ -16,7 +14,6 @@ void Wait(double seconds); double GetClock(); 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 @@ -45,5 +42,3 @@ private: MUTEX_ID m_semaphore; DISALLOW_COPY_AND_ASSIGN(Timer); }; - -#endif diff --git a/wpilibc/wpilibC++Sim/include/Utility.h b/wpilibc/wpilibC++Sim/include/Utility.h index f2a5e3ec2f..25c56e0006 100644 --- a/wpilibc/wpilibC++Sim/include/Utility.h +++ b/wpilibc/wpilibC++Sim/include/Utility.h @@ -3,12 +3,10 @@ /* 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 -#ifndef UTILITY_H_ -#define UTILITY_H_ - #define wpi_assert(condition) wpi_assert_impl(condition, #condition, NULL, __FILE__, __LINE__, __FUNCTION__) #define wpi_assertWithMessage(condition, message) wpi_assert_impl(condition, #condition, message, __FILE__, __LINE__, __FUNCTION__) @@ -26,5 +24,3 @@ void wpi_suspendOnAssertEnabled(bool enabled); void wpi_stackTraceOnAssertEnable(bool enabled); uint32_t GetFPGATime(); - -#endif // UTILITY_H_ diff --git a/wpilibc/wpilibC++Sim/include/Victor.h b/wpilibc/wpilibC++Sim/include/Victor.h index 486c4f8f96..5873176afa 100644 --- a/wpilibc/wpilibC++Sim/include/Victor.h +++ b/wpilibc/wpilibC++Sim/include/Victor.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef VICTOR_H -#define VICTOR_H +#pragma once #include "simulation/SimContinuousOutput.h" #include "SpeedController.h" @@ -18,18 +16,14 @@ class Victor : public SpeedController { public: explicit Victor(uint32_t channel); - Victor(uint8_t moduleNumber, uint32_t channel); virtual ~Victor(); - virtual void Set(float value, uint8_t syncGroup=0); + virtual void Set(float value, uint8_t syncGroup = 0); virtual float Get(); virtual void Disable(); virtual void PIDWrite(float output); private: - void InitVictor(int slot, int channel); - SimContinuousOutput* impl; + void InitVictor(int channel); + SimContinuousOutput* impl; }; - -#endif - diff --git a/wpilibc/wpilibC++Sim/include/WPIErrors.h b/wpilibc/wpilibC++Sim/include/WPIErrors.h index 01adf0e27e..3968c7d8c1 100644 --- a/wpilibc/wpilibC++Sim/include/WPIErrors.h +++ b/wpilibc/wpilibC++Sim/include/WPIErrors.h @@ -3,9 +3,7 @@ /* 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. */ /*----------------------------------------------------------------------------*/ - -#ifndef __WPI_ERRORS_H__ -#define __WPI_ERRORS_H__ +#pragma once #ifdef WPI_ERRORS_DEFINE_STRINGS #define S(label, offset, message) const char *wpi_error_s_##label = message ; @@ -47,6 +45,7 @@ S(LineNotOutput, -27, "Cannot SetDigitalOutput for a line not configured for out S(ParameterOutOfRange, -28, "A parameter is out of range."); S(SPIClockRateTooLow, -29, "SPI clock rate was below the minimum supported"); S(JaguarVersionError, -30, "Jaguar firmware version error"); +S(JaguarMessageNotFound, -31, "Jaguar message not found"); S(NetworkTablesReadError, -40, "Error reading NetworkTables socket"); S(NetworkTablesBufferFull, -41, "Buffer full writing to NetworkTables socket"); S(NetworkTablesWrongType, -42, "The wrong type was read from the NetworkTables entry"); @@ -75,5 +74,3 @@ S(SPIReadNoData, 14, "No data available to read from SPI"); S(IncompatibleState, 15, "Incompatible State: The operation cannot be completed"); #undef S - -#endif // __WPI_ERRORS_H__ diff --git a/wpilibc/wpilibC++Sim/include/WPILib.h b/wpilibc/wpilibC++Sim/include/WPILib.h index 318fd55c91..1890ab4af9 100644 --- a/wpilibc/wpilibC++Sim/include/WPILib.h +++ b/wpilibc/wpilibC++Sim/include/WPILib.h @@ -4,12 +4,13 @@ * Created on: May 29, 2014 * Author: alex */ - -#ifndef WPILIB_H_ -#define WPILIB_H_ +#pragma once #define SIMULATION "gazebo" +#include "string.h" +#include + #include "Buttons/Trigger.h" #include "Buttons/Button.h" #include "Buttons/InternalButton.h" @@ -41,7 +42,7 @@ #include "Solenoid.h" #include "DoubleSolenoid.h" #include "interfaces/Potentiometer.h" -#include "AnalogChannel.h" +#include "AnalogInput.h" #include "AnalogPotentiometer.h" #include "Counter.h" #include "DigitalInput.h" @@ -53,4 +54,3 @@ #include "RobotDrive.h" #include "LiveWindow/LiveWindow.h" -#endif /* WPILIB_H_ */ diff --git a/wpilibc/wpilibC++Sim/include/interfaces/Potentiometer.h b/wpilibc/wpilibC++Sim/include/interfaces/Potentiometer.h index f6b38d467a..f5f45674cb 100644 --- a/wpilibc/wpilibC++Sim/include/interfaces/Potentiometer.h +++ b/wpilibc/wpilibC++Sim/include/interfaces/Potentiometer.h @@ -25,4 +25,3 @@ public: }; #endif - diff --git a/wpilibc/wpilibC++Sim/src/AnalogChannel.cpp b/wpilibc/wpilibC++Sim/src/AnalogInput.cpp similarity index 51% rename from wpilibc/wpilibC++Sim/src/AnalogChannel.cpp rename to wpilibc/wpilibC++Sim/src/AnalogInput.cpp index aff1d6abc4..7ac26ff21a 100644 --- a/wpilibc/wpilibC++Sim/src/AnalogChannel.cpp +++ b/wpilibc/wpilibC++Sim/src/AnalogInput.cpp @@ -1,62 +1,51 @@ /*----------------------------------------------------------------------------*/ -/* 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. */ /*----------------------------------------------------------------------------*/ -#include "AnalogChannel.h" +#include "AnalogInput.h" #include "WPIErrors.h" #include "LiveWindow/LiveWindow.h" /** * Common initialization. */ -void AnalogChannel::InitChannel(uint8_t moduleNumber, uint32_t channel) +void AnalogInput::InitAnalogInput(uint32_t channel) { m_table = NULL; - m_module = moduleNumber; - m_channel = channel; - char buffer[50]; - int n = sprintf(buffer, "analog/%d/%d", moduleNumber, channel); - m_impl = new SimFloatInput(buffer); - LiveWindow::GetInstance()->AddSensor("AnalogChannel",channel, GetModuleNumber(), this); + m_channel = channel; + char buffer[50]; + int n = sprintf(buffer, "analog/1/%d", channel); + m_impl = new SimFloatInput(buffer); + + LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this); } /** - * Construct an analog channel on a specified module. - * - * @param moduleNumber The analog module (1 or 2). - * @param channel The channel number to represent. - */ -AnalogChannel::AnalogChannel(uint8_t moduleNumber, uint32_t channel) -{ - InitChannel(moduleNumber, channel); -} - -/** - * Construct an analog channel on the default module. + * Construct an analog input. * * @param channel The channel number to represent. */ -AnalogChannel::AnalogChannel(uint32_t channel) +AnalogInput::AnalogInput(uint32_t channel) { - InitChannel(GetDefaultAnalogModule(), channel); + InitAnalogInput(channel); } /** * Channel destructor. */ -AnalogChannel::~AnalogChannel() +AnalogInput::~AnalogInput() { } /** - * Get a scaled sample straight from this channel on the module. + * 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 on the module. + * @return A scaled sample straight from this channel. */ -float AnalogChannel::GetVoltage() +float AnalogInput::GetVoltage() { return m_impl->Get(); } @@ -68,7 +57,7 @@ float AnalogChannel::GetVoltage() * 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 AnalogChannel::GetAverageVoltage() +float AnalogInput::GetAverageVoltage() { return m_impl->Get(); } @@ -77,55 +66,44 @@ float AnalogChannel::GetAverageVoltage() * Get the channel number. * @return The channel number. */ -uint32_t AnalogChannel::GetChannel() +uint32_t AnalogInput::GetChannel() { return m_channel; } -/** - * Get the module number. - * @return The module number. - */ -uint8_t AnalogChannel::GetModuleNumber() -{ - return m_module; -} - /** * Get the Average value for the PID Source base object. * * @return The average voltage. */ -double AnalogChannel::PIDGet() +double AnalogInput::PIDGet() { return GetAverageVoltage(); } -void AnalogChannel::UpdateTable() { +void AnalogInput::UpdateTable() { if (m_table != NULL) { m_table->PutNumber("Value", GetAverageVoltage()); } } -void AnalogChannel::StartLiveWindowMode() { +void AnalogInput::StartLiveWindowMode() { } -void AnalogChannel::StopLiveWindowMode() { +void AnalogInput::StopLiveWindowMode() { } -std::string AnalogChannel::GetSmartDashboardType() { +std::string AnalogInput::GetSmartDashboardType() { return "Analog Input"; } -void AnalogChannel::InitTable(ITable *subTable) { +void AnalogInput::InitTable(ITable *subTable) { m_table = subTable; UpdateTable(); } -ITable * AnalogChannel::GetTable() { +ITable * AnalogInput::GetTable() { return m_table; } - - diff --git a/wpilibc/wpilibC++Sim/src/AnalogPotentiometer.cpp b/wpilibc/wpilibC++Sim/src/AnalogPotentiometer.cpp index 98ca8a5a2b..47e0361bd4 100644 --- a/wpilibc/wpilibC++Sim/src/AnalogPotentiometer.cpp +++ b/wpilibc/wpilibC++Sim/src/AnalogPotentiometer.cpp @@ -1,43 +1,66 @@ #include "AnalogPotentiometer.h" -void AnalogPotentiometer::initPot(int slot, int channel, double scale, double offset) { - char buffer[50]; - int n = sprintf(buffer, "analog/%d/%d", slot, channel); - impl = new SimFloatInput(buffer); - this->module = slot; - this->channel = channel; -} - -AnalogPotentiometer::AnalogPotentiometer(int slot, int channel, double scale, double offset) { - initPot(slot, channel, scale, offset); +/** + * 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; } AnalogPotentiometer::AnalogPotentiometer(int channel, double scale, double offset) { - initPot(1, channel, scale, offset); + m_init_analog_input = true; + initPot(new AnalogInput(channel), scale, offset); } -AnalogPotentiometer::AnalogPotentiometer(int channel, double scale) { - initPot(1, channel, scale, 0); +AnalogPotentiometer::AnalogPotentiometer(AnalogInput *input, double scale, double offset) { + m_init_analog_input = false; + initPot(input, scale, offset); } -AnalogPotentiometer::AnalogPotentiometer(int channel) { - initPot(1, channel, 1, 0); +AnalogPotentiometer::AnalogPotentiometer(AnalogInput &input, double scale, double offset) { + m_init_analog_input = false; + initPot(&input, scale, offset); } +AnalogPotentiometer::~AnalogPotentiometer() { + if(m_init_analog_input){ + delete m_analog_input; + m_init_analog_input = false; + } +} +/** + * Get the current reading of the potentiomere. + * + * @return The current position of the potentiometer. + */ double AnalogPotentiometer::Get() { - return impl->Get(); + return m_analog_input->GetVoltage() * m_scale + m_offset; } - + +/** + * Implement the PIDSource interface. + * + * @return The current reading. + */ double AnalogPotentiometer::PIDGet() { return Get(); } + +/** + * @return the Smart Dashboard Type + */ std::string AnalogPotentiometer::GetSmartDashboardType() { return "Analog Input"; } +/** + * Live Window code, only does anything if live window is activated. + */ void AnalogPotentiometer::InitTable(ITable *subtable) { m_table = subtable; UpdateTable(); diff --git a/wpilibc/wpilibC++Sim/src/Commands/Command.cpp b/wpilibc/wpilibC++Sim/src/Commands/Command.cpp index b90e4efb5c..f6a5bd1c9c 100644 --- a/wpilibc/wpilibC++Sim/src/Commands/Command.cpp +++ b/wpilibc/wpilibC++Sim/src/Commands/Command.cpp @@ -129,7 +129,7 @@ double Command::TimeSinceInitialized() if (m_startTime < 0.0) return 0.0; else - return Timer::GetFPGATimestamp() - m_startTime; + return Timer::GetFPGATimestamp() - m_startTime; } /** @@ -239,7 +239,7 @@ void Command::_End() */ void Command::StartTiming() { - m_startTime = Timer::GetFPGATimestamp(); + m_startTime = Timer::GetFPGATimestamp(); } /** diff --git a/wpilibc/wpilibC++Sim/src/Commands/WaitUntilCommand.cpp b/wpilibc/wpilibC++Sim/src/Commands/WaitUntilCommand.cpp index 8acddb1525..3d521b8156 100644 --- a/wpilibc/wpilibC++Sim/src/Commands/WaitUntilCommand.cpp +++ b/wpilibc/wpilibC++Sim/src/Commands/WaitUntilCommand.cpp @@ -39,7 +39,7 @@ void WaitUntilCommand::Execute() */ bool WaitUntilCommand::IsFinished() { - return DriverStation::GetInstance()->GetMatchTime() >= m_time; + return DriverStation::GetInstance()->GetMatchTime() >= m_time; } void WaitUntilCommand::End() diff --git a/wpilibc/wpilibC++Sim/src/DigitalInput.cpp b/wpilibc/wpilibC++Sim/src/DigitalInput.cpp index f1c8055f98..671be61fb3 100644 --- a/wpilibc/wpilibC++Sim/src/DigitalInput.cpp +++ b/wpilibc/wpilibC++Sim/src/DigitalInput.cpp @@ -9,16 +9,16 @@ /** * Create an instance of a DigitalInput. - * Creates a digital input given a slot and channel. Common creation routine - * for all constructors. + * Creates a digital input given a channel. Common creation routine for all + * constructors. */ -void DigitalInput::InitDigitalInput(uint8_t moduleNumber, uint32_t channel) +void DigitalInput::InitDigitalInput(uint32_t channel) { m_table = NULL; + char buf[64]; m_channel = channel; - char buffer[50]; - int n = sprintf(buffer, "dio/%d/%d", moduleNumber, channel); - m_impl = new SimDigitalInput(buffer); + int n = sprintf(buf, "dio/1/%d", channel); + m_impl = new SimDigitalInput(buf); } /** @@ -29,19 +29,7 @@ void DigitalInput::InitDigitalInput(uint8_t moduleNumber, uint32_t channel) */ DigitalInput::DigitalInput(uint32_t channel) { - InitDigitalInput(1, channel); -} - -/** - * Create an instance of a Digital Input class. - * Creates a digital input given an channel and module. - * - * @param moduleNumber The digital module (1 or 2). - * @param channel The digital channel (1..14). - */ -DigitalInput::DigitalInput(uint8_t moduleNumber, uint32_t channel) -{ - InitDigitalInput(moduleNumber, channel); + InitDigitalInput(channel); } /** @@ -75,11 +63,11 @@ void DigitalInput::UpdateTable() { } void DigitalInput::StartLiveWindowMode() { - + } void DigitalInput::StopLiveWindowMode() { - + } std::string DigitalInput::GetSmartDashboardType() { diff --git a/wpilibc/wpilibC++Sim/src/DriverStation.cpp b/wpilibc/wpilibC++Sim/src/DriverStation.cpp index 098b0472da..71c23e3efe 100644 --- a/wpilibc/wpilibC++Sim/src/DriverStation.cpp +++ b/wpilibc/wpilibC++Sim/src/DriverStation.cpp @@ -22,7 +22,6 @@ TLogLevel dsLogLevel = logDEBUG; if (level > dsLogLevel) ; \ else Log().Get(level) -const uint32_t DriverStation::kBatteryModuleNumber; const uint32_t DriverStation::kBatteryChannel; const uint32_t DriverStation::kJoystickPorts; const uint32_t DriverStation::kJoystickAxes; @@ -36,7 +35,7 @@ uint8_t DriverStation::m_updateNumber = 0; * This is only called once the first time GetInstance() is called */ DriverStation::DriverStation() - : m_digitalOut (0) + : m_digitalOut (0) , m_waitForDataSem(0) , m_approxMatchTimeOffset(-1.0) , m_userInDisabled(false) @@ -46,25 +45,25 @@ DriverStation::DriverStation() { // Create a new semaphore m_waitForDataSem = initializeMultiWait(); - m_stateSemaphore = initializeMutexRecursive(); - m_joystickSemaphore = initializeMutexRecursive(); + m_stateSemaphore = initializeMutexRecursive(); + m_joystickSemaphore = initializeMutexRecursive(); - state = msgs::DriverStationPtr(new msgs::DriverStation()); - stateSub = MainNode::Subscribe("~/ds/state", - &DriverStation::stateCallback, this); - // TODO: for loop + boost bind - joysticks[0] = msgs::JoystickPtr(new msgs::Joystick()); - joysticksSub[0] = MainNode::Subscribe("~/ds/joysticks/1", - &DriverStation::joystickCallback1, this); - joysticks[1] = msgs::JoystickPtr(new msgs::Joystick()); - joysticksSub[1] = MainNode::Subscribe("~/ds/joysticks/2", - &DriverStation::joystickCallback2, this); - joysticks[2] = msgs::JoystickPtr(new msgs::Joystick()); - joysticksSub[2] = MainNode::Subscribe("~/ds/joysticks/3", - &DriverStation::joystickCallback3, this); - joysticks[3] = msgs::JoystickPtr(new msgs::Joystick()); - joysticksSub[3] = MainNode::Subscribe("~/ds/joysticks/4", - &DriverStation::joystickCallback4, this); + state = msgs::DriverStationPtr(new msgs::DriverStation()); + stateSub = MainNode::Subscribe("~/ds/state", + &DriverStation::stateCallback, this); + // TODO: for loop + boost bind + joysticks[0] = msgs::JoystickPtr(new msgs::Joystick()); + joysticksSub[0] = MainNode::Subscribe("~/ds/joysticks/1", + &DriverStation::joystickCallback1, this); + joysticks[1] = msgs::JoystickPtr(new msgs::Joystick()); + joysticksSub[1] = MainNode::Subscribe("~/ds/joysticks/2", + &DriverStation::joystickCallback2, this); + joysticks[2] = msgs::JoystickPtr(new msgs::Joystick()); + joysticksSub[2] = MainNode::Subscribe("~/ds/joysticks/3", + &DriverStation::joystickCallback3, this); + joysticks[3] = msgs::JoystickPtr(new msgs::Joystick()); + joysticksSub[3] = MainNode::Subscribe("~/ds/joysticks/4", + &DriverStation::joystickCallback4, this); AddToSingletonList(); } @@ -73,6 +72,7 @@ DriverStation::~DriverStation() { m_instance = NULL; deleteMultiWait(m_waitForDataSem); + // TODO: Release m_stateSemaphore and m_joystickSemaphore? } /** @@ -88,16 +88,16 @@ DriverStation* DriverStation::GetInstance() } /** - * Read the battery voltage from the specified AnalogChannel. - * + * Read the battery voltage from the specified AnalogInput. + * * This accessor assumes that the battery voltage is being measured * through the voltage divider on an analog breakout. - * + * * @return The battery voltage. */ float DriverStation::GetBatteryVoltage() { - return 12.0; // 12 volts all the time! + return 12.0; // 12 volts all the time! } /** @@ -156,7 +156,7 @@ bool DriverStation::GetStickButton(uint32_t stick, uint32_t button) /** * 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. */ @@ -189,14 +189,14 @@ short DriverStation::GetStickButtons(uint32_t stick) * 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; + wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetAnalogIn"); + return 0.0; } /** @@ -207,8 +207,8 @@ float DriverStation::GetAnalogIn(uint32_t channel) */ bool DriverStation::GetDigitalIn(uint32_t channel) { - wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalIn"); - return false; + wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalIn"); + return false; } /** @@ -220,7 +220,7 @@ bool DriverStation::GetDigitalIn(uint32_t channel) * @param channel The digital output to set. Valid range is 1 - 8. * @param value The state to set the digital output. */ -void DriverStation::SetDigitalOut(uint32_t channel, bool value) +void DriverStation::SetDigitalOut(uint32_t channel, bool value) { wpi_setWPIErrorWithContext(UnsupportedInSimulation, "SetDigitalOut"); } @@ -230,7 +230,7 @@ void DriverStation::SetDigitalOut(uint32_t channel, bool value) * @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) +bool DriverStation::GetDigitalOut(uint32_t channel) { wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalOut"); } diff --git a/wpilibc/wpilibC++Sim/src/Encoder.cpp b/wpilibc/wpilibC++Sim/src/Encoder.cpp index 5914c68fc8..7a1a4f58a4 100644 --- a/wpilibc/wpilibC++Sim/src/Encoder.cpp +++ b/wpilibc/wpilibC++Sim/src/Encoder.cpp @@ -19,62 +19,33 @@ * 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 slotA, int channelA, int slotB, int channelB, - bool reverseDirection, EncodingType encodingType) +void Encoder::InitEncoder(int channelA, int channelB, bool reverseDirection, EncodingType encodingType) { - this->slotA = slotA; + m_table = NULL; this->channelA = channelA; - this->slotB = slotB; this->channelB = channelB; reversed = reverseDirection; m_encodingType = encodingType; - - m_table = NULL; + int32_t index = 0; m_distancePerPulse = 1.0; m_pidSource = kDistance; - LiveWindow::GetInstance()->AddSensor("Encoder", slotA, channelA, this); + LiveWindow::GetInstance()->AddSensor("Encoder", channelA, this); - if ((slotB < slotA) || ((slotB == slotA) && (channelB < channelA))) { // Swap ports - int slot = slotB; + if (channelB < channelA) { // Swap ports int channel = channelB; - slotB = slotA; channelB = channelA; - slotA = slot; channelA = channel; } char buffer[50]; - int n = sprintf(buffer, "dio/%d/%d/%d/%d", slotA, channelA, slotB, channelB); + int n = sprintf(buffer, "dio/1/%d/1/%d", channelA, channelB); impl = new SimEncoder(buffer); } /** * Encoder constructor. - * Construct a Encoder given a and b modules and channels fully specified. - * @param aModuleNumber The a channel digital input module. - * @param aChannel The a channel digital input channel. - * @param bModuleNumber The b channel digital input module. - * @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. - */ -Encoder::Encoder(uint8_t aModuleNumber, uint32_t aChannel, - uint8_t bModuleNumber, uint32_t bChannel, - bool reverseDirection, EncodingType encodingType) -{ - InitEncoder(aModuleNumber, aChannel, bModuleNumber, bChannel, - reverseDirection, encodingType); -} - -/** - * Encoder constructor. - * Construct a Encoder given a and b channels assuming the default module. + * Construct a Encoder given a and b channels. * @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 @@ -87,7 +58,7 @@ Encoder::Encoder(uint8_t aModuleNumber, uint32_t aChannel, */ Encoder::Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection, EncodingType encodingType) { - InitEncoder(1, aChannel, 1, bChannel, reverseDirection, encodingType); + InitEncoder(aChannel, bChannel, reverseDirection, encodingType); } /** @@ -216,12 +187,66 @@ double Encoder::DecodingScaleFactor() return 0.25; default: return 0.0; - } + } +} + +/** + * 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() +{ + 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. + */ +int32_t Encoder::Get() +{ + 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(). + * + * @return Period in seconds of the most recent pulse. + */ +double Encoder::GetPeriod() +{ + 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. + * 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(). + * + * @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."; } /** * 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(). */ double Encoder::GetDistance() @@ -232,7 +257,7 @@ double Encoder::GetDistance() /** * Get the current rate of the encoder. * Units are distance per second as scaled by the value from SetDistancePerPulse(). - * + * * @return The current rate of the encoder. */ double Encoder::GetRate() @@ -242,7 +267,7 @@ double Encoder::GetRate() /** * 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(). */ void Encoder::SetMinRate(double minRate) @@ -258,7 +283,7 @@ void Encoder::SetMinRate(double minRate) * 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) @@ -279,7 +304,7 @@ void Encoder::SetReverseDirection(bool reverseDirection) /** * Set which parameter of the encoder you are using as a process control variable. - * + * * @param pidSource An enum to select the parameter. */ void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource) @@ -289,7 +314,7 @@ void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource) /** * Implement the PIDSource interface. - * + * * @return The current value of the selected source parameter. */ double Encoder::PIDGet() @@ -314,11 +339,11 @@ void Encoder::UpdateTable() { } void Encoder::StartLiveWindowMode() { - + } void Encoder::StopLiveWindowMode() { - + } std::string Encoder::GetSmartDashboardType() { @@ -336,4 +361,3 @@ void Encoder::InitTable(ITable *subTable) { ITable * Encoder::GetTable() { return m_table; } - diff --git a/wpilibc/wpilibC++Sim/src/Error.cpp b/wpilibc/wpilibC++Sim/src/Error.cpp index 19894db61b..c9494bda55 100644 --- a/wpilibc/wpilibC++Sim/src/Error.cpp +++ b/wpilibc/wpilibC++Sim/src/Error.cpp @@ -12,7 +12,6 @@ #include "Timer.h" #include "Utility.h" - bool Error::m_stackTraceEnabled = false; bool Error::m_suspendOnErrorEnabled = false; diff --git a/wpilibc/wpilibC++Sim/src/ErrorBase.cpp b/wpilibc/wpilibC++Sim/src/ErrorBase.cpp index c5142180f9..c973798c10 100644 --- a/wpilibc/wpilibC++Sim/src/ErrorBase.cpp +++ b/wpilibc/wpilibC++Sim/src/ErrorBase.cpp @@ -96,7 +96,7 @@ void ErrorBase::SetImaqError(int success, const char *contextMessage, const char { // If there was an error if (success <= 0) { - char err[256]; + //TODO: ??? char err[256]; // XXX: sprintf(err, "%s: %s", contextMessage, imaqGetErrorText(imaqGetLastError())); // Set the current error information for this object. diff --git a/wpilibc/wpilibC++Sim/src/Gyro.cpp b/wpilibc/wpilibC++Sim/src/Gyro.cpp index 3c1b80a523..68d311114d 100644 --- a/wpilibc/wpilibC++Sim/src/Gyro.cpp +++ b/wpilibc/wpilibC++Sim/src/Gyro.cpp @@ -23,39 +23,26 @@ constexpr float Gyro::kDefaultVoltsPerDegreePerSecond; * in progress, this is typically done when the robot is first turned on while it's sitting at * rest before the competition starts. */ -void Gyro::InitGyro(int slot, int channel) +void Gyro::InitGyro(int channel) { m_table = NULL; SetPIDSourceParameter(kAngle); - char buffer[50]; - int n = sprintf(buffer, "analog/%d/%d", slot, channel); - impl = new SimGyro(buffer); + char buffer[50]; + int n = sprintf(buffer, "analog/1/%d", channel); + impl = new SimGyro(buffer); - LiveWindow::GetInstance()->AddSensor("Gyro", slot, channel, this); + LiveWindow::GetInstance()->AddSensor("Gyro", channel, this); } /** - * Gyro constructor given a slot and a channel. - * - * @param moduleNumber The analog module the gyro is connected to (1). - * @param channel The analog channel the gyro is connected to (1 or 2). - */ -Gyro::Gyro(uint8_t moduleNumber, uint32_t channel) -{ - InitGyro(moduleNumber, channel); -} - -/** - * Gyro constructor with only a channel. - * - * Use the default analog module slot. - * + * Gyro constructor with only a channel.. + * * @param channel The analog channel the gyro is connected to. */ Gyro::Gyro(uint32_t channel) { - InitGyro(1, channel); + InitGyro(channel); } /** @@ -73,7 +60,6 @@ void Gyro::Reset() */ Gyro::~Gyro() { - } /** @@ -95,9 +81,9 @@ float Gyro::GetAngle( void ) /** * Return the rate of rotation of the gyro - * + * * The rate is based on the most recent reading of the gyro analog value - * + * * @return the current rate in degrees per second */ double Gyro::GetRate( void ) @@ -114,7 +100,7 @@ void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource) /** * Get the angle in degrees for the PIDSource base object. - * + * * @return The angle in degrees. */ double Gyro::PIDGet() @@ -136,11 +122,11 @@ void Gyro::UpdateTable() { } void Gyro::StartLiveWindowMode() { - + } void Gyro::StopLiveWindowMode() { - + } std::string Gyro::GetSmartDashboardType() { @@ -155,4 +141,3 @@ void Gyro::InitTable(ITable *subTable) { ITable * Gyro::GetTable() { return m_table; } - diff --git a/wpilibc/wpilibC++Sim/src/IterativeRobot.cpp b/wpilibc/wpilibC++Sim/src/IterativeRobot.cpp index 9d1dcf74c2..caa1052433 100644 --- a/wpilibc/wpilibC++Sim/src/IterativeRobot.cpp +++ b/wpilibc/wpilibC++Sim/src/IterativeRobot.cpp @@ -10,7 +10,6 @@ #include "SmartDashboard/SmartDashboard.h" #include "LiveWindow/LiveWindow.h" #include "networktables/NetworkTable.h" - #include constexpr double IterativeRobot::kDefaultPeriod; @@ -118,7 +117,7 @@ void IterativeRobot::StartCompetition() } if (NextPeriodReady()) { - // TODO: HALNetworkCommunicationObserveUserProgramDisabled(); + // TODO: HALNetworkCommunicationObserveUserProgramDisabled(); DisabledPeriodic(); } } @@ -184,7 +183,7 @@ void IterativeRobot::StartCompetition() } } // wait for driver station data so the loop doesn't hog the CPU - m_ds->WaitForData(); + m_ds->WaitForData(); } } diff --git a/wpilibc/wpilibC++Sim/src/Jaguar.cpp b/wpilibc/wpilibC++Sim/src/Jaguar.cpp index 4e1129ec2e..da6945756c 100644 --- a/wpilibc/wpilibC++Sim/src/Jaguar.cpp +++ b/wpilibc/wpilibC++Sim/src/Jaguar.cpp @@ -11,7 +11,7 @@ /** * Common initialization code called by all constructors. */ -void Jaguar::InitJaguar(int slot, int channel) +void Jaguar::InitJaguar(int channel) { /* * Input profile defined by Luminary Micro. @@ -22,32 +22,19 @@ void Jaguar::InitJaguar(int slot, int channel) * Proportional forward ranges from 1.5517922ms to 2.3027789ms * Full forward ranges from 2.3027789ms to 2.328675ms */ - char buffer[50]; - int n = sprintf(buffer, "pwm/%d/%d", slot, channel); - impl = new SimContinuousOutput(buffer); + char buffer[50]; + int n = sprintf(buffer, "pwm/1/%d", channel); + impl = new SimContinuousOutput(buffer); - // TODO: LiveWindow::GetInstance()->AddActuator("Jaguar", GetModuleNumber(), GetChannel(), this); + // TODO: LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this); } /** - * Constructor that assumes the default digital module. - * - * @param channel The PWM channel on the digital module that the Jaguar is attached to. + * @param channel The PWM channel that the Jaguar is attached to. */ Jaguar::Jaguar(uint32_t channel) { - InitJaguar(1, channel); -} - -/** - * Constructor that specifies the digital module. - * - * @param moduleNumber The digital module (1 or 2). - * @param channel The PWM channel on the digital module that the Jaguar is attached to. - */ -Jaguar::Jaguar(uint8_t moduleNumber, uint32_t channel) -{ - InitJaguar(moduleNumber, channel); + InitJaguar(channel); } Jaguar::~Jaguar() @@ -55,11 +42,11 @@ Jaguar::~Jaguar() } /** - * Set the PWM value. - * + * Set the PWM value. + * * 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 syncGroup Unused interface. */ @@ -70,7 +57,7 @@ void Jaguar::Set(float speed, uint8_t syncGroup) /** * 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() @@ -88,7 +75,7 @@ void Jaguar::Disable() /** * 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) diff --git a/wpilibc/wpilibC++Sim/src/LiveWindow/LiveWindow.cpp b/wpilibc/wpilibC++Sim/src/LiveWindow/LiveWindow.cpp index 409892d463..6d28f0d79a 100644 --- a/wpilibc/wpilibC++Sim/src/LiveWindow/LiveWindow.cpp +++ b/wpilibc/wpilibC++Sim/src/LiveWindow/LiveWindow.cpp @@ -102,10 +102,10 @@ void LiveWindow::AddActuator(const char *subsystem, const char *name, /** * INTERNAL */ -void LiveWindow::AddSensor(std::string type, int module, int channel, LiveWindowSendable *component) +void LiveWindow::AddSensor(std::string type, int channel, LiveWindowSendable *component) { std::ostringstream oss; - oss << type << "[" << module << "," << channel << "]"; + oss << type << "[" << channel << "]"; std::string types(oss.str()); char* cc = new char[types.size() + 1]; types.copy(cc, types.size()); @@ -118,10 +118,10 @@ void LiveWindow::AddSensor(std::string type, int module, int channel, LiveWindow /** * INTERNAL */ -void LiveWindow::AddActuator(std::string type, int module, int channel, LiveWindowSendable *component) +void LiveWindow::AddActuator(std::string type, int channel, LiveWindowSendable *component) { std::ostringstream oss; - oss << type << "[" << module << "," << channel << "]"; + oss << type << "[" << channel << "]"; std::string types(oss.str()); char* cc = new char[types.size() + 1]; types.copy(cc, types.size()); @@ -129,6 +129,20 @@ void LiveWindow::AddActuator(std::string type, int module, int channel, LiveWind AddActuator("Ungrouped", cc, component); } +/** + * INTERNAL + */ +void LiveWindow::AddActuator(std::string type, int module, int channel, LiveWindowSendable *component) +{ + std::ostringstream oss; + oss << type << "[" << module << "," << channel << "]"; + std::string types(oss.str()); + char* cc = new char[types.size() + 1]; + types.copy(cc, types.size()); + cc[types.size()]='\0'; + AddActuator("Ungrouped", cc, component); +} + /** * Tell all the sensors to update (send) their values * Actuators are handled through callbacks on their value changing from the @@ -184,4 +198,3 @@ void LiveWindow::InitializeLiveWindowComponents() } } } - diff --git a/wpilibc/wpilibC++Sim/src/Notifier.cpp b/wpilibc/wpilibC++Sim/src/Notifier.cpp index 98b7331ae7..bf16c1002a 100644 --- a/wpilibc/wpilibC++Sim/src/Notifier.cpp +++ b/wpilibc/wpilibC++Sim/src/Notifier.cpp @@ -31,15 +31,13 @@ Notifier::Notifier(TimerEventHandler handler, void *param) m_nextEvent = NULL; m_queued = false; m_handlerSemaphore = initializeSemaphore(SEMAPHORE_FULL); - - { + { Synchronized sync(queueSemaphore); // do the first time intialization of static variables if (refcount == 0) { - task = new Task("NotifierTask", (FUNCPTR)Notifier::Run, - Task::kDefaultPriority, 64000); - task->Start(); + task = new Task("NotifierTask", (FUNCPTR)Notifier::Run, Task::kDefaultPriority, 64000); + task->Start(); } refcount++; } @@ -56,11 +54,11 @@ Notifier::~Notifier() Synchronized sync(queueSemaphore); DeleteFromQueue(); - // Delete the static variables when the last one is going away + // Delete the static variables when the last one is going away if (!(--refcount)) { - task->Stop(); - delete task; + task->Stop(); + delete task; } } @@ -144,7 +142,7 @@ void Notifier::InsertInQueue(bool reschedule) } else { - m_expirationTime = GetClock() + m_period; + m_expirationTime = GetClock() + m_period; } if (timerQueueHead == NULL || timerQueueHead->m_expirationTime >= this->m_expirationTime) { @@ -186,7 +184,7 @@ void Notifier::DeleteFromQueue() if (m_queued) { m_queued = false; - wpi_assert(timerQueueHead != NULL); + wpi_assert(timerQueueHead != NULL); if (timerQueueHead == this) { // remove the first item in the list - update the alarm diff --git a/wpilibc/wpilibC++Sim/src/RobotBase.cpp b/wpilibc/wpilibC++Sim/src/RobotBase.cpp index 93ae2b043c..aa40f949d5 100644 --- a/wpilibc/wpilibC++Sim/src/RobotBase.cpp +++ b/wpilibc/wpilibC++Sim/src/RobotBase.cpp @@ -30,8 +30,9 @@ RobotBase &RobotBase::getInstance() * 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(); +RobotBase::RobotBase() +{ + m_ds = DriverStation::GetInstance(); } /** @@ -39,7 +40,9 @@ RobotBase::RobotBase() { * This includes deleting all classes that might have been allocated as Singletons to they * would never be deleted except here. */ -RobotBase::~RobotBase() {} +RobotBase::~RobotBase() +{ +} /** * Determine if the Robot is currently enabled. @@ -83,7 +86,7 @@ bool RobotBase::IsOperatorControl() */ bool RobotBase::IsTest() { - return m_ds->IsTest(); + return m_ds->IsTest(); } /** diff --git a/wpilibc/wpilibC++Sim/src/RobotDrive.cpp b/wpilibc/wpilibC++Sim/src/RobotDrive.cpp index 36d66f268b..2261236c98 100644 --- a/wpilibc/wpilibC++Sim/src/RobotDrive.cpp +++ b/wpilibc/wpilibC++Sim/src/RobotDrive.cpp @@ -46,8 +46,8 @@ void RobotDrive::InitRobotDrive() { * 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 Jaguars for controlling the motors. - * @param leftMotorChannel The PWM channel number on the default digital module that drives the left motor. - * @param rightMotorChannel The PWM channel number on the default digital module that drives the right motor. + * @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) { @@ -67,10 +67,10 @@ RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel) * Set up parameters for a four wheel drive system where all four motor * pwm channels are specified in the call. * This call assumes Jaguars for controlling the motors. - * @param frontLeftMotor Front left motor channel number on the default digital module - * @param rearLeftMotor Rear Left motor channel number on the default digital module - * @param frontRightMotor Front right motor channel number on the default digital module - * @param rearRightMotor Rear Right motor channel number on the default digital module + * @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 */ RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor, uint32_t frontRightMotor, uint32_t rearRightMotor) @@ -468,7 +468,7 @@ void RobotDrive::ArcadeDrive(float moveValue, float rotateValue, bool squaredInp * 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. - * + * * 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] @@ -509,7 +509,7 @@ void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup); // CANJaguar::UpdateSyncGroup(syncGroup); - + // FIXME: m_safetyHelper->Feed(); } @@ -673,7 +673,7 @@ 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) */ diff --git a/wpilibc/wpilibC++Sim/src/SensorBase.cpp b/wpilibc/wpilibC++Sim/src/SensorBase.cpp index 9b5d8788c5..80db28c5c3 100644 --- a/wpilibc/wpilibC++Sim/src/SensorBase.cpp +++ b/wpilibc/wpilibC++Sim/src/SensorBase.cpp @@ -9,13 +9,12 @@ #include "WPIErrors.h" const uint32_t SensorBase::kDigitalChannels; -const uint32_t SensorBase::kAnalogChannels; -const uint32_t SensorBase::kAnalogModules; -const uint32_t SensorBase::kDigitalModules; +const uint32_t SensorBase::kAnalogInputs; const uint32_t SensorBase::kSolenoidChannels; const uint32_t SensorBase::kSolenoidModules; const uint32_t SensorBase::kPwmChannels; const uint32_t SensorBase::kRelayChannels; +const uint32_t SensorBase::kPDPChannels; const uint32_t SensorBase::kChassisSlots; SensorBase *SensorBase::m_singletonList = NULL; @@ -64,61 +63,21 @@ void SensorBase::DeleteSingletons() m_singletonList = NULL; } -/** - * Check that the analog module number is valid. - * - * @return Analog module is valid and present - */ -bool SensorBase::CheckAnalogModule(uint8_t moduleNumber) -{ - return 0 < moduleNumber && moduleNumber <= 2; // TODO: Fix for Athena -} - -/** - * Check that the digital module number is valid. - * - * @return Digital module is valid and present - */ -bool SensorBase::CheckDigitalModule(uint8_t moduleNumber) -{ - return 0 < moduleNumber && moduleNumber <= 2; // TODO: Fix for Athena -} - -/** - * Check that the digital module number is valid. - * - * @return Digital module is valid and present - */ -bool SensorBase::CheckPWMModule(uint8_t moduleNumber) -{ - return CheckDigitalModule(moduleNumber); -} - -/** - * Check that the digital module number is valid. - * - * @return Digital module is valid and present - */ -bool SensorBase::CheckRelayModule(uint8_t moduleNumber) -{ - return CheckDigitalModule(moduleNumber); -} - /** * Check that the solenoid module number is valid. - * + * * @return Solenoid module is valid and present */ bool SensorBase::CheckSolenoidModule(uint8_t moduleNumber) { - return 0 < moduleNumber && moduleNumber <= 2; // TODO: Fix for Athena + 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. - * + * * @return Digital channel is valid */ bool SensorBase::CheckDigitalChannel(uint32_t channel) @@ -132,7 +91,7 @@ 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. - * + * * @return Relay channel is valid */ bool SensorBase::CheckRelayChannel(uint32_t channel) @@ -146,7 +105,7 @@ 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. - * + * * @return PWM channel is valid */ bool SensorBase::CheckPWMChannel(uint32_t channel) @@ -157,19 +116,33 @@ bool SensorBase::CheckPWMChannel(uint32_t channel) } /** - * Check that the analog channel number is value. - * Verify that the analog channel number is one of the legal channel numbers. Channel numbers + * 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. - * + * * @return Analog channel is valid */ -bool SensorBase::CheckAnalogChannel(uint32_t channel) +bool SensorBase::CheckAnalogInput(uint32_t channel) { - if (channel > 0 && channel <= kAnalogChannels) + 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. + * + * @return Analog channel is valid + */ +bool SensorBase::CheckAnalogOutput(uint32_t channel) +{ + if (channel > 0 && channel <= kAnalogOutputs) + return true; + return false; +} + /** * Verify that the solenoid channel number is within limits. * @@ -182,3 +155,14 @@ bool SensorBase::CheckSolenoidChannel(uint32_t channel) return false; } +/** + * Verify that the power distribution channel number is within limits. + * + * @return PDP channel is valid + */ +bool SensorBase::CheckPDPChannel(uint32_t channel) +{ + if (channel > 0 && channel <= kPDPChannels) + return true; + return false; +} diff --git a/wpilibc/wpilibC++Sim/src/SimpleRobot.cpp b/wpilibc/wpilibC++Sim/src/SimpleRobot.cpp index 7bd9dcb621..2667d7db68 100644 --- a/wpilibc/wpilibC++Sim/src/SimpleRobot.cpp +++ b/wpilibc/wpilibC++Sim/src/SimpleRobot.cpp @@ -8,8 +8,6 @@ #include #include - -// #include "DriverStation.h" #include "Timer.h" #include "SmartDashboard/SmartDashboard.h" #include "LiveWindow/LiveWindow.h" @@ -99,49 +97,49 @@ void SimpleRobot::RobotMain() */ void SimpleRobot::StartCompetition() { - LiveWindow *lw = LiveWindow::GetInstance(); + LiveWindow *lw = LiveWindow::GetInstance(); - SmartDashboard::init(); + SmartDashboard::init(); NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false); - + RobotMain(); if (!m_robotMainOverridden) { // first and one-time initialization - //lw->SetEnabled(false); + lw->SetEnabled(false); RobotInit(); while (true) { if (IsDisabled()) { - // TODO: m_ds->InDisabled(true); + m_ds->InDisabled(true); Disabled(); - // TODO: m_ds->InDisabled(false); + m_ds->InDisabled(false); while (IsDisabled()) sleep(1); //m_ds->WaitForData(); } else if (IsAutonomous()) { - // TODO: m_ds->InAutonomous(true); + m_ds->InAutonomous(true); Autonomous(); - // TODO: m_ds->InAutonomous(false); + m_ds->InAutonomous(false); while (IsAutonomous() && IsEnabled()) sleep(1); //m_ds->WaitForData(); } else if (IsTest()) { - // lw->SetEnabled(true); - // TODO: m_ds->InTest(true); + lw->SetEnabled(true); + m_ds->InTest(true); Test(); - // TODO: m_ds->InTest(false); + m_ds->InTest(false); while (IsTest() && IsEnabled()) sleep(1); //m_ds->WaitForData(); - //lw->SetEnabled(false); + lw->SetEnabled(false); } else { - // TODO: m_ds->InOperatorControl(true); + m_ds->InOperatorControl(true); OperatorControl(); - // TODO: m_ds->InOperatorControl(false); + m_ds->InOperatorControl(false); while (IsOperatorControl() && IsEnabled()) sleep(1); //m_ds->WaitForData(); } } diff --git a/wpilibc/wpilibC++Sim/src/SmartDashboard/SmartDashboard.cpp b/wpilibc/wpilibC++Sim/src/SmartDashboard/SmartDashboard.cpp index 87b8ae9146..8976278053 100644 --- a/wpilibc/wpilibC++Sim/src/SmartDashboard/SmartDashboard.cpp +++ b/wpilibc/wpilibC++Sim/src/SmartDashboard/SmartDashboard.cpp @@ -58,7 +58,7 @@ void SmartDashboard::PutData(NamedSendable *value) * @param keyName the key * @return the value */ -Sendable *SmartDashboard::GetData(std::string key) +/*Sendable *SmartDashboard::GetData(std::string key) { ITable* subtable = m_table->GetSubTable(key); Sendable *data = m_tablesToData[subtable]; @@ -68,7 +68,7 @@ Sendable *SmartDashboard::GetData(std::string key) return NULL; } return data; -} +}*/ /** * Maps the specified key to the specified complex value (such as an array) in this table. diff --git a/wpilibc/wpilibC++Sim/src/Talon.cpp b/wpilibc/wpilibC++Sim/src/Talon.cpp index c661413e43..a0e9420be8 100644 --- a/wpilibc/wpilibC++Sim/src/Talon.cpp +++ b/wpilibc/wpilibC++Sim/src/Talon.cpp @@ -15,40 +15,27 @@ * 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 Talon User Manual available from CTRE. - * + * * - 211 = full "forward" * - 133 = the "high end" of the deadband range * - 129 = center of the deadband range (off) * - 125 = the "low end" of the deadband range * - 49 = full "reverse" */ -void Talon::InitTalon(int slot, int channel) { - char buffer[50]; - int n = sprintf(buffer, "pwm/%d/%d", slot, channel); - impl = new SimContinuousOutput(buffer); +void Talon::InitTalon(int channel) { + char buffer[50]; + int n = sprintf(buffer, "pwm/1/%d", channel); + impl = new SimContinuousOutput(buffer); // TODO: LiveWindow::GetInstance()->AddActuator("Talon", slot, channel, this); } /** - * Constructor that assumes the default digital module. - * - * @param channel The PWM channel on the digital module that the Talon is attached to. + * @param channel The PWM channel that the Talon is attached to. */ Talon::Talon(uint32_t channel) { - InitTalon(1, channel); -} - -/** - * Constructor that specifies the digital module. - * - * @param moduleNumber The digital module (1 or 2). - * @param channel The PWM channel on the digital module that the Talon is attached to (1..10). - */ -Talon::Talon(uint8_t moduleNumber, uint32_t channel) -{ - InitTalon(moduleNumber, channel); + InitTalon(channel); } Talon::~Talon() @@ -56,11 +43,11 @@ Talon::~Talon() } /** - * Set the PWM value. - * + * Set the PWM value. + * * 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 syncGroup Unused interface. */ @@ -71,7 +58,7 @@ void Talon::Set(float speed, uint8_t syncGroup) /** * 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() @@ -89,11 +76,10 @@ void Talon::Disable() /** * 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) +void Talon::PIDWrite(float output) { Set(output); } - diff --git a/wpilibc/wpilibC++Sim/src/Utility.cpp b/wpilibc/wpilibC++Sim/src/Utility.cpp index 925d26d3ab..37e6ab7d59 100644 --- a/wpilibc/wpilibC++Sim/src/Utility.cpp +++ b/wpilibc/wpilibC++Sim/src/Utility.cpp @@ -162,6 +162,5 @@ uint32_t GetFPGATime() { boost::posix_time::ptime time = boost::posix_time::microsec_clock::universal_time(); return time.time_of_day().total_microseconds(); - // return 0; } diff --git a/wpilibc/wpilibC++Sim/src/Victor.cpp b/wpilibc/wpilibC++Sim/src/Victor.cpp index 7fbd623ac7..721053d241 100644 --- a/wpilibc/wpilibC++Sim/src/Victor.cpp +++ b/wpilibc/wpilibC++Sim/src/Victor.cpp @@ -16,40 +16,27 @@ * Victor 884 controllers as well 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 Victor 884 User Manual available from IFI. - * + * * - 206 = full "forward" * - 131 = the "high end" of the deadband range * - 128 = center of the deadband range (off) * - 125 = the "low end" of the deadband range * - 56 = full "reverse" */ -void Victor::InitVictor(int slot, int channel) { - char buffer[50]; - int n = sprintf(buffer, "pwm/%d/%d", slot, channel); - impl = new SimContinuousOutput(buffer); - +void Victor::InitVictor(int channel) { + char buffer[50]; + int n = sprintf(buffer, "pwm/1/%d", channel); + impl = new SimContinuousOutput(buffer); + // TODO: LiveWindow::GetInstance()->AddActuator("Victor", slot, channel, this); } /** - * Constructor that assumes the default digital module. - * - * @param channel The PWM channel on the digital module that the Victor is attached to. + * @param channel The PWM channel that the Victor is attached to. */ Victor::Victor(uint32_t channel) { - InitVictor(1, channel); -} - -/** - * Constructor that specifies the digital module. - * - * @param moduleNumber The digital module (1 or 2). - * @param channel The PWM channel on the digital module that the Victor is attached to (1..10). - */ -Victor::Victor(uint8_t moduleNumber, uint32_t channel) -{ - InitVictor(moduleNumber, channel); + InitVictor(channel); } Victor::~Victor() @@ -57,8 +44,8 @@ Victor::~Victor() } /** - * Set the PWM value. - * + * Set the PWM value. + * * The PWM value is set using a range of -1.0 to 1.0, appropriately * scaling the value for the FPGA. * @@ -72,7 +59,7 @@ void Victor::Set(float speed, uint8_t syncGroup) /** * 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() @@ -90,11 +77,10 @@ void Victor::Disable() /** * 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) +void Victor::PIDWrite(float output) { Set(output); } -