Merged with feature/command-based

.env

@@ -2,14 +2,14 @@ function mb(){
   vagrant ssh -c "cd /vagrant/src;make clean 2>&1 >/dev/null;make"
 }
 function mc(){
-  vagrant ssh -c "cd /vagrant/src;make clean"
+  vagrant ssh -c "cd /vagrant/src;make clean 2>&1 >/dev/null"
 }
 function mk(){
   vagrant ssh -c "cd /vagrant/src;make"
 }
-function md(){
+function me(){
   vagrant ssh -c "cd /vagrant/src;make deploy"
 }
 function ma(){
-  vagrant ssh -c "cd /vagrant/src;make clean;make;make deploy"
+  vagrant ssh -c "cd /vagrant/src;make clean 2>&1 >/dev/null;make;make deploy"
 }

CommandBase.cpp

@@ -2,13 +2,9 @@
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
-#include "Subsystems/DIO.h"
-#include "Subsystems/AirCompressor.h"
 Drivetrain* CommandBase::drivetrain = NULL;
 Collector* CommandBase::collector = NULL;
 Elevator* CommandBase::elevator = NULL;
-DIO* CommandBase::dio = NULL;
-AirCompressor* CommandBase::airCompressor = NULL;
 OI* CommandBase::oi = NULL;
 CommandBase::CommandBase(char const *name) : Command(name) {
 }
@@ -18,7 +14,6 @@ void CommandBase::init(){
   drivetrain = new Drivetrain();
   collector = new Collector();
   elevator = new Elevator();
-  dio = new DIO();
-  airCompressor = new AirCompressor();
   oi = new OI();
 }
+// vim: ts2:sw=2:et

CommandBase.h

@@ -5,8 +5,6 @@
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
-#include "Subsystems/DIO.h"
-#include "Subsystems/AirCompressor.h"
 #include "OI.h"
 #include "WPILib.h"
 
@@ -18,8 +16,7 @@ class CommandBase: public Command {
     static Drivetrain *drivetrain;
     static Collector *collector;
     static Elevator *elevator;
-    static DIO* dio;
-    static AirCompressor *airCompressor;
     static OI *oi;
 };
 #endif
+// vim: ts2:sw=2:et

Commands/Autonomous/AutoDrive.cpp

@@ -13,10 +13,11 @@ void AutoDrive::Execute(){
   DentRobot::drivetrain->DriveMecanum(0.5,0,0,0.9,0);
 }
 bool AutoDrive::IsFinished(){
-  return false;
+  return IsTimedOut();
 }
 void AutoDrive::End(){
 }
 void AutoDrive::Interrupted(){
   End();
 }
+// vim: ts2:sw=2:et

Commands/Autonomous/AutoDrive.h

@@ -16,3 +16,4 @@ class AutoDrive: public Command{
     void Interrupted();
 };
 #endif
+// vim: ts2:sw=2:et

Commands/Autonomous/Autonomous.cpp

@@ -1,7 +1,11 @@
 #include "Autonomous.h"
 #include "Commands/CommandGroup.h"
 #include "../../DentRobot.h"
-#include ""
+#include "AutoDrive.h"
+#include "../../DentRobot.h"
+#include "../Elevator/Raise.h"
 Autonomous::Autonomous(){
   AddSequential(new AutoDrive());
+  AddSequential(new Raise());
 }
+// vim: ts2:sw=2:et

Commands/Autonomous/Autonomous.h

@@ -1,7 +1,7 @@
 #ifndef AUTONOMOUS_H
 #define AUTONOMOUS_H
 
-#include "Commands/Command.h"
+#include "Commands/CommandGroup.h"
 #include "../../CommandBase.h"
 #include "../../DentRobot.h"
 #include "WPILib.h"
@@ -9,10 +9,6 @@
 class Autonomous: public CommandGroup{
   public:
     Autonomous();
-    void Initialize();
-    void Execute();
-    bool IsFinished();
-    void End();
-    void Interrupted();
 };
 #endif
+// vim: ts2:sw=2:et

Commands/Collector/CloseCollector.cpp

@@ -17,3 +17,4 @@ void CloseCollector::End(){
 void CloseCollector::Interrupted(){
   End();
 }
+// vim: ts2:sw=2:et

Commands/Collector/CloseCollector.h

@@ -17,3 +17,4 @@ class CloseCollector: public Command{
 };
 
 #endif
+// vim: ts2:sw=2:et

Commands/Collector/CollectTote.cpp

@@ -7,7 +7,6 @@ void CollectTote::Initialize(){
 }
 void CollectTote::Execute(){
   //TODO check this value to move the motors in the right direction
-  printf("collecting tote\n");
   DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
 }
 bool CollectTote::IsFinished(){
@@ -19,3 +18,4 @@ void CollectTote::End(){
 void CollectTote::Interrupted(){
   End();
 }
+// vim: ts2:sw=2:et

Commands/Collector/CollectTote.h

@@ -17,3 +17,4 @@ class CollectTote: public Command{
 };
 
 #endif
+// vim: ts2:sw=2:et

Commands/Collector/OpenCollector.cpp

@@ -18,3 +18,4 @@ void OpenCollector::End(){
 void OpenCollector::Interrupted(){
   End();
 }
+// vim: ts2:sw=2:et

Commands/Collector/OpenCollector.h

@@ -17,3 +17,4 @@ class OpenCollector: public Command{
 };
 
 #endif
+// vim: ts2:sw=2:et

Commands/Collector/ReleaseTote.cpp

@@ -7,7 +7,6 @@ void ReleaseTote::Initialize(){
 }
 void ReleaseTote::Execute(){
   //TODO check this value to move the motors in the right direction
-  printf("releasing tote\n");
   // Devide by 2 twice because this speed should be half the collector speed
   DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2/2);
 }
@@ -20,3 +19,4 @@ void ReleaseTote::End(){
 void ReleaseTote::Interrupted(){
   End();
 }
+// vim: ts2:sw=2:et

Commands/Collector/ReleaseTote.h

@@ -17,3 +17,4 @@ class ReleaseTote: public Command{
 };
 
 #endif
+// vim: ts2:sw=2:et

Commands/Compressor/StartCompressing.cpp

@@ -1,18 +0,0 @@
-#include "StartCompressing.h"
-#include "../../DentRobot.h"
-StartCompressing::StartCompressing() : Command("StartCompressing"){
-  Requires(DentRobot::airCompressor);
-}
-void StartCompressing::Initialize(){
-}
-void StartCompressing::Execute(){
-  DentRobot::airCompressor->StartCompressing();
-}
-bool StartCompressing::IsFinished(){
-  return false;
-}
-void StartCompressing::End(){
-}
-void StartCompressing::Interrupted(){
-  End();
-}

Commands/Compressor/StartCompressing.h

@@ -1,18 +0,0 @@
-#ifndef STARTCOMPRESSING_H
-#define STARTCOMPRESSING_H
-
-#include "../../CommandBase.h"
-#include "../../DentRobot.h"
-#include "Commands/Command.h"
-#include "WPILib.h"
-
-class StartCompressing: public Command{
-  public:
-    StartCompressing();
-    void Initialize();
-    void Execute();
-    bool IsFinished();
-    void End();
-    void Interrupted();
-};
-#endif

Commands/Compressor/StopCompressing.cpp

@@ -1,18 +0,0 @@
-#include "StopCompressing.h"
-#include "../../DentRobot.h"
-StopCompressing::StopCompressing() : Command("StopCompressing"){
-  Requires(DentRobot::airCompressor);
-}
-void StopCompressing::Initialize(){
-}
-void StopCompressing::Execute(){
-  DentRobot::airCompressor->StopCompressing();
-}
-bool StopCompressing::IsFinished(){
-  return false;
-}
-void StopCompressing::End(){
-}
-void StopCompressing::Interrupted(){
-  End();
-}

Commands/Compressor/StopCompressing.h

@@ -1,18 +0,0 @@
-#ifndef STOPCOMPRESSING_H
-#define STOPCOMPRESSING_H
-
-#include "../../CommandBase.h"
-#include "../../DentRobot.h"
-#include "Commands/Command.h"
-#include "WPILib.h"
-
-class StopCompressing: public Command{
-  public:
-    StopCompressing();
-    void Initialize();
-    void Execute();
-    bool IsFinished();
-    void End();
-    void Interrupted();
-};
-#endif

Commands/Drivetrain/Drive.cpp

@@ -18,10 +18,6 @@ void Drive::Execute(){
   if (DentRobot::oi->GetLeftStick()->GetRawButton(2)){
     y=0;
   }
-  if (DentRobot::oi->GetLeftStick()->GetRawAxis(3)<=0.5){
-    y /= 2;
-    z /= 2;
-  }
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
   DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
 }
@@ -33,3 +29,4 @@ void Drive::End(){
 void Drive::Interrupted(){
   End();
 }
+// vim: ts2:sw=2:et

Commands/Drivetrain/Drive.h

@@ -16,3 +16,4 @@ class Drive: public Command{
     void Interrupted();
 };
 #endif
+// vim: ts2:sw=2:et

Commands/Elevator/Calibrate.cpp

@@ -10,7 +10,7 @@ void Calibrate::Execute(){
   DentRobot::elevator->Run(-0.4f);
 }
 bool Calibrate::IsFinished(){
-  if(DentRobot::dio->Get(DIO::ELEVATORBOTTOM)){
+  if(DentRobot::elevator->GetElevatorBottom()){
     DentRobot::elevator->ResetEncoder();
     DentRobot::elevator->SetOffset(0.99);
     return true;
@@ -23,3 +23,4 @@ void Calibrate::End(){
 void Calibrate::Interrupted(){
   End();
 }
+// vim: ts2:sw=2:et

Commands/Elevator/Calibrate.h

@@ -14,3 +14,4 @@ class Calibrate: public Command{
     void Interrupted();
 };
 #endif
+// vim: ts2:sw=2:et

Commands/Elevator/Lower.cpp

@@ -4,13 +4,14 @@
 Lower::Lower() : Command("Lower"){
 }
 void Lower::Initialize(){
-    SetTimeout(2.0);
+  SetTimeout(1.0);
 }
 void Lower::Execute(){
-  DentRobot::elevator->Run((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
+  DentRobot::elevator->Run(-1.0);
 }
 bool Lower::IsFinished(){
-  if (!DentRobot::dio->Get(DentRobot::dio->ELEVATORBOTTOM) || IsTimedOut()){
+  if (!DentRobot::elevator->GetElevatorBottom()||IsTimedOut()){
+    printf("Robot stoped lowering. Sensor based? %d\n", !DentRobot::elevator->GetElevatorBottom());
     return true;
   }else{
     return false;
@@ -22,3 +23,4 @@ void Lower::End(){
 void Lower::Interrupted(){
   End();
 }
+// vim: ts2:sw=2:et

Commands/Elevator/Lower.h

@@ -14,3 +14,4 @@ class Lower: public Command{
     void Interrupted();
 };
 #endif
+// vim: ts2:sw=2:et

Commands/Elevator/Raise.cpp

@@ -4,13 +4,14 @@
 Raise::Raise() : Command("Raise"){
 }
 void Raise::Initialize(){
-    SetTimeout(2.0);
+  SetTimeout(2.5);
 }
 void Raise::Execute(){
-  DentRobot::elevator->Run(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2);
+  DentRobot::elevator->Run(1.0);
 }
 bool Raise::IsFinished(){
-  if (!DentRobot::dio->Get(DentRobot::dio->ELEVATORBOTTOM) || IsTimedOut()){
+  if (!DentRobot::elevator->GetElevatorTop()||IsTimedOut()){
+    printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop());
     return true;
   }else{
     return false;
@@ -22,3 +23,4 @@ void Raise::End(){
 void Raise::Interrupted(){
   End();
 }
+// vim: ts2:sw=2:et

Commands/Elevator/Raise.h

@@ -15,3 +15,4 @@ class Raise: public Command{
 };
 
 #endif
+// vim: ts2:sw=2:et

Commands/Test/Check.h

@@ -1,20 +0,0 @@
-#ifndef CHECK_H
-#define CHECK_H
-
-#include "Commands/Command.h"
-#include "../../CommandBase.h"
-#include "../../DentRobot.h"
-#include "WPILib.h"
-
-class Check: public CommandGroup{
-  private:
-    int motor;
-  public:
-    Check();
-    void Initialize();
-    void Execute();
-    bool IsFinished();
-    void End();
-    void Interrupted();
-};
-#endif

Commands/Test/CheckRobot.cpp

@@ -1,11 +1,13 @@
-#include "Check.h"
 #include "Commands/CommandGroup.h"
 #include "../../DentRobot.h"
 #include "../../RobotMap.h"
+#include "../Elevator/Raise.h"
+#include "CheckRobot.h"
 #include "CheckDrive.h"
-Check::Check(){
+CheckRobot::CheckRobot(){
   AddSequential(new CheckDrive(DRIVE_FRONT_LEFT_CAN));
   AddSequential(new CheckDrive(DRIVE_FRONT_RIGHT_CAN));
   AddSequential(new CheckDrive(DRIVE_BACK_LEFT_CAN));
   AddSequential(new CheckDrive(DRIVE_BACK_RIGHT_CAN));
 }
+// vim: ts2:sw=2:et

Commands/Test/CheckRobot.h

@@ -0,0 +1,14 @@
+#ifndef CHECKROBOT_H
+#define CHECKROBOT_H
+
+#include "Commands/CommandGroup.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class CheckRobot: public CommandGroup{
+  public:
+    CheckRobot();
+};
+#endif
+// vim: ts2:sw=2:et

DentRobot.cpp

@@ -1,17 +1,16 @@
 #include "DentRobot.h"
+#include "Commands/Autonomous/Autonomous.h"
 OI* DentRobot::oi=NULL;
 Collector* DentRobot::collector=NULL;
 Drivetrain* DentRobot::drivetrain=NULL;
 Elevator* DentRobot::elevator=NULL;
-DIO* DentRobot::dio = NULL;
+CommandGroup* DentRobot::aut=NULL;
-AirCompressor * DentRobot::airCompressor=NULL;
 DentRobot::DentRobot(){
   oi=new OI();
   collector=new Collector();
   drivetrain=new Drivetrain();
   elevator=new Elevator();
-  dio = new DIO();
+  aut=new Autonomous();
-  airCompressor=new AirCompressor();
   printf("Initialized");
 }
 void DentRobot::RobotInit(){
@@ -21,11 +20,17 @@ void DentRobot::DisabledPeriodic(){
   Scheduler::GetInstance()->Run();
 }
 void DentRobot::AutonomousInit(){
+  if(aut != NULL){
+    aut->Start();
+  }
 }
 void DentRobot::AutonomousPeriodic(){
   Scheduler::GetInstance()->Run();
 }
 void DentRobot::TeleopInit(){
+  //if (aut != NULL){
+  //  aut->Cancel();
+  //}
 }
 void DentRobot::TeleopPeriodic(){
   Scheduler::GetInstance()->Run();
@@ -33,3 +38,4 @@ void DentRobot::TeleopPeriodic(){
 void DentRobot::TestPeriodic(){
 }
 START_ROBOT_CLASS(DentRobot);
+// vim: ts2:sw=2:et

DentRobot.h

@@ -3,21 +3,19 @@
 #include "WPILib.h"
 #include "OI.h"
 #include "Subsystems/Elevator.h"
-#include "Subsystems/DIO.h"
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
-#include "Subsystems/AirCompressor.h"
+#include "Commands/Autonomous/Autonomous.h"
 class DentRobot: public IterativeRobot {
   private:
     Command *driveCommand = NULL;
   public:
+    DentRobot();
     static OI* oi;
     static Collector* collector;
     static Drivetrain* drivetrain;
     static Elevator* elevator;
-    static DIO* dio;
+    static CommandGroup* aut;
-    static AirCompressor* airCompressor;
-    DentRobot();
     void RobotInit();
     void DisabledPeriodic();
     void AutonomousInit();

OI.cpp

@@ -5,24 +5,44 @@
 #include "Commands/Collector/CloseCollector.h"
 #include "Commands/Collector/CollectTote.h"
 #include "Commands/Collector/ReleaseTote.h"
-#include "Commands/Compressor/StartCompressing.h"
+#include "Commands/Test/CheckRobot.h"
 
 OI::OI() {
+  // Joysticks
   leftStick=new Joystick(0);
   rightStick=new Joystick(1);
-  //TODO name these buttons to their functions rather to their number
+  // Collector
-  JoystickButton *left1=new JoystickButton(leftStick, 1);
+  JoystickButton *right1=new JoystickButton(rightStick, 1);
-  JoystickButton *left2=new JoystickButton(leftStick, 2);
+  JoystickButton *right2=new JoystickButton(rightStick, 2);
   JoystickButton *left3=new JoystickButton(leftStick, 3);
   JoystickButton *left4=new JoystickButton(leftStick, 4);
-  JoystickButton *left5=new JoystickButton(leftStick, 5);
+  right1->WhileHeld(new OpenCollector());
-  JoystickButton *left6=new JoystickButton(leftStick, 6);
+  right2->WhileHeld(new CloseCollector());
-  left1->WhileHeld(new OpenCollector());
-  left2->WhileHeld(new CloseCollector());
   left3->WhileHeld(new CollectTote());
   left4->WhileHeld(new ReleaseTote());
-  left5->WhenPressed(new Raise());
+  // Elevator
-  left6->WhenPressed(new Lower());
+  Raise* raise=new Raise();
+  Lower* lower=new Lower();
+  JoystickButton *right3=new JoystickButton(rightStick, 3);
+  JoystickButton *right4=new JoystickButton(rightStick, 4);
+  JoystickButton *right5=new JoystickButton(rightStick, 5);
+  JoystickButton *right6=new JoystickButton(rightStick, 6);
+  right3->WhenPressed(lower);
+  right4->WhenPressed(lower);
+  right3->CancelWhenPressed(raise);
+  right4->CancelWhenPressed(raise);
+  right5->WhenPressed(raise);
+  right6->WhenPressed(raise);
+  right5->CancelWhenPressed(lower);
+  right6->CancelWhenPressed(lower);
+  // Cancel
+  JoystickButton *right8=new JoystickButton(rightStick, 8);
+  right8->CancelWhenPressed(raise);
+  right8->CancelWhenPressed(lower);
+  // Basic motor test
+  CheckRobot* checkRobot=new CheckRobot();
+  JoystickButton *left7=new JoystickButton(leftStick, 7);
+  left7->WhenPressed(checkRobot);
 }
 Joystick* OI::GetRightStick(){
   return rightStick;
@@ -30,3 +50,4 @@ Joystick* OI::GetRightStick(){
 Joystick* OI::GetLeftStick(){
   return leftStick;
 }
+// vim: ts2:sw=2:et

OI.h

@@ -13,3 +13,4 @@ class OI
     Joystick* GetLeftStick();
 };
 #endif
+// vim: ts2:sw=2:et

RobotMap.h

@@ -5,6 +5,12 @@
 
 // Elevator
 #define ELEVATOR_CAN 1
+#define ELEVATOR_BOTTOM_DIO 0
+#define ELEVATOR_COLELCT_TOTE_DIO 1
+#define ELEVATOR_READY_TOTE_DIO 2
+#define ELEVATOR_COLELCT_CAN_DIO 3
+#define ELEVATOR_READY_CAN_DIO 4
+#define ELEVATOR_TOP_DIO 5
 
 // Drivetrain
 #define DRIVE_FRONT_LEFT_CAN 2
@@ -17,9 +23,6 @@
 #define COLLECTOR_WINDOW_RIGHT_CAN 7
 #define COLLECTOR_LEFT_CAN 8
 #define COLLECTOR_RIGHT_CAN 9
-#define COLLECTOR_CALIBRATOR_DIO 0
-
-// Compressor
-#define COMPRESSOR_CAN 10
 
 #endif
+// vim: ts2:sw=2:et

Subsystems/AirCompressor.cpp

@@ -1,20 +0,0 @@
-#include "AirCompressor.h"
-#include "../RobotMap.h"
-
-AirCompressor::AirCompressor() : Subsystem("AirCompressor") {
-  compressor = new Compressor(COMPRESSOR_CAN);
-}
-void AirCompressor::InitDefaultCommand() {
-}
-void AirCompressor::StartCompressing() {
-  if(compressor->Enabled()){
-    printf("Starting compressor\n");
-    compressor->Start();
-  }
-}
-void AirCompressor::StopCompressing() {
-  if(compressor->Enabled()){
-    printf("Stopping compressor\n");
-    compressor->Stop();
-  }
-}

Subsystems/AirCompressor.h

@@ -1,15 +0,0 @@
-#ifndef AIRCOMPRESSOR_H
-#define AIRCOMPRESSOR_H
-
-#include "WPILib.h"
-class AirCompressor: public Subsystem
-{
-  private:
-    Compressor *compressor;
-  public:
-    AirCompressor();
-    void InitDefaultCommand();
-    void StartCompressing();
-    void StopCompressing();
-};
-#endif

Subsystems/Collector.cpp

@@ -6,7 +6,6 @@ Collector::Collector() : Subsystem("Collector") {
   windowMotorRight=new CANTalon(COLLECTOR_WINDOW_RIGHT_CAN);
   collectorMotorLeft=new CANTalon(COLLECTOR_LEFT_CAN);
   collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN);
-  boxSwitch=new DigitalInput(COLLECTOR_CALIBRATOR_DIO);
 }
 void Collector::InitDefaultCommand() {
 }
@@ -15,7 +14,6 @@ void Collector::MoveArms(double a){
   windowMotorRight->Set(-a);
 }
 void Collector::MoveRollers(double a){
-  printf("Collector: %f\n", a);
   collectorMotorLeft->Set(a);
   collectorMotorRight->Set(-a);
 }
@@ -27,3 +25,4 @@ bool Collector::BoxCollected(){
   return false;
   //return boxSwitch->Get();
 }
+// vim: ts2:sw=2:et

Subsystems/Collector.h

@@ -6,7 +6,6 @@ class Collector: public Subsystem
 {
   private:
     CANTalon *windowMotorLeft, *windowMotorRight, *collectorMotorLeft, *collectorMotorRight;
-    DigitalInput *boxSwitch;
   public:
     Collector();
     void InitDefaultCommand();
@@ -16,3 +15,4 @@ class Collector: public Subsystem
     bool BoxCollected();
 };
 #endif
+// vim: ts2:sw=2:et

Subsystems/DIO.cpp

@@ -1,21 +0,0 @@
-#include "DIO.h"
-#include "../RobotMap.h"
-DIO::DIO(){
-  elevatorTop=new DigitalInput(0);
-  elevatorBottom=new DigitalInput(1);
-}
-void DIO::InitDefaultCommand(){
-}
-bool DIO::Get(e_dioSig dioSig){
-  switch (dioSig){
-    case ELEVATORTOP:
-        return elevatorTop->Get();
-        break;
-    case ELEVATORBOTTOM:
-        return elevatorBottom->Get();
-        break;
-    default:
-        return false;
-        break;
-  }
-}

Subsystems/DIO.h

@@ -1,17 +0,0 @@
-#ifndef DIO_H
-#define DIO_H
-
-#include "WPILib.h"
-class DIO{
-  private:
-    DigitalInput *elevatorTop, *elevatorBottom;
-  public:
-    DIO();
-    enum e_dioSig{
-      ELEVATORTOP,
-      ELEVATORBOTTOM
-    };
-    void InitDefaultCommand();
-    bool Get(e_dioSig);
-};
-#endif

Subsystems/Drivetrain.cpp

@@ -40,3 +40,4 @@ void Drivetrain::TestMotor(e_motors motor, float power){
       break;
   }
 }
+// vim: ts2:sw=2:et

Subsystems/Drivetrain.h

@@ -20,3 +20,4 @@ class Drivetrain: public Subsystem{
     void TestMotor(e_motors,float);
 };
 #endif
+// vim: ts2:sw=2:et

Subsystems/Elevator.cpp

@@ -1,16 +1,17 @@
 #include "Elevator.h"
 #include "../RobotMap.h"
-Elevator::Elevator()/* : PIDSubsystem("Elevator", kP_real, kI_real, 0.0)*/{
+Elevator::Elevator(){
   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 Elevator::InitDefaultCommand(){
 }
 void Elevator::Run(double power){
-  printf("Elevator Power: %f\n",power);
   motor->Set(power);
 }
 void Elevator::SetOffset(double ht){
@@ -22,3 +23,10 @@ void Elevator::ResetEncoder(){
 double Elevator::GetHeight(){
   return elevatorEncoder->Get()+offset;
 }
+bool Elevator::GetElevatorBottom(){
+  return elevatorBottom->Get();
+}
+bool Elevator::GetElevatorTop(){
+  return elevatorTop->Get();
+}
+// vim: ts2:sw=2:et

Subsystems/Elevator.h

@@ -3,12 +3,13 @@
 
 #include "WPILib.h"
 #include "Commands/PIDSubsystem.h"
-class Elevator/*: public PIDSubsystem*/{
+class Elevator{
   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:
     Elevator();
     void InitDefaultCommand();
@@ -16,5 +17,8 @@ class Elevator/*: public PIDSubsystem*/{
     void SetOffset(double);
     void ResetEncoder();
     double GetHeight();
+    bool GetElevatorTop();
+    bool GetElevatorBottom();
 };
 #endif
+// vim: ts2:sw=2:et

TODO.txt

@@ -1,2 +0,0 @@
-Compressor isn't in use
-  (dos compressor on compressor start or stop)?