Autonomous Mode

src/AutonomousMode.h

@@ -0,0 +1,24 @@
+/*
+ * AutonomousMode.h
+ *
+ *  Created on: Feb 18, 2016
+ *      Author: Benjamin
+ */
+
+#ifndef AUTONOMOUSMODE_H
+#define AUTONOMOUSMODE_H
+
+
+class AutonomousMode{
+
+private:
+
+public:
+	int slotnum;
+
+	AutonomousMode(int slot): slotnum(slot){};
+};
+
+
+
+#endif /* SRC_AUTONOMOUSMODES_H_ */

src/ModeNum.h

@@ -0,0 +1,17 @@
+/*
+ * ModeNum.h
+ *
+ *  Created on: Feb 18, 2016
+ *      Author: Benjamin
+ */
+
+#ifndef MODENUM_H
+#define MODENUM_H
+
+enum slot{
+	slot1, slot2, slot3, slot4, slot5
+};
+
+
+
+#endif /* SRC_MODENUM_H_ */

src/Robot.cpp

@@ -1,3 +1,4 @@
+#include <AutonomousMode.h>
 #include "WPILib.h"
 #include <Subsystems/Drivetrain.h>
 
@@ -6,6 +7,8 @@
 
 #include "Subsystems/Shooter.h"
 #include "Subsystems/BallCollector.h"
+#include "ModeNum.h"
+#include <chrono>
 
 class Robot: public IterativeRobot {
 private:
@@ -17,6 +20,17 @@ class Robot: public IterativeRobot {
 	std::unique_ptr<Shooter> shooter;
 	std::unique_ptr<BallCollector> ball_collector;
 
+	// Chooosing an autonomous program
+	SendableChooser* chooser;
+	int slot_number;
+
+	std::chrono::milliseconds ms;
+
+	float distance1[5] = {22.20244104688751*12, 24.60806716851051*12, 17.39119351980729*12, 18.61805812566858*12, 25.83493649053858*12};
+	float angle1[5] = {0, 0, -M_PI/6, M_PI/6, 0};
+	float distance2[5] = {0, 0, 12*25/3, 12*25/3, 0};
+	float angle2[5] = {M_PI/3, M_PI/3, M_PI/2, -M_PI/2, -M_PI/3};
+
 	void RobotInit() {
 		// Input devices
 		controller = std::unique_ptr<XboxController>(new XboxController(0));
@@ -25,6 +39,15 @@ class Robot: public IterativeRobot {
 		drive_train = std::unique_ptr<Drivetrain> (new Drivetrain());
 		shooter = std::unique_ptr<Shooter> (new Shooter());
 		ball_collector = std::unique_ptr<BallCollector> (new BallCollector());
+
+		// Choosing an autonomous program
+		chooser = new SendableChooser();
+		chooser->AddDefault("Slot 1", new AutonomousMode(0));
+		chooser->AddObject("Slot 2", new AutonomousMode(1));
+		chooser->AddObject("Slot 3", new AutonomousMode(2));
+		chooser->AddObject("Slot 4", new AutonomousMode(3));
+		chooser->AddObject("Slot 5", new AutonomousMode(4));
+		SmartDashboard::PutData("Autonomous modes:", chooser);
 	}
 
 	/**
@@ -49,12 +72,25 @@ class Robot: public IterativeRobot {
 	 * or additional comparisons to the if-else structure below with additional strings & commands.
 	 */
 	void AutonomousInit() {
-		/* std::string autoSelected = SmartDashboard::GetString("Auto Selector", "Default");
-		if(autoSelected == "My Auto") {
-			autonomousCommand.reset(new MyAutoCommand());
-		} else {
-			autonomousCommand.reset(new ExampleCommand());
-		} */
+
+		slot_number = ((AutonomousMode*) chooser->GetSelected())->slotnum;
+		drive_train->DriveDistance(distance1[slot_number]);
+		drive_train->TurnAngle(angle1[slot_number]);
+		drive_train->DriveDistance(distance2[slot_number]);
+		drive_train->TurnAngle(angle2[slot_number]);
+		shooter->run_shooter();
+		ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+			std::chrono::system_clock::now().time_since_epoch()
+		);
+		while (!ms.count()+500<std::chrono::system_clock::now().time_since_epoch().count()){
+			//waits for 500 ms
+		}
+		ball_collector->Start();
+		while (!ms.count()+1000<std::chrono::system_clock::now().time_since_epoch().count()){
+			//waits for 1000 ms
+		}
+		ball_collector->Stop();
+		shooter->stop_shooter();
 	}
 
 	void AutonomousPeriodic() {

src/Subsystems/Drivetrain.h → src/Subsystems/DriveTrain.h

@@ -23,6 +23,7 @@ class Drivetrain: public Subsystem {
 	void ArcadeDrive(float magnitude, float rotate);
 
 	void DriveDistance(float distance);
+	void TurnAngle(float angle);
 };
 
 #endif