Ben Katz
/
Hobbyking_Cheetah_V1
FOC Implementation for putting multirotor motors in robots
Transforms/Transforms.cpp@7:dc5f27756e02, 2016-03-29 (annotated)
- Committer:
- benkatz
- Date:
- Tue Mar 29 01:05:46 2016 +0000
- Revision:
- 7:dc5f27756e02
- Parent:
- 5:51c6560bf624
- Child:
- 10:370851e6e132
propper svm;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benkatz | 0:4e1c4df6aabd | 1 | #include "mbed.h" |
benkatz | 0:4e1c4df6aabd | 2 | #include "Transforms.h" |
benkatz | 0:4e1c4df6aabd | 3 | #include "FastMath.h" |
benkatz | 0:4e1c4df6aabd | 4 | |
benkatz | 0:4e1c4df6aabd | 5 | using namespace FastMath; |
benkatz | 0:4e1c4df6aabd | 6 | |
benkatz | 0:4e1c4df6aabd | 7 | void Transforms::Park(float alpha, float beta, float theta, float *d, float *q){ |
benkatz | 7:dc5f27756e02 | 8 | //float cosine = cos(theta); |
benkatz | 7:dc5f27756e02 | 9 | //float sine = sin(theta); |
benkatz | 7:dc5f27756e02 | 10 | float cosine = FastCos(theta); |
benkatz | 7:dc5f27756e02 | 11 | float sine = FastSin(theta); |
benkatz | 0:4e1c4df6aabd | 12 | *d = alpha*cosine - beta*sine; |
benkatz | 0:4e1c4df6aabd | 13 | *q = -beta*cosine - alpha*sine; |
benkatz | 5:51c6560bf624 | 14 | //*d = alpha*cosine + beta*sine; |
benkatz | 5:51c6560bf624 | 15 | //*q = beta*cosine - alpha*sine; |
benkatz | 0:4e1c4df6aabd | 16 | //DAC->DHR12R1 = (int) (*q*49.648f) + 2048; |
benkatz | 0:4e1c4df6aabd | 17 | //DAC->DHR12R1 = (int) (*q*2048.0f) + 2048; |
benkatz | 0:4e1c4df6aabd | 18 | } |
benkatz | 0:4e1c4df6aabd | 19 | |
benkatz | 0:4e1c4df6aabd | 20 | void Transforms::InvPark(float d, float q, float theta, float *alpha, float *beta){ |
benkatz | 7:dc5f27756e02 | 21 | //float cosine = cos(theta); |
benkatz | 7:dc5f27756e02 | 22 | //float sine = sin(theta); |
benkatz | 7:dc5f27756e02 | 23 | float cosine = FastCos(theta); |
benkatz | 7:dc5f27756e02 | 24 | float sine = FastSin(theta); |
benkatz | 0:4e1c4df6aabd | 25 | *alpha = d*cosine - q*sine; |
benkatz | 0:4e1c4df6aabd | 26 | *beta = q*cosine + d*sine; |
benkatz | 0:4e1c4df6aabd | 27 | } |
benkatz | 0:4e1c4df6aabd | 28 | |
benkatz | 0:4e1c4df6aabd | 29 | void Transforms::Clarke(float a, float b, float *alpha, float *beta){ |
benkatz | 0:4e1c4df6aabd | 30 | *alpha = a; |
benkatz | 0:4e1c4df6aabd | 31 | *beta = 0.57735026919f*(a + 2.0f*b); |
benkatz | 0:4e1c4df6aabd | 32 | } |
benkatz | 0:4e1c4df6aabd | 33 | |
benkatz | 0:4e1c4df6aabd | 34 | void Transforms::InvClarke(float alpha, float beta, float *a, float *b, float *c){ |
benkatz | 0:4e1c4df6aabd | 35 | *a = alpha; |
benkatz | 0:4e1c4df6aabd | 36 | *b = 0.5f*(-alpha + 1.73205080757f*beta); |
benkatz | 0:4e1c4df6aabd | 37 | *c = 0.5f*(-alpha - 1.73205080757f*beta); |
benkatz | 0:4e1c4df6aabd | 38 | } |
benkatz | 0:4e1c4df6aabd | 39 |