1 files changed, 31 insertions, 0 deletions
diff --git a/Navigation.h b/Navigation.h
new file mode 100644
index 0000000..561e342
--- /dev/null
+++ b/Navigation.h
@@ -0,0 +1,31 @@
+#ifndef _ROBOCUP_NAVIGATION_H_
+#define _ROBOCUP_NAVIGATION_H_
+
+#include "Motor.h"
+#include "util.h"
+
+
+class Navigation {
+  private:
+    Motor *motors[3];
+    float motorPos[3];
+    
+    float speed;
+    float direction;
+    float spin;
+    
+    void update();
+  public:
+    Navigation(Motor *m1, float p1, Motor *m2, float p2, Motor *m3, float p3);
+    
+    void setSpeed(float speed) {this->speed = CLAMP(0.0, speed, 1.0); update();}
+    float getSpeed() {return speed;}
+    
+    void setDirection(float direction) {this->direction = direction; update();}
+    float getDirection() {return direction;}
+    
+    void setSpin(float spin) {this->spin = CLAMP(-1.0, spin, 1.0); update();}
+    float getSpin() {return spin;}
+};
+
+#endif