From e2a9a695ae0360862382ac57c1dab0fbb7ba38b3 Mon Sep 17 00:00:00 2001
From: Adam Long <macadam.long@gmail.com>
Date: Sat, 7 Feb 2015 14:17:20 +0000
Subject: [PATCH] Added pretest code (not working)

---
 Commands/Autonomous/Autonomous.cpp | 17 ++------------
 Commands/Test/Check.cpp            | 11 +++++++++
 Commands/Test/Check.h              | 20 ++++++++++++++++
 Commands/Test/CheckDrive.cpp       | 37 ++++++++++++++++++++++++++++++
 Commands/Test/CheckDrive.h         | 20 ++++++++++++++++
 Subsystems/Drivetrain.cpp          | 20 ++++++++++++++++
 Subsystems/Drivetrain.h            |  7 ++++++
 7 files changed, 117 insertions(+), 15 deletions(-)
 create mode 100644 Commands/Test/Check.cpp
 create mode 100644 Commands/Test/Check.h
 create mode 100644 Commands/Test/CheckDrive.cpp
 create mode 100644 Commands/Test/CheckDrive.h

diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
index c3e2e3b..eaf9669 100644
--- a/Commands/Autonomous/Autonomous.cpp
+++ b/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();
-}
diff --git a/Commands/Test/Check.cpp b/Commands/Test/Check.cpp
new file mode 100644
index 0000000..6151d27
--- /dev/null
+++ b/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));
+}
diff --git a/Commands/Test/Check.h b/Commands/Test/Check.h
new file mode 100644
index 0000000..37b14bf
--- /dev/null
+++ b/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
diff --git a/Commands/Test/CheckDrive.cpp b/Commands/Test/CheckDrive.cpp
new file mode 100644
index 0000000..73aff31
--- /dev/null
+++ b/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();
+}
diff --git a/Commands/Test/CheckDrive.h b/Commands/Test/CheckDrive.h
new file mode 100644
index 0000000..ec76d13
--- /dev/null
+++ b/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
diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp
index 80df4e7..cdb094e 100644
--- a/Subsystems/Drivetrain.cpp
+++ b/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;
+  }
+}
diff --git a/Subsystems/Drivetrain.h b/Subsystems/Drivetrain.h
index 81a5168..d4ca877 100644
--- a/Subsystems/Drivetrain.h
+++ b/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