Diff: gyro.cpp

Revision:

1:1b01dbd85589

Parent:

0:6371d70a58f2

--- a/gyro.cpp	Mon Jun 10 14:23:00 2019 +0000
+++ b/gyro.cpp	Sat Jun 22 00:38:40 2019 +0000
@@ -10,6 +10,7 @@
     wheelRad[num] = rad;
 }
 
+
 double gyro::hMove(double way, int num)
 {
     delete[] wheelRad;
@@ -23,6 +24,11 @@
     ideal = angle;
 }
 
+void gyro::setErrorRange(double range)
+{
+    errorRange = range;
+}
+
 double gyro::cAngle(double angle)
 {
     toChange = 180 - ideal;
@@ -31,7 +37,7 @@
     {
         changed -= 360;
     }
-    if(fabs(changed) < 179)
+    if(fabs(changed) < (180 - errorRange))
     {
         if(changed >= 0)
         {
@@ -52,3 +58,49 @@
     }
     return 0;
 }
+
+void gyro::limitPower(double limit)
+{
+    absLimit = limit / 180;
+    _limit = limit;
+}
+
+double gyro::pAngle(double angle, double k)
+{
+    toChange =  -1 * ideal;
+    changed = angle + toChange;
+    if (changed > 180)
+    {
+        changed -= 360;
+    }
+    if(changed < -180)
+    {
+        changed += 360;
+    }
+    rVal = k * changed * absLimit;
+    if((int)(angled + 0.5) == (int)(angle + 0.5))
+    {
+        rVal += redVal;
+    }
+    if(rVal < (-1 * _limit))
+    {
+        rVal = -1 * _limit;
+    }
+    else if(rVal > _limit)
+    {
+        rVal = _limit;
+    }
+    redVal = rVal;
+    angled = angle;
+    return rVal;
+}
+
+double gyro::lFunc(double num,double gain)
+{
+    rFunc = num * gain;
+    if(rFunc >= 1)
+    {
+        rFunc = 1;
+    }
+    return rFunc;
+}