hMove and cAngle
gyro.cpp@1:1b01dbd85589, 2019-06-22 (annotated)
- Committer:
- THtakahiro702286
- Date:
- Sat Jun 22 00:38:40 2019 +0000
- Revision:
- 1:1b01dbd85589
- Parent:
- 0:6371d70a58f2
2019 NHK B manual machine designated
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
THtakahiro702286 | 0:6371d70a58f2 | 1 | #include "gyro.h" |
THtakahiro702286 | 0:6371d70a58f2 | 2 | |
THtakahiro702286 | 0:6371d70a58f2 | 3 | gyro::gyro(int num) |
THtakahiro702286 | 0:6371d70a58f2 | 4 | { |
THtakahiro702286 | 0:6371d70a58f2 | 5 | wheelRad = new double[num]; |
THtakahiro702286 | 0:6371d70a58f2 | 6 | } |
THtakahiro702286 | 0:6371d70a58f2 | 7 | |
THtakahiro702286 | 0:6371d70a58f2 | 8 | void gyro::setRad(int num,double rad) |
THtakahiro702286 | 0:6371d70a58f2 | 9 | { |
THtakahiro702286 | 0:6371d70a58f2 | 10 | wheelRad[num] = rad; |
THtakahiro702286 | 0:6371d70a58f2 | 11 | } |
THtakahiro702286 | 0:6371d70a58f2 | 12 | |
THtakahiro702286 | 1:1b01dbd85589 | 13 | |
THtakahiro702286 | 0:6371d70a58f2 | 14 | double gyro::hMove(double way, int num) |
THtakahiro702286 | 0:6371d70a58f2 | 15 | { |
THtakahiro702286 | 0:6371d70a58f2 | 16 | delete[] wheelRad; |
THtakahiro702286 | 0:6371d70a58f2 | 17 | if(way == 10) {return 0;} |
THtakahiro702286 | 0:6371d70a58f2 | 18 | |
THtakahiro702286 | 0:6371d70a58f2 | 19 | return sin(way - wheelRad[num]); |
THtakahiro702286 | 0:6371d70a58f2 | 20 | } |
THtakahiro702286 | 0:6371d70a58f2 | 21 | |
THtakahiro702286 | 0:6371d70a58f2 | 22 | void gyro::setIdeal(double angle) |
THtakahiro702286 | 0:6371d70a58f2 | 23 | { |
THtakahiro702286 | 0:6371d70a58f2 | 24 | ideal = angle; |
THtakahiro702286 | 0:6371d70a58f2 | 25 | } |
THtakahiro702286 | 0:6371d70a58f2 | 26 | |
THtakahiro702286 | 1:1b01dbd85589 | 27 | void gyro::setErrorRange(double range) |
THtakahiro702286 | 1:1b01dbd85589 | 28 | { |
THtakahiro702286 | 1:1b01dbd85589 | 29 | errorRange = range; |
THtakahiro702286 | 1:1b01dbd85589 | 30 | } |
THtakahiro702286 | 1:1b01dbd85589 | 31 | |
THtakahiro702286 | 0:6371d70a58f2 | 32 | double gyro::cAngle(double angle) |
THtakahiro702286 | 0:6371d70a58f2 | 33 | { |
THtakahiro702286 | 0:6371d70a58f2 | 34 | toChange = 180 - ideal; |
THtakahiro702286 | 0:6371d70a58f2 | 35 | changed = angle + toChange; |
THtakahiro702286 | 0:6371d70a58f2 | 36 | if (changed > 180) |
THtakahiro702286 | 0:6371d70a58f2 | 37 | { |
THtakahiro702286 | 0:6371d70a58f2 | 38 | changed -= 360; |
THtakahiro702286 | 0:6371d70a58f2 | 39 | } |
THtakahiro702286 | 1:1b01dbd85589 | 40 | if(fabs(changed) < (180 - errorRange)) |
THtakahiro702286 | 0:6371d70a58f2 | 41 | { |
THtakahiro702286 | 0:6371d70a58f2 | 42 | if(changed >= 0) |
THtakahiro702286 | 0:6371d70a58f2 | 43 | { |
THtakahiro702286 | 0:6371d70a58f2 | 44 | if(180 - changed >= 25) |
THtakahiro702286 | 0:6371d70a58f2 | 45 | { |
THtakahiro702286 | 0:6371d70a58f2 | 46 | return -0.2; |
THtakahiro702286 | 0:6371d70a58f2 | 47 | } |
THtakahiro702286 | 0:6371d70a58f2 | 48 | return -0.1; |
THtakahiro702286 | 0:6371d70a58f2 | 49 | } |
THtakahiro702286 | 0:6371d70a58f2 | 50 | else |
THtakahiro702286 | 0:6371d70a58f2 | 51 | { |
THtakahiro702286 | 0:6371d70a58f2 | 52 | if(180 + changed >= 25) |
THtakahiro702286 | 0:6371d70a58f2 | 53 | { |
THtakahiro702286 | 0:6371d70a58f2 | 54 | return 0.2; |
THtakahiro702286 | 0:6371d70a58f2 | 55 | } |
THtakahiro702286 | 0:6371d70a58f2 | 56 | return 0.1; |
THtakahiro702286 | 0:6371d70a58f2 | 57 | } |
THtakahiro702286 | 0:6371d70a58f2 | 58 | } |
THtakahiro702286 | 0:6371d70a58f2 | 59 | return 0; |
THtakahiro702286 | 0:6371d70a58f2 | 60 | } |
THtakahiro702286 | 1:1b01dbd85589 | 61 | |
THtakahiro702286 | 1:1b01dbd85589 | 62 | void gyro::limitPower(double limit) |
THtakahiro702286 | 1:1b01dbd85589 | 63 | { |
THtakahiro702286 | 1:1b01dbd85589 | 64 | absLimit = limit / 180; |
THtakahiro702286 | 1:1b01dbd85589 | 65 | _limit = limit; |
THtakahiro702286 | 1:1b01dbd85589 | 66 | } |
THtakahiro702286 | 1:1b01dbd85589 | 67 | |
THtakahiro702286 | 1:1b01dbd85589 | 68 | double gyro::pAngle(double angle, double k) |
THtakahiro702286 | 1:1b01dbd85589 | 69 | { |
THtakahiro702286 | 1:1b01dbd85589 | 70 | toChange = -1 * ideal; |
THtakahiro702286 | 1:1b01dbd85589 | 71 | changed = angle + toChange; |
THtakahiro702286 | 1:1b01dbd85589 | 72 | if (changed > 180) |
THtakahiro702286 | 1:1b01dbd85589 | 73 | { |
THtakahiro702286 | 1:1b01dbd85589 | 74 | changed -= 360; |
THtakahiro702286 | 1:1b01dbd85589 | 75 | } |
THtakahiro702286 | 1:1b01dbd85589 | 76 | if(changed < -180) |
THtakahiro702286 | 1:1b01dbd85589 | 77 | { |
THtakahiro702286 | 1:1b01dbd85589 | 78 | changed += 360; |
THtakahiro702286 | 1:1b01dbd85589 | 79 | } |
THtakahiro702286 | 1:1b01dbd85589 | 80 | rVal = k * changed * absLimit; |
THtakahiro702286 | 1:1b01dbd85589 | 81 | if((int)(angled + 0.5) == (int)(angle + 0.5)) |
THtakahiro702286 | 1:1b01dbd85589 | 82 | { |
THtakahiro702286 | 1:1b01dbd85589 | 83 | rVal += redVal; |
THtakahiro702286 | 1:1b01dbd85589 | 84 | } |
THtakahiro702286 | 1:1b01dbd85589 | 85 | if(rVal < (-1 * _limit)) |
THtakahiro702286 | 1:1b01dbd85589 | 86 | { |
THtakahiro702286 | 1:1b01dbd85589 | 87 | rVal = -1 * _limit; |
THtakahiro702286 | 1:1b01dbd85589 | 88 | } |
THtakahiro702286 | 1:1b01dbd85589 | 89 | else if(rVal > _limit) |
THtakahiro702286 | 1:1b01dbd85589 | 90 | { |
THtakahiro702286 | 1:1b01dbd85589 | 91 | rVal = _limit; |
THtakahiro702286 | 1:1b01dbd85589 | 92 | } |
THtakahiro702286 | 1:1b01dbd85589 | 93 | redVal = rVal; |
THtakahiro702286 | 1:1b01dbd85589 | 94 | angled = angle; |
THtakahiro702286 | 1:1b01dbd85589 | 95 | return rVal; |
THtakahiro702286 | 1:1b01dbd85589 | 96 | } |
THtakahiro702286 | 1:1b01dbd85589 | 97 | |
THtakahiro702286 | 1:1b01dbd85589 | 98 | double gyro::lFunc(double num,double gain) |
THtakahiro702286 | 1:1b01dbd85589 | 99 | { |
THtakahiro702286 | 1:1b01dbd85589 | 100 | rFunc = num * gain; |
THtakahiro702286 | 1:1b01dbd85589 | 101 | if(rFunc >= 1) |
THtakahiro702286 | 1:1b01dbd85589 | 102 | { |
THtakahiro702286 | 1:1b01dbd85589 | 103 | rFunc = 1; |
THtakahiro702286 | 1:1b01dbd85589 | 104 | } |
THtakahiro702286 | 1:1b01dbd85589 | 105 | return rFunc; |
THtakahiro702286 | 1:1b01dbd85589 | 106 | } |