2
0
mirror of https://github.com/team2059/Dent synced 2024-12-18 20:52:29 -05:00
dent/Commands/Elevator/Raise.cpp

39 lines
1.1 KiB
C++
Raw Normal View History

2015-01-16 19:29:55 -05:00
#include "Raise.h"
#include "../../DentRobot.h"
#include "../../OI.h"
2015-03-09 07:25:11 -04:00
Raise::Raise(double timeout): Command("Raise"){
2015-03-07 13:27:11 -05:00
SetTimeout(timeout);
2015-01-16 19:29:55 -05:00
}
void Raise::Initialize(){
}
void Raise::Execute(){
2015-02-07 17:04:16 -05:00
DentRobot::elevator->Run(1.0);
2015-01-16 19:29:55 -05:00
}
bool Raise::IsFinished(){
//if(!DentRobot::elevator->GetElevatorMiddle()){
// DentRobot::elevator->stoppedAtSensor=true;
//}
2015-03-09 07:25:11 -04:00
//if((DentRobot::elevator->stoppedAtSensor)){
// printf("Stopped at the middle sensor\n");
// DentRobot::elevator->stoppedAtSensor=false;
// return true;
2015-03-09 07:25:11 -04:00
//}else if(!DentRobot::elevator->GetElevatorTop()){
if(!DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle()||IsTimedOut()){
2015-02-15 17:10:20 -05:00
printf("Robot stopped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop()||!DentRobot::elevator->GetElevatorMiddle());
2015-02-07 12:50:36 -05:00
return true;
}else{
2015-02-07 12:50:36 -05:00
return false;
}
2015-01-16 19:29:55 -05:00
}
void Raise::End(){
// If the elevator is at the top
if(DentRobot::elevator->GetElevatorTop()){
DentRobot::elevator->SetUseEncoder(true);
}
DentRobot::elevator->Run(0.0f);
2015-01-16 19:29:55 -05:00
}
void Raise::Interrupted(){
End();
2015-01-16 19:29:55 -05:00
}
2015-02-08 12:26:15 -05:00
// vim: ts=2:sw=2:et