Diff: gyro.cpp

Revision:

0:6371d70a58f2

Child:

1:1b01dbd85589

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gyro.cpp	Mon Jun 10 14:23:00 2019 +0000
@@ -0,0 +1,54 @@
+#include "gyro.h"
+
+gyro::gyro(int num)
+{
+    wheelRad = new double[num];
+}
+
+void gyro::setRad(int num,double rad)
+{
+    wheelRad[num] = rad;
+}
+
+double gyro::hMove(double way, int num)
+{
+    delete[] wheelRad;
+    if(way == 10) {return 0;}
+
+    return sin(way - wheelRad[num]);
+}
+
+void gyro::setIdeal(double angle)
+{
+    ideal = angle;
+}
+
+double gyro::cAngle(double angle)
+{
+    toChange = 180 - ideal;
+    changed = angle + toChange;
+    if (changed > 180)
+    {
+        changed -= 360;
+    }
+    if(fabs(changed) < 179)
+    {
+        if(changed >= 0)
+        {
+            if(180 - changed >= 25)
+            {
+                return -0.2;
+            }
+            return -0.1;
+        }
+        else
+        {
+            if(180 + changed >= 25)
+            {
+                return 0.2;
+            }
+            return 0.1;
+        }
+    }
+    return 0;
+}