Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: Calibration/Calibration.cpp
- Revision:
- 117:97da9eb4300e
- Child:
- 118:2b6dab10b69d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Calibration/Calibration.cpp Sun Apr 23 07:17:48 2017 +0000 @@ -0,0 +1,91 @@ +#include <math.h> + +#include "BREMSStructs.h" +#include "Calibration.h" +#include "MathHelpers.h" +#include "Transforms.h" +#include "config_pins.h" +#include "config_motor.h" +#include "config_inverter.h" + +//output is in modulation depth +void abc(float theta, float vd, float vq, float *a, float *b, float *c) { + float valpha, vbeta; + float va, vb, vc, voff; + + invpark(vd, vq, sinf(theta), cosf(theta), &valpha, &vbeta); + invclarke(valpha, vbeta, &va, &vb); + vc = -va - vb; + + voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it + va = va - voff; + vb = vb - voff; + vc = vc - voff; + + *a = va; + *b = vb; + *c = vc; +} + +void calibrate_position(IOStruct *io, ControlStruct *control) { + io->pc->printf("%s\n", "Starting calibration procedure"); + + const int n = (int) (128 * POLE_PAIRS); + const int n2 = 10; + + float delta = 2 * PI * POLE_PAIRS / (n * n2); + + float error_f[n] = {0}; + float error_b[n] = {0}; + + float theta_ref = 0.0f; + float theta_actual = 0.0f; + + float vd = 0.1f; + float vq = 0.0f; + float va, vb, vc = 0.0f; + + abc(theta_ref, vd, vq, &va, &vb, &vc); + + for (int i = 0; i < 40000; i++) { + set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX); + set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX); + set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX); + wait_us(100); + } + + for (int i = 0; i < n; i++) { + for (int j = 0; j < n2; j++) { + theta_ref += delta; + abc(theta_ref, vd, vq, &va, &vb, &vc); + + set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX); + set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX); + set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX); + wait_us(100); + } + theta_actual = io->pos->GetMechPosition(); + error_f[i] = theta_ref / POLE_PAIRS - theta_actual; + } + + for (int i = 0; i < n; i++) { + for (int j = 0; j < n2; j++) { + theta_ref -= delta; + abc(theta_ref, vd, vq, &va, &vb, &vc); + + set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX); + set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX); + set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX); + wait_us(100); + } + theta_actual = io->pos->GetMechPosition(); + error_b[i] = theta_ref / POLE_PAIRS - theta_actual; + } + + float offset = 0.0f; + for (int i = 0; i < n; i++) { + offset += (error_f[i] + error_b[n - 1 - i]) / (2.0f * n); + } + offset = fmodf(offset * POLE_PAIRS, 2 * PI); + io->pc->printf("Offset: %f\n", offset); +} \ No newline at end of file