NagaokaRoboticsClub_mbedTeam / omni_wheel

omniwheel lib

Dependents:   NHK2017_octopus NHK2017_octopus2 NHK2017_octopus2_drive 2018NHK_gakugaku_robo ... more

wheel.cpp@5:cfec945ea421, 2019-01-21 (annotated)

Committer:
shinjitakano
Date:
Mon Jan 21 17:57:44 2019 +0900
Revision:
5:cfec945ea421
Parent:
3:86fee122b81d 
wheel.cpp???????

Who changed what in which revision?

UserRevisionLine numberNew 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 {
shinjitakano 5:cfec945ea421 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 }