Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: Transforms/Transforms.cpp
- Revision:
- 0:bac9c3a3a6ca
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Transforms/Transforms.cpp Wed Mar 09 06:44:51 2016 +0000 @@ -0,0 +1,28 @@ +#include "mbed.h" +#include "Transforms.h" + +void Transforms::Park(float alpha, float beta, float theta, float *d, float *q){ + float cosine = cos(theta); + float sine = sin(theta); + *d = alpha * cosine - beta * sine; + *q = -beta * cosine - alpha * sine; +} + +void Transforms::InvPark(float d, float q, float theta, float *alpha, float *beta){ + float cosine = cos(theta); + float sine = sin(theta); + *alpha = d * cosine - q * sine; + *beta = q * cosine + d * sine; +} + +void Transforms::Clarke(float a, float b, float *alpha, float *beta){ + *alpha = a; + *beta = sqrtf(3.0f) / 3.0f * (a + 2.0f * b); +} + +void Transforms::InvClarke(float alpha, float beta, float *a, float *b, float *c){ + *a = alpha; + *b = 0.5f * (-alpha + sqrtf(3.0f) * beta); + *c = 0.5f * (-alpha - sqrtf(3.0f) * beta); +} +