hMove and cAngle
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; +}