Merge feature/command-based with t

Commands/Calibrate.cpp

@@ -11,8 +11,8 @@ void Calibrate::Execute(){
 }
 bool Calibrate::IsFinished(){
   if(DentRobot::dio->Get(DIO::ELEVATORBOTTOM)){
-    // 0.99 is a placeholder for the height of the limit switches
-    DentRobot::elevator->SetHeight(0.99);
+    DentRobot::elevator->ResetEncoder();
+    DentRobot::elevator->SetOffset(0.99);
     return true;
   }
   return false;

Subsystems/Elevator.cpp

@@ -2,8 +2,10 @@
 #include "../RobotMap.h"
 Elevator::Elevator()/* : PIDSubsystem("Elevator", kP_real, kI_real, 0.0)*/{
   pot=new AnalogPotentiometer(0);
-  leftMotor=new Talon(1);
-  rightMotor=new Talon(0);
+  leftMotor=new CANTalon(1);
+  rightMotor=new CANTalon(0);
+  elevatorEncoder=new Encoder(0,1,false);
+  offset=0;
   height=0;
   //SetAbsoluteTolerance(0.004);
 }
@@ -13,14 +15,15 @@ float Elevator::GetPotValue(){
   return pot->Get();
 }
 void Elevator::Run(double power){
-  // Height supposed to be the location of the elevator
-  //height+=power;
   leftMotor->Set(power);
   rightMotor->Set(power);
 }
-void Elevator::SetHeight(double ht){
-  height=ht;
+void Elevator::SetOffset(double ht){
+  offset=ht;
+}
+void Elevator::ResetEncoder(){
+  elevatorEncoder->Reset();
 }
 double Elevator::GetHeight(){
-  return height;
+  return elevatorEncoder->Get()+offset;
 }

Subsystems/Elevator.h

@@ -6,15 +6,16 @@
 class Elevator/*: public PIDSubsystem*/{
   private:
     AnalogPotentiometer *pot;
-    Talon *leftMotor, *rightMotor;
+    CANTalon *leftMotor, *rightMotor;
+    Encoder *elevatorEncoder;
     static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2;
-    double height;
+    double offset, height;
   public:
     Elevator();
     void InitDefaultCommand();
     float GetPotValue();
     void Run(double);
-    void SetHeight(double);
+    void SetOffset(double);
     double GetHeight();
 };
 #endif