hMove and cAngle
gyro.cpp
- Committer:
- THtakahiro702286
- Date:
- 2019-06-22
- Revision:
- 1:1b01dbd85589
- Parent:
- 0:6371d70a58f2
File content as of revision 1:1b01dbd85589:
#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; } void gyro::setErrorRange(double range) { errorRange = range; } double gyro::cAngle(double angle) { toChange = 180 - ideal; changed = angle + toChange; if (changed > 180) { changed -= 360; } if(fabs(changed) < (180 - errorRange)) { 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; } 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; }