Added encoders

src/Commands/DriveDistance.cpp

@@ -0,0 +1,9 @@
+/*
+ * DriveDistance.cpp
+ *
+ *  Created on: Dec 28, 2015
+ *      Author: tony
+ */
+
+#include <Commands/DriveDistance.h>
+

src/Commands/DriveDistance.h

@@ -0,0 +1,16 @@
+/*
+ * DriveDistance.h
+ *
+ *  Created on: Dec 28, 2015
+ *      Author: tony
+ */
+
+#ifndef SRC_COMMANDS_DRIVEDISTANCE_H_
+#define SRC_COMMANDS_DRIVEDISTANCE_H_
+
+#include <CommandBase.h>
+
+class DriveDistance: public CommandBase {
+};
+
+#endif /* SRC_COMMANDS_DRIVEDISTANCE_H_ */

src/Subsystems/Drive.cpp

@@ -1,6 +1,7 @@
 #include "Drive.h"
 #include <Victor.h>
 #include <DoubleSolenoid.h>
+#include <Encoder.h>
 
 Drive::Drive() :
 		Subsystem("Drive") {
@@ -11,6 +12,12 @@ Drive::Drive() :
 	right2 = new Victor(4);
 	right3 = new Victor(5);
 	gearSolenoid = new DoubleSolenoid(0, 1);
+	leftEncoder = new Encoder(0, 1);
+	rightEncoder = new Encoder(2, 3);
+	leftEncoder->SetDistancePerPulse(1.0 / 360.0);
+	rightEncoder->SetDistancePerPulse(1.0 / 360.0);
+	leftEncoder->SetReverseDirection(true);
+	rightEncoder->SetReverseDirection(false);
 }
 
 Drive::~Drive() {
@@ -21,6 +28,8 @@ Drive::~Drive() {
 	delete right2;
 	delete right3;
 	delete gearSolenoid;
+	delete leftEncoder;
+	delete rightEncoder;
 }
 
 void Drive::SetSpeeds(float leftSpeed, float rightSpeed) {
@@ -39,6 +48,17 @@ void Drive::SetHighGear() {
 	gearSolenoid->Set(DoubleSolenoid::kForward);
 
 }
-void Drive::SetLowGear(){
+void Drive::SetLowGear() {
 	gearSolenoid->Set(DoubleSolenoid::kReverse);
 }
+
+double Drive::GetDistanceTraveled() {
+	double leftDistance = leftEncoder->GetDistance() * 19.867432;
+	double rightDistance = rightEncoder->GetDistance() * 19.867432;
+	return (leftDistance + rightDistance) / 2;
+}
+
+void Drive::ResetEncoders() {
+	leftEncoder->Reset();
+	rightEncoder->Reset();
+}

src/Subsystems/Drive.h

@@ -5,6 +5,7 @@
 
 class Victor;
 class DoubleSolenoid;
+class Encoder;
 
 class Drive: public Subsystem {
 public:
@@ -15,6 +16,8 @@ class Drive: public Subsystem {
 	void ResetSpeeds();
 	void SetHighGear();
 	void SetLowGear();
+	double GetDistanceTraveled();
+	void ResetEncoders();
 
 private:
 	Victor* left1;
@@ -24,6 +27,8 @@ class Drive: public Subsystem {
 	Victor* right2;
 	Victor* right3;
 	DoubleSolenoid* gearSolenoid;
+	Encoder* leftEncoder;
+	Encoder* rightEncoder;
 };
 
 #endif /* SRC_SUBSYSTEMS_DRIVE_H_ */