diff --git a/CommandBase.cpp b/CommandBase.cpp index b98ffac..c4defc3 100644 --- a/CommandBase.cpp +++ b/CommandBase.cpp @@ -2,9 +2,11 @@ #include "Subsystems/Drivetrain.h" #include "Subsystems/Collector.h" #include "Subsystems/Elevator.h" +#include "Subsystems/BinElevator.h" Drivetrain* CommandBase::drivetrain = NULL; Collector* CommandBase::collector = NULL; Elevator* CommandBase::elevator = NULL; +BinElevator* CommandBase::binElevator = NULL; OI* CommandBase::oi = NULL; CommandBase::CommandBase(char const *name) : Command(name) { } @@ -14,6 +16,7 @@ void CommandBase::init(){ drivetrain = new Drivetrain(); collector = new Collector(); elevator = new Elevator(); + binElevator = new BinElevator(); oi = new OI(); } // vim: ts=2:sw=2:et diff --git a/CommandBase.h b/CommandBase.h index ca66699..7443ebd 100644 --- a/CommandBase.h +++ b/CommandBase.h @@ -5,6 +5,7 @@ #include "Subsystems/Drivetrain.h" #include "Subsystems/Collector.h" #include "Subsystems/Elevator.h" +#include "Subsystems/BinElevator.h" #include "OI.h" #include "WPILib.h" @@ -16,6 +17,7 @@ class CommandBase: public Command { static Drivetrain *drivetrain; static Collector *collector; static Elevator *elevator; + static BinElevator *binElevator; static OI *oi; }; #endif diff --git a/Commands/BinElevator/BinLower.cpp b/Commands/BinElevator/BinLower.cpp new file mode 100644 index 0000000..158f98a --- /dev/null +++ b/Commands/BinElevator/BinLower.cpp @@ -0,0 +1,26 @@ +#include "BinLower.h" +#include "../../DentRobot.h" +#include "../../OI.h" +BinLower::BinLower() : Command("BinLower"){ +} +void BinLower::Initialize(){ + SetTimeout(2.5); +} +void BinLower::Execute(){ + DentRobot::binElevator->Run(-1.0); +} +bool BinLower::IsFinished(){ + if (!DentRobot::binElevator->GetElevatorBottom()||IsTimedOut()){ + printf("Robot stoped BinLowering. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorBottom()); + return true; + }else{ + return false; + } +} +void BinLower::End(){ + DentRobot::binElevator->Run(0.0f); +} +void BinLower::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/Elevator/Calibrate.h b/Commands/BinElevator/BinLower.h similarity index 68% rename from Commands/Elevator/Calibrate.h rename to Commands/BinElevator/BinLower.h index d2bb7b2..a3504b4 100644 --- a/Commands/Elevator/Calibrate.h +++ b/Commands/BinElevator/BinLower.h @@ -1,12 +1,12 @@ -#ifndef CALIBRATE_H -#define CALIBRATE_H +#ifndef BINLOWER_H +#define BINLOWER_H #include "Commands/Command.h" #include "WPILib.h" -class Calibrate: public Command{ +class BinLower: public Command{ public: - Calibrate(); + BinLower(); void Initialize(); void Execute(); bool IsFinished(); diff --git a/Commands/BinElevator/BinRaise.cpp b/Commands/BinElevator/BinRaise.cpp new file mode 100644 index 0000000..aa765a4 --- /dev/null +++ b/Commands/BinElevator/BinRaise.cpp @@ -0,0 +1,26 @@ +#include "BinRaise.h" +#include "../../DentRobot.h" +#include "../../OI.h" +BinRaise::BinRaise() : Command("BinRaise"){ +} +void BinRaise::Initialize(){ + SetTimeout(3.0); +} +void BinRaise::Execute(){ + DentRobot::binElevator->Run(1.0); +} +bool BinRaise::IsFinished(){ + if (!DentRobot::binElevator->GetElevatorTop()||IsTimedOut()){ + printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorTop()); + return true; + }else{ + return false; + } +} +void BinRaise::End(){ + DentRobot::binElevator->Run(0.0f); +} +void BinRaise::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/BinElevator/BinRaise.h b/Commands/BinElevator/BinRaise.h new file mode 100644 index 0000000..ea73454 --- /dev/null +++ b/Commands/BinElevator/BinRaise.h @@ -0,0 +1,18 @@ +#ifndef BINRAISE_H +#define BINRAISE_H + +#include "Commands/Command.h" +#include "WPILib.h" + +class BinRaise: public Command{ + public: + BinRaise(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; + +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/Elevator/Calibrate.cpp b/Commands/Elevator/Calibrate.cpp deleted file mode 100644 index a55c8c4..0000000 --- a/Commands/Elevator/Calibrate.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "Calibrate.h" -#include "../../DentRobot.h" -// Lowers elevator until it hits the limit switch then sets the height of the elevator to the height of the limit switches -Calibrate::Calibrate() : Command("Calibrate"){ -} -void Calibrate::Initialize(){ -} -void Calibrate::Execute(){ - // Lower collector until End() - DentRobot::elevator->Run(-0.4f); -} -bool Calibrate::IsFinished(){ - if(DentRobot::elevator->GetElevatorBottom()){ - DentRobot::elevator->ResetEncoder(); - DentRobot::elevator->SetOffset(0.99); - return true; - } - return false; -} -void Calibrate::End(){ - DentRobot::elevator->Run(0.0f); -} -void Calibrate::Interrupted(){ - End(); -} -// vim: ts=2:sw=2:et diff --git a/DentRobot.cpp b/DentRobot.cpp index d0399a6..4588bf6 100644 --- a/DentRobot.cpp +++ b/DentRobot.cpp @@ -4,12 +4,14 @@ OI* DentRobot::oi=NULL; Collector* DentRobot::collector=NULL; Drivetrain* DentRobot::drivetrain=NULL; Elevator* DentRobot::elevator=NULL; +BinElevator* DentRobot::binElevator=NULL; CommandGroup* DentRobot::aut=NULL; DentRobot::DentRobot(){ oi=new OI(); collector=new Collector(); drivetrain=new Drivetrain(); elevator=new Elevator(); + binElevator=new BinElevator(); aut=new Autonomous(); printf("Initialized"); } diff --git a/DentRobot.h b/DentRobot.h index e089405..68b17ba 100644 --- a/DentRobot.h +++ b/DentRobot.h @@ -3,6 +3,7 @@ #include "WPILib.h" #include "OI.h" #include "Subsystems/Elevator.h" +#include "Subsystems/BinElevator.h" #include "Subsystems/Drivetrain.h" #include "Subsystems/Collector.h" #include "Commands/Autonomous/Autonomous.h" @@ -15,6 +16,7 @@ class DentRobot: public IterativeRobot { static Collector* collector; static Drivetrain* drivetrain; static Elevator* elevator; + static BinElevator* binElevator; static CommandGroup* aut; void RobotInit(); void DisabledPeriodic(); diff --git a/Subsystems/BinElevator.cpp b/Subsystems/BinElevator.cpp new file mode 100644 index 0000000..2ada4b9 --- /dev/null +++ b/Subsystems/BinElevator.cpp @@ -0,0 +1,32 @@ +#include "BinElevator.h" +#include "../RobotMap.h" +BinElevator::BinElevator(){ + motor=new CANTalon(ELEVATOR_CAN); + elevatorEncoder=new Encoder(0,1,false); + offset=0; + height=0; + elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO); + elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO); + //SetAbsoluteTolerance(0.004); +} +void BinElevator::InitDefaultCommand(){ +} +void BinElevator::Run(double power){ + motor->Set(power); +} +void BinElevator::SetOffset(double ht){ + offset=ht; +} +void BinElevator::ResetEncoder(){ + elevatorEncoder->Reset(); +} +double BinElevator::GetHeight(){ + return elevatorEncoder->Get()+offset; +} +bool BinElevator::GetElevatorBottom(){ + return elevatorBottom->Get(); +} +bool BinElevator::GetElevatorTop(){ + return elevatorTop->Get(); +} +// vim: ts=2:sw=2:et diff --git a/Subsystems/BinElevator.h b/Subsystems/BinElevator.h new file mode 100644 index 0000000..5e9e72d --- /dev/null +++ b/Subsystems/BinElevator.h @@ -0,0 +1,24 @@ +#ifndef BINELEVATOR_H +#define BINELEVATOR_H + +#include "WPILib.h" +#include "Commands/PIDSubsystem.h" +class BinElevator{ + private: + CANTalon *motor; + Encoder *elevatorEncoder; + static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2; + double offset, height; + DigitalInput *elevatorBottom, *elevatorTop; + public: + BinElevator(); + void InitDefaultCommand(); + void Run(double); + void SetOffset(double); + void ResetEncoder(); + double GetHeight(); + bool GetElevatorTop(); + bool GetElevatorBottom(); +}; +#endif +// vim: ts=2:sw=2:et