Revision 1:1b01dbd85589, committed 2019-06-22

Comitter:

THtakahiro702286

Date:

Sat Jun 22 00:38:40 2019 +0000

Parent:

0:6371d70a58f2

Commit message:

2019 NHK B manual machine designated

Changed in this revision

diff -r 6371d70a58f2 -r 1b01dbd85589 gyro.cpp
--- 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;
+}

diff -r 6371d70a58f2 -r 1b01dbd85589 gyro.h
--- a/gyro.h	Mon Jun 10 14:23:00 2019 +0000
+++ b/gyro.h	Sat Jun 22 00:38:40 2019 +0000
@@ -12,10 +12,25 @@
     double hMove(double way, int num);
     void setIdeal(double angle);
     double cAngle(double angle);
+    double pAngle(double angle, double k);
+    void limitPower(double limit);
+    void setErrorRange(double range);
+    double lFunc(double num,double gain);
+//    double slowIdeal(double changed);
 private:
     double* wheelRad;
     double ideal;
     double toChange;
     double changed;
+    double rVal;
+    double _limit;
+    double redVal;
+    double angled;
+    double absLimit;
+    double errorRange;
+    double rFunc;
+//    double stepwise;
+//    double rIdeal;
+//    bool checkStep;
 };
 #endif