diff --git a/Commands/Elevator/Lower.cpp b/Commands/Elevator/Lower.cpp index d853678..8d75a74 100644 --- a/Commands/Elevator/Lower.cpp +++ b/Commands/Elevator/Lower.cpp @@ -1,14 +1,20 @@ #include "Lower.h" #include "../../DentRobot.h" +#include "../../OI.h" Lower::Lower() : Command("Lower"){ } void Lower::Initialize(){ + SetTimeout(2.0); } void Lower::Execute(){ DentRobot::elevator->Run((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2); } bool Lower::IsFinished(){ - return !DentRobot::dio->Get(DentRobot::dio->ELEVATORBOTTOM); + if (!DentRobot::dio->Get(DentRobot::dio->ELEVATORBOTTOM) || IsTimedOut()){ + return true; + }else{ + return false; + } } void Lower::End(){ DentRobot::elevator->Run(0.0f); diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp index 8d556c5..35372ea 100644 --- a/Commands/Elevator/Raise.cpp +++ b/Commands/Elevator/Raise.cpp @@ -1,15 +1,20 @@ #include "Raise.h" #include "../../DentRobot.h" #include "../../OI.h" -Raise::Raise(){ +Raise::Raise() : Command("Raise"){ } void Raise::Initialize(){ + SetTimeout(2.0); } void Raise::Execute(){ DentRobot::elevator->Run(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2); } bool Raise::IsFinished(){ - return !DentRobot::dio->Get(DentRobot::dio->ELEVATORTOP); + if (!DentRobot::dio->Get(DentRobot::dio->ELEVATORBOTTOM) || IsTimedOut()){ + return true; + }else{ + return false; + } } void Raise::End(){ DentRobot::elevator->Run(0.0f); diff --git a/DentRobot.cpp b/DentRobot.cpp index 0d9e4ab..819efba 100644 --- a/DentRobot.cpp +++ b/DentRobot.cpp @@ -15,6 +15,7 @@ DentRobot::DentRobot(){ printf("Initialized"); } void DentRobot::RobotInit(){ + SmartDashboard::PutNumber("CodeVersion",0.001); } void DentRobot::DisabledPeriodic(){ Scheduler::GetInstance()->Run(); diff --git a/Subsystems/DIO.cpp b/Subsystems/DIO.cpp index b845c3a..470bf3a 100644 --- a/Subsystems/DIO.cpp +++ b/Subsystems/DIO.cpp @@ -9,11 +9,9 @@ void DIO::InitDefaultCommand(){ bool DIO::Get(e_dioSig dioSig){ switch (dioSig){ case ELEVATORTOP: - printf("Hit top limit switch\n"); return elevatorTop->Get(); break; case ELEVATORBOTTOM: - printf("Hit top bottom switch\n"); return elevatorBottom->Get(); break; default: