diff --git a/src/IAMRobot.cpp b/src/IAMRobot.cpp index 2fe3ab1..8372165 100644 --- a/src/IAMRobot.cpp +++ b/src/IAMRobot.cpp @@ -7,25 +7,21 @@ #include using namespace frc; -IAMRobot::IAMRobot() { - flTalon = new CANTalon(flTalID); - frTalon = new CANTalon(frTalID); - rlTalon = new CANTalon(rlTalID); - rrTalon = new CANTalon(rrTalID); - drive = new RobotDrive(flTalon,rlTalon,frTalon,rrTalon); +IAMRobot::IAMRobot() { mainController = new XboxController(mainControllerID); - driveEncoder1 = new Encoder(driveEncoder1Port1,driveEncoder1Port2,true,Encoder::EncodingType::k4X); - driveEncoder2 = new Encoder(driveEncoder2Port1,driveEncoder2Port2,true,Encoder::EncodingType::k4X); + driveSystem = new DriveSystem(mainController); + pickup = new GearPickup(mainController); + baller = new Ball_Pickup(mainController); + lifter = new Lifter(mainController); } IAMRobot::~IAMRobot() { - delete drive; delete mainController; - delete flTalon; - delete frTalon; - delete rlTalon; - delete rrTalon; + delete driveSystem; + delete pickup; + delete baller; + delete lifter; } void IAMRobot::RobotInit(){ @@ -41,9 +37,10 @@ void IAMRobot::TeleopInit(){ } void IAMRobot::TeleopPeriodic(){ - drive->ArcadeDrive(mainController->GetY(Joystick::kLeftHand),mainController->GetX(Joystick::kRightHand),false); - SmartDashboard::PutNumber("Encoder1 Get:", driveEncoder1->Get()); - SmartDashboard::PutNumber("Encoder2 Get:", driveEncoder2->Get()); + driveSystem->ArcadeDriveStickSquare(); + pickup->teleopGearLoop(); + baller->TeleopBallLoop(); + lifter->TeleopLoop(); } void IAMRobot::TestPeriodic(){ diff --git a/src/IAMRobot.h b/src/IAMRobot.h index 93550ff..49ee5ce 100644 --- a/src/IAMRobot.h +++ b/src/IAMRobot.h @@ -14,7 +14,13 @@ #include #include +#include "systems/DriveSystem.h" +#include "util/DeviceIDs.h" +#include "systems/GearPickup.h" +#include "systems/BallPickup.h" +#include "systems/Lifter.h" using namespace frc; + class IAMRobot: public IterativeRobot { public: @@ -23,24 +29,10 @@ class IAMRobot: public IterativeRobot { private: XboxController *mainController; - CANTalon *flTalon; - CANTalon *frTalon; - CANTalon *rlTalon; - CANTalon *rrTalon; - RobotDrive *drive; - Encoder* driveEncoder1; - Encoder* driveEncoder2; - - int driveEncoder1Port1; - int driveEncoder1Port2; - int driveEncoder2Port1; - int driveEncoder2Port2; - int flTalID = 1; - int frTalID = 3; - int rlTalID = 2; - int rrTalID = 4; - int mainControllerID = 0; - + DriveSystem* driveSystem; + GearPickup* pickup; + Ball_Pickup* baller; + Lifter* lifter; void RobotInit() override; void AutonomousInit() override; void AutonomousPeriodic() override; diff --git a/src/systems/BallPickup.cpp b/src/systems/BallPickup.cpp new file mode 100644 index 0000000..a582c4e --- /dev/null +++ b/src/systems/BallPickup.cpp @@ -0,0 +1,64 @@ +/* + * BallPickup.cpp + * + * Created on: Feb 17, 2017 + * Author: Anish + */ + +#include + +Ball_Pickup::Ball_Pickup(XboxController* controller) { + this->control = controller; + innerRoller = new CANTalon(innerRollerID); + outerRoller = new CANTalon(outerRollerID); + Ball_BackDoor = new DoubleSolenoid(backDoorID1,backDoorID2); +} + +Ball_Pickup::~Ball_Pickup() { + delete control; + delete innerRoller; + delete outerRoller; + delete Ball_BackDoor; + +} + +void Ball_Pickup::TeleopBallLoop(){ + if(control->GetXButton() && !control->GetYButton()){ + BallInput(); + } + else if(control->GetYButton() && !control->GetXButton()){ + BallOutput(); + } + + else if(control->GetTriggerAxis(Joystick::kRightHand) && !control->GetTriggerAxis(Joystick::kLeftHand)){ + RaiseElevator(); + } + else if(control->GetTriggerAxis(Joystick::kRightHand) && !control->GetTriggerAxis(Joystick::kLeftHand)){ + LowElevator(); + } + + else{ + innerRoller->Set(0); + outerRoller->Set(0); + } +} + +void Ball_Pickup::BallInput(){ + innerRoller->Set(1); + outerRoller->Set(1); +} + +void Ball_Pickup::BallOutput(){ + innerRoller->Set(-1); + outerRoller->Set(-1); +} + +void Ball_Pickup::RaiseElevator(){ + Ball_BackDoor->Set(DoubleSolenoid::Value::kForward); +} + +void Ball_Pickup::LowElevator(){ + Ball_BackDoor->Set(DoubleSolenoid::Value::kReverse); +} + + diff --git a/src/systems/BallPickup.h b/src/systems/BallPickup.h new file mode 100644 index 0000000..429c68c --- /dev/null +++ b/src/systems/BallPickup.h @@ -0,0 +1,31 @@ +/* + * BallPickup.h + * + * Created on: Feb 17, 2017 + * Author: Anish + */ + +#ifndef SRC_SYSTEMS_BALLPICKUP_H_ +#define SRC_SYSTEMS_BALLPICKUP_H_ +#include "WPILib.h" +#include "CANTalon.h" +#include "teleop/controls/Deadband.h" +#include "util/DeviceIDs.h" +#include "systems/Constants.h" +using namespace frc; +class Ball_Pickup { +public: + Ball_Pickup(XboxController* controller); + virtual ~Ball_Pickup(); + CANTalon* outerRoller; + CANTalon* innerRoller; + XboxController* control; + DoubleSolenoid* Ball_BackDoor; + void TeleopBallLoop(); + void BallInput(); + void BallOutput(); + void RaiseElevator(); + void LowElevator(); +}; + +#endif /* SRC_SYSTEMS_BALLPICKUP_H_ */ diff --git a/src/systems/Constants.h b/src/systems/Constants.h new file mode 100644 index 0000000..78ff259 --- /dev/null +++ b/src/systems/Constants.h @@ -0,0 +1,9 @@ +#ifndef CONST +#define CONST +const double LowLimitDriveDeadband = 0.25; +const double HighLimitDriveDeadband = -0.25; +const double GearPickWaitTime = 0.5; +const double SpeedScale = 0.75; +const double RollerSpeed = 0.75; +const double LiftSpeed = 0.75; +#endif diff --git a/src/systems/DriveSystem.cpp b/src/systems/DriveSystem.cpp index 9cf93b3..d310460 100644 --- a/src/systems/DriveSystem.cpp +++ b/src/systems/DriveSystem.cpp @@ -5,14 +5,57 @@ * Author: Anish */ -#include -DriveSystem::DriveSystem() { +#include "DriveSystem.h" +using namespace frc; +DriveSystem::DriveSystem(XboxController* mainController) { + flTalon = new CANTalon(flTalID); + frTalon = new CANTalon(frTalID); + rlTalon = new CANTalon(rlTalID); + rrTalon = new CANTalon(rrTalID); + drive = new RobotDrive(flTalon,rlTalon,frTalon,rrTalon); + driveEncoder1 = new Encoder(flEncID1,flEncID2,true,Encoder::EncodingType::k4X); + driveEncoder2 = new Encoder(frEncID1,frEncID2,true,Encoder::EncodingType::k4X); + controller = mainController; + driveband = new Deadband(LowLimitDriveDeadband,HighLimitDriveDeadband); } DriveSystem::~DriveSystem() { + delete drive; + delete driveEncoder1; + delete driveEncoder2; + delete flTalon; + delete frTalon; + delete rlTalon; + delete rrTalon; + delete controller; + delete driveband; +} + + + +void DriveSystem::ArcadeDriveStick(){ + drive->ArcadeDrive(SpeedScale*driveband->ReturnBoundedValue(controller->GetY(Joystick::kLeftHand)), SpeedScale*driveband->ReturnBoundedValue(controller->GetX(Joystick::kRightHand)), false); + +} + +void DriveSystem::TankDriveStick(){ + drive->TankDrive(SpeedScale*driveband->ReturnBoundedValue(controller->GetY(Joystick::kLeftHand)),SpeedScale*driveband->ReturnBoundedValue(controller->GetY(Joystick::kRightHand)),false); +} +void DriveSystem::ArcadeDriveStickSquare(){ + drive->ArcadeDrive(SpeedScale*driveband->ReturnBoundedValue(controller->GetY(Joystick::kLeftHand)), SpeedScale*driveband->ReturnBoundedValue(controller->GetX(Joystick::kRightHand)), true); + + if(controller->GetPOV()==0){ + drive->ArcadeDrive(-1,0,true); + } + else if(controller->GetPOV()==180){ + drive->ArcadeDrive(1,0,true); + } +} +void DriveSystem::TankDriveStickSquare(){ + drive->TankDrive(driveband->ReturnBoundedValue(controller->GetY(Joystick::kLeftHand)),driveband->ReturnBoundedValue(controller->GetY(Joystick::kRightHand)),true); } diff --git a/src/systems/DriveSystem.h b/src/systems/DriveSystem.h index 42a2756..ebaca7e 100644 --- a/src/systems/DriveSystem.h +++ b/src/systems/DriveSystem.h @@ -8,13 +8,33 @@ #ifndef SRC_SYSTEMS_DRIVESYSTEM_H_ #define SRC_SYSTEMS_DRIVESYSTEM_H_ #include "WPILib.h" +#include "CANTalon.h" +#include "teleop/controls/Deadband.h" +#include "util/DeviceIDs.h" +#include "systems/Constants.h" +using namespace frc; + class DriveSystem { public: - DriveSystem(); virtual ~DriveSystem(); + CANTalon *flTalon; + CANTalon *frTalon; + CANTalon *rlTalon; + CANTalon *rrTalon; + RobotDrive *drive; + Encoder* driveEncoder1; + Encoder* driveEncoder2; + XboxController* controller; + DriveSystem(XboxController* mainController); void DriveArcade(); - -private: + void AnishArcadeDrive(); + void AnishArcadeDriveSquare(); + void ArcadeDriveStick(); + void ArcadeDriveStickSquare(); + void TankDriveStick(); + void TankDriveStickSquare(); + Deadband* driveband; +//private: }; diff --git a/src/systems/GearPickup.cpp b/src/systems/GearPickup.cpp new file mode 100644 index 0000000..f3986f7 --- /dev/null +++ b/src/systems/GearPickup.cpp @@ -0,0 +1,93 @@ +/* + * GearPickup.cpp + * + * Created on: Feb 11, 2017 + * Author: Anish + */ + +#include + +GearPickup::GearPickup(XboxController*controller) { + this->controller = controller; + liftSolenoid = new DoubleSolenoid(PCM_ID, liftSolenoidID1,liftSolenoidID2); + clampSolenoid = new DoubleSolenoid(PCM_ID,clampSolenoidID1,clampSolenoidID2); + banner = new DigitalInput(BannerID); +} + +GearPickup::~GearPickup() { + delete controller; + delete liftSolenoid; + delete clampSolenoid; + delete banner; +} + +void GearPickup::teleopGearLoop(){ + banner->UpdateTable(); + SmartDashboard::PutBoolean("Banner", banner->Get()); + if(isliftSolenoid && controller->GetBumper(Joystick::kRightHand)){ + isliftSolenoid = false; + liftSolenoid->Set(DoubleSolenoid::Value::kReverse); + Wait(GearPickWaitTime); + + } + else if(!isliftSolenoid && controller->GetBumper(Joystick::kRightHand)){ + isliftSolenoid = true; + liftSolenoid->Set(DoubleSolenoid::Value::kForward); + Wait(GearPickWaitTime); + } + else if(isclampSolenoid && controller->GetBumper(Joystick::kLeftHand)){ + isclampSolenoid = false; + clampSolenoid->Set(DoubleSolenoid::Value::kReverse); + Wait(GearPickWaitTime); + } + else if(!isclampSolenoid && controller->GetBumper(Joystick::kLeftHand)){ + isclampSolenoid = true; + clampSolenoid->Set(DoubleSolenoid::Value::kForward); + Wait(GearPickWaitTime); + } + +} + +void GearPickup::teleopGearLoopMod(){ + if(liftSolenoid->Get() == DoubleSolenoid::Value::kForward && controller->GetBumper(Joystick::kRightHand)){ + liftSolenoid->Set(DoubleSolenoid::Value::kReverse); + Wait(GearPickWaitTime); + } + else if(liftSolenoid->Get() == DoubleSolenoid::Value::kReverse && controller->GetBumper(Joystick::kRightHand)){ + liftSolenoid->Set(DoubleSolenoid::Value::kForward); + Wait(GearPickWaitTime); + } + else if (clampSolenoid->Get() == DoubleSolenoid::Value::kReverse && controller->GetBumper(Joystick::kLeftHand)){ + clampSolenoid->Set(DoubleSolenoid::Value::kForward); + Wait(GearPickWaitTime); + } + else if(clampSolenoid->Get() == DoubleSolenoid::Value::kForward && controller->GetBumper(Joystick::kLeftHand)){ + clampSolenoid->Set(DoubleSolenoid::Value::kReverse); + Wait(GearPickWaitTime); + } +} + +void GearPickup::teleopGearLoopMod2(){ + if(liftSolenoid->Get() == DoubleSolenoid::Value::kForward && controller->GetBumper(Joystick::kRightHand)){ + while(liftSolenoid->Get() != DoubleSolenoid::Value::kReverse){ + liftSolenoid->Set(DoubleSolenoid::Value::kReverse); + } + } + else if(liftSolenoid->Get() == DoubleSolenoid::Value::kReverse && controller->GetBumper(Joystick::kRightHand)){ + while(liftSolenoid->Get() != DoubleSolenoid::Value::kForward){ + liftSolenoid->Set(DoubleSolenoid::Value::kForward); + } + } + else if (clampSolenoid->Get() == DoubleSolenoid::Value::kReverse && controller->GetBumper(Joystick::kLeftHand)){ + while(clampSolenoid->Get() != DoubleSolenoid::Value::kForward){ + clampSolenoid->Set(DoubleSolenoid::Value::kForward); + } + } + else if(clampSolenoid->Get() == DoubleSolenoid::Value::kForward && controller->GetBumper(Joystick::kLeftHand)){ + while(liftSolenoid->Get() != DoubleSolenoid::Value::kReverse){ + liftSolenoid->Set(DoubleSolenoid::Value::kReverse); + } + } +} + + diff --git a/src/systems/GearPickup.h b/src/systems/GearPickup.h new file mode 100644 index 0000000..7d7d6ba --- /dev/null +++ b/src/systems/GearPickup.h @@ -0,0 +1,33 @@ +/* + * GearPickup.h + * + * Created on: Feb 11, 2017 + * Author: Anish + */ + +#ifndef SRC_SYSTEMS_GEARPICKUP_H_ +#define SRC_SYSTEMS_GEARPICKUP_H_ +#include "WPILib.h" +#include "util/DeviceIDs.h" +#include "systems/Constants.h" +using namespace frc; + +class GearPickup { +public: + GearPickup(XboxController* controller); + virtual ~GearPickup(); + DoubleSolenoid* liftSolenoid; + DoubleSolenoid* clampSolenoid; + XboxController* controller; + DigitalInput* banner; + void teleopGearLoop(); + void teleopGearLoopMod(); + bool isliftSolenoid = true; + bool isclampSolenoid = true; + bool yButtonPressed = false; + bool xButtonPressed = false; + void teleopGearLoopMod2(); + +}; + +#endif /* SRC_SYSTEMS_GEARPICKUP_H_ */ diff --git a/src/systems/Lifter.cpp b/src/systems/Lifter.cpp new file mode 100644 index 0000000..48602a9 --- /dev/null +++ b/src/systems/Lifter.cpp @@ -0,0 +1,44 @@ +/* + * Lifter.cpp + * + * Created on: Feb 17, 2017 + * Author: Anish + */ + +#include + +Lifter::Lifter(XboxController* controller) { + this->controller = controller; + leftInRoller = new CANTalon(leftInRollerID); + rightInRoller = new CANTalon(rightInRollerID); + liftTalon = new CANTalon(lifterTalonID); +} + +Lifter::~Lifter() { + delete controller; + delete leftInRoller; + delete rightInRoller; + delete liftTalon; +} + +void Lifter::TeleopLoop(){ + if(controller->GetAButton()&& !controller->GetBButton()){ + leftInRoller->Set(RollerSpeed); + rightInRoller->Set(RollerSpeed); + liftTalon->Set(LiftSpeed); + } + else if(controller->GetBButton() && !controller->GetAButton()){ + leftInRoller->Set(-1*RollerSpeed); + rightInRoller->Set(-1*RollerSpeed); + liftTalon->Set(-1*LiftSpeed); + } + else{ + leftInRoller->Set(0); + rightInRoller->Set(0); + liftTalon->Set(0); + } +} + + + + diff --git a/src/systems/Lifter.h b/src/systems/Lifter.h new file mode 100644 index 0000000..5182ba2 --- /dev/null +++ b/src/systems/Lifter.h @@ -0,0 +1,29 @@ +/* + * Lifter.h + * + * Created on: Feb 17, 2017 + * Author: Anish + */ + +#ifndef SRC_SYSTEMS_LIFTER_H_ +#define SRC_SYSTEMS_LIFTER_H_ + +#include "WPILib.h" +#include "CANTalon.h" +#include "teleop/controls/Deadband.h" +#include "util/DeviceIDs.h" +#include "systems/Constants.h" +using namespace frc; + +class Lifter { +public: + Lifter(XboxController* controller); + virtual ~Lifter(); + void TeleopLoop(); + CANTalon* leftInRoller; + CANTalon* rightInRoller; + CANTalon* liftTalon; + XboxController* controller; +}; + +#endif /* SRC_SYSTEMS_LIFTER_H_ */ diff --git a/src/teleop/controls/Deadband.cpp b/src/teleop/controls/Deadband.cpp new file mode 100644 index 0000000..3c9dc71 --- /dev/null +++ b/src/teleop/controls/Deadband.cpp @@ -0,0 +1,20 @@ +/* + * Deadband.cpp + * + * Created on: Feb 11, 2017 + * Author: Anish + */ + +#include "Deadband.h" + +Deadband::Deadband(double LowBound, double HighBound){ + this->LowBound = LowBound; + this->HighBound = HighBound; +} +double Deadband::ReturnBoundedValue(double value){ + if(value<=LowBound || value>=HighBound){ + return value; + }else{ + return 0; + } +} diff --git a/src/teleop/controls/Deadband.h b/src/teleop/controls/Deadband.h new file mode 100644 index 0000000..5024a5c --- /dev/null +++ b/src/teleop/controls/Deadband.h @@ -0,0 +1,21 @@ +/* + * Deadband.h + * + * Created on: Feb 11, 2017 + * Author: Anish + */ + +#ifndef SRC_TELEOP_CONTROLS_DEADBAND_H_ +#define SRC_TELEOP_CONTROLS_DEADBAND_H_ + +class Deadband { +public: + Deadband(double LowBound, double HighBound); + double ReturnBoundedValue(double value); + double LowBound; + double HighBound; + + +}; + +#endif /* SRC_TELEOP_CONTROLS_DEADBAND_H_ */ diff --git a/src/util/DeviceIDs.h b/src/util/DeviceIDs.h index 803d5dd..e111bf9 100644 --- a/src/util/DeviceIDs.h +++ b/src/util/DeviceIDs.h @@ -1,6 +1,44 @@ +#ifndef IDS +#define IDS +enum DriveID{ + flTalID = 1, + frTalID = 4, + rlTalID = 2, + rrTalID = 5, + mainControllerID = 0, +}; - int flTalID = 1; - int frTalID = 3; - int rlTalID = 2; - int rrTalID = 4; - int mainControllerID = 0; +enum SensorID{ + flEncID1, + flEncID2, + frEncID1, + frEncID2, + gyroID, + BannerID + +}; + +enum PnuematicIDS{ + liftSolenoidID1=0, + liftSolenoidID2=1, + clampSolenoidID1=2, + clampSolenoidID2=3, + PCM_ID=1 +}; + +enum BallPickup{ + innerRollerID=9, + outerRollerID=8, + backDoorID1 = 4, + backDoorID2 =5 +}; + +enum LifterID{ + leftInRollerID=7, + rightInRollerID=6, + lifterTalonID=3 + +}; + + +#endif