Added pretest code (not working)

Commands/Autonomous/Autonomous.cpp

@@ -1,20 +1,7 @@
 #include "Autonomous.h"
-#include "AutoDrive.h"
-#include <cmath>
 #include "Commands/CommandGroup.h"
 #include "../../DentRobot.h"
-Autonomous::Autonomous() : CommandGroup("Autonomous"){
+#include ""
+Autonomous::Autonomous(){
   AddSequential(new AutoDrive());
 }
-void Autonomous::Initialize(){
-}
-void Autonomous::Execute(){
-}
-bool Autonomous::IsFinished(){
-  return false;
-}
-void Autonomous::End(){
-}
-void Autonomous::Interrupted(){
-  End();
-}

Commands/Test/Check.cpp

@@ -0,0 +1,11 @@
+#include "Check.h"
+#include "Commands/CommandGroup.h"
+#include "../../DentRobot.h"
+#include "../../RobotMap.h"
+#include "CheckDrive.h"
+Check::Check(){
+  AddSequential(new CheckDrive(DRIVE_FRONT_LEFT_CAN));
+  AddSequential(new CheckDrive(DRIVE_FRONT_RIGHT_CAN));
+  AddSequential(new CheckDrive(DRIVE_BACK_LEFT_CAN));
+  AddSequential(new CheckDrive(DRIVE_BACK_RIGHT_CAN));
+}

Commands/Test/Check.h

@@ -0,0 +1,20 @@
+#ifndef CHECK_H
+#define CHECK_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class Check: public CommandGroup{
+  private:
+    int motor;
+  public:
+    Check();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif

Commands/Test/CheckDrive.cpp

@@ -0,0 +1,37 @@
+#include "CheckDrive.h"
+#include <cmath>
+#include "Commands/CommandGroup.h"
+#include "../../DentRobot.h"
+#include "../../RobotMap.h"
+CheckDrive::CheckDrive(int motorID) : CommandGroup("CheckDrive"){
+  Requires(DentRobot::drivetrain);
+}
+void CheckDrive::Initialize(){
+  SetTimeout(1.0);
+}
+void CheckDrive::Execute(){
+  switch(motor){
+    case DRIVE_FRONT_LEFT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTLEFT,1);
+      break;
+    case DRIVE_FRONT_RIGHT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTRIGHT,1);
+      break;
+    case DRIVE_BACK_LEFT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKLEFT,1);
+      break;
+    case DRIVE_BACK_RIGHT_CAN:
+      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKRIGHT,1);
+      break;
+    default:
+      break;
+  }
+}
+bool CheckDrive::IsFinished(){
+  return false;
+}
+void CheckDrive::End(){
+}
+void CheckDrive::Interrupted(){
+  End();
+}

Commands/Test/CheckDrive.h

@@ -0,0 +1,20 @@
+#ifndef CHECKDRIVE_H
+#define CHECKDRIVE_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class CheckDrive: public CommandGroup{
+  private:
+    int motor;
+  public:
+    CheckDrive(int);
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif

Subsystems/Drivetrain.cpp

@@ -20,3 +20,23 @@ void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, floa
   rightRear->Set((correctX + correctY - correctZ));
   leftRear->Set((-correctX + correctY + correctZ)*-1);
 }
+
+//Used in pretest
+void Drivetrain::TestMotor(e_motors motor, float power){
+  switch(motor){
+    case FRONTRIGHT:
+      rightFront->Set(power);
+      break;
+    case FRONTLEFT:
+      leftFront->Set(power);
+      break;
+    case BACKRIGHT:
+      rightRear->Set(power);
+      break;
+    case BACKLEFT:
+      leftRear->Set(power);
+      break;
+    default:
+      break;
+  }
+}

Subsystems/Drivetrain.h

@@ -8,8 +8,15 @@ class Drivetrain: public Subsystem{
     RobotDrive *drive;
   public:
     Drivetrain();
+    enum e_motors{
+      FRONTRIGHT,
+      FRONTLEFT,
+      BACKRIGHT,
+      BACKLEFT
+    };
     void InitDefaultCommand();
     void DriveMecanum(float,float,float,float,float);
     void DriveArcade(float, float);
+    void TestMotor(e_motors,float);
 };
 #endif