Continued working on auto. Works, but not well

Commands/Autonomous/AutoDrive.cpp

@@ -15,6 +15,7 @@ void AutoDrive::Initialize(){
 void AutoDrive::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
   printf("z: %f\n", z);
+  printf("(x, y, z): (%f, %f, %f)\n", x, y, z);
   DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, gyro);
 }
 bool AutoDrive::IsFinished(){

Commands/Autonomous/Autonomous.cpp

@@ -16,7 +16,13 @@ Autonomous::Autonomous(int seq){
     case 0:
       // Just for testing
       // Turn testing
-      AddSequential(new Turn(3.8));
+      //AddSequential(new AutoDrive(2.0, -0.75, 0.0, true));
+      //AddSequential(new CollectTote());
+      //AddSequential(new CollectTote());
+      //AddSequential(new CollectTote());
+      //AddSequential(new CollectTote());
+      //AddSequential(new Turn(3.8));
+      AddSequential(new Turn(SmartDashboard::GetNumber("180Turn"), 0.8));
       break;
     case 1:
       // Drive to Auto Zone (TM)
@@ -44,8 +50,10 @@ Autonomous::Autonomous(int seq){
       // Collect one, two, or three totes, drive to Auto Zone (TM), release totes
       AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn")));
       if(SmartDashboard::GetBoolean("Two totes")){
-        AddParallel(new Turn(0.81));
+        AddParallel(new Turn(SmartDashboard::GetNumber("TwoToteTurn")));
+        AddParallel(new RollIn(3.5));
         AddSequential(new Raise(3.5));
+        AddParallel(new RollIn(SmartDashboard::GetNumber("Two Tote Distance")));
         AddSequential(new AutoDrive(SmartDashboard::GetNumber("Two Tote Distance"), 0.0, 0.75));
         AddSequential(new CollectTote());
         AddSequential(new Lower(3.0));
@@ -74,6 +82,14 @@ Autonomous::Autonomous(int seq){
       // Same as auto 4 with (Three|Two) totes checked, collect bin, drive to Auto Zone (TM), release totes
       //TODO: Implement this
       break;
+    case 8:
+      AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn")));
+      AddSequential(new Turn(SmartDashboard::GetNumber("180Turn"), 0.8));
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Two Tote Distance"), 0.0, 0.75));
+      AddSequential(new CollectTote());
+      AddSequential(new AutoDrive(SmartDashboard::GetNumber("Three Tote Distance"), 0.0, 0.75));
+      AddSequential(new CollectTote());
+      break;
     default:
       printf("Invalid seq: %d\n", seq);
       break;

Commands/Autonomous/Turn.cpp

@@ -1,14 +1,15 @@
 #include "Turn.h"
 #include "../../DentRobot.h"
-Turn::Turn(double timeout): Command("Turn"){
+Turn::Turn(double timeout, double pwr): Command("Turn"){
   Requires(DentRobot::drivetrain);
   SetTimeout(timeout);
+  power=pwr;
 }
 void Turn::Initialize(){
 }
 void Turn::Execute(){
   //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle)
-  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.6, 0.9);
+  DentRobot::drivetrain->DriveMecanum(0.0, 0.0, power, 0.9);
 }
 bool Turn::IsFinished(){
   return IsTimedOut();

Commands/Autonomous/Turn.h

@@ -13,13 +13,14 @@
  */
 class Turn: public Command{
   private:
+    double power; //<! Power to turn (default: 0.6)
   public:
     /**
      * @brief Constructs Turn
      *
      * @param timeout Timeout in seconds
      */
-    Turn(double timeout);
+    Turn(double timeout, double pwr=0.6);
     /**
      * @brief Initializes the class
      */

DentRobot.cpp

@@ -26,6 +26,8 @@ void DentRobot::RobotInit(){
   // Calibration
   // Amount to turn while collecting the initial tote in auto 4
   SmartDashboard::PutNumber("CollectToteTurn", 0.25);
+  SmartDashboard::PutNumber("180Turn", 1.8);
+  SmartDashboard::PutNumber("TwoToteTurn", 0.81);
   // Amount of time to collect a tote
   SmartDashboard::PutNumber("DriveTime", 1.3);
   // Sequence of autonomous command
@@ -41,7 +43,6 @@ void DentRobot::RobotInit(){
   SmartDashboard::PutNumber("Three Tote Distance", 2.5);
   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);
@@ -63,7 +64,6 @@ void DentRobot::AutonomousInit(){
   }
 }
 void DentRobot::AutonomousPeriodic(){
-  printf("Running auto.\n");
   Scheduler::GetInstance()->Run();
 }
 void DentRobot::TeleopInit(){

Subsystems/Drivetrain.cpp

@@ -27,9 +27,9 @@ void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity,
   if(DentRobot::oi->GetLeftStick()->GetRawButton(9)){
     correctY /= SmartDashboard::GetNumber("DriveSpeedReductionThresh");
   }
-  rightFront->Set((-correctX + correctY - correctZ));
+  rightFront->Set(-correctX + correctY - correctZ);
   leftFront->Set((correctX + correctY + correctZ)*-1);
-  rightRear->Set((correctX + correctY - correctZ));
+  rightRear->Set(correctX + correctY - correctZ);
   leftRear->Set((-correctX + correctY + correctZ)*-1);
 }
 //Used in pretest