From 4c6e75969e9145e03343ba48f01554b8cc319a9c Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Sat, 7 Feb 2015 17:04:16 -0500
Subject: [PATCH] Robot fully works

---
 .env                              |  2 +-
 Commands/Autonomous/AutoDrive.cpp |  2 +-
 Commands/Drivetrain/Drive.cpp     |  4 ----
 Commands/Elevator/Lower.cpp       |  7 ++++---
 Commands/Elevator/Raise.cpp       |  7 ++++---
 OI.cpp                            | 19 +++++++++++++------
 6 files changed, 23 insertions(+), 18 deletions(-)

diff --git a/.env b/.env
index 7bcd405..fe3ac67 100644
--- a/.env
+++ b/.env
@@ -7,7 +7,7 @@ function mc(){
 function mk(){
   vagrant ssh -c "cd /vagrant/src;make"
 }
-function md(){
+function me(){
   vagrant ssh -c "cd /vagrant/src;make deploy"
 }
 function ma(){
diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
index 511039f..e7f85fc 100644
--- a/Commands/Autonomous/AutoDrive.cpp
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -13,7 +13,7 @@ void AutoDrive::Execute(){
   DentRobot::drivetrain->DriveMecanum(0.5,0,0,0.9,0);
 }
 bool AutoDrive::IsFinished(){
-  return false;
+  return IsTimedOut();
 }
 void AutoDrive::End(){
 }
diff --git a/Commands/Drivetrain/Drive.cpp b/Commands/Drivetrain/Drive.cpp
index b6bb601..dfa803b 100644
--- a/Commands/Drivetrain/Drive.cpp
+++ b/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);
 }
diff --git a/Commands/Elevator/Lower.cpp b/Commands/Elevator/Lower.cpp
index ca38e6a..35d2159 100644
--- a/Commands/Elevator/Lower.cpp
+++ b/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::elevator->GetElevatorBottom()||IsTimedOut()){
+  if (!DentRobot::elevator->GetElevatorBottom()||IsTimedOut()){
+    printf("Robot stoped lowering. Sensor based? %d\n", !DentRobot::elevator->GetElevatorBottom());
     return true;
   }else{
     return false;
diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp
index ab41f45..050b571 100644
--- a/Commands/Elevator/Raise.cpp
+++ b/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::elevator->GetElevatorTop()||IsTimedOut()){
+  if (!DentRobot::elevator->GetElevatorTop()||IsTimedOut()){
+    printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop());
     return true;
   }else{
     return false;
diff --git a/OI.cpp b/OI.cpp
index 838c7a9..f3a6d28 100644
--- a/OI.cpp
+++ b/OI.cpp
@@ -7,21 +7,28 @@
 #include "Commands/Collector/ReleaseTote.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 *right1=new JoystickButton(rightStick, 1);
   JoystickButton *right2=new JoystickButton(rightStick, 2);
+  JoystickButton *left3=new JoystickButton(leftStick, 3);
+  JoystickButton *left4=new JoystickButton(leftStick, 4);
+  right1->WhileHeld(new OpenCollector());
+  right2->WhileHeld(new CloseCollector());
+  left3->WhileHeld(new CollectTote());
+  left4->WhileHeld(new ReleaseTote());
+  // Elevator
   JoystickButton *right3=new JoystickButton(rightStick, 3);
   JoystickButton *right4=new JoystickButton(rightStick, 4);
   JoystickButton *right5=new JoystickButton(rightStick, 5);
   JoystickButton *right6=new JoystickButton(rightStick, 6);
-  right1->WhileHeld(new OpenCollector());
-  right2->WhileHeld(new CloseCollector());
-  right3->WhileHeld(new CollectTote());
-  right4->WhileHeld(new ReleaseTote());
+  right3->WhenPressed(new Lower());
+  right4->WhenPressed(new Lower());
   right5->WhenPressed(new Raise());
-  right6->WhenPressed(new Lower());
+  right6->WhenPressed(new Raise());
 }
 Joystick* OI::GetRightStick(){
   return rightStick;