From 28d266c8c2cb15afdd41bd829d54b84f155359cf Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Fri, 6 Feb 2015 13:45:01 -0500
Subject: [PATCH] Started working on autonomous

---
 CommandBase.cpp                    |  8 --------
 Commands/Autonomous/AutoDrive.cpp  | 21 +++++++++++++++++++++
 Commands/Autonomous/AutoDrive.h    | 18 ++++++++++++++++++
 Commands/Autonomous/Autonomous.cpp | 20 ++++++++++++++++++++
 Commands/Autonomous/Autonomous.h   | 18 ++++++++++++++++++
 5 files changed, 77 insertions(+), 8 deletions(-)
 create mode 100644 Commands/Autonomous/AutoDrive.cpp
 create mode 100644 Commands/Autonomous/AutoDrive.h
 create mode 100644 Commands/Autonomous/Autonomous.cpp
 create mode 100644 Commands/Autonomous/Autonomous.h

diff --git a/CommandBase.cpp b/CommandBase.cpp
index 0b28ca5..c68ce0e 100644
--- a/CommandBase.cpp
+++ b/CommandBase.cpp
@@ -4,14 +4,6 @@
 #include "Subsystems/Elevator.h"
 #include "Subsystems/DIO.h"
 #include "Subsystems/AirCompressor.h"
-//#include "Commands/Drive.h"
-//#include "Commands/CloseCollector.h"
-//#include "Commands/OpenCollector.h"
-//#include "Commands/CollectTote.h"
-//#include "Commands/ReleaseTote.h"
-//#include "Commands/Raise.h"
-//#include "Commands/Lower.h"
-//#include "Commands/Calibrate.h"
 Drivetrain* CommandBase::drivetrain = NULL;
 Collector* CommandBase::collector = NULL;
 Elevator* CommandBase::elevator = NULL;
diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp
new file mode 100644
index 0000000..01718f2
--- /dev/null
+++ b/Commands/Autonomous/AutoDrive.cpp
@@ -0,0 +1,21 @@
+#include "AutoDrive.h"
+#include <cmath>
+#include "../../DentRobot.h"
+AutoDrive::AutoDrive() : Command("AutoDrive"){
+  Requires(DentRobot::drivetrain);
+  SetTimeout(1.0);
+}
+void AutoDrive::Initialize(){
+}
+void AutoDrive::Execute(){
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
+  DentRobot::drivetrain->DriveMecanum(0.5,0,0,0.9,0);
+}
+bool AutoDrive::IsFinished(){
+  return false;
+}
+void AutoDrive::End(){
+}
+void AutoDrive::Interrupted(){
+  End();
+}
diff --git a/Commands/Autonomous/AutoDrive.h b/Commands/Autonomous/AutoDrive.h
new file mode 100644
index 0000000..f7538b2
--- /dev/null
+++ b/Commands/Autonomous/AutoDrive.h
@@ -0,0 +1,18 @@
+#ifndef AUTODRIVE_H
+#define AUTODRIVE_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class AutoDrive: public Command{
+  public:
+    AutoDrive();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif
diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp
new file mode 100644
index 0000000..c3e2e3b
--- /dev/null
+++ b/Commands/Autonomous/Autonomous.cpp
@@ -0,0 +1,20 @@
+#include "Autonomous.h"
+#include "AutoDrive.h"
+#include <cmath>
+#include "Commands/CommandGroup.h"
+#include "../../DentRobot.h"
+Autonomous::Autonomous() : CommandGroup("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/Autonomous/Autonomous.h b/Commands/Autonomous/Autonomous.h
new file mode 100644
index 0000000..e584b17
--- /dev/null
+++ b/Commands/Autonomous/Autonomous.h
@@ -0,0 +1,18 @@
+#ifndef AUTONOMOUS_H
+#define AUTONOMOUS_H
+
+#include "Commands/Command.h"
+#include "../../CommandBase.h"
+#include "../../DentRobot.h"
+#include "WPILib.h"
+
+class Autonomous: public CommandGroup{
+  public:
+    Autonomous();
+    void Initialize();
+    void Execute();
+    bool IsFinished();
+    void End();
+    void Interrupted();
+};
+#endif