omniwheel lib
Dependents: NHK2017_octopus NHK2017_octopus2 NHK2017_octopus2_drive 2018NHK_gakugaku_robo ... more
wheel.cpp@3:86fee122b81d, 2017-11-06 (annotated)
- Committer:
- takeuchi
- Date:
- Mon Nov 06 01:43:39 2017 +0900
- Revision:
- 3:86fee122b81d
- Parent:
- 2:9d972b2dad0f
- Child:
- 5:cfec945ea421
fix calc
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCHITAKE | 0:952a0ff1bf46 | 1 | #include "wheel.h" |
UCHITAKE | 0:952a0ff1bf46 | 2 | |
UCHITAKE | 0:952a0ff1bf46 | 3 | Wheel::Wheel() : |
UCHITAKE | 0:952a0ff1bf46 | 4 | radian(0), |
UCHITAKE | 0:952a0ff1bf46 | 5 | distance(1.0) |
UCHITAKE | 0:952a0ff1bf46 | 6 | { |
UCHITAKE | 0:952a0ff1bf46 | 7 | } |
UCHITAKE | 0:952a0ff1bf46 | 8 | |
UCHITAKE | 0:952a0ff1bf46 | 9 | Wheel::Wheel(double radian) : |
UCHITAKE | 0:952a0ff1bf46 | 10 | radian(radian), |
UCHITAKE | 0:952a0ff1bf46 | 11 | distance(1.0) |
UCHITAKE | 0:952a0ff1bf46 | 12 | { |
UCHITAKE | 0:952a0ff1bf46 | 13 | } |
UCHITAKE | 0:952a0ff1bf46 | 14 | |
UCHITAKE | 0:952a0ff1bf46 | 15 | Wheel::Wheel(double radian, double distance) : |
UCHITAKE | 0:952a0ff1bf46 | 16 | radian(radian), |
UCHITAKE | 0:952a0ff1bf46 | 17 | distance(distance) |
UCHITAKE | 0:952a0ff1bf46 | 18 | { |
UCHITAKE | 0:952a0ff1bf46 | 19 | } |
UCHITAKE | 0:952a0ff1bf46 | 20 | |
UCHITAKE | 0:952a0ff1bf46 | 21 | Wheel::Wheel(const Wheel &a) : |
UCHITAKE | 0:952a0ff1bf46 | 22 | radian(a.radian), |
UCHITAKE | 0:952a0ff1bf46 | 23 | distance(a.distance) |
UCHITAKE | 0:952a0ff1bf46 | 24 | { |
UCHITAKE | 0:952a0ff1bf46 | 25 | } |
UCHITAKE | 0:952a0ff1bf46 | 26 | |
UCHITAKE | 0:952a0ff1bf46 | 27 | Wheel &Wheel::operator=(const Wheel &a) |
UCHITAKE | 0:952a0ff1bf46 | 28 | { |
UCHITAKE | 0:952a0ff1bf46 | 29 | if(this != &a) { |
UCHITAKE | 0:952a0ff1bf46 | 30 | radian = a.radian; |
UCHITAKE | 0:952a0ff1bf46 | 31 | distance = a.distance; |
UCHITAKE | 0:952a0ff1bf46 | 32 | } |
UCHITAKE | 0:952a0ff1bf46 | 33 | return *this; |
UCHITAKE | 0:952a0ff1bf46 | 34 | } |
UCHITAKE | 0:952a0ff1bf46 | 35 | |
UCHITAKE | 0:952a0ff1bf46 | 36 | Wheel &Wheel::operator=(double value) |
UCHITAKE | 0:952a0ff1bf46 | 37 | { |
UCHITAKE | 0:952a0ff1bf46 | 38 | setOutput(value); |
UCHITAKE | 0:952a0ff1bf46 | 39 | return *this; |
UCHITAKE | 0:952a0ff1bf46 | 40 | } |
UCHITAKE | 0:952a0ff1bf46 | 41 | |
uchitake |
2:9d972b2dad0f | 42 | Wheel &Wheel::operator=(float value) |
uchitake |
2:9d972b2dad0f | 43 | { |
uchitake |
2:9d972b2dad0f | 44 | setOutput(value); |
uchitake |
2:9d972b2dad0f | 45 | return *this; |
uchitake |
2:9d972b2dad0f | 46 | } |
uchitake |
2:9d972b2dad0f | 47 | |
UCHITAKE | 0:952a0ff1bf46 | 48 | Wheel::operator double() |
UCHITAKE | 0:952a0ff1bf46 | 49 | { |
UCHITAKE | 0:952a0ff1bf46 | 50 | return output; |
UCHITAKE | 0:952a0ff1bf46 | 51 | } |
UCHITAKE | 0:952a0ff1bf46 | 52 | |
UCHITAKE | 0:952a0ff1bf46 | 53 | Wheel::operator float() |
UCHITAKE | 0:952a0ff1bf46 | 54 | { |
UCHITAKE | 0:952a0ff1bf46 | 55 | return float(output); |
UCHITAKE | 0:952a0ff1bf46 | 56 | } |
UCHITAKE | 0:952a0ff1bf46 | 57 | |
UCHITAKE | 0:952a0ff1bf46 | 58 | void Wheel::setRadian(double tRadian) |
UCHITAKE | 0:952a0ff1bf46 | 59 | { |
UCHITAKE | 0:952a0ff1bf46 | 60 | radian = tRadian; |
UCHITAKE | 0:952a0ff1bf46 | 61 | } |
UCHITAKE | 0:952a0ff1bf46 | 62 | |
UCHITAKE | 0:952a0ff1bf46 | 63 | void Wheel::setDistance(double tDistance) |
UCHITAKE | 0:952a0ff1bf46 | 64 | { |
UCHITAKE | 0:952a0ff1bf46 | 65 | distance = tDistance; |
UCHITAKE | 0:952a0ff1bf46 | 66 | } |
UCHITAKE | 0:952a0ff1bf46 | 67 | |
UCHITAKE | 0:952a0ff1bf46 | 68 | void Wheel::setOutput(double value) |
UCHITAKE | 0:952a0ff1bf46 | 69 | { |
UCHITAKE | 0:952a0ff1bf46 | 70 | output = value; |
UCHITAKE | 0:952a0ff1bf46 | 71 | } |
UCHITAKE | 0:952a0ff1bf46 | 72 | |
UCHITAKE | 0:952a0ff1bf46 | 73 | double Wheel::calculateShift(double r, double theta) |
UCHITAKE | 0:952a0ff1bf46 | 74 | { |
UCHITAKE | 0:952a0ff1bf46 | 75 | outputShift = sin(theta + radian) * r; |
UCHITAKE | 0:952a0ff1bf46 | 76 | return outputShift; |
UCHITAKE | 0:952a0ff1bf46 | 77 | } |
UCHITAKE | 0:952a0ff1bf46 | 78 | |
UCHITAKE | 0:952a0ff1bf46 | 79 | double Wheel::calculateRotate(double X, double Y, double value) |
UCHITAKE | 0:952a0ff1bf46 | 80 | { |
UCHITAKE | 0:952a0ff1bf46 | 81 | outputRotate = fabs(X + tan(radian) * Y - cos(radian) - sin(radian) * tan(radian)) / hypot(1.0, tan(radian)); |
UCHITAKE | 0:952a0ff1bf46 | 82 | outputRotate *= value; |
takeuchi |
3:86fee122b81d | 83 | if(Y > 0) { |
takeuchi |
3:86fee122b81d | 84 | if(Y > (-1 * (X - sin(radian) * tan(radian))) / tan(radian)) outputRotate *= -1; |
takeuchi |
3:86fee122b81d | 85 | } |
takeuchi |
3:86fee122b81d | 86 | if(Y < 0) { |
takeuchi |
3:86fee122b81d | 87 | if(Y < (-1 * (X - sin(radian) * tan(radian))) / tan(radian)) outputRotate *= -1; |
takeuchi |
3:86fee122b81d | 88 | } |
UCHITAKE | 0:952a0ff1bf46 | 89 | return outputRotate; |
UCHITAKE | 0:952a0ff1bf46 | 90 | } |
UCHITAKE | 0:952a0ff1bf46 | 91 | |
UCHITAKE | 0:952a0ff1bf46 | 92 | double Wheel::getOutput() |
UCHITAKE | 0:952a0ff1bf46 | 93 | { |
UCHITAKE | 0:952a0ff1bf46 | 94 | return output; |
UCHITAKE | 1:e9b590a5b27a | 95 | } |