diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
index 9832bf6..475224b 100644
--- a/Commands/Autonomous/AutoDrive.cpp
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -8,10 +8,11 @@ AutoDrive::AutoDrive(double duration, double xtmp, double ytmp): Command("AutoDr
   y=ytmp;
 }
 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);
+  DentRobot::drivetrain->DriveMecanum(x, y, 0.0, 0.9, true);
 }
 bool AutoDrive::IsFinished(){
   return IsTimedOut();
diff --git a/Commands/BinElevator/BinLower.h b/Commands/BinElevator/BinLower.h
index 765c1d6..f9a6068 100644
--- a/Commands/BinElevator/BinLower.h
+++ b/Commands/BinElevator/BinLower.h
@@ -13,7 +13,7 @@ class BinLower: public Command{
     /**
      * @brief Constructs BinLower
      *
-     * @param float The timeout
+     * @param timeout The timeout
      */
     BinLower(float timeout);
     /**
diff --git a/Commands/Collector/RollIn.h b/Commands/Collector/RollIn.h
index e5bbf37..10b9189 100644
--- a/Commands/Collector/RollIn.h
+++ b/Commands/Collector/RollIn.h
@@ -13,11 +13,10 @@ class RollIn: public Command{
   private:
     double rawSpeed; //<! Raw speed of the collector
   public:
-    RollIn();
     /**
      * @brief Constructs RollIn
      *
-     * @param double Speed to roll the collector
+     * @param speed Speed to roll the collector
      */
     RollIn(double speed);
     /**
diff --git a/Commands/Collector/RollOut.cpp b/Commands/Collector/RollOut.cpp
index bc54cdf..09864dd 100644
--- a/Commands/Collector/RollOut.cpp
+++ b/Commands/Collector/RollOut.cpp
@@ -6,7 +6,6 @@ RollOut::RollOut(double timeout): Command("RollOut"){
 void RollOut::Initialize(){
 }
 void RollOut::Execute(){
-  //TODO check this value to move the motors in the right direction
   // Divide by 2 twice because this speed should be half the collector speed
   DentRobot::collector->MoveRollers(-DentRobot::oi->GetLeftThrottle() * 0.8);
   SmartDashboard::PutNumber("DriveThrottle", -DentRobot::oi->GetLeftThrottle());
diff --git a/Commands/Elevator/Lower.h b/Commands/Elevator/Lower.h
index 9cc3cfa..faf46e7 100644
--- a/Commands/Elevator/Lower.h
+++ b/Commands/Elevator/Lower.h
@@ -9,11 +9,10 @@
  */
 class Lower: public Command{
   public:
-    Lower(double timeout=3.0);
     /**
      * @brief Constructs Lower
      */
-    Lower();
+    Lower(double timeout=3.0);
     /**
      * @brief Initializes the class
      */
diff --git a/Commands/Elevator/Raise.h b/Commands/Elevator/Raise.h
index 5dd122f..56535d0 100644
--- a/Commands/Elevator/Raise.h
+++ b/Commands/Elevator/Raise.h
@@ -9,11 +9,10 @@
  */
 class Raise: public Command{
   public:
-    Raise(double timeout=3.5);
     /**
      * @brief Constructs Raise
      */
-    Raise();
+    Raise(double timeout=3.5);
     /**
      * @brief Initializes the class
      */
diff --git a/Commands/Test/CheckDrive.h b/Commands/Test/CheckDrive.h
index 2501755..f398fb6 100644
--- a/Commands/Test/CheckDrive.h
+++ b/Commands/Test/CheckDrive.h
@@ -16,7 +16,7 @@ class CheckDrive: public CommandGroup{
     /**
      * @brief TODO
      *
-     * @param int
+     * @param motorID
      */
     CheckDrive(int);
     /**
diff --git a/DentRobot.cpp b/DentRobot.cpp
index be86154..eb45120 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -29,7 +29,6 @@ void DentRobot::RobotInit(){
   // If the robot will be picking up three totes in sequence 3
   SmartDashboard::PutBoolean("Two totes", true);
   SmartDashboard::PutBoolean("Three totes", false);
-  // TODO: Calibrate the following two values
   // Distance (in time) to auto zone
   SmartDashboard::PutNumber("Auto Zone Distance", 2.1);
   // Distance (in time) to auto tote (used in sequence 3)
@@ -43,7 +42,7 @@ void DentRobot::RobotInit(){
   SmartDashboard::PutBoolean("Elevator Bottom", false);
   SmartDashboard::PutBoolean("Elevator Top", false);
   //Drive speed
-  SmartDashboard::PutNumber("DriveSpeedReductionThresh", 2);
+  SmartDashboard::PutNumber("DriveSpeedReductionThresh", 2.0);
 }
 void DentRobot::DisabledPeriodic(){
   Scheduler::GetInstance()->Run();
@@ -66,7 +65,7 @@ void DentRobot::TeleopInit(){
 }
 void DentRobot::TeleopPeriodic(){
   Scheduler::GetInstance()->Run();
-  // TODO: Calibrate 1.0 to the height we want the elevator to automatically raise
+  //TODO: Calibrate 1.0 to the height we want the elevator to automatically raise
   if(elevator->GetUseEncoder()&&elevator->GetHeight()<=-1.0){
     // Raise the elevator if it dips below elevatorTop
     oi->raise->Start();
diff --git a/Subsystems/BinElevator.h b/Subsystems/BinElevator.h
index 5923c4d..5e6f27f 100644
--- a/Subsystems/BinElevator.h
+++ b/Subsystems/BinElevator.h
@@ -24,11 +24,11 @@ class BinElevator{
     /**
      * @brief Runs the bin elevator
      *
-     * @param double The power to run the bin elevator
+     * @param power The power to run the bin elevator
      * 
      * Ranges from -1.0 to 1.0
      */
-    void Run(double);
+    void Run(double power);
     /**
      * @brief Sets the encoder value to 0 (unused)
      */
diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp
index ddae882..89eb781 100644
--- a/Subsystems/Collector.cpp
+++ b/Subsystems/Collector.cpp
@@ -10,12 +10,12 @@ Collector::Collector(): Subsystem("Collector"){
 }
 void Collector::InitDefaultCommand(){
 }
-void Collector::MoveRollers(double a){
-  collectorMotorLeft->Set(a);
-  collectorMotorBottom->Set(-a);
-  collectorMotorRamp->Set(a);
-  collectorMotorRight->Set(-a);
-  printf("Roller power: %f\n", a);
+void Collector::MoveRollers(double power){
+  collectorMotorLeft->Set(power);
+  collectorMotorBottom->Set(-power);
+  collectorMotorRamp->Set(power);
+  collectorMotorRight->Set(-power);
+  printf("Roller power: %f\n", power);
 }
 double Collector::GetSonarDistance(){
   return sonarAnalog->GetAverageVoltage();
diff --git a/Subsystems/Collector.h b/Subsystems/Collector.h
index 2c541d9..3cbc646 100644
--- a/Subsystems/Collector.h
+++ b/Subsystems/Collector.h
@@ -33,9 +33,9 @@ class Collector: public Subsystem{
     /**
      * @brief Moves the collectors
      *
-     * @param double The speed to run the collectors
+     * @param power The speed to run the collectors
      */
-    void MoveRollers(double);
+    void MoveRollers(double power);
     /**
      * @brief Gets the distance of the sonar (unused)
      *
diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp
index ac93f2c..4587e8c 100644
--- a/Subsystems/Drivetrain.cpp
+++ b/Subsystems/Drivetrain.cpp
@@ -19,6 +19,7 @@ void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity,
   double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
   double correctZ;
   if(driveStraight){
+    printf("Driving straight at: %f\n", -gyro->GetAngle()*kP);
     correctZ = -gyro->GetAngle()*kP;
   }else{
     correctZ = -z * 0.5;
diff --git a/Subsystems/Elevator.cpp b/Subsystems/Elevator.cpp
index abba7e3..8470b34 100644
--- a/Subsystems/Elevator.cpp
+++ b/Subsystems/Elevator.cpp
@@ -35,8 +35,8 @@ bool Elevator::GetElevatorTop(){
   SmartDashboard::PutBoolean("Elevator Top", !elevatorTop->Get());
   return elevatorTop->Get();
 }
-void Elevator::SetUseEncoder(bool param){
-  useEncoder=param;
+void Elevator::SetUseEncoder(bool use){
+  useEncoder=use;
 }
 bool Elevator::GetUseEncoder(){
   return useEncoder;
diff --git a/Subsystems/Elevator.h b/Subsystems/Elevator.h
index ebd1e9d..c33856b 100644
--- a/Subsystems/Elevator.h
+++ b/Subsystems/Elevator.h
@@ -25,9 +25,9 @@ class Elevator{
     /**
      * @brief Runs the elevator
      *
-     * @param double The power to run the bin elevator (-1.0 to 1.0)
+     * @param power The power to run the bin elevator (-1.0 to 1.0)
      */
-    void Run(double);
+    void Run(double power);
     /**
      * @brief Sets the encoder value to 0 (unused)
      */
@@ -59,9 +59,9 @@ class Elevator{
     /**
      * @brief Use the elevator encoder (unused)
      *
-     * @param bool State to use encoder
+     * @param use State to use encoder
      */
-    void SetUseEncoder(bool);
+    void SetUseEncoder(bool use);
     /**
      * @brief Gets the state of useEncoder (unused)
      *
diff --git a/Subsystems/Pneumatics.h b/Subsystems/Pneumatics.h
index 0a46928..d61c084 100644
--- a/Subsystems/Pneumatics.h
+++ b/Subsystems/Pneumatics.h
@@ -23,9 +23,9 @@ class Pneumatics: public Subsystem{
     /**
      * @brief Sets the state of the arms
      *
-     * @param bool State of the arms
+     * @param state State of the arms
      */
-    void SetOpen(bool);
+    void SetOpen(bool state);
 };
 #endif
 // vim: ts=2:sw=2:et