From cb95167104f4ff6ffcbec1493ba573565af138a8 Mon Sep 17 00:00:00 2001 From: Adam Newhouse Date: Mon, 20 Feb 2017 00:24:53 -0500 Subject: [PATCH 1/7] I made the changes I think fix everything --- src/Commands/Acquisition/AcquisitionIn.cpp | 2 -- src/Commands/Auto/AutonomousCommand.h | 2 +- src/Commands/Shooter/FireBalls.cpp | 38 ++++++++++++++++++++++ src/Commands/Shooter/FireBalls.h | 24 ++++++++++++++ src/Commands/Storage/RunStorage.h | 1 + src/OI.cpp | 12 +++++-- src/OI.h | 2 ++ src/Robot.cpp | 1 - src/Robot.h | 2 +- src/Subsystems/Shooter.cpp | 14 ++++++++ src/Subsystems/Shooter.h | 3 ++ src/Subsystems/Storage.cpp | 21 ++---------- src/Subsystems/Storage.h | 3 -- 13 files changed, 97 insertions(+), 28 deletions(-) create mode 100644 src/Commands/Shooter/FireBalls.cpp create mode 100644 src/Commands/Shooter/FireBalls.h diff --git a/src/Commands/Acquisition/AcquisitionIn.cpp b/src/Commands/Acquisition/AcquisitionIn.cpp index a3c2e08..e599581 100644 --- a/src/Commands/Acquisition/AcquisitionIn.cpp +++ b/src/Commands/Acquisition/AcquisitionIn.cpp @@ -14,8 +14,6 @@ void AcquisitionIn::Initialize() { // Called repeatedly when this Command is scheduled to run void AcquisitionIn::Execute() { Robot::acquisition->Intake(); - Robot::storage->MoveStorage(0.2); - Robot::storage->SetGatePosition(true); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/Commands/Auto/AutonomousCommand.h b/src/Commands/Auto/AutonomousCommand.h index 9ea7a3d..a48b244 100644 --- a/src/Commands/Auto/AutonomousCommand.h +++ b/src/Commands/Auto/AutonomousCommand.h @@ -14,7 +14,7 @@ #include "Commands/Subsystem.h" -#include "../Robot.h" +#include "Robot.h" /** * diff --git a/src/Commands/Shooter/FireBalls.cpp b/src/Commands/Shooter/FireBalls.cpp new file mode 100644 index 0000000..8fc9c32 --- /dev/null +++ b/src/Commands/Shooter/FireBalls.cpp @@ -0,0 +1,38 @@ +#include + +#define SHOOTER_ACCEPTABLE_ERROR 50 + +FireBalls::FireBalls() { + // Use Requires() here to declare subsystem dependencies + // eg. Requires(Robot::chassis.get()); +} + +// Called just before this Command runs the first time +void FireBalls::Initialize() { + +} + +// Called repeatedly when this Command is scheduled to run +void FireBalls::Execute() { + if (fabs(Robot::shooter->GetFlywheelSpeed() - RunShooter::GetDesiredSpeed()) < SHOOTER_ACCEPTABLE_ERROR) + Robot::shooter->SetGatePosition(false); //TODO make the shooter alternate sides while held + else + Robot::shooter->SetGatePosition(true); +} + +// Make this return true when this Command no longer needs to run execute() +bool FireBalls::IsFinished() { + return false; +} + +// Called once after isFinished returns true +void FireBalls::End() { + Robot::shooter->SetGatePosition(true); +} + +// Called when another command which requires one or more of the same +// subsystems is scheduled to run +void FireBalls::Interrupted() { + +} + diff --git a/src/Commands/Shooter/FireBalls.h b/src/Commands/Shooter/FireBalls.h new file mode 100644 index 0000000..23f2037 --- /dev/null +++ b/src/Commands/Shooter/FireBalls.h @@ -0,0 +1,24 @@ +/* + * FireBalls.h + * + * Created on: Feb 19, 2017 + * Author: adnew + */ + +#ifndef SRC_COMMANDS_SHOOTER_FIREBALLS_H_ +#define SRC_COMMANDS_SHOOTER_FIREBALLS_H_ + +#include "../../CommandBase.h" +#include "../../Robot.h" +class FireBalls: public Command { +public: + FireBalls(); + void Initialize() override; + void Execute() override; + bool IsFinished() override; + void End() override; + void Interrupted() override; +}; + + +#endif /* SRC_COMMANDS_SHOOTER_FIREBALLS_H_ */ diff --git a/src/Commands/Storage/RunStorage.h b/src/Commands/Storage/RunStorage.h index e66cc49..7f9624a 100644 --- a/src/Commands/Storage/RunStorage.h +++ b/src/Commands/Storage/RunStorage.h @@ -3,6 +3,7 @@ #include #include "../../CommandBase.h" +#include "Robot.h" class RunStorage: public Command { public: diff --git a/src/OI.cpp b/src/OI.cpp index 54cee13..9c7b40e 100644 --- a/src/OI.cpp +++ b/src/OI.cpp @@ -13,7 +13,7 @@ #include "Commands/Gear/GearOut.h" #include "Commands/Storage/RunStorage.h" #include "Commands/Shooter/RunShooter.h" - +#include "Commands/Shooter/FireBalls.h" bool OI::controllerLeft; @@ -65,7 +65,7 @@ void OI::MapButtons(){ toggleVisionShooter->WhenPressed(new RunVisionShooter()); storageFeedShooter.reset(new JoystickButton(controllerL.get(), STORAGE_FEED_SHOOTER)); - storageFeedShooter->WhileHeld(new RunStorage()); + storageFeedShooter->WhileHeld(new FireBalls()); extendGear.reset(new JoystickButton(controllerL.get(), EXTEND_GEAR)); extendGear->WhenPressed(new GearOut()); @@ -109,3 +109,11 @@ double OI::GetYDriverR(){ else return lJoystick2->GetY(); } + +bool OI::GetAcquisition(){ + return acqIn->Get(); +} + +bool OI::GetFiring(){ + return storageFeedShooter->Get(); +} diff --git a/src/OI.h b/src/OI.h index 7e01287..6120620 100644 --- a/src/OI.h +++ b/src/OI.h @@ -33,6 +33,8 @@ class OI { double GetYControllerR(); double GetYDriverL(); double GetYDriverR(); + bool GetAcquisition(); + bool GetFiring(); private: static bool controllerLeft; std::shared_ptr rJoystick1; diff --git a/src/Robot.cpp b/src/Robot.cpp index f6c4490..6c00cf6 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -7,7 +7,6 @@ #include #include -#include "Commands/ExampleCommand.h" #include "Commands/Drive/AutoDriveDistance.h" #include "Commands/Drive/AutoDriveTurn.h" #include "CommandBase.h" diff --git a/src/Robot.h b/src/Robot.h index dc82899..c336db6 100644 --- a/src/Robot.h +++ b/src/Robot.h @@ -18,7 +18,7 @@ #include "LiveWindow/LiveWindow.h" // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES -#include "Commands/AutonomousCommand.h" +#include "Commands/Auto/AutonomousCommand.h" #include "Subsystems/Acquisition.h" #include "Subsystems/Climbing.h" #include "Subsystems/Drive.h" diff --git a/src/Subsystems/Shooter.cpp b/src/Subsystems/Shooter.cpp index c360fd7..037259a 100644 --- a/src/Subsystems/Shooter.cpp +++ b/src/Subsystems/Shooter.cpp @@ -63,3 +63,17 @@ bool Shooter::IsRunning() { return isRunning; } + +void Shooter::SetGatePosition(bool pos) { + RobotMap::leftGate->Set(pos); + RobotMap::rightGate->Set(pos); +} + +void Shooter::SetRGatePosition(bool pos) { + RobotMap::rightGate->Set(pos); +} + +void Shooter::SetLGatePosition(bool pos) { + RobotMap::leftGate->Set(pos); +} + diff --git a/src/Subsystems/Shooter.h b/src/Subsystems/Shooter.h index 21bc284..909b966 100644 --- a/src/Subsystems/Shooter.h +++ b/src/Subsystems/Shooter.h @@ -38,6 +38,9 @@ class Shooter: public Subsystem { void RunFlywheel(double speed); double GetFlywheelSpeed(); bool IsRunning(); + void SetRGatePosition(bool pos); + void SetLGatePosition(bool pos); + void SetGatePosition(bool pos); // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS }; diff --git a/src/Subsystems/Storage.cpp b/src/Subsystems/Storage.cpp index 8806b19..6808924 100644 --- a/src/Subsystems/Storage.cpp +++ b/src/Subsystems/Storage.cpp @@ -8,23 +8,20 @@ // update. Deleting the comments indicating the section will prevent // it from being updated in the future. - #include "CANTalon.h" #include "Robot.h" #include "Storage.h" -#include "../RobotMap.h" - - +#include "RobotMap.h" +#include "Commands/Storage/RunStorage.h" Storage::Storage() : Subsystem("Storage") { -// std::shared_ptr Storage::lGatingPiston; -// std::shared_ptr Storage::rGatingPiston; ballControlMotor = RobotMap::storageballControlMotor; } void Storage::InitDefaultCommand() { + SetDefaultCommand(new RunStorage()); } void Storage::SetControlMode(CANTalon::ControlMode cMode){ @@ -45,16 +42,4 @@ void Storage::MoveStorage(double speed) ballControlMotor->Set(speed); } -void Storage::SetGatePosition(bool pos) { - lGatingPiston->Set(pos); - rGatingPiston->Set(pos); -} - -void Storage::SetLGatePosition(bool pos) { - lGatingPiston->Set(pos); -} - -void Storage::SetRGatePosition(bool pos) { - rGatingPiston->Set(pos); -} diff --git a/src/Subsystems/Storage.h b/src/Subsystems/Storage.h index b3e3626..b958500 100644 --- a/src/Subsystems/Storage.h +++ b/src/Subsystems/Storage.h @@ -41,9 +41,6 @@ class Storage: public Subsystem { void InitDefaultCommand(); void SetControlMode(CANTalon::ControlMode cMode); void MoveStorage(double speed); - void SetRGatePosition(bool pos); - void SetLGatePosition(bool pos); - void SetGatePosition(bool pos); }; #endif From d571db06173c024b349e87edc22956dbc22c89ca Mon Sep 17 00:00:00 2001 From: Adam Newhouse Date: Mon, 20 Feb 2017 00:40:21 -0500 Subject: [PATCH 2/7] More fixes --- src/Commands/Shooter/FireBalls.cpp | 4 +--- src/Commands/Shooter/RunShooter.cpp | 2 +- src/Commands/Storage/RunStorage.cpp | 8 ++++---- src/Subsystems/Shooter.cpp | 14 ++++++-------- src/Subsystems/Shooter.h | 1 + 5 files changed, 13 insertions(+), 16 deletions(-) diff --git a/src/Commands/Shooter/FireBalls.cpp b/src/Commands/Shooter/FireBalls.cpp index 8fc9c32..92f72ae 100644 --- a/src/Commands/Shooter/FireBalls.cpp +++ b/src/Commands/Shooter/FireBalls.cpp @@ -1,7 +1,5 @@ #include -#define SHOOTER_ACCEPTABLE_ERROR 50 - FireBalls::FireBalls() { // Use Requires() here to declare subsystem dependencies // eg. Requires(Robot::chassis.get()); @@ -14,7 +12,7 @@ void FireBalls::Initialize() { // Called repeatedly when this Command is scheduled to run void FireBalls::Execute() { - if (fabs(Robot::shooter->GetFlywheelSpeed() - RunShooter::GetDesiredSpeed()) < SHOOTER_ACCEPTABLE_ERROR) + if (Robot::shooter->UpToSpeed()) //Is the shooter up to speed? Robot::shooter->SetGatePosition(false); //TODO make the shooter alternate sides while held else Robot::shooter->SetGatePosition(true); diff --git a/src/Commands/Shooter/RunShooter.cpp b/src/Commands/Shooter/RunShooter.cpp index 31f3444..0472820 100644 --- a/src/Commands/Shooter/RunShooter.cpp +++ b/src/Commands/Shooter/RunShooter.cpp @@ -12,7 +12,7 @@ void RunShooter::Initialize() { // Called repeatedly when this Command is scheduled to run void RunShooter::Execute() { - if (!Robot::shooter->IsRunning()) + if (!Robot::shooter->IsRunning()) //Toggle the shooter flywheel Robot::shooter->RunFlywheel(GetDesiredSpeed()); else Robot::shooter->RunFlywheel(0); diff --git a/src/Commands/Storage/RunStorage.cpp b/src/Commands/Storage/RunStorage.cpp index 85328a9..11ebf41 100644 --- a/src/Commands/Storage/RunStorage.cpp +++ b/src/Commands/Storage/RunStorage.cpp @@ -1,5 +1,4 @@ #include "RunStorage.h" -#define SHOOTER_ACCEPTABLE_ERROR 50 RunStorage::RunStorage() { // Use Requires() here to declare subsystem dependencies @@ -13,11 +12,12 @@ void RunStorage::Initialize() { // Called repeatedly when this Command is scheduled to run void RunStorage::Execute() { - if (fabs(Robot::shooter->GetFlywheelSpeed() - RunShooter::GetDesiredSpeed()) < SHOOTER_ACCEPTABLE_ERROR){ + //Either Acquisition is running or the shooter is sunning and ready to fire + if (Robot::oi->GetAcquisition() || (Robot::oi->GetFiring() && Robot::shooter->UpToSpeed())){ Robot::storage->MoveStorage(0.2); - } - else + } else { Robot::storage->MoveStorage(0.0); + } } // Make this return true when this Command no longer needs to run execute() diff --git a/src/Subsystems/Shooter.cpp b/src/Subsystems/Shooter.cpp index 037259a..f41e3e8 100644 --- a/src/Subsystems/Shooter.cpp +++ b/src/Subsystems/Shooter.cpp @@ -10,16 +10,11 @@ #include "CANTalon.h" - - #include "Robot.h" #include "Shooter.h" #include "../RobotMap.h" -// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES -// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES -// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS -// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS +#define SHOOTER_ACCEPTABLE_ERROR 50 Shooter::Shooter() : Subsystem("Shooter") { flywheel = RobotMap::shooterflywheel; @@ -54,11 +49,14 @@ void Shooter::ChangeControlMode(CANTalon::ControlMode cMode){ } } -double Shooter::GetFlywheelSpeed() -{ +double Shooter::GetFlywheelSpeed(){ return flywheel->GetEncVel(); } +bool Shooter::UpToSpeed(){ + return abs(flywheel->GetClosedLoopError()) < SHOOTER_ACCEPTABLE_ERROR; +} + bool Shooter::IsRunning() { return isRunning; diff --git a/src/Subsystems/Shooter.h b/src/Subsystems/Shooter.h index 909b966..ea54fb2 100644 --- a/src/Subsystems/Shooter.h +++ b/src/Subsystems/Shooter.h @@ -37,6 +37,7 @@ class Shooter: public Subsystem { void ChangeControlMode(CANTalon::ControlMode cMode); void RunFlywheel(double speed); double GetFlywheelSpeed(); + bool UpToSpeed(); bool IsRunning(); void SetRGatePosition(bool pos); void SetLGatePosition(bool pos); From 4ee37068e2630b1dfc4724ad41511cdae882144b Mon Sep 17 00:00:00 2001 From: Adam Newhouse Date: Mon, 20 Feb 2017 01:06:19 -0500 Subject: [PATCH 3/7] Lol. Forgot to remove this. --- src/Commands/Acquisition/AcquisitionIn.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Commands/Acquisition/AcquisitionIn.cpp b/src/Commands/Acquisition/AcquisitionIn.cpp index e599581..632452c 100644 --- a/src/Commands/Acquisition/AcquisitionIn.cpp +++ b/src/Commands/Acquisition/AcquisitionIn.cpp @@ -3,7 +3,6 @@ AcquisitionIn::AcquisitionIn() { // Use Requires() here to declare subsystem dependencies // eg. Requires(Robot::chassis.get()); - Requires(Robot::storage.get()); } // Called just before this Command runs the first time From dfdf4ca5323a0d7476e853e8e000376ed694a7a3 Mon Sep 17 00:00:00 2001 From: Adam Newhouse Date: Mon, 20 Feb 2017 22:22:22 -0500 Subject: [PATCH 4/7] JoystickDrive no longer broken Split FireBalls command into one that shoots high and and one that shoots low angle GearIn and GearOut commands can no longer cause race conditions RunStorage requires storage subsystem and nobody else may touch it Shooter subsystem now knows how to set shooter angle --- src/Commands/Acquisition/AcquisitionIn.cpp | 6 +-- src/Commands/Drive/JoystickDrive.cpp | 1 - src/Commands/Drive/ShiftHigh.cpp | 1 - src/Commands/Drive/ShiftLow.cpp | 1 - src/Commands/Gear/GearIn.cpp | 3 +- src/Commands/Gear/GearOut.cpp | 3 +- src/Commands/Shooter/FireBallsHigh.cpp | 37 +++++++++++++++++++ src/Commands/Shooter/FireBallsHigh.h | 24 ++++++++++++ .../{FireBalls.cpp => FireBallsLow.cpp} | 15 ++++---- .../Shooter/{FireBalls.h => FireBallsLow.h} | 10 ++--- src/Commands/Storage/RunStorage.cpp | 1 + src/OI.cpp | 20 +++++----- src/OI.h | 21 ++++++----- src/Robot.cpp | 9 +---- src/Subsystems/Shooter.cpp | 4 ++ src/Subsystems/Shooter.h | 1 + 16 files changed, 108 insertions(+), 49 deletions(-) create mode 100644 src/Commands/Shooter/FireBallsHigh.cpp create mode 100644 src/Commands/Shooter/FireBallsHigh.h rename src/Commands/Shooter/{FireBalls.cpp => FireBallsLow.cpp} (72%) rename src/Commands/Shooter/{FireBalls.h => FireBallsLow.h} (66%) diff --git a/src/Commands/Acquisition/AcquisitionIn.cpp b/src/Commands/Acquisition/AcquisitionIn.cpp index 632452c..24a5fec 100644 --- a/src/Commands/Acquisition/AcquisitionIn.cpp +++ b/src/Commands/Acquisition/AcquisitionIn.cpp @@ -1,8 +1,7 @@ #include "AcquisitionIn.h" AcquisitionIn::AcquisitionIn() { - // Use Requires() here to declare subsystem dependencies - // eg. Requires(Robot::chassis.get()); + Requires(Robot::acquisition.get()); } // Called just before this Command runs the first time @@ -13,13 +12,14 @@ void AcquisitionIn::Initialize() { // Called repeatedly when this Command is scheduled to run void AcquisitionIn::Execute() { Robot::acquisition->Intake(); + Robot::acquisition->LowerArm(); } // Make this return true when this Command no longer needs to run execute() bool AcquisitionIn::IsFinished() { return false; } - + Robot::acquisition->Stop(); // Called once after isFinished returns true void AcquisitionIn::End() { diff --git a/src/Commands/Drive/JoystickDrive.cpp b/src/Commands/Drive/JoystickDrive.cpp index 8ee384d..1ac60d7 100644 --- a/src/Commands/Drive/JoystickDrive.cpp +++ b/src/Commands/Drive/JoystickDrive.cpp @@ -15,7 +15,6 @@ void JoystickDrive::Initialize() { // Called repeatedly when this Command is scheduled to run void JoystickDrive::Execute() { - printf("running"); Robot::drive->TankDrive(Robot::oi->GetYDriverL() * fabs(Robot::oi->GetYDriverL()), Robot::oi->GetYDriverR() * fabs(Robot::oi->GetYDriverR()), false); } diff --git a/src/Commands/Drive/ShiftHigh.cpp b/src/Commands/Drive/ShiftHigh.cpp index 82c8b7c..5457fd2 100644 --- a/src/Commands/Drive/ShiftHigh.cpp +++ b/src/Commands/Drive/ShiftHigh.cpp @@ -2,7 +2,6 @@ ShiftHigh::ShiftHigh() { // Use Requires() here to declare subsystem dependencies - Requires(Robot::drive.get()); } // Called just before this Command runs the first time diff --git a/src/Commands/Drive/ShiftLow.cpp b/src/Commands/Drive/ShiftLow.cpp index 1b30a8a..efd95b7 100644 --- a/src/Commands/Drive/ShiftLow.cpp +++ b/src/Commands/Drive/ShiftLow.cpp @@ -2,7 +2,6 @@ ShiftLow::ShiftLow() { // Use Requires() here to declare subsystem dependencies - Requires(Robot::drive.get()); } // Called just before this Command runs the first time diff --git a/src/Commands/Gear/GearIn.cpp b/src/Commands/Gear/GearIn.cpp index d598d7b..200c235 100644 --- a/src/Commands/Gear/GearIn.cpp +++ b/src/Commands/Gear/GearIn.cpp @@ -1,8 +1,7 @@ #include "GearIn.h" GearIn::GearIn() { - // Use Requires() here to declare subsystem dependencies - // eg. Requires(Robot::chassis.get()); + Requires(Robot::gear.get()); //Don't let controller confuse the robot by mashing both buttons } // Called just before this Command runs the first time diff --git a/src/Commands/Gear/GearOut.cpp b/src/Commands/Gear/GearOut.cpp index cf082cf..907f8c3 100644 --- a/src/Commands/Gear/GearOut.cpp +++ b/src/Commands/Gear/GearOut.cpp @@ -1,8 +1,7 @@ #include "GearOut.h" GearOut::GearOut() { - // Use Requires() here to declare subsystem dependencies - // eg. Requires(Robot::chassis.get()); + Requires(Robot::gear.get()); //Don't let controller confuse the robot by mashing both buttons } // Called just before this Command runs the first time diff --git a/src/Commands/Shooter/FireBallsHigh.cpp b/src/Commands/Shooter/FireBallsHigh.cpp new file mode 100644 index 0000000..2dff3c3 --- /dev/null +++ b/src/Commands/Shooter/FireBallsHigh.cpp @@ -0,0 +1,37 @@ +#include + +FireBallsHigh::FireBallsHigh() { + // Use Requires() here to declare subsystem dependencies + // eg. Requires(Robot::chassis.get()); +} + +// Called just before this Command runs the first time +void FireBallsHigh::Initialize() { + +} + +// Called repeatedly when this Command is scheduled to run +void FireBallsHigh::Execute() { + Robot::shooter->SetAngle(true); + if (Robot::shooter->UpToSpeed()) //Is the shooter up to speed? + Robot::shooter->SetGatePosition(false); //TODO make the shooter alternate sides while held + else + Robot::shooter->SetGatePosition(true); +} + +// Make this return true when this Command no longer needs to run execute() +bool FireBallsHigh::IsFinished() { + return false; +} + +// Called once after isFinished returns true +void FireBallsHigh::End() { + Robot::shooter->SetGatePosition(true); +} + +// Called when another command which requires one or more of the same +// subsystems is scheduled to run +void FireBallsHigh::Interrupted() { + +} + diff --git a/src/Commands/Shooter/FireBallsHigh.h b/src/Commands/Shooter/FireBallsHigh.h new file mode 100644 index 0000000..3c4256b --- /dev/null +++ b/src/Commands/Shooter/FireBallsHigh.h @@ -0,0 +1,24 @@ +/* + * FireBallsHigh.h + * + * Created on: Feb 19, 2017 + * Author: adnew + */ + +#ifndef SRC_COMMANDS_SHOOTER_FIREBALLSHIGH_H_ +#define SRC_COMMANDS_SHOOTER_FIREBALLSHIGH_H_ + +#include "../../CommandBase.h" +#include "../../Robot.h" +class FireBallsHigh: public Command { +public: + FireBallsHigh(); + void Initialize() override; + void Execute() override; + bool IsFinished() override; + void End() override; + void Interrupted() override; +}; + + +#endif /* SRC_COMMANDS_SHOOTER_FIREBALLSHIGH_H_ */ diff --git a/src/Commands/Shooter/FireBalls.cpp b/src/Commands/Shooter/FireBallsLow.cpp similarity index 72% rename from src/Commands/Shooter/FireBalls.cpp rename to src/Commands/Shooter/FireBallsLow.cpp index 92f72ae..3796976 100644 --- a/src/Commands/Shooter/FireBalls.cpp +++ b/src/Commands/Shooter/FireBallsLow.cpp @@ -1,17 +1,18 @@ -#include +#include -FireBalls::FireBalls() { +FireBallsLow::FireBallsLow() { // Use Requires() here to declare subsystem dependencies // eg. Requires(Robot::chassis.get()); } // Called just before this Command runs the first time -void FireBalls::Initialize() { +void FireBallsLow::Initialize() { } // Called repeatedly when this Command is scheduled to run -void FireBalls::Execute() { +void FireBallsLow::Execute() { + Robot::shooter->SetAngle(false); if (Robot::shooter->UpToSpeed()) //Is the shooter up to speed? Robot::shooter->SetGatePosition(false); //TODO make the shooter alternate sides while held else @@ -19,18 +20,18 @@ void FireBalls::Execute() { } // Make this return true when this Command no longer needs to run execute() -bool FireBalls::IsFinished() { +bool FireBallsLow::IsFinished() { return false; } // Called once after isFinished returns true -void FireBalls::End() { +void FireBallsLow::End() { Robot::shooter->SetGatePosition(true); } // Called when another command which requires one or more of the same // subsystems is scheduled to run -void FireBalls::Interrupted() { +void FireBallsLow::Interrupted() { } diff --git a/src/Commands/Shooter/FireBalls.h b/src/Commands/Shooter/FireBallsLow.h similarity index 66% rename from src/Commands/Shooter/FireBalls.h rename to src/Commands/Shooter/FireBallsLow.h index 23f2037..5678d0f 100644 --- a/src/Commands/Shooter/FireBalls.h +++ b/src/Commands/Shooter/FireBallsLow.h @@ -1,18 +1,18 @@ /* - * FireBalls.h + * FireBallsLow.h * * Created on: Feb 19, 2017 * Author: adnew */ -#ifndef SRC_COMMANDS_SHOOTER_FIREBALLS_H_ -#define SRC_COMMANDS_SHOOTER_FIREBALLS_H_ +#ifndef SRC_COMMANDS_SHOOTER_FIREBALLSLOW_H_ +#define SRC_COMMANDS_SHOOTER_FIREBALLSLOW_H_ #include "../../CommandBase.h" #include "../../Robot.h" -class FireBalls: public Command { +class FireBallsLow: public Command { public: - FireBalls(); + FireBallsLow(); void Initialize() override; void Execute() override; bool IsFinished() override; diff --git a/src/Commands/Storage/RunStorage.cpp b/src/Commands/Storage/RunStorage.cpp index 11ebf41..bcbc21b 100644 --- a/src/Commands/Storage/RunStorage.cpp +++ b/src/Commands/Storage/RunStorage.cpp @@ -3,6 +3,7 @@ RunStorage::RunStorage() { // Use Requires() here to declare subsystem dependencies // eg. Requires(Robot::chassis.get()); + Requires(Robot::storage.get()); } // Called just before this Command runs the first time diff --git a/src/OI.cpp b/src/OI.cpp index 9c7b40e..ae225aa 100644 --- a/src/OI.cpp +++ b/src/OI.cpp @@ -1,3 +1,5 @@ +#include +#include #include #include #include "OI.h" @@ -13,7 +15,6 @@ #include "Commands/Gear/GearOut.h" #include "Commands/Storage/RunStorage.h" #include "Commands/Shooter/RunShooter.h" -#include "Commands/Shooter/FireBalls.h" bool OI::controllerLeft; @@ -52,10 +53,10 @@ void OI::MapButtons(){ driveShiftHighGear.reset(new JoystickButton(driverR.get(), SHIFT_HIGH_GEAR)); driveShiftHighGear->WhenPressed(new ShiftHigh()); - drivePTOOn.reset(new JoystickButton(controllerR.get(), SHIFT_PTO_ON)); + drivePTOOn.reset(new JoystickButton(driverR.get(), SHIFT_PTO_ON)); drivePTOOn->WhenPressed(new StartClimber()); - drivePTOOff.reset(new JoystickButton(controllerL.get(), SHIFT_PTO_OFF)); + drivePTOOff.reset(new JoystickButton(driverL.get(), SHIFT_PTO_OFF)); drivePTOOff->WhenPressed(new StopClimber()); toggleShooter.reset(new JoystickButton(controllerL.get(), TOGGLE_SHOOTER)); @@ -64,9 +65,6 @@ void OI::MapButtons(){ toggleVisionShooter.reset(new JoystickButton(controllerL.get(), TOGGLE_VISION_SHOOTER)); toggleVisionShooter->WhenPressed(new RunVisionShooter()); - storageFeedShooter.reset(new JoystickButton(controllerL.get(), STORAGE_FEED_SHOOTER)); - storageFeedShooter->WhileHeld(new FireBalls()); - extendGear.reset(new JoystickButton(controllerL.get(), EXTEND_GEAR)); extendGear->WhenPressed(new GearOut()); @@ -78,8 +76,12 @@ void OI::MapButtons(){ turnToBoiler.reset(new JoystickButton(driverR.get(), DRIVE_TO_BOILER)); turnToGear.reset(new JoystickButton(driverR.get(), DRIVE_TO_GEAR)); - shooterAngleFar.reset(new JoystickButton(driverR.get(), SHOOTER_ANGLE_FAR)); - shooterAngleShort.reset(new JoystickButton(driverR.get(), SHOOTER_ANGLE_SHORT)); + + shooterAngleFar.reset(new JoystickButton(controllerL.get(), SHOOT_FAR)); + shooterAngleFar->WhileHeld(new FireBallsHigh()); + + shooterAngleShort.reset(new JoystickButton(controllerL.get(), SHOOT_SHORT)); + shooterAngleFar->WhileHeld(new FireBallsLow()); } double OI::GetYControllerL(){ @@ -115,5 +117,5 @@ bool OI::GetAcquisition(){ } bool OI::GetFiring(){ - return storageFeedShooter->Get(); + return shooterAngleFar->Get() || shooterAngleShort->Get(); } diff --git a/src/OI.h b/src/OI.h index 6120620..f8fb106 100644 --- a/src/OI.h +++ b/src/OI.h @@ -10,16 +10,17 @@ #define SHIFT_LOW_GEAR 6 //driver left #define SHIFT_PTO_ON 1 //driver right #define SHIFT_PTO_OFF 1 //driver left -#define TOGGLE_SHOOTER 8 // -#define STORAGE_FEED_SHOOTER 9 -#define EXTEND_GEAR 6 -#define RETRACT_GEAR 7 -#define ACQ_IN 10 -#define DRIVE_TO_BOILER 8 -#define DRIVE_TO_GEAR 9 -#define SHOOTER_ANGLE_FAR 6 -#define SHOOTER_ANGLE_SHORT 7 -#define TOGGLE_VISION_SHOOTER 2 +#define DRIVE_TO_BOILER 8 //Driver right +#define DRIVE_TO_GEAR 9 //Driver right + +//All controller buttons below +#define TOGGLE_SHOOTER 6 +#define EXTEND_GEAR 5 +#define RETRACT_GEAR 4 +#define ACQ_IN 2 +#define SHOOT_FAR 3 +#define SHOOT_SHORT 1 +#define TOGGLE_VISION_SHOOTER 7 #include "WPILib.h" diff --git a/src/Robot.cpp b/src/Robot.cpp index 6c00cf6..c65daf3 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -23,15 +23,8 @@ Robot::TestMode Robot::tMode; std::shared_ptr Robot::table; std::unique_ptr autonomousCommand; -//frc::SendableChooser sideChooser; void Robot::RobotInit() { -// sideChooser = new SendableChooser(); -// NetworkTable::SetServerMode(); -// NetworkTable::SetIPAddress("0.0.0.0"); -//// NetworkTable::SetTeam(639); -// table = NetworkTable::GetTable("CameraTracker"); - SmartDashboard::PutBoolean("controller_left_side", true); tMode = TestMode::NONE; RobotMap::init(); @@ -134,7 +127,7 @@ void Robot::RobotInit() { } bool cLeftSide = SmartDashboard::GetBoolean("controller_left_side", true); oi->SetControllerSide(cLeftSide); - //oi->MapButtons(); + oi->MapButtons(); } void Robot::TeleopPeriodic(){ diff --git a/src/Subsystems/Shooter.cpp b/src/Subsystems/Shooter.cpp index f41e3e8..492665c 100644 --- a/src/Subsystems/Shooter.cpp +++ b/src/Subsystems/Shooter.cpp @@ -75,3 +75,7 @@ void Shooter::SetLGatePosition(bool pos) { RobotMap::leftGate->Set(pos); } +void Shooter::SetAngle(bool high){ + angleShift->Set(high); +} + diff --git a/src/Subsystems/Shooter.h b/src/Subsystems/Shooter.h index ea54fb2..ce6eff3 100644 --- a/src/Subsystems/Shooter.h +++ b/src/Subsystems/Shooter.h @@ -42,6 +42,7 @@ class Shooter: public Subsystem { void SetRGatePosition(bool pos); void SetLGatePosition(bool pos); void SetGatePosition(bool pos); + void SetAngle(bool high); // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS }; From 45debb0a389b34f06e283905e1fce154f093a2ee Mon Sep 17 00:00:00 2001 From: Adam Newhouse Date: Mon, 20 Feb 2017 22:22:36 -0500 Subject: [PATCH 5/7] One more thing --- src/Commands/Acquisition/AcquisitionIn.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/Commands/Acquisition/AcquisitionIn.cpp b/src/Commands/Acquisition/AcquisitionIn.cpp index 24a5fec..0b02bf1 100644 --- a/src/Commands/Acquisition/AcquisitionIn.cpp +++ b/src/Commands/Acquisition/AcquisitionIn.cpp @@ -19,10 +19,11 @@ void AcquisitionIn::Execute() { bool AcquisitionIn::IsFinished() { return false; } - Robot::acquisition->Stop(); + // Called once after isFinished returns true void AcquisitionIn::End() { - + Robot::acquisition->Stop(); + Robot::acquisition->RaiseArm(); } // Called when another command which requires one or more of the same From 39460d2e707e94d4f66a0ed9fa517bfd44388714 Mon Sep 17 00:00:00 2001 From: Adam Newhouse Date: Tue, 21 Feb 2017 14:23:03 -0500 Subject: [PATCH 6/7] Hopper is a thing now Minor fixes as well --- src/Commands/Acquisition/AcquisitionIn.cpp | 8 ++--- src/Commands/Acquisition/ToggleHopper.cpp | 35 ++++++++++++++++++++++ src/Commands/Acquisition/ToggleHopper.h | 24 +++++++++++++++ src/OI.cpp | 6 +++- src/OI.h | 4 ++- src/RobotMap.cpp | 21 +++++-------- src/RobotMap.h | 29 ++++-------------- src/Subsystems/Acquisition.cpp | 19 ++++++------ src/Subsystems/Acquisition.h | 7 +++-- src/Subsystems/Drive.h | 5 ---- 10 files changed, 98 insertions(+), 60 deletions(-) create mode 100644 src/Commands/Acquisition/ToggleHopper.cpp create mode 100644 src/Commands/Acquisition/ToggleHopper.h diff --git a/src/Commands/Acquisition/AcquisitionIn.cpp b/src/Commands/Acquisition/AcquisitionIn.cpp index 0b02bf1..00e62fe 100644 --- a/src/Commands/Acquisition/AcquisitionIn.cpp +++ b/src/Commands/Acquisition/AcquisitionIn.cpp @@ -11,8 +11,8 @@ void AcquisitionIn::Initialize() { // Called repeatedly when this Command is scheduled to run void AcquisitionIn::Execute() { - Robot::acquisition->Intake(); - Robot::acquisition->LowerArm(); + //Robot::acquisition->Intake(); + //Robot::acquisition->LowerArm(); } // Make this return true when this Command no longer needs to run execute() @@ -22,8 +22,8 @@ bool AcquisitionIn::IsFinished() { // Called once after isFinished returns true void AcquisitionIn::End() { - Robot::acquisition->Stop(); - Robot::acquisition->RaiseArm(); + //Robot::acquisition->Stop(); + //Robot::acquisition->RaiseArm(); } // Called when another command which requires one or more of the same diff --git a/src/Commands/Acquisition/ToggleHopper.cpp b/src/Commands/Acquisition/ToggleHopper.cpp new file mode 100644 index 0000000..4232f1d --- /dev/null +++ b/src/Commands/Acquisition/ToggleHopper.cpp @@ -0,0 +1,35 @@ +#include "ToggleHopper.h" + +ToggleHopper::ToggleHopper() { +} + +// Called just before this Command runs the first time +void ToggleHopper::Initialize() { + +} + +// Called repeatedly when this Command is scheduled to run +void ToggleHopper::Execute() { + if(RobotMap::doorPiston->Get() == true) { + Robot::acquisition->CloseHopper(); + } + else + { + Robot::acquisition->OpenHopper(); + } +} + +// Make this return true when this Command no longer needs to run execute() +bool ToggleHopper::IsFinished() { + return true; +} + +// Called once after isFinished returns true +void ToggleHopper::End() { +} + +// Called when another command which requires one or more of the same +// subsystems is scheduled to run +void ToggleHopper::Interrupted() { + +} diff --git a/src/Commands/Acquisition/ToggleHopper.h b/src/Commands/Acquisition/ToggleHopper.h new file mode 100644 index 0000000..c1f3439 --- /dev/null +++ b/src/Commands/Acquisition/ToggleHopper.h @@ -0,0 +1,24 @@ +/* + * ToggleHopper.h + * + * Created on: Feb 21, 2017 + * Author: adnew + */ + +#ifndef SRC_COMMANDS_ACQUISITION_TOGGLEHOPPER_H_ +#define SRC_COMMANDS_ACQUISITION_TOGGLEHOPPER_H_ + +#include "../../CommandBase.h" +#include "../../Robot.h" + +class ToggleHopper: public CommandBase { +public: + ToggleHopper(); + void Initialize() override; + void Execute() override; + bool IsFinished() override; + void End() override; + void Interrupted() override; +}; + +#endif /* SRC_COMMANDS_ACQUISITION_TOGGLEHOPPER_H_ */ diff --git a/src/OI.cpp b/src/OI.cpp index ae225aa..2915966 100644 --- a/src/OI.cpp +++ b/src/OI.cpp @@ -7,6 +7,7 @@ #include #include "Robot.h" #include "Commands/Acquisition/AcquisitionIn.h" +#include "Commands/Acquisition/ToggleHopper.h" #include "Commands/Climbing/StartClimber.h" #include "Commands/Climbing/StopClimber.h" #include "Commands/Drive/ShiftHigh.h" @@ -74,6 +75,9 @@ void OI::MapButtons(){ acqIn.reset(new JoystickButton(controllerL.get(), ACQ_IN)); acqIn->WhileHeld(new AcquisitionIn()); + toggleHopper.reset(new JoystickButton(controllerL.get(), TOGGLE_HOPPER)); + toggleHopper->WhenPressed(new ToggleHopper()); + turnToBoiler.reset(new JoystickButton(driverR.get(), DRIVE_TO_BOILER)); turnToGear.reset(new JoystickButton(driverR.get(), DRIVE_TO_GEAR)); @@ -81,7 +85,7 @@ void OI::MapButtons(){ shooterAngleFar->WhileHeld(new FireBallsHigh()); shooterAngleShort.reset(new JoystickButton(controllerL.get(), SHOOT_SHORT)); - shooterAngleFar->WhileHeld(new FireBallsLow()); + shooterAngleShort->WhileHeld(new FireBallsLow()); } double OI::GetYControllerL(){ diff --git a/src/OI.h b/src/OI.h index f8fb106..cbe1e9d 100644 --- a/src/OI.h +++ b/src/OI.h @@ -17,7 +17,8 @@ #define TOGGLE_SHOOTER 6 #define EXTEND_GEAR 5 #define RETRACT_GEAR 4 -#define ACQ_IN 2 +#define ACQ_IN 8 +#define TOGGLE_HOPPER 2 #define SHOOT_FAR 3 #define SHOOT_SHORT 1 #define TOGGLE_VISION_SHOOTER 7 @@ -53,6 +54,7 @@ class OI { std::shared_ptr extendGear; std::shared_ptr retractGear; std::shared_ptr acqIn; + std::shared_ptr toggleHopper; std::shared_ptr turnToBoiler; std::shared_ptr turnToGear; std::shared_ptr shooterAngleFar; diff --git a/src/RobotMap.cpp b/src/RobotMap.cpp index 238dd4f..ebebbd2 100644 --- a/src/RobotMap.cpp +++ b/src/RobotMap.cpp @@ -18,16 +18,14 @@ std::shared_ptr RobotMap::driverDrive1; std::shared_ptr RobotMap::driverDrive2; std::shared_ptr RobotMap::drivelDrive1; std::shared_ptr RobotMap::drivelDrive2; -std::shared_ptr RobotMap::drivebrakes; + std::shared_ptr RobotMap::shooterflywheel; std::shared_ptr RobotMap::shooterangleShift; std::shared_ptr RobotMap::gearpiston; std::shared_ptr RobotMap::climbingdriveShiftPiston; -std::shared_ptr RobotMap::acquisitionarmPiston; -std::shared_ptr RobotMap::acquisitionintakeRoller; +//std::shared_ptr RobotMap::acquisitionarmPiston; +//std::shared_ptr RobotMap::acquisitionintakeRoller; std::shared_ptr RobotMap::storageballControlMotor; -//std::shared_ptr RobotMap::driverEnc; -//std::shared_ptr RobotMap::drivelEnc; std::shared_ptr RobotMap::driveahrs; std::shared_ptr RobotMap::drivegearShift; std::shared_ptr RobotMap::rightGate; @@ -70,29 +68,24 @@ void RobotMap::init() { drivegearShift.reset(new Solenoid(PCM_ID,SHIFTERS)); lw->AddActuator("GearShifting","driveGearShift",drivegearShift); - leftGate.reset(new Solenoid(PCM_ID,LFUEL_GATE)); + leftGate.reset(new Solenoid(PCM_ID,LFUEL_GATE)); //Shooter ball control piston lw->AddActuator("leftGate","leftGate",leftGate); rightGate.reset(new Solenoid(PCM_ID,RFUEL_GATE)); lw->AddActuator("rightGate","rightGate",rightGate); - doorPiston.reset(new Solenoid(PCM_ID,PASSIVE_ACQ)); + doorPiston.reset(new Solenoid(PCM_ID,PASSIVE_ACQ)); //Hopper piston lw->AddActuator("doorPiston","doorPiston",doorPiston); - // acquisitionarmPiston.reset(new Solenoid()); // lw->AddActuator("Acquisition", "armPiston", acquisitionarmPiston); - acquisitionintakeRoller.reset(new CANTalon(INTAKE_ROLLER)); - lw->AddActuator("Acquisition", "intakeRoller", acquisitionintakeRoller); +// //acquisitionintakeRoller.reset(new CANTalon(INTAKE_ROLLER)); //Active floor pickup acquisition +// //lw->AddActuator("Acquisition", "intakeRoller", acquisitionintakeRoller); storageballControlMotor.reset(new CANTalon(STORAGE_ROLLER)); lw->AddActuator("Storage", "ballControlMotor", storageballControlMotor); -// driverEnc.reset(new Encoder(); - -// drivelEnc.reset(new Encoder(LEFT_DRIVE_ENC)); - driveahrs.reset(new AHRS(frc::SerialPort::kMXP)); // drivegearShift.reset(new Solenoid(GEAR_DRIVE_DOWN, GEAR_DRIVE_UP)); diff --git a/src/RobotMap.h b/src/RobotMap.h index e1df9f3..96df74e 100644 --- a/src/RobotMap.h +++ b/src/RobotMap.h @@ -9,17 +9,13 @@ #define R_DRIVE_2 4 #define L_DRIVE_1 1 #define L_DRIVE_2 2 -//#define BRAKES_OUT 4 -//#define BRAKES_IN 0 + #define FLYWHEEL 5 #define INTAKE_ROLLER 0 //Change #define STORAGE_ROLLER 6 -//#define RIGHT_DRIVE_ENC 123123 -//#define LEFT_DRIVE_ENC 321321 -#define GEAR_DRIVE 987897 -//#define GEAR_DRIVE_UP 666 + //Pistons #define PCM_ID 61 @@ -29,25 +25,14 @@ #define RFUEL_GATE 6 #define GEAR_PISTON 5 #define SHOOTER_PISTON 4 - #define PASSIVE_ACQ 2 -//anthony is a pretty fly guy +//anthony is a pretty fly guy i guess /** * The RobotMap is a mapping from the ports sensors and actuators are wired into * to a variable name. This provides flexibility changing wiring, makes checking * the wiring easier and significantly reduces the number of magic numbers * floating around. */ - -// For example to map the left and right motors, you could define the -// following variables to use with your drivetrain subsystem. -// constexpr int LEFTMOTOR = 1; -// constexpr int RIGHTMOTOR = 2; - -// If you are using multiple modules, make sure to define both the port -// number and the module. For example you with a rangefinder: -// constexpr int RANGE_FINDER_PORT = 1; -// constexpr int RANGE_FINDER_MODULE = 1; class RobotMap { public: @@ -55,12 +40,10 @@ class RobotMap static std::shared_ptr driverDrive2; static std::shared_ptr drivelDrive1; static std::shared_ptr drivelDrive2; - static std::shared_ptr drivebrakes; + static std::shared_ptr shooterflywheel; - static std::shared_ptr acquisitionintakeRoller; + //static std::shared_ptr acquisitionintakeRoller; static std::shared_ptr storageballControlMotor; -// static std::shared_ptr driverEnc; -// static std::shared_ptr drivelEnc; static std::shared_ptr driveahrs; @@ -69,7 +52,7 @@ class RobotMap static std::shared_ptr shooterangleShift; static std::shared_ptr gearpiston; static std::shared_ptr climbingdriveShiftPiston; - static std::shared_ptr acquisitionarmPiston; + //static std::shared_ptr acquisitionarmPiston; static std::shared_ptr rightGate; static std::shared_ptr leftGate; static std::shared_ptr doorPiston; diff --git a/src/Subsystems/Acquisition.cpp b/src/Subsystems/Acquisition.cpp index 2a1d986..61eda0d 100644 --- a/src/Subsystems/Acquisition.cpp +++ b/src/Subsystems/Acquisition.cpp @@ -22,8 +22,9 @@ // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS Acquisition::Acquisition() : Subsystem("Acquisition") { - armPiston = RobotMap::acquisitionarmPiston; - intakeRoller = RobotMap::acquisitionintakeRoller; + hopperPiston = RobotMap::doorPiston; + //armPiston = RobotMap::acquisitionarmPiston; + //intakeRoller = RobotMap::acquisitionintakeRoller; } void Acquisition::InitDefaultCommand() @@ -36,24 +37,24 @@ void Acquisition::InitDefaultCommand() // here. Call these from Commands. void Acquisition::Intake() { - intakeRoller->Set(0.6); + //intakeRoller->Set(0.6); } void Acquisition::Output() { - intakeRoller->Set(-0.6); + //intakeRoller->Set(-0.6); } void Acquisition::Stop() { - intakeRoller->Set(0); + //intakeRoller->Set(0); } -void Acquisition::RaiseArm() +void Acquisition::OpenHopper() { - armPiston->Set(true); + hopperPiston->Set(true); } -void Acquisition::LowerArm() +void Acquisition::CloseHopper() { - armPiston->Set(false); + hopperPiston->Set(false); } diff --git a/src/Subsystems/Acquisition.h b/src/Subsystems/Acquisition.h index 524db8f..d30037c 100644 --- a/src/Subsystems/Acquisition.h +++ b/src/Subsystems/Acquisition.h @@ -23,7 +23,8 @@ class Acquisition: public Subsystem { private: // It's desirable that everything possible is private except // for methods that implement subsystem capabilities - std::shared_ptr armPiston; + //std::shared_ptr armPiston; + std::shared_ptr hopperPiston; std::shared_ptr intakeRoller; public: @@ -32,8 +33,8 @@ class Acquisition: public Subsystem { void Intake(); void Output(); void Stop(); - void RaiseArm(); - void LowerArm(); + void OpenHopper(); + void CloseHopper(); }; #endif diff --git a/src/Subsystems/Drive.h b/src/Subsystems/Drive.h index 88d42b7..dd5c330 100644 --- a/src/Subsystems/Drive.h +++ b/src/Subsystems/Drive.h @@ -30,10 +30,7 @@ class Drive: public Subsystem { std::shared_ptr rDrive2; std::shared_ptr lDrive1; std::shared_ptr lDrive2; -// std::shared_ptr brakes; std::shared_ptr gearShift; -// std::shared_ptr rEnc; -// std::shared_ptr lEnc; std::shared_ptr ahrs; double vTurnP = 0.0; @@ -66,8 +63,6 @@ class Drive: public Subsystem { void SetControlMode(DriveControlMode cMode); void ChangeGear(bool high); bool GetGear(); -// void EnableBraking(); -// void DisableBraking(); void InitDefaultCommand(); int GetLEncoder(); int GetREncoder(); From 93f403ecd4ee8eb7e547cb192592199a92e8aae2 Mon Sep 17 00:00:00 2001 From: Adam Newhouse Date: Mon, 20 Feb 2017 23:03:23 -0600 Subject: [PATCH 7/7] Set default command correctly --- src/Subsystems/Drive.cpp | 327 +++++++++++++++++++-------------------- 1 file changed, 161 insertions(+), 166 deletions(-) diff --git a/src/Subsystems/Drive.cpp b/src/Subsystems/Drive.cpp index 655f607..ad58397 100644 --- a/src/Subsystems/Drive.cpp +++ b/src/Subsystems/Drive.cpp @@ -1,167 +1,162 @@ -// RobotBuilder Version: 2.0 -// -// This file was generated by RobotBuilder. It contains sections of -// code that are automatically generated and assigned by robotbuilder. -// These sections will be updated in the future when you export to -// C++ from RobotBuilder. Do not put any code or make any change in -// the blocks indicating autogenerated code or it will be lost on an -// update. Deleting the comments indicating the section will prevent -// it from being updated in the future. - - +// RobotBuilder Version: 2.0 +// +// This file was generated by RobotBuilder. It contains sections of +// code that are automatically generated and assigned by robotbuilder. +// These sections will be updated in the future when you export to +// C++ from RobotBuilder. Do not put any code or make any change in +// the blocks indicating autogenerated code or it will be lost on an +// update. Deleting the comments indicating the section will prevent +// it from being updated in the future. + + #include "CANTalon.h" - -#include "Robot.h" -#include "Drive.h" -#include "../RobotMap.h" -#include "Commands/Drive/JoystickDrive.h" -// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES -// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES - -// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS -// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS - -Drive::Drive() : Subsystem("Drive") { - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS - rDrive1 = RobotMap::driverDrive1; - rDrive2 = RobotMap::driverDrive2; - lDrive1 = RobotMap::drivelDrive1; - lDrive2 = RobotMap::drivelDrive2; - -// brakes = RobotMap::drivebrakes; - gearShift = RobotMap::drivegearShift; -// rEnc = RobotMap::driverEnc; -// lEnc = RobotMap::drivelEnc; - ahrs = RobotMap::driveahrs; - //SetDefaultCommand(new JoystickDrive()); - SetDefaultCommand(new JoystickDrive()); - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS - -} - -void Drive::InitDefaultCommand() { - // Set the default command for a subsystem here. - // SetDefaultCommand(new MySpecialCommand()); - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND - - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND -} - -int Drive::GetLEncoderRate(){ - return lDrive1->GetEncVel(); -} - -int Drive::GetREncoderRate(){ - return rDrive1->GetEncVel(); -} - -// Put methods for controlling this subsystem -// here. Call these from Commands. - -void Drive::TankDrive(double lSpeed, double rSpeed, bool speedClosed){ - if (!speedClosed){ - rDrive1->Set(rSpeed); - lDrive1->Set(lSpeed); - } - else{ - rDrive1->Set(rSpeed * MAX_ENC_VEL); - lDrive1->Set(lSpeed * MAX_ENC_VEL); - } -} - -void Drive::ChangeGear(bool high) -{ - gearShift->Set(!high); -} - -//void Drive::EnableBraking() -//{ -// brakes->Set(true); -//} -// -//void Drive::DisableBraking() -//{ -// brakes->Set(false); -//} - -int Drive::GetLEncoder() -{ - return lDrive1->GetEncPosition(); -} - -int Drive::GetREncoder() -{ - return rDrive1->GetEncPosition(); -} - -double Drive::GetYaw() -{ - return ahrs->GetYaw(); -} - -bool Drive::GetGear(){ - return gearShift->Get(); -} - -//Should be run once every time we change into a control mode -void Drive::SetControlMode(DriveControlMode cMode){ - switch (cMode) - { - case DriveControlMode::MotionProfile: - rDrive1->SetControlMode(CANTalon::ControlMode::kMotionProfile); - lDrive1->SetControlMode(CANTalon::ControlMode::kMotionProfile); - if (Robot::tMode == Robot::TestMode::DRIVE_MOTION_PROFILE) - { - motionP = frc::SmartDashboard::GetNumber("test_pCons", 0.0); - motionI = frc::SmartDashboard::GetNumber("test_iCons", 0.0); - motionD = frc::SmartDashboard::GetNumber("test_dCons", 0.0); - motionF = frc::SmartDashboard::GetNumber("test_fCons", 0.0); - } - rDrive1->SetPID(motionP, motionI, motionD, motionF); - lDrive1->SetPID(motionP, motionI, motionD, motionF); - break; - //case DriveControlMode::Position: -// rDrive1->SetControlMode(CANTalon::ControlMode::kPosition); -// lDrive1->SetControlMode(CANTalon::ControlMode::kPosition); -// if (Robot::tMode == Robot::TestMode::DRIVE_POSITION) -// { -// posP = frc::SmartDashboard::GetNumber("test_pCons", 0.0); -// posI = frc::SmartDashboard::GetNumber("test_iCons", 0.0); -// posD = frc::SmartDashboard::GetNumber("test_dCons", 0.0); -// posF = frc::SmartDashboard::GetNumber("test_fCons", 0.0); -// } -// rDrive1->SetPID(posP, posI, posD, posF); -// lDrive1->SetPID(posP, posI, posD, posF); -// break; - case DriveControlMode::VelocityDriving: - rDrive1->SetControlMode(CANTalon::ControlMode::kSpeed); - lDrive1->SetControlMode(CANTalon::ControlMode::kSpeed); - if (Robot::tMode == Robot::TestMode::DRIVE_SPEED) - { - vDriveP = frc::SmartDashboard::GetNumber("test_pCons", 0.0); - vDriveI = frc::SmartDashboard::GetNumber("test_iCons", 0.0); - vDriveD = frc::SmartDashboard::GetNumber("test_dCons", 0.0); - vDriveF = frc::SmartDashboard::GetNumber("test_fCons", 0.0); - } - rDrive1->SetPID(vDriveP, vDriveI, vTurnD, vDriveF); - lDrive1->SetPID(vDriveP, vDriveI, vTurnD, vDriveF); - break; - case DriveControlMode::VelocityTurning: - rDrive1->SetControlMode(CANTalon::ControlMode::kSpeed); - lDrive1->SetControlMode(CANTalon::ControlMode::kSpeed); - if (Robot::tMode == Robot::TestMode::DRIVE_TURN_SPEED) - { - vTurnP = frc::SmartDashboard::GetNumber("test_pCons", 0.0); - vTurnI = frc::SmartDashboard::GetNumber("test_iCons", 0.0); - vTurnD = frc::SmartDashboard::GetNumber("test_dCons", 0.0); - vTurnF = frc::SmartDashboard::GetNumber("test_fCons", 0.0); - } - rDrive1->SetPID(vTurnP, vTurnI, vTurnD, vTurnF); - lDrive1->SetPID(vTurnP, vTurnI, vTurnD, vTurnF); - break; - case DriveControlMode::Voltage: - rDrive1->SetControlMode(CANTalon::ControlMode::kPercentVbus); - lDrive1->SetControlMode(CANTalon::ControlMode::kPercentVbus); - break; - }; -} + +#include "Robot.h" +#include "Drive.h" +#include "../RobotMap.h" +#include "Commands/Drive/JoystickDrive.h" +// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES +// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES + +// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS +// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS + +Drive::Drive() : Subsystem("Drive") { + // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS + rDrive1 = RobotMap::driverDrive1; + rDrive2 = RobotMap::driverDrive2; + lDrive1 = RobotMap::drivelDrive1; + lDrive2 = RobotMap::drivelDrive2; + gearShift = RobotMap::drivegearShift; + ahrs = RobotMap::driveahrs; + + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS + +} + +void Drive::InitDefaultCommand() { + // Set the default command for a subsystem here. + // SetDefaultCommand(new MySpecialCommand()); + // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND + SetDefaultCommand(new JoystickDrive()); + // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND +} + +int Drive::GetLEncoderRate(){ + return lDrive1->GetEncVel(); +} + +int Drive::GetREncoderRate(){ + return rDrive1->GetEncVel(); +} + +// Put methods for controlling this subsystem +// here. Call these from Commands. + +void Drive::TankDrive(double lSpeed, double rSpeed, bool speedClosed){ + if (!speedClosed){ + rDrive1->Set(rSpeed); + lDrive1->Set(lSpeed); + } + else{ + rDrive1->Set(rSpeed * MAX_ENC_VEL); + lDrive1->Set(lSpeed * MAX_ENC_VEL); + } +} + +void Drive::ChangeGear(bool high) +{ + gearShift->Set(!high); +} + +//void Drive::EnableBraking() +//{ +// brakes->Set(true); +//} +// +//void Drive::DisableBraking() +//{ +// brakes->Set(false); +//} + +int Drive::GetLEncoder() +{ + return lDrive1->GetEncPosition(); +} + +int Drive::GetREncoder() +{ + return rDrive1->GetEncPosition(); +} + +double Drive::GetYaw() +{ + return ahrs->GetYaw(); +} + +bool Drive::GetGear(){ + return gearShift->Get(); +} + +//Should be run once every time we change into a control mode +void Drive::SetControlMode(DriveControlMode cMode){ + switch (cMode) + { + case DriveControlMode::MotionProfile: + rDrive1->SetControlMode(CANTalon::ControlMode::kMotionProfile); + lDrive1->SetControlMode(CANTalon::ControlMode::kMotionProfile); + if (Robot::tMode == Robot::TestMode::DRIVE_MOTION_PROFILE) + { + motionP = frc::SmartDashboard::GetNumber("test_pCons", 0.0); + motionI = frc::SmartDashboard::GetNumber("test_iCons", 0.0); + motionD = frc::SmartDashboard::GetNumber("test_dCons", 0.0); + motionF = frc::SmartDashboard::GetNumber("test_fCons", 0.0); + } + rDrive1->SetPID(motionP, motionI, motionD, motionF); + lDrive1->SetPID(motionP, motionI, motionD, motionF); + break; + //case DriveControlMode::Position: +// rDrive1->SetControlMode(CANTalon::ControlMode::kPosition); +// lDrive1->SetControlMode(CANTalon::ControlMode::kPosition); +// if (Robot::tMode == Robot::TestMode::DRIVE_POSITION) +// { +// posP = frc::SmartDashboard::GetNumber("test_pCons", 0.0); +// posI = frc::SmartDashboard::GetNumber("test_iCons", 0.0); +// posD = frc::SmartDashboard::GetNumber("test_dCons", 0.0); +// posF = frc::SmartDashboard::GetNumber("test_fCons", 0.0); +// } +// rDrive1->SetPID(posP, posI, posD, posF); +// lDrive1->SetPID(posP, posI, posD, posF); +// break; + case DriveControlMode::VelocityDriving: + rDrive1->SetControlMode(CANTalon::ControlMode::kSpeed); + lDrive1->SetControlMode(CANTalon::ControlMode::kSpeed); + if (Robot::tMode == Robot::TestMode::DRIVE_SPEED) + { + vDriveP = frc::SmartDashboard::GetNumber("test_pCons", 0.0); + vDriveI = frc::SmartDashboard::GetNumber("test_iCons", 0.0); + vDriveD = frc::SmartDashboard::GetNumber("test_dCons", 0.0); + vDriveF = frc::SmartDashboard::GetNumber("test_fCons", 0.0); + } + rDrive1->SetPID(vDriveP, vDriveI, vTurnD, vDriveF); + lDrive1->SetPID(vDriveP, vDriveI, vTurnD, vDriveF); + break; + case DriveControlMode::VelocityTurning: + rDrive1->SetControlMode(CANTalon::ControlMode::kSpeed); + lDrive1->SetControlMode(CANTalon::ControlMode::kSpeed); + if (Robot::tMode == Robot::TestMode::DRIVE_TURN_SPEED) + { + vTurnP = frc::SmartDashboard::GetNumber("test_pCons", 0.0); + vTurnI = frc::SmartDashboard::GetNumber("test_iCons", 0.0); + vTurnD = frc::SmartDashboard::GetNumber("test_dCons", 0.0); + vTurnF = frc::SmartDashboard::GetNumber("test_fCons", 0.0); + } + rDrive1->SetPID(vTurnP, vTurnI, vTurnD, vTurnF); + lDrive1->SetPID(vTurnP, vTurnI, vTurnD, vTurnF); + break; + case DriveControlMode::Voltage: + rDrive1->SetControlMode(CANTalon::ControlMode::kPercentVbus); + lDrive1->SetControlMode(CANTalon::ControlMode::kPercentVbus); + break; + }; +}