Attempted writing Raise and Lower commands (untested)

src/CommandBase.cpp

@@ -2,6 +2,7 @@
 #include "Subsystems/Drivetrain.h"
 #include "Subsystems/Collector.h"
 #include "Subsystems/Elevator.h"
+#include "Commands/Drive.h"
 #include "Commands/Collect.h"
 #include "Commands/Eject.h"
 #include "Commands/Raise.h"
@@ -21,4 +22,3 @@ void CommandBase::init()
   elevator = new Elevator();
   oi = new OI();
 }
-

src/Commands/Collect.cpp

@@ -9,7 +9,6 @@ bool Collect::IsFinished(){
   return false;
 }
 void Collect::End(){
-
 }
 void Collect::Interrupted(){
 }

src/Commands/Drive.cpp

@@ -1,11 +1,22 @@
 #include "Drive.h"
+#include <cmath>
 #include "../DentRobot.h"
 Drive::Drive() : Command("Drive"){
 }
 void Drive::Initialize(){
 }
 void Drive::Execute(){
-  DentRobot::drivetrain->DriveMecanum(DentRobot::oi->GetLeftStick()->GetRawAxis(2), DentRobot::oi->GetLeftStick()->GetRawAxis(1), DentRobot::oi->GetLeftStick()->GetRawAxis(0));
+  static float sens=0.7;
+  float x, y, twist;
+  x=DentRobot::oi->GetLeftStick()->GetRawAxis(0);
+  y=DentRobot::oi->GetLeftStick()->GetRawAxis(1);
+  twist=DentRobot::oi->GetLeftStick()->GetRawAxis(2);
+  if(true){
+    x=sens*std::pow(x, 3)+(1.0f-sens)*x;
+    y=sens*std::pow(y, 3)+(1.0f-sens)*y;
+  }
+  //DentRobot::drivetrain->DriveMecanum(DentRobot::oi->GetLeftStick()->GetRawAxis(2), DentRobot::oi->GetLeftStick()->GetRawAxis(1), DentRobot::oi->GetLeftStick()->GetRawAxis(0));
+  DentRobot::drivetrain->DriveMecanum(x, y, twist);
 }
 bool Drive::IsFinished(){
   return false;

src/Commands/Eject.cpp

@@ -12,7 +12,6 @@ bool Eject::IsFinished(){
   return false;
 }
 void Eject::End(){
-
 }
 void Eject::Interrupted(){
 }

src/Commands/Lower.cpp

@@ -1,15 +1,19 @@
 #include "Lower.h"
+#include "../DentRobot.h"
 Lower::Lower() : Command("Lower"){
 }
 void Lower::Initialize(){
 }
 void Lower::Execute(){
+  DentRobot::elevator->Run(-0.4f);
 }
 bool Lower::IsFinished(){
-  return false;
+  // 0.1f is a placeholder for the min elevator value
+  return DentRobot::elevator->GetPotValue()>=0.1f;
 }
 void Lower::End(){
-
+  DentRobot::elevator->Run(0.0f);
 }
 void Lower::Interrupted(){
+  End();
 }

src/Commands/Lower.h

@@ -13,5 +13,4 @@ class Lower: public Command{
     void End();
     void Interrupted();
 };
-
 #endif

src/Commands/Raise.cpp

@@ -1,15 +1,19 @@
 #include "Raise.h"
+#include "../DentRobot.h"
 Raise::Raise(){
 }
 void Raise::Initialize(){
 }
 void Raise::Execute(){
+  DentRobot::elevator->Run(0.4f);
 }
 bool Raise::IsFinished(){
-  return false;
+  // 0.9f is a placeholder for the max elevator value
+  return DentRobot::elevator->GetPotValue()>=0.9f;
 }
 void Raise::End(){
-
+  DentRobot::elevator->Run(0.0f);
 }
 void Raise::Interrupted(){
+  End();
 }

src/Subsystems/Elevator.cpp

@@ -1,7 +1,6 @@
 #include "Elevator.h"
 #include "../RobotMap.h"
-//Elevator::Elevator() : PIDSubsystem("Elevator", kP_real, kI_real, 0.0){
+Elevator::Elevator() : PIDSubsystem("Elevator", kP_real, kI_real, 0.0){
-Elevator::Elevator() : Subsystem("Elevator"){
   pot=new AnalogPotentiometer(0);
   leftMotor=new Talon(1);
   rightMotor=new Talon(0);

src/Subsystems/Elevator.h

@@ -2,9 +2,8 @@
 #define ELEVATOR_H
 
 #include "WPILib.h"
-//#include "Commands/PIDSubsystem.h"
+#include "Commands/PIDSubsystem.h"
-//class Elevator: public PIDSubsystem{
+class Elevator: public PIDSubsystem{
-class Elevator: public Subsystem{
   private:
     AnalogPotentiometer *pot;
     Talon *leftMotor, *rightMotor;