1 files changed, 48 insertions, 0 deletions
diff --git a/Navigation.cpp b/Navigation.cpp
new file mode 100644
index 0000000..796bb86
--- /dev/null
+++ b/Navigation.cpp
@@ -0,0 +1,48 @@
+#include "Navigation.h"
+#include "hardware.h"
+
+#include <math.h>
+
+
+Navigation::Navigation(Motor *m1, float p1, Motor *m2, float p2, Motor *m3, float p3) {
+  motors[0] = m1;
+  motorPos[0] = p1;
+  motors[1] = m2;
+  motorPos[1] = p2;
+  motors[2] = m3;
+  motorPos[2] = p3;
+  
+  speed = direction = spin = 0.0;
+  
+  update();
+}
+
+void Navigation::update() {
+  float s1, s2, s3;
+  
+  s1 = s2 = s3 = spin;
+  
+  s1 += speed*sin((direction-motorPos[0])*M_PI/180);
+  s2 += speed*sin((direction-motorPos[1])*M_PI/180);
+  s3 += speed*sin((direction-motorPos[2])*M_PI/180);
+  
+  if(ABS(s1) > 1.0) {
+    s1 /= ABS(s1);
+    s2 /= ABS(s1);
+    s3 /= ABS(s1);
+  }
+  if(ABS(s2) > 1.0) {
+    s1 /= ABS(s2);
+    s2 /= ABS(s2);
+    s3 /= ABS(s2);
+  }
+  if(ABS(s3) > 1.0) {
+    s1 /= ABS(s3);
+    s2 /= ABS(s3);
+    s3 /= ABS(s3);
+  }
+  
+  motors[0]->setSpeed(int(s1*255));
+  motors[1]->setSpeed(int(s2*255));
+  motors[2]->setSpeed(int(s3*255));
+}