hirromu takaki / gyro

hMove and cAngle

Dependents:   moveTest

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?

UserRevisionLine numberNew 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 }