Started working on autonomous

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;

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();
+}

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

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();
+}

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