From 6c62797e074702d9273afeb1aafbf0287a2aa57e Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sat, 17 Jan 2015 13:28:27 -0500
Subject: [PATCH] Attempted writing Raise and Lower commands (untested)

---
 src/CommandBase.cpp         |  2 +-
 src/Commands/Collect.cpp    |  1 -
 src/Commands/Drive.cpp      | 13 ++++++++++++-
 src/Commands/Eject.cpp      |  1 -
 src/Commands/Lower.cpp      |  8 ++++++--
 src/Commands/Lower.h        |  1 -
 src/Commands/Raise.cpp      |  8 ++++++--
 src/Subsystems/Elevator.cpp |  3 +--
 src/Subsystems/Elevator.h   |  5 ++---
 9 files changed, 28 insertions(+), 14 deletions(-)

diff --git a/src/CommandBase.cpp b/src/CommandBase.cpp
index 3d51e70..42344fb 100644
--- a/src/CommandBase.cpp
+++ b/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();
 }
-
diff --git a/src/Commands/Collect.cpp b/src/Commands/Collect.cpp
index e863ff3..2ce95e3 100644
--- a/src/Commands/Collect.cpp
+++ b/src/Commands/Collect.cpp
@@ -9,7 +9,6 @@ bool Collect::IsFinished(){
   return false;
 }
 void Collect::End(){
-
 }
 void Collect::Interrupted(){
 }
diff --git a/src/Commands/Drive.cpp b/src/Commands/Drive.cpp
index 05ad2d1..f8f5a0f 100644
--- a/src/Commands/Drive.cpp
+++ b/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;
diff --git a/src/Commands/Eject.cpp b/src/Commands/Eject.cpp
index 0e72eea..f94ae8a 100644
--- a/src/Commands/Eject.cpp
+++ b/src/Commands/Eject.cpp
@@ -12,7 +12,6 @@ bool Eject::IsFinished(){
   return false;
 }
 void Eject::End(){
-
 }
 void Eject::Interrupted(){
 }
diff --git a/src/Commands/Lower.cpp b/src/Commands/Lower.cpp
index d16cd09..b30b0c8 100644
--- a/src/Commands/Lower.cpp
+++ b/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();
 }
diff --git a/src/Commands/Lower.h b/src/Commands/Lower.h
index 68be397..1f39059 100644
--- a/src/Commands/Lower.h
+++ b/src/Commands/Lower.h
@@ -13,5 +13,4 @@ class Lower: public Command{
     void End();
     void Interrupted();
 };
-
 #endif
diff --git a/src/Commands/Raise.cpp b/src/Commands/Raise.cpp
index 9eb7559..2d6dd30 100644
--- a/src/Commands/Raise.cpp
+++ b/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();
 }
diff --git a/src/Subsystems/Elevator.cpp b/src/Subsystems/Elevator.cpp
index 2aa7368..490cc7c 100644
--- a/src/Subsystems/Elevator.cpp
+++ b/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() : Subsystem("Elevator"){
+Elevator::Elevator() : PIDSubsystem("Elevator", kP_real, kI_real, 0.0){
   pot=new AnalogPotentiometer(0);
   leftMotor=new Talon(1);
   rightMotor=new Talon(0);
diff --git a/src/Subsystems/Elevator.h b/src/Subsystems/Elevator.h
index 9108aa5..d80ddd0 100644
--- a/src/Subsystems/Elevator.h
+++ b/src/Subsystems/Elevator.h
@@ -2,9 +2,8 @@
 #define ELEVATOR_H
 
 #include "WPILib.h"
-//#include "Commands/PIDSubsystem.h"
-//class Elevator: public PIDSubsystem{
-class Elevator: public Subsystem{
+#include "Commands/PIDSubsystem.h"
+class Elevator: public PIDSubsystem{
   private:
     AnalogPotentiometer *pot;
     Talon *leftMotor, *rightMotor;