Added elevator calibrate function (untested)

CommandBase.cpp

@@ -2,15 +2,18 @@
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
+#include "Subsystems/DIO.h"
 #include "Subsystems/AirCompressor.h"
 #include "Commands/Drive.h"
 #include "Commands/Collect.h"
 #include "Commands/Eject.h"
 #include "Commands/Raise.h"
 #include "Commands/Lower.h"
+#include "Commands/Calibrate.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) {
@@ -22,6 +25,7 @@ void CommandBase::init()
   drivetrain = new Drivetrain();
   collector = new Collector();
   elevator = new Elevator();
+  dio = new DIO();
   airCompressor = new AirCompressor();
   oi = new OI();
 }

CommandBase.h

@@ -5,6 +5,7 @@
 #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"
@@ -17,6 +18,7 @@ class CommandBase: public Command {
     static Drivetrain *drivetrain;
     static Collector *collector;
     static Elevator *elevator;
+    static DIO* dio;
     static AirCompressor *airCompressor;
     static OI *oi;
 };

Commands/Calibrate.cpp

@@ -0,0 +1,18 @@
+#include "Calibrate.h"
+#include "../DentRobot.h"
+Calibrate::Calibrate() : Command("Calibrate"){
+}
+void Calibrate::Initialize(){
+}
+void Calibrate::Execute(){
+  DentRobot::elevator->Run(-0.4f);
+}
+bool Calibrate::IsFinished(){
+  return DentRobot::elevator->GetPotValue()>=0.1f;
+}
+void Calibrate::End(){
+  DentRobot::elevator->Run(0.0f);
+}
+void Calibrate::Interrupted(){
+  End();
+}

Commands/Calibrate.h

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

DentRobot.cpp

@@ -3,12 +3,14 @@ OI* DentRobot::oi=NULL;
 Collector* DentRobot::collector=NULL;
 Drivetrain* DentRobot::drivetrain=NULL;
 Elevator* DentRobot::elevator=NULL;
+DIO* DentRobot::dio = NULL;
 AirCompressor * DentRobot::airCompressor=NULL;
 DentRobot::DentRobot(){
   oi=new OI();
   collector=new Collector();
   drivetrain=new Drivetrain();
   elevator=new Elevator();
+  dio = new DIO();
   airCompressor=new AirCompressor();
   printf("Initialized");
 }

DentRobot.h

@@ -3,6 +3,7 @@
 #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"
@@ -14,6 +15,7 @@ public:
   static Collector* collector;
   static Drivetrain* drivetrain;
   static Elevator* elevator;
+  static DIO* dio;
   static AirCompressor* airCompressor;
   DentRobot();
   void RobotInit();

Subsystems/DIO.cpp

@@ -0,0 +1,18 @@
+#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){
+  if(dioSig == ELEVATORTOP){
+    // The top elevator digitalinput was triggered
+    return elevatorTop->Get();
+  }else if(dioSig == ELEVATORBOTTOM){
+    // The buttom elevator digitalinput was triggered
+    return elevatorBottom->Get();
+  }
+  return false;
+}

Subsystems/DIO.h

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

Subsystems/Drivetrain.h

@@ -2,8 +2,7 @@
 #define DRIVETRAIN_H
 
 #include "WPILib.h"
-class Drivetrain: public Subsystem
-{
+class Drivetrain: public Subsystem{
   private:
     CANTalon *rightFront, *leftFront, *rightRear, *leftRear;
     RobotDrive *drive;

Subsystems/Elevator.cpp

@@ -4,6 +4,7 @@ Elevator::Elevator()/* : PIDSubsystem("Elevator", kP_real, kI_real, 0.0)*/{
   pot=new AnalogPotentiometer(0);
   leftMotor=new Talon(1);
   rightMotor=new Talon(0);
+  height=0;
   //SetAbsoluteTolerance(0.004);
 }
 void Elevator::InitDefaultCommand(){
@@ -15,3 +16,9 @@ void Elevator::Run(double power){
   leftMotor->Set(power);
   rightMotor->Set(power);
 }
+void Elevator::SetHeight(double ht){
+  height=ht;
+}
+double Elevator::GetHeight(){
+  return height;
+}

Subsystems/Elevator.h

@@ -8,10 +8,13 @@ class Elevator/*: public PIDSubsystem*/{
     AnalogPotentiometer *pot;
     Talon *leftMotor, *rightMotor;
     static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
+    double height;
   public:
     Elevator();
     void InitDefaultCommand();
     float GetPotValue();
     void Run(double);
+    void SetHeight(double);
+    double GetHeight();
 };
 #endif