From 40da44008f8ce0034fdaabf1950d1f58d24fc684 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Tue, 17 Mar 2015 09:50:38 -0400
Subject: [PATCH] Worked on Autonomous

---
 Commands/Autonomous/AutoDrive.cpp   |  7 +++++--
 Commands/Autonomous/AutoDrive.h     |  8 ++++++--
 Commands/Autonomous/Autonomous.cpp  | 19 ++++++++-----------
 Commands/Autonomous/CollectTote.cpp |  4 ++--
 Commands/Autonomous/CollectTote.h   |  4 +++-
 DentRobot.cpp                       | 12 +++++++++---
 Makefile                            |  2 +-
 7 files changed, 34 insertions(+), 22 deletions(-)

diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
index 475224b..711de7e 100644
--- a/Commands/Autonomous/AutoDrive.cpp
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -1,18 +1,21 @@
 #include "AutoDrive.h"
 #include "../../DentRobot.h"
 // Drive for a short while then stop. Just for testing
-AutoDrive::AutoDrive(double duration, double xtmp, double ytmp): Command("AutoDrive"){
+AutoDrive::AutoDrive(double duration, double xtmp, double ytmp, double ztmp, bool useGyro): Command("AutoDrive"){
   Requires(DentRobot::drivetrain);
   SetTimeout(duration);
   x=xtmp;
   y=ytmp;
+  z=ztmp;
+  gyro=useGyro;
 }
 void AutoDrive::Initialize(){
   DentRobot::drivetrain->ResetGyro();
 }
 void AutoDrive::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
-  DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9, true);
+  printf("z: %f\n", z);
+  DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, gyro);
 }
 bool AutoDrive::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/Autonomous/AutoDrive.h b/Commands/Autonomous/AutoDrive.h
index 96fa8a4..e2fa088 100644
--- a/Commands/Autonomous/AutoDrive.h
+++ b/Commands/Autonomous/AutoDrive.h
@@ -14,7 +14,9 @@
 class AutoDrive: public Command{
   private:
     double x, //<! The x value of the simulated joystick value
-           y; //<! The y value of the simulated joystick value
+           y, //<! The y value of the simulated joystick value
+           z; //<! The z value of the simulated joystick value
+    bool gyro;
   public:
     /**
      * @brief Constructs AutoDrive
@@ -22,8 +24,10 @@ class AutoDrive: public Command{
      * @param duration Timeout in seconds
      * @param xtmp Joystick x value (default: 0.0)
      * @param ytmp Joystick y value (default: 0.75)
+     * @param ztmp Joystick z value (default: 0.0)
+     * @param useGyro Use the gyro when driving
      */
-    AutoDrive(double duration, double xtmp = 0.0, double ytmp = -0.75);
+    AutoDrive(double duration, double xtmp = 0.0, double ytmp = -0.75, double ztmp = 0.0, bool useGyro = true);
     /**
      * @brief Initializes the class
      */
diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index 80a14f2..a22310f 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -11,17 +11,16 @@
 #include "CollectTote.h"
 #include "ReleaseTote.h"
 Autonomous::Autonomous(int seq){
-  //SmartDashboard::GetNumber("Auto Wait Time");
+  Wait(SmartDashboard::GetNumber("Auto Wait Time"));
   switch(seq){
     case 0:
       // Just for testing
-      // Strafe at .25 power
-      AddSequential(new AutoDrive(0.5, 0.25, 0.0));
+      // Turn testing
+      AddSequential(new Turn(3.8));
       break;
     case 1:
       // Drive to Auto Zone (TM)
-      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
-      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8, 0.01));
       AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
       break;
     case 2:
@@ -35,7 +34,6 @@ Autonomous::Autonomous(int seq){
       break;
     case 3:
       // Collect a tote with BinElevator, turn, drive to Auto Zone (TM)
-      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
       AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount")));
       AddSequential(new BinRaise(1.2));
       AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75));
@@ -44,18 +42,17 @@ Autonomous::Autonomous(int seq){
       break;
     case 4:
       // Collect one, two, or three totes, drive to Auto Zone (TM), release totes
-      printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time"));
-      Wait(SmartDashboard::GetNumber("Auto Wait Time"));
-      printf("Done");
-      AddSequential(new CollectTote());
+      AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn")));
       if(SmartDashboard::GetBoolean("Two totes")){
+        AddParallel(new Turn(0.81));
         AddSequential(new Raise(3.5));
         AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0.0, 0.75));
         AddSequential(new CollectTote());
         AddSequential(new Lower(3.0));
         AddSequential(new Raise(3.5));
         if(SmartDashboard::GetBoolean("Three totes")){
-          AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0.0, 0.75));
+          AddSequential(new Turn(3.8));
+          AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance")*2.5, 0.0, 0.75));
           AddSequential(new CollectTote());
           AddSequential(new Lower(3.0));
           AddSequential(new Raise(3.5));
diff --git a/Commands/Autonomous/CollectTote.cpp b/Commands/Autonomous/CollectTote.cpp
index 2eb6634..44b653e 100644
--- a/Commands/Autonomous/CollectTote.cpp
+++ b/Commands/Autonomous/CollectTote.cpp
@@ -2,8 +2,8 @@
 #include "../../DentRobot.h"
 #include "AutoDrive.h"
 #include "../Collector/RollIn.h"
-CollectTote::CollectTote(){
-  AddParallel(new AutoDrive(1.0, 0.0, 0.75));
+CollectTote::CollectTote(double z){
+  AddParallel(new AutoDrive(SmartDashboard::GetNumber("DriveTime"), 0.0, 0.75, z, false));
   AddSequential(new RollIn(1.0));
 }
 // vim: ts=2:sw=2:et
diff --git a/Commands/Autonomous/CollectTote.h b/Commands/Autonomous/CollectTote.h
index aba8974..6d6fb61 100644
--- a/Commands/Autonomous/CollectTote.h
+++ b/Commands/Autonomous/CollectTote.h
@@ -15,8 +15,10 @@ class CollectTote: public CommandGroup{
   public:
     /**
      * @brief Constructs CollectTote
+     * 
+     * @param z Joystick z value (default: 0.0)
      */
-    CollectTote();
+    CollectTote(double z = 0.0);
 };
 #endif
 // vim: ts=2:sw=2:et
diff --git a/DentRobot.cpp b/DentRobot.cpp
index 3fd72c1..069ee3a 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -23,18 +23,24 @@ DentRobot::DentRobot(){
 void DentRobot::RobotInit(){
   SmartDashboard::PutNumber("CodeVersion", CODE_VERSION);
   // Autonomous
+  // Calibration
+  // Amount to turn while collecting the initial tote in auto 4
+  SmartDashboard::PutNumber("CollectToteTurn", 0.25);
+  // Amount of time to collect a tote
+  SmartDashboard::PutNumber("DriveTime", 1.3);
   // Sequence of autonomous command
-  SmartDashboard::PutNumber("Auto Sequence", 1.0);
+  SmartDashboard::PutNumber("Auto Sequence", 4.0);
   SmartDashboard::PutNumber("Auto Wait Time", 0.5);
   // If the robot will be picking up three totes in sequence 3
-  SmartDashboard::PutBoolean("Two totes", true);
+  SmartDashboard::PutBoolean("Two totes", false);
   SmartDashboard::PutBoolean("Three totes", false);
   // Distance (in time) to auto zone
   SmartDashboard::PutNumber("Auto Zone Distance", 2.1);
   // Distance (in time) to auto tote (used in sequence 3)
-  SmartDashboard::PutNumber("Auto Tote Distance", 0.5);
+  SmartDashboard::PutNumber("Auto Tote Distance", 1.0);
   SmartDashboard::PutNumber("Auto Bin Distance", 0.25);
   SmartDashboard::PutNumber("TurnAmount", 1.8);
+
   // Elevators
   SmartDashboard::PutBoolean("Bin Elevator Bottom", false);
   SmartDashboard::PutBoolean("Bin Elevator Top", false);
diff --git a/Makefile b/Makefile
index d49cc80..6bb3d52 100644
--- a/Makefile
+++ b/Makefile
@@ -19,7 +19,7 @@ clean:
 	$(CLEANSER) $(OBJECTS) bin/FRCUserProgram
 
 deploy:
-		@cat bin/FRCUserProgram | ssh admin@$(REMOTEIP) '(rm /home/lvuser/FRCUserProgram)</dev/null;cat > /home/lvuser/FRCUserProgram;chmod a+x /home/lvuser/FRCUserProgram'
+		@/home/stonewareslord/git/pv-1.5.7/pv bin/FRCUserProgram | ssh admin@$(REMOTEIP) '(rm /home/lvuser/FRCUserProgram)</dev/null;cat > /home/lvuser/FRCUserProgram;chmod a+x /home/lvuser/FRCUserProgram'
 
 debug:
 		@cat bin/FRCUserProgram | ssh admin@$(REMOTEIP) '(rm /home/lvuser/FRCUserProgram)</dev/null;cat > /home/lvuser/FRCUserProgram;chmod a+x /home/lvuser/FRCUserProgram;/home/lvuser/run.sh'