diff --git a/.clang-format b/.clang-format index afc2395..fc58cb1 100644 --- a/.clang-format +++ b/.clang-format @@ -1,26 +1,13 @@ ---- +--- BasedOnStyle: Chromium -AlignEscapedNewlinesLeft: 'true' +AlignAfterOpenBracket: Align AlignTrailingComments: 'true' -AllowAllParametersOfDeclarationOnNextLine: 'false' -AllowShortBlocksOnASingleLine: 'true' AllowShortFunctionsOnASingleLine: All -AllowShortIfStatementsOnASingleLine: 'false' -AllowShortLoopsOnASingleLine: 'false' +AllowShortIfStatementsOnASingleLine: 'true' AlwaysBreakBeforeMultilineStrings: 'false' -AlwaysBreakTemplateDeclarations: 'false' -BinPackParameters: 'true' -BreakBeforeBinaryOperators: 'false' -BreakBeforeBraces: Stroustrup -BreakBeforeTernaryOperators: 'false' -BreakConstructorInitializersBeforeComma: 'false' -ColumnLimit: '160' -ConstructorInitializerAllOnOneLineOrOnePerLine: 'false' -Cpp11BracedListStyle: 'false' -DerivePointerAlignment: 'true' +ColumnLimit: '100' Language: Cpp -SpacesInParentheses: 'true' -Standard: Auto -UseTab: Always +SortIncludes: 'true' +SpaceBeforeParens: Never ... diff --git a/src/main/cpp/CommandBase.cpp b/src/main/cpp/CommandBase.cpp index 37283ba..fedc6e7 100644 --- a/src/main/cpp/CommandBase.cpp +++ b/src/main/cpp/CommandBase.cpp @@ -5,40 +5,39 @@ // static variables in C++ have to be declared here // (not in the .hpp file) -DriveTrain *CommandBase::pDriveTrain = NULL; -Generic *CommandBase::pGeneric = NULL; -OI *CommandBase::pOI = NULL; +DriveTrain* CommandBase::pDriveTrain = NULL; +Generic* CommandBase::pGeneric = NULL; +OI* CommandBase::pOI = NULL; -CommandBase::CommandBase() : Command() -{ - LOG( "[CommandBase] Constructed" ); +CommandBase::CommandBase() : + Command() { + LOG("[CommandBase] Constructed"); - // Note: These aren't "this" because they are static, - // so there is only one instance of these variables in - // the CommandBase class. "this" pointers refer to - // regular variables that are created (and are unique) - // to the 'instance' of the object. + // Note: These aren't "this" because they are static, + // so there is only one instance of these variables in + // the CommandBase class. "this" pointers refer to + // regular variables that are created (and are unique) + // to the 'instance' of the object. - if ( CommandBase::pOI == nullptr ) { - CommandBase::pOI = new OI(); - } + if (CommandBase::pOI == nullptr) { + CommandBase::pOI = new OI(); + } - if ( CommandBase::pDriveTrain == nullptr ) { - CommandBase::pDriveTrain = new DriveTrain(); - } + if (CommandBase::pDriveTrain == nullptr) { + CommandBase::pDriveTrain = new DriveTrain(); + } - if ( CommandBase::pGeneric == nullptr ) { - CommandBase::pGeneric = new Generic(); - } + if (CommandBase::pGeneric == nullptr) { + CommandBase::pGeneric = new Generic(); + } - return; + return; } -CommandBase::~CommandBase() -{ - delete this->pDriveTrain; - delete this->pGeneric; - delete this->pOI; +CommandBase::~CommandBase() { + delete this->pDriveTrain; + delete this->pGeneric; + delete this->pOI; - return; + return; } diff --git a/src/main/cpp/Commands/Autonomous/CommandGroups/Auto.cpp b/src/main/cpp/Commands/Autonomous/CommandGroups/Auto.cpp index 2c6e8bd..ef893d5 100644 --- a/src/main/cpp/Commands/Autonomous/CommandGroups/Auto.cpp +++ b/src/main/cpp/Commands/Autonomous/CommandGroups/Auto.cpp @@ -1,4 +1,7 @@ #include #include -JustDriveForward::JustDriveForward() { LOG( "[Auto] Constructed" ); } +JustDriveForward::JustDriveForward() { + LOG("[Auto] Constructed"); + +} diff --git a/src/main/cpp/Commands/DriveWithJoystick.cpp b/src/main/cpp/Commands/DriveWithJoystick.cpp index 0bd0c54..da1d851 100644 --- a/src/main/cpp/Commands/DriveWithJoystick.cpp +++ b/src/main/cpp/Commands/DriveWithJoystick.cpp @@ -1,88 +1,91 @@ // Include Required Files -#include // STD math lib -#include // Header File -#include // LOG tool. Used for printing to the RIOlog console -#include "../RobotCFG.hpp" // Robot-wide configuration file - -DriveWithJoystick::DriveWithJoystick() -{ - LOG( "[DriveWithJoystick] Constructed" ); - - if ( CommandBase::pDriveTrain != nullptr ) { - Requires( CommandBase::pDriveTrain ); - } - else { - LOG( "[DriveWithJoystick] driveTrain is null!" ); - } - - return; +#include // Header File +#include // STD math lib +#include // LOG tool. Used for printing to the RIOlog console +#include "../RobotCFG.hpp" // Robot-wide configuration file + + + +DriveWithJoystick::DriveWithJoystick() { + LOG("[DriveWithJoystick] Constructed"); + + if (CommandBase::pDriveTrain != nullptr) { + Requires(CommandBase::pDriveTrain); + } else { + LOG("[DriveWithJoystick] driveTrain is null!"); + } + + return; } -DriveWithJoystick::~DriveWithJoystick() -{ - LOG( "[DriveWithJoystick] Destroyed" ); +DriveWithJoystick::~DriveWithJoystick() { + LOG("[DriveWithJoystick] Destroyed"); - return; + return; } -void DriveWithJoystick::Initialize() -{ - LOG( "[DriveWithJoystick] Initialized" ); - // Store pointer to SmartDashboard in table - table = NetworkTable::GetTable( "SmartDashboard" ); - this->isReverse = false; +void DriveWithJoystick::Initialize() { + LOG("[DriveWithJoystick] Initialized"); + // Store pointer to SmartDashboard in table + table = NetworkTable::GetTable("SmartDashboard"); + this->isReverse = false; - return; + return; } -void DriveWithJoystick::Execute() -{ - // Pointer to Xbox Controller - frc::XboxController *pJoyDrive = CommandBase::pOI->GetJoystickDrive(); - - // Trigger Reverse Mode if X Button Tapped - if ( pJoyDrive->GetXButtonReleased() ) { - this->isReverse = !this->isReverse; - } - - // The Y-axis goes from -1 (forward) to 1 (backwards) but we want to - // set motor from 1 (forward) to -1 (reverse) so multiply by -1 - double xSpeed = pJoyDrive->GetY( XboxController::kLeftHand ) * -1; // Speed = Joustick Y Axis - double zRotation = pJoyDrive->GetX( XboxController::kLeftHand ); // Rotation = Joystick X Axis - - // Set Slow and Reverse Modes - double dSlow = ( pJoyDrive->GetBumper( XboxController::kRightHand ) ) ? 0.5 : 1; - double dReverse = ( this->isReverse ) ? -1 : 1; - - // Joystick Deadzone - if ( fabs( xSpeed ) <= XBOX_DEADZONE_LEFT_JOY ) { - xSpeed = 0.0; - } - - if ( fabs( zRotation ) <= XBOX_DEADZONE_LEFT_JOY ) { - zRotation = 0.0; - } - - // Calculate final speed and rotation - xSpeed = ( xSpeed * dSlow * dReverse ); - zRotation = ( zRotation * dSlow ); - - // If the vision mode is enabled, get info from table and store in vars - if ( Vision && pJoyDrive->GetBButton() ) { - double Speed, Rotation; - std::tie( Speed, Rotation ) = GetMotorSpeeds( table ); // Get Movement Data - xSpeed = Speed; - zRotation = Rotation; - } - - // Call DriveTrain - CommandBase::pDriveTrain->ArcadeDrive( xSpeed, zRotation ); - - return; +void DriveWithJoystick::Execute() { + // Pointer to Xbox Controller + frc::XboxController* pJoyDrive = CommandBase::pOI->GetJoystickDrive(); + + // Trigger Reverse Mode if X Button Tapped + if (pJoyDrive->GetXButtonReleased()) { + this->isReverse = !this->isReverse; + } + + // The Y-axis goes from -1 (forward) to 1 (backwards) but we want to + // set motor from 1 (forward) to -1 (reverse) so multiply by -1 + double xSpeed = pJoyDrive->GetY(XboxController::kLeftHand) * -1; // Speed = Joustick Y Axis + double zRotation = pJoyDrive->GetX(XboxController::kLeftHand); // Rotation = Joystick X Axis + + // Set Slow and Reverse Modes + double dSlow = (pJoyDrive->GetBumper(XboxController::kRightHand)) ? 0.5 : 1; + double dReverse = (this->isReverse) ? -1 : 1; + + // Joystick Deadzone + if (fabs(xSpeed) <= XBOX_DEADZONE_LEFT_JOY) { + xSpeed = 0.0; + } + + if (fabs(zRotation) <= XBOX_DEADZONE_LEFT_JOY) { + zRotation = 0.0; + } + + // Calculate final speed and rotation + xSpeed = (xSpeed * dSlow * dReverse); + zRotation = (zRotation * dSlow); + + // If the vision mode is enabled, get info from table and store in vars + if(Vision && pJoyDrive->GetBButton()){ + double Speed, Rotation; + std::tie(Speed, Rotation) = GetMotorSpeeds(table); // Get Movement Data + xSpeed = Speed; + zRotation = Rotation; + } + + // Call DriveTrain + CommandBase::pDriveTrain->ArcadeDrive(xSpeed, zRotation); + + return; } -bool DriveWithJoystick::IsFinished() { return false; } +bool DriveWithJoystick::IsFinished() { + return false; +} -void DriveWithJoystick::End() { return; } +void DriveWithJoystick::End() { + return; +} -void DriveWithJoystick::Interrupted() { return; } +void DriveWithJoystick::Interrupted() { + return; +} diff --git a/src/main/cpp/Commands/DriveWithTriggers.cpp b/src/main/cpp/Commands/DriveWithTriggers.cpp index 0876f95..479a510 100644 --- a/src/main/cpp/Commands/DriveWithTriggers.cpp +++ b/src/main/cpp/Commands/DriveWithTriggers.cpp @@ -1,82 +1,84 @@ // Include Required Files -#include // STD math lib -#include // Header File -#include // LOG tool. Used for printing to the RIOlog console -#include "../RobotCFG.hpp" // Robot-wide configuration file - -DriveWithTriggers::DriveWithTriggers() -{ - LOG( "[DriveWithTriggers] Constructed" ); - - if ( CommandBase::pDriveTrain != nullptr ) { - Requires( CommandBase::pDriveTrain ); - } - else { - LOG( "[DriveWithTriggers] driveTrain is null!" ); - } - - return; +#include // Header File +#include // STD math lib +#include // LOG tool. Used for printing to the RIOlog console +#include "../RobotCFG.hpp" // Robot-wide configuration file + + +DriveWithTriggers::DriveWithTriggers() { + LOG("[DriveWithTriggers] Constructed"); + + if (CommandBase::pDriveTrain != nullptr) { + Requires(CommandBase::pDriveTrain); + } else { + LOG("[DriveWithTriggers] driveTrain is null!"); + } + + return; } -DriveWithTriggers::~DriveWithTriggers() -{ - LOG( "[DriveWithTriggers] Destroyed" ); +DriveWithTriggers::~DriveWithTriggers() { + LOG("[DriveWithTriggers] Destroyed"); - return; + return; } -void DriveWithTriggers::Initialize() -{ - LOG( "[DriveWithTriggers] Initialized" ); - table = NetworkTable::GetTable( "SmartDashboard" ); +void DriveWithTriggers::Initialize() { + LOG("[DriveWithTriggers] Initialized"); + table = NetworkTable::GetTable("SmartDashboard"); - return; + return; } -void DriveWithTriggers::Execute() -{ - // Pointer to Xbox Controller - frc::XboxController *pJoyDrive = CommandBase::pOI->GetJoystickDrive(); - - // Trigger Reverse Mode if X Button Tapped - if ( pJoyDrive->GetXButtonReleased() ) { - this->isReverse = !this->isReverse; - } - - // The Y-axis goes from -1 (forward) to 1 (backwards) but we want to - // set motor from 1 (forward) to -1 (reverse) so multiply by -1 - double xSpeed = pJoyDrive->GetTriggerAxis( frc::XboxController::kRightHand ) - pJoyDrive->GetTriggerAxis( frc::XboxController::kLeftHand ); - double zRotation = pJoyDrive->GetX( XboxController::kLeftHand ); - - // Test the right bumper for setting slow mode - double dSlow = ( pJoyDrive->GetBumper( XboxController::kRightHand ) ) ? 0.5 : 1; - // Set dReverse based on value of isReversed - double dReverse = ( this->isReverse ) ? -1 : 1; - - if ( fabs( zRotation ) <= XBOX_DEADZONE_LEFT_JOY ) { - zRotation = 0.0; - } - - // Calculate final speed and rotation - xSpeed = ( xSpeed * dSlow * dReverse ); - zRotation = ( zRotation * dSlow ); - - // If the vision mode is enabled, get info from table and store in vars - if ( Vision && pJoyDrive->GetBButton() ) { - double Speed, Rotation; - std::tie( Speed, Rotation ) = GetMotorSpeeds( table ); // Get Movement Data - xSpeed = Speed; - zRotation = Rotation; - } - - // Call DriveTrain - CommandBase::pDriveTrain->ArcadeDrive( xSpeed, zRotation ); - - return; +void DriveWithTriggers::Execute() { + // Pointer to Xbox Controller + frc::XboxController* pJoyDrive = CommandBase::pOI->GetJoystickDrive(); + + // Trigger Reverse Mode if X Button Tapped + if (pJoyDrive->GetXButtonReleased()) { + this->isReverse = !this->isReverse; + } + + // The Y-axis goes from -1 (forward) to 1 (backwards) but we want to + // set motor from 1 (forward) to -1 (reverse) so multiply by -1 + double xSpeed = pJoyDrive->GetTriggerAxis(frc::XboxController::kRightHand) - pJoyDrive->GetTriggerAxis(frc::XboxController::kLeftHand); + double zRotation = pJoyDrive->GetX(XboxController::kLeftHand); + + // Test the right bumper for setting slow mode + double dSlow = (pJoyDrive->GetBumper(XboxController::kRightHand)) ? 0.5 : 1; + // Set dReverse based on value of isReversed + double dReverse = (this->isReverse) ? -1 : 1; + + if (fabs(zRotation) <= XBOX_DEADZONE_LEFT_JOY) { + zRotation = 0.0; + } + + // Calculate final speed and rotation + xSpeed = (xSpeed * dSlow * dReverse); + zRotation = (zRotation * dSlow); + + // If the vision mode is enabled, get info from table and store in vars + if(Vision && pJoyDrive->GetBButton()){ + double Speed, Rotation; + std::tie(Speed, Rotation) = GetMotorSpeeds(table); // Get Movement Data + xSpeed = Speed; + zRotation = Rotation; + } + + // Call DriveTrain + CommandBase::pDriveTrain->ArcadeDrive(xSpeed, zRotation); + + return; } -bool DriveWithTriggers::IsFinished() { return false; } +bool DriveWithTriggers::IsFinished() { + return false; +} -void DriveWithTriggers::End() { return; } +void DriveWithTriggers::End() { + return; +} -void DriveWithTriggers::Interrupted() { return; } +void DriveWithTriggers::Interrupted() { + return; +} diff --git a/src/main/cpp/Commands/GenericControl.cpp b/src/main/cpp/Commands/GenericControl.cpp index 8c2c734..cac63f2 100644 --- a/src/main/cpp/Commands/GenericControl.cpp +++ b/src/main/cpp/Commands/GenericControl.cpp @@ -1,60 +1,65 @@ -#include #include +#include #include #include "../RobotCFG.hpp" -GenericControl::GenericControl() -{ - LOG( "[DriveWithJoystick] Constructed" ); +GenericControl::GenericControl() { + LOG("[DriveWithJoystick] Constructed"); - if ( CommandBase::pGeneric != nullptr ) { - Requires( CommandBase::pGeneric ); - } - else { - LOG( "[GenericControl] Generic is null!" ); - } + if (CommandBase::pGeneric != nullptr) { + Requires(CommandBase::pGeneric); + } else { + LOG("[GenericControl] Generic is null!"); + } - return; + return; } // Called just before this Command runs the first time -void GenericControl::Initialize() {} +void GenericControl::Initialize() { + +} // Called repeatedly when this Command is scheduled to run -void GenericControl::Execute() -{ - frc::XboxController *pJoyOp = CommandBase::pOI->GetJoystickOperator(); - - // Left Hand (Proto Motor 1) - double xSpeed1 = pJoyOp->GetY( XboxController::kLeftHand ) * -1; // Joystick - if ( pJoyOp->GetTriggerAxis( frc::XboxController::kLeftHand ) > 0.9 ) { - xSpeed1 = 1.0; - } - if ( pJoyOp->GetBumper( XboxController::kLeftHand ) ) { - xSpeed1 = -1.0; - } - - // Right Hand (Proto Motor 2) - double xSpeed2 = pJoyOp->GetY( XboxController::kRightHand ) * -1; // Joystick - if ( pJoyOp->GetTriggerAxis( frc::XboxController::kRightHand ) > 0.9 ) { - xSpeed2 = 1.0; - } - if ( pJoyOp->GetBumper( XboxController::kRightHand ) ) { - xSpeed2 = -1.0; - } - - double dSlow = ( pJoyOp->GetAButton() ) ? 0.5 : 1; - - CommandBase::pGeneric->setSpeed( 1, xSpeed1 * dSlow ); - CommandBase::pGeneric->setSpeed( 2, xSpeed2 * dSlow ); +void GenericControl::Execute() { + frc::XboxController* pJoyOp = CommandBase::pOI->GetJoystickOperator(); + + // Left Hand (Proto Motor 1) + double xSpeed1 = pJoyOp->GetY(XboxController::kLeftHand) * -1; // Joystick + if (pJoyOp->GetTriggerAxis(frc::XboxController::kLeftHand) > 0.9) { + xSpeed1 = 1.0; + } + if (pJoyOp->GetBumper(XboxController::kLeftHand)) { + xSpeed1 = -1.0; + } + + // Right Hand (Proto Motor 2) + double xSpeed2 = pJoyOp->GetY(XboxController::kRightHand) * -1; // Joystick + if (pJoyOp->GetTriggerAxis(frc::XboxController::kRightHand) > 0.9) { + xSpeed2 = 1.0; + } + if (pJoyOp->GetBumper(XboxController::kRightHand)) { + xSpeed2 = -1.0; + } + + double dSlow = (pJoyOp->GetAButton()) ? 0.5 : 1; + + CommandBase::pGeneric->setSpeed(1, xSpeed1 * dSlow); + CommandBase::pGeneric->setSpeed(2, xSpeed2 * dSlow); } // Make this return true when this Command no longer needs to run execute() -bool GenericControl::IsFinished() { return false; } +bool GenericControl::IsFinished() { + return false; +} // Called once after isFinished returns true -void GenericControl::End() {} +void GenericControl::End() { + +} // Called when another command which requires one or more of the same // subsystems is scheduled to run -void GenericControl::Interrupted() {} +void GenericControl::Interrupted() { + +} diff --git a/src/main/cpp/OI.cpp b/src/main/cpp/OI.cpp index 509870d..95b7b91 100644 --- a/src/main/cpp/OI.cpp +++ b/src/main/cpp/OI.cpp @@ -2,27 +2,28 @@ #include #include "RobotCFG.hpp" -OI::OI() -{ - LOG( "[OI] Constructed" ); +OI::OI() { + LOG("[OI] Constructed"); - // NOTE: For some reason, Eclipse complains about not finding - // the XboxController class, even though it compiles 100% OK. - // This appears to be because we are using the depricated wpi class and not the frc one. - this->pJoystickDrive = new XboxController( XBOX_CONTROLLER_DRIVE_PORT ); - this->pJoystickOperator = new XboxController( XBOX_CONTROLLER_OPERATOR_PORT ); + // NOTE: For some reason, Eclipse complains about not finding + // the XboxController class, even though it compiles 100% OK. + this->pJoystickDrive = new XboxController(XBOX_CONTROLLER_DRIVE_PORT); + this->pJoystickOperator = new XboxController(XBOX_CONTROLLER_OPERATOR_PORT); - return; + return; } -OI::~OI() -{ - delete this->pJoystickDrive; - delete this->pJoystickOperator; +OI::~OI() { + delete this->pJoystickDrive; + delete this->pJoystickOperator; - return; + return; } -frc::XboxController *OI::GetJoystickDrive() { return this->pJoystickDrive; } +frc::XboxController* OI::GetJoystickDrive() { + return this->pJoystickDrive; +} -frc::XboxController *OI::GetJoystickOperator() { return this->pJoystickOperator; } +frc::XboxController* OI::GetJoystickOperator() { + return this->pJoystickOperator; +} diff --git a/src/main/cpp/Subsystems/CVServer.cpp b/src/main/cpp/Subsystems/CVServer.cpp index f69d77a..1bc2130 100644 --- a/src/main/cpp/Subsystems/CVServer.cpp +++ b/src/main/cpp/Subsystems/CVServer.cpp @@ -1,10 +1,9 @@ -#include #include +#include -std::tuple GetMotorSpeeds( std::shared_ptr &ntable ) -{ - double Rotation = ( ntable->GetNumber( "cvserv_rotation", 0.00 ) / 100 ); - double Speed = ( ntable->GetNumber( "cvserv_speed", 0.00 ) / 100 ); - - return std::make_tuple( Speed, Rotation ); +std::tuple GetMotorSpeeds(std::shared_ptr &ntable){ + double Rotation = (ntable->GetNumber("cvserv_rotation", 0.00) / 100); + double Speed = (ntable->GetNumber("cvserv_speed", 0.00) / 100); + + return std::make_tuple(Speed, Rotation); } \ No newline at end of file diff --git a/src/main/cpp/Subsystems/DriveTrain.cpp b/src/main/cpp/Subsystems/DriveTrain.cpp index 48a2699..f319ed3 100644 --- a/src/main/cpp/Subsystems/DriveTrain.cpp +++ b/src/main/cpp/Subsystems/DriveTrain.cpp @@ -1,9 +1,10 @@ #include #include #include -#include #include "DriveTrainMap.hpp" +#include +<<<<<<< HEAD DriveTrain::DriveTrain() : frc::Subsystem( "DriveTrain" ) { LOG( "[DriveTrain] Constructed" ); @@ -55,64 +56,411 @@ DriveTrain::~DriveTrain() delete this->pGyro; return; +======= +DriveTrain::DriveTrain() : + frc::Subsystem("DriveTrain") { + LOG("[DriveTrain] Constructed"); + + // Initialize the motors + this->pLeftFrontMotor = new can::WPI_TalonSRX( + DRIVETRAIN_LEFT_FRONT_MOTOR_ID); + this->pLeftRearMotor = new can::WPI_TalonSRX(DRIVETRAIN_LEFT_REAR_MOTOR_ID); + this->pLeftRearMotor->Follow(*pLeftFrontMotor); + + this->pLeftFrontMotor->SetInverted(false); // change this based on test or production robot + this->pLeftRearMotor->SetInverted(false); // change this based on test or production robot + this->pLeftFrontMotor->SetNeutralMode(NeutralMode::Brake); + this->pLeftRearMotor->SetNeutralMode(NeutralMode::Brake); + + this->pRightFrontMotor = new can::WPI_TalonSRX( + DRIVETRAIN_RIGHT_FRONT_MOTOR_ID); + this->pRightRearMotor = new can::WPI_TalonSRX( + DRIVETRAIN_RIGHT_REAR_MOTOR_ID); + this->pRightRearMotor->Follow(*pRightFrontMotor); + + this->pRightFrontMotor->SetInverted(true); // change this based on test or production robot + this->pRightRearMotor->SetInverted(true); // change this based on test or production robot + this->pRightFrontMotor->SetNeutralMode(NeutralMode::Brake); + this->pRightRearMotor->SetNeutralMode(NeutralMode::Brake); + + this->pRobotDrive = new frc::DifferentialDrive(*pLeftFrontMotor, + *pRightFrontMotor); + + this->pLeftFrontMotor->SetSafetyEnabled(false); + this->pLeftRearMotor->SetSafetyEnabled(false); + this->pRightFrontMotor->SetSafetyEnabled(false); + this->pRightRearMotor->SetSafetyEnabled(false); + this->pRobotDrive->SetSafetyEnabled(false); + + // Initialize the gyro + // (See comment here about which port. We are using MXP, the one physically on top of the RoboRio + // https://www.pdocs.kauailabs.com/navx-mxp/software/roborio-libraries/c/) + this->pGyro = new AHRS(SPI::Port::kMXP); + this->pGyro->Reset(); + + // Initialize the turn controller + this->pTurnController = new PIDController(GYRO_PID_P, GYRO_PID_I, + GYRO_PID_D, GYRO_PID_F, pGyro, this); + this->pTurnController->SetInputRange(-180.0f, 180.0f); + this->pTurnController->SetOutputRange(-1.0, 1.0); + this->pTurnController->SetAbsoluteTolerance(GYRO_TOLERANCE_DEGREES); + this->pTurnController->SetContinuous(true); + + this->dRotateToAngleRate = 0.0f; + + return; +>>>>>>> parent of 21efe0c... formatted all code } -void DriveTrain::InitAutonomousMode() -{ - LOG( "[DriveTrain] Autonomous Mode Initialized" ); +DriveTrain::~DriveTrain() { + delete this->pRobotDrive; + delete this->pRightFrontMotor; + delete this->pRightRearMotor; + delete this->pLeftFrontMotor; + delete this->pLeftRearMotor; +<<<<<<< HEAD return; +======= + delete this->pGyro; + delete this->pTurnController; + + return; +>>>>>>> parent of 21efe0c... formatted all code } -void DriveTrain::InitDefaultCommand() -{ - LOG( "[DriveTrain] Initialized Default Command" ); +void DriveTrain::InitAutonomousMode() { + LOG("[DriveTrain] Autonomous Mode Initialized"); - SetDefaultCommand( new DriveWithTriggers() ); + /* choose the sensor and sensor direction */ + this->pLeftFrontMotor->ConfigSelectedFeedbackSensor( + FeedbackDevice::CTRE_MagEncoder_Relative, PID_LOOP_INDEX, + TIMEOUT_MS); + this->pLeftFrontMotor->SetSensorPhase(true); - return; + /* set the peak and nominal outputs, 12V means full */ + this->pLeftFrontMotor->ConfigNominalOutputForward(0, TIMEOUT_MS); + this->pLeftFrontMotor->ConfigNominalOutputReverse(0, TIMEOUT_MS); + this->pLeftFrontMotor->ConfigPeakOutputForward(1, TIMEOUT_MS); + this->pLeftFrontMotor->ConfigPeakOutputReverse(-1, TIMEOUT_MS); + + this->pLeftFrontMotor->ConfigAllowableClosedloopError(SLOT_INDEX, 0, + TIMEOUT_MS); + + /* set closed loop gains in slot0 */ + this->pLeftFrontMotor->Config_kF(PID_LOOP_INDEX, 0.00, TIMEOUT_MS); + this->pLeftFrontMotor->Config_kP(PID_LOOP_INDEX, 0.29, TIMEOUT_MS); + this->pLeftFrontMotor->Config_kI(PID_LOOP_INDEX, 0.00, TIMEOUT_MS); + this->pLeftFrontMotor->Config_kD(PID_LOOP_INDEX, 0.00, TIMEOUT_MS); + +// int abLeftPosition = this->pLeftFrontMotor->GetSelectedSensorPosition(SLOT_INDEX) & 0xFFF; + int abLeftPosition = + this->pLeftFrontMotor->GetSensorCollection().GetPulseWidthPosition(); + this->pLeftFrontMotor->SetSelectedSensorPosition(abLeftPosition, + PID_LOOP_INDEX, TIMEOUT_MS); + + /* choose the sensor and sensor direction */ + this->pRightFrontMotor->ConfigSelectedFeedbackSensor( + FeedbackDevice::CTRE_MagEncoder_Relative, PID_LOOP_INDEX, + TIMEOUT_MS); + this->pRightFrontMotor->SetSensorPhase(true); + + /* set the peak and nominal outputs, 12V means full */ + this->pRightFrontMotor->ConfigNominalOutputForward(0, TIMEOUT_MS); + this->pRightFrontMotor->ConfigNominalOutputReverse(0, TIMEOUT_MS); + this->pRightFrontMotor->ConfigPeakOutputForward(1, TIMEOUT_MS); + this->pRightFrontMotor->ConfigPeakOutputReverse(-1, TIMEOUT_MS); + + this->pRightFrontMotor->ConfigAllowableClosedloopError(SLOT_INDEX, 0, + TIMEOUT_MS); + + /* set closed loop gains in slot0 */ + this->pRightFrontMotor->Config_kF(PID_LOOP_INDEX, 0.00, TIMEOUT_MS); + this->pRightFrontMotor->Config_kP(PID_LOOP_INDEX, 0.29, TIMEOUT_MS); + this->pRightFrontMotor->Config_kI(PID_LOOP_INDEX, 0.00, TIMEOUT_MS); + this->pRightFrontMotor->Config_kD(PID_LOOP_INDEX, 0.00, TIMEOUT_MS); + +// int abRightPosition = this->pRightFrontMotor->GetSelectedSensorPosition(SLOT_INDEX) & 0xFFF; + int abRightPosition = + this->pRightFrontMotor->GetSensorCollection().GetPulseWidthPosition(); + this->pRightFrontMotor->SetSelectedSensorPosition(abRightPosition, + PID_LOOP_INDEX, TIMEOUT_MS); + + return; } +<<<<<<< HEAD void DriveTrain::DriveSetup() { DriveTrain::ResetEncoders(); +======= +void DriveTrain::InitDefaultCommand() { + LOG("[DriveTrain] Initialized Default Command"); - this->pRightFrontMotor->Follow( *pLeftFrontMotor ); + SetDefaultCommand(new DriveWithTriggers()); - return; + return; +} + +void DriveTrain::InitMotionProfilingMode() { + LOG("[DriveTrain] Motion Profiling Mode Initialized"); + + this->pLeftFrontMotor->ConfigSelectedFeedbackSensor( + FeedbackDevice::CTRE_MagEncoder_Relative, PID_LOOP_INDEX, + TIMEOUT_MS); + this->pLeftFrontMotor->SetSensorPhase(true); + this->pLeftFrontMotor->ConfigNeutralDeadband( + NEUTRAL_DEADBAND_PERCENT * 0.01, TIMEOUT_MS); + + this->pLeftFrontMotor->Config_kF(SLOT_INDEX, 0.0, TIMEOUT_MS); + this->pLeftFrontMotor->Config_kP(SLOT_INDEX, 0.0, TIMEOUT_MS); + this->pLeftFrontMotor->Config_kI(SLOT_INDEX, 0.0, TIMEOUT_MS); + this->pLeftFrontMotor->Config_kD(SLOT_INDEX, 0.0, TIMEOUT_MS); + + this->pLeftFrontMotor->ConfigMotionProfileTrajectoryPeriod(10, TIMEOUT_MS); //Our profile uses 10 ms timing + /* status 10 provides the trajectory target for motion profile AND motion magic */ + this->pLeftFrontMotor->SetStatusFramePeriod( + StatusFrameEnhanced::Status_10_MotionMagic, 10, TIMEOUT_MS); + this->pLeftFrontMotor->ConfigMotionCruiseVelocity(6800, TIMEOUT_MS); + this->pLeftFrontMotor->ConfigMotionAcceleration(6800, TIMEOUT_MS); + + this->pRightFrontMotor->ConfigSelectedFeedbackSensor( + FeedbackDevice::CTRE_MagEncoder_Relative, PID_LOOP_INDEX, + TIMEOUT_MS); + this->pRightFrontMotor->SetSensorPhase(true); + this->pRightFrontMotor->ConfigNeutralDeadband( + NEUTRAL_DEADBAND_PERCENT * 0.01, TIMEOUT_MS); + + this->pRightFrontMotor->Config_kF(SLOT_INDEX, 0.0, TIMEOUT_MS); + this->pRightFrontMotor->Config_kP(SLOT_INDEX, 0.0, TIMEOUT_MS); + this->pRightFrontMotor->Config_kI(SLOT_INDEX, 0.0, TIMEOUT_MS); + this->pRightFrontMotor->Config_kD(SLOT_INDEX, 0.0, TIMEOUT_MS); + + this->pRightFrontMotor->ConfigMotionProfileTrajectoryPeriod(10, TIMEOUT_MS); //Our profile uses 10 ms timing + /* status 10 provides the trajectory target for motion profile AND motion magic */ + this->pRightFrontMotor->SetStatusFramePeriod( + StatusFrameEnhanced::Status_10_MotionMagic, 10, TIMEOUT_MS); + this->pRightFrontMotor->ConfigMotionCruiseVelocity(6800, TIMEOUT_MS); + this->pRightFrontMotor->ConfigMotionAcceleration(6800, TIMEOUT_MS); + + return; } +void DriveTrain::DriveSetup() { + DriveTrain::ResetEncoders(); +>>>>>>> parent of 21efe0c... formatted all code + + this->pRightFrontMotor->Follow(*pLeftFrontMotor); + + return; +} + +<<<<<<< HEAD void DriveTrain::ArcadeDrive( double xSpeed, double zRotation ) { this->pRobotDrive->ArcadeDrive( zRotation, xSpeed ); // API parameter order is incorrect +======= +/** + * Used by Autonomous Commands + */ +void DriveTrain::Drive(double distance, double speed) { + double targetPositionRotations = (distance / INCHES_PER_REVOLUTION) + * TICKS_PER_REVOLUTION; - return; + SetTargetPosition(targetPositionRotations * speed); + + this->pLeftFrontMotor->Set(ControlMode::Position, GetTargetPosition()); +// this->pRightFrontMotor->Set(ControlMode::Position, GetTargetPosition()); + + return; } -void DriveTrain::CurvatureDrive( double xSpeed, double zRotation, bool isQuickTurn ) -{ - this->pRobotDrive->CurvatureDrive( xSpeed, zRotation, isQuickTurn ); +void DriveTrain::TurnSetup() { + return; +} - return; +/** + * Used by Autonomous Commands + */ +void DriveTrain::Turn() { + this->pTurnController->Enable(); + + double dTurnRate = GetRotateToAngleRate(); + + DriveTrain::ArcadeDrive(0.0, dTurnRate); + + return; } -void DriveTrain::TankDrive( double leftSpeed, double rightSpeed ) -{ - this->pRobotDrive->TankDrive( leftSpeed, rightSpeed ); +void DriveTrain::ArcadeDrive(double xSpeed, double zRotation) { + this->pRobotDrive->ArcadeDrive(zRotation, xSpeed); // API parameter order is incorrect +>>>>>>> parent of 21efe0c... formatted all code - return; + return; +} + +void DriveTrain::CurvatureDrive(double xSpeed, double zRotation, + bool isQuickTurn) { + this->pRobotDrive->CurvatureDrive(xSpeed, zRotation, isQuickTurn); + + return; } -double DriveTrain::GetAngle() { return pGyro->GetAngle(); } +void DriveTrain::TankDrive(double leftSpeed, double rightSpeed) { + this->pRobotDrive->TankDrive(leftSpeed, rightSpeed); + return; +} + +double DriveTrain::GetAngle() { + return pGyro->GetAngle(); +} + +<<<<<<< HEAD bool DriveTrain::IsDriving() { return this->pGyro->IsMoving(); } +======= +can::WPI_TalonSRX* DriveTrain::GetLeftFrontMotor() { + return this->pLeftFrontMotor; +} + +int DriveTrain::GetLeftClosedLoopError() { + return this->pLeftFrontMotor->GetClosedLoopError(SLOT_INDEX); +} + +double DriveTrain::GetLeftDistance() { + return GetLeftPosition() / TICKS_PER_REVOLUTION * INCHES_PER_REVOLUTION; +} + +double DriveTrain::GetLeftPosition() { + return this->pLeftFrontMotor->GetSelectedSensorPosition(SLOT_INDEX); +} + +int DriveTrain::GetLeftVelocity() { + return this->pLeftFrontMotor->GetSelectedSensorVelocity(SLOT_INDEX); +} + +can::WPI_TalonSRX* DriveTrain::GetRightFrontMotor() { + return this->pRightFrontMotor; +} + +int DriveTrain::GetRightClosedLoopError() { + return this->pRightFrontMotor->GetClosedLoopError(SLOT_INDEX); +} -bool DriveTrain::IsTurning() { return this->pGyro->IsRotating(); } +double DriveTrain::GetRightDistance() { + return GetRightPosition() / TICKS_PER_REVOLUTION * INCHES_PER_REVOLUTION; +} + +double DriveTrain::GetRightPosition() { + return this->pRightFrontMotor->GetSelectedSensorPosition(SLOT_INDEX); +} + +int DriveTrain::GetRightVelocity() { + return this->pRightFrontMotor->GetSelectedSensorVelocity(SLOT_INDEX); +} + +double DriveTrain::GetRotateToAngleRate() { + return this->dRotateToAngleRate; +} +double DriveTrain::GetTargetPosition() { + return this->dTargetPostionRotations; +} + +bool DriveTrain::IsDriving() { + return this->pGyro->IsMoving(); +} +>>>>>>> parent of 21efe0c... formatted all code + +bool DriveTrain::IsTurning() { + return this->pGyro->IsRotating(); +} + +<<<<<<< HEAD void DriveTrain::ResetGyro() { pGyro->ZeroYaw(); +======= +void DriveTrain::ResetDrive() { + // reset the motors + this->pLeftFrontMotor->Set(ControlMode::PercentOutput, 0); + this->pLeftRearMotor->Set(ControlMode::PercentOutput, 0); + this->pRightFrontMotor->Set(ControlMode::PercentOutput, 0); + this->pRightRearMotor->Set(ControlMode::PercentOutput, 0); - return; + this->pLeftRearMotor->Follow(*pLeftFrontMotor); + this->pLeftFrontMotor->SetInverted(false); + this->pLeftRearMotor->SetInverted(false); + + this->pRightRearMotor->Follow(*pRightFrontMotor); + this->pRightFrontMotor->SetInverted(true); + this->pRightRearMotor->SetInverted(true); + + // Disable the turn controller + this->pTurnController->Disable(); + + return; +} + +void DriveTrain::ResetEncoders() { + LOG("[DriveTrain] Resetting encoders"); + + this->pLeftFrontMotor->SetSelectedSensorPosition(0, PID_LOOP_INDEX, + TIMEOUT_MS); + this->pRightFrontMotor->SetSelectedSensorPosition(0, PID_LOOP_INDEX, + TIMEOUT_MS); + + return; +} + +void DriveTrain::ResetGyro() { + pGyro->ZeroYaw(); +>>>>>>> parent of 21efe0c... formatted all code + + return; +} +<<<<<<< HEAD +======= + +void DriveTrain::SetEncoders() { + return; +} + +void DriveTrain::SetRotateToAngleRate(double dRate) { + this->dRotateToAngleRate = dRate; + + return; +} + +/** + * Used by Autonomous Commands + */ +void DriveTrain::SetSetpoint(double dSetpoint) { + this->pTurnController->SetSetpoint(dSetpoint); + + return; +} + +void DriveTrain::SetTargetPosition(double dTargetPosition) { + this->dTargetPostionRotations = dTargetPosition; + + return; +} + +void DriveTrain::Trace() { + return; +} + +/* This function is invoked periodically by the PID Controller, */ +/* based upon navX MXP yaw angle input and PID Coefficients. */ +void DriveTrain::PIDWrite(double output) { + SetRotateToAngleRate(output); + + if (this->pTurnController->IsEnabled()) { + DriveTrain::Turn(); + } + + return; } +>>>>>>> parent of 21efe0c... formatted all code diff --git a/src/main/cpp/Subsystems/Elevator.cpp b/src/main/cpp/Subsystems/Elevator.cpp deleted file mode 100644 index 37bf652..0000000 --- a/src/main/cpp/Subsystems/Elevator.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include //!< Elevator subsystem header -#include //!< Printing to the Riolog -#include "../RobotCFG.hpp" //!< Robot config - -//!< Constructor -Elevator::Elevator() : frc::Subsystem( "Elevator" ) -{ - LOG( "[Elevator] Constructed" ); //!< Print to screen - - //!< Construct a new tallon - this->pLiftMotor = new can::WPI_TalonSRX( 5 ); - //!< Enable brake mode - this->pLiftMotor->SetNeutralMode( ctre::phoenix::motorcontrol::NeutralMode::ProtoBrakeMode ); - - //!< Disable saftey mode - this->pLiftMotor->SetSafetyEnabled( false ); - return; -} - -Elevator::~Elevator() -{ - LOG( "[Elevator] Destructed" ) - delete this->pLiftMotor; - return; -} - -//!< Sets the speed of a motor -void setSpeed( double speed ) { this->pLiftMotor->set( speed ); } \ No newline at end of file diff --git a/src/main/cpp/Subsystems/Generic.cpp b/src/main/cpp/Subsystems/Generic.cpp index 3504184..75f249f 100644 --- a/src/main/cpp/Subsystems/Generic.cpp +++ b/src/main/cpp/Subsystems/Generic.cpp @@ -1,45 +1,46 @@ #include #include +#include "./GenericMap.hpp" #include #include "../RobotCFG.hpp" -#include "./GenericMap.hpp" -Generic::Generic() : frc::Subsystem( "Generic" ) -{ - LOG( "[Generic] Constructed" ); +Generic::Generic() : + frc::Subsystem("Generic") { + LOG("[Generic] Constructed"); - // Initialize the motors - this->pMotor1 = new can::WPI_TalonSRX( PROTO_1_MOTOR_ID ); - this->pMotor2 = new can::WPI_TalonSRX( PROTO_2_MOTOR_ID ); + // Initialize the motors + this->pMotor1 = new can::WPI_TalonSRX(PROTO_1_MOTOR_ID); + this->pMotor2 = new can::WPI_TalonSRX(PROTO_2_MOTOR_ID); - this->pMotor1->SetNeutralMode( ctre::phoenix::motorcontrol::NeutralMode::ProtoBrakeMode ); - this->pMotor2->SetNeutralMode( ctre::phoenix::motorcontrol::NeutralMode::ProtoBrakeMode ); - // - // this->pRightFrontMotor = new can::WPI_TalonSRX( - // Generic_RIGHT_FRONT_MOTOR_ID); - // this->pRightRearMotor = new can::WPI_TalonSRX( - // Generic_RIGHT_REAR_MOTOR_ID); - // this->pRightRearMotor->Follow(*pRightFrontMotor); + this->pMotor1->SetNeutralMode( + ctre::phoenix::motorcontrol::NeutralMode::ProtoBrakeMode); + this->pMotor2->SetNeutralMode( + ctre::phoenix::motorcontrol::NeutralMode::ProtoBrakeMode); +// +// this->pRightFrontMotor = new can::WPI_TalonSRX( +// Generic_RIGHT_FRONT_MOTOR_ID); +// this->pRightRearMotor = new can::WPI_TalonSRX( +// Generic_RIGHT_REAR_MOTOR_ID); +// this->pRightRearMotor->Follow(*pRightFrontMotor); - // this->pRobotDrive = new frc::DifferentialDrive(*pLeftFrontMotor, - // *pRightFrontMotor); +// this->pRobotDrive = new frc::DifferentialDrive(*pLeftFrontMotor, +// *pRightFrontMotor); - this->pMotor1->SetSafetyEnabled( false ); - this->pMotor2->SetSafetyEnabled( false ); + this->pMotor1->SetSafetyEnabled(false); + this->pMotor2->SetSafetyEnabled(false); - return; + return; } -Generic::~Generic() -{ - // delete this->pRobotDrive; - delete this->pMotor1; - delete this->pMotor2; +Generic::~Generic() { +// delete this->pRobotDrive; + delete this->pMotor1; + delete this->pMotor2; - return; + return; } -// void Generic::InitDefaultCommand() { +//void Generic::InitDefaultCommand() { // LOG("[Generic] Initialized Default Command"); // // SetDefaultCommand(new GenericControl()); @@ -47,15 +48,12 @@ Generic::~Generic() // return; //} -void Generic::setSpeed( int ID, double speed ) -{ - if ( ID == 1 ) { - this->pMotor1->Set( speed ); - } - else if ( ID == 2 ) { - this->pMotor2->Set( speed ); - } - else { - LOG( "[Generic] ERROR!! INVALID MOTOR ID IN setSpeed" ); - } +void Generic::setSpeed(int ID, double speed) { + if (ID == 1) { + this->pMotor1->Set(speed); + } else if (ID == 2) { + this->pMotor2->Set(speed); + } else { + LOG("[Generic] ERROR!! INVALID MOTOR ID IN setSpeed"); + } } diff --git a/src/main/cpp/robot.cpp b/src/main/cpp/robot.cpp index 10b6e30..0a1434f 100644 --- a/src/main/cpp/robot.cpp +++ b/src/main/cpp/robot.cpp @@ -1,107 +1,105 @@ // Include required files -#include // Robot header file (containes includes for all other files) -#include // LOG tool. Used for printing to the RIOlog console -#include "RobotCFG.hpp" // Robot-wide configuration file +#include // Robot header file (containes includes for all other files) +#include "RobotCFG.hpp" // Robot-wide configuration file +#include // LOG tool. Used for printing to the RIOlog console // Robot Destructor -Robot::~Robot() -{ - // Delete command pointers - delete this->pDriveWithJoystick; - delete this->pDriveWithTriggers; +Robot::~Robot() { + // Delete command pointers + delete this->pDriveWithJoystick; + delete this->pDriveWithTriggers; - if ( this->pAutonomousCommand != nullptr ) - delete this->pAutonomousCommand; + if (this->pAutonomousCommand != nullptr) + delete this->pAutonomousCommand; - return; + return; } // Robot Initialization -void Robot::RobotInit() -{ - LOG( "[Robot] Initialized" ); - - // Initialize the drive commands - this->pDriveWithJoystick = new DriveWithJoystick(); - this->pDriveWithTriggers = new DriveWithTriggers(); - - if ( Camera_Server ) { // If the Camera_server setting is set in RobotCFG. - // Initialize the camera server - LOG( "CSCORE INIT" ); - CameraServer::GetInstance()->StartAutomaticCapture(); - } - else { - LOG( "CSCORE not running due to config setting" ); - } - - return; +void Robot::RobotInit() { + LOG("[Robot] Initialized"); + + // Initialize the drive commands + this->pDriveWithJoystick = new DriveWithJoystick(); + this->pDriveWithTriggers = new DriveWithTriggers(); + + + if(Camera_Server){ // If the Camera_server setting is set in RobotCFG. + // Initialize the camera server + LOG("CSCORE INIT"); + CameraServer::GetInstance()->StartAutomaticCapture(); + }else{ + LOG("CSCORE not running due to config setting"); + } + + return; } -void Robot::DisabledInit() { return; } +void Robot::DisabledInit() { + return; +} -void Robot::DisabledPeriodic() -{ - frc::Scheduler::GetInstance()->Run(); +void Robot::DisabledPeriodic() { + frc::Scheduler::GetInstance()->Run(); - return; + return; } -void Robot::AutonomousInit() -{ - LOG( "[Robot] Autonomous Initialized" ); +void Robot::AutonomousInit() { + LOG("[Robot] Autonomous Initialized"); - // Remove this when auto is added - LOG( "[Robot] Auto aborted due to no code" ) + // Remove this when auto is added + LOG("[Robot] Auto aborted due to no code") - return; + return; } -void Robot::AutonomousPeriodic() -{ - frc::Scheduler::GetInstance()->Run(); +void Robot::AutonomousPeriodic() { + frc::Scheduler::GetInstance()->Run(); - return; + return; } // Robot Teleop Initialization -void Robot::TeleopInit() -{ - LOG( "[Robot] Teleop Initialized" ); - - // If Joydrive setting is enabled, use joystick drive, else use trigger drive - if ( JoyDrive == true ) { - if ( this->pDriveWithJoystick != nullptr ) { - this->pDriveWithJoystick->Start(); - } - } - else { - if ( this->pDriveWithTriggers != nullptr ) { - this->pDriveWithTriggers->Start(); - } - } - - // Use Generic Control to control other on-board motors - if ( this->pGenericControl != nullptr ) { - this->pGenericControl->Start(); - LOG( "USE OP CONTROLLER FOR PROTOTYPING" ) - } - // Replace above with commands for other hardware controlls - - return; +void Robot::TeleopInit() { + LOG("[Robot] Teleop Initialized"); + + // If Joydrive setting is enabled, use joystick drive, else use trigger drive + if (JoyDrive == true) { + if (this->pDriveWithJoystick != nullptr) { + this->pDriveWithJoystick->Start(); + } + } else { + if (this->pDriveWithTriggers != nullptr) { + this->pDriveWithTriggers->Start(); + } + } + + // Use Generic Control to control other on-board motors + if (this->pGenericControl != nullptr) { + this->pGenericControl->Start(); + LOG("USE OP CONTROLLER FOR PROTOTYPING") + } + // Replace above with commands for other hardware controlls + + return; } -void Robot::TeleopPeriodic() -{ - // Run Teleop - frc::Scheduler::GetInstance()->Run(); +void Robot::TeleopPeriodic() { + // Run Teleop + frc::Scheduler::GetInstance()->Run(); - return; + return; } -void Robot::TestInit() { return; } +void Robot::TestInit() { + return; +} -void Robot::TestPeriodic() { return; } +void Robot::TestPeriodic() { + return; +} // The main() function is hidden in this // pre-processor macro... -START_ROBOT_CLASS( Robot ) +START_ROBOT_CLASS(Robot) diff --git a/src/main/include/Subsystems/Elevator.hpp b/src/main/include/Subsystems/Elevator.hpp deleted file mode 100644 index 616cd48..0000000 --- a/src/main/include/Subsystems/Elevator.hpp +++ /dev/null @@ -1,22 +0,0 @@ -//! Subsystem to control the elevator - -#ifndef _ELEVATOR_HG_ -#define _ELEVATOR_HG_ - -#include -#include -#include - -//!< Create a class for our subsystem -class Elevator: public frc::Subsystem { -public: - Elevator(); //!< Class constructor - ~Elevator(); //!< Class destructor - - void setSpeed(double speed); - -private: - can::WPI_TalonSRX* pLiftMotor; //!< Pointer to a Talon - -}; -#endif