Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Calibration/Calibration.cpp
- Committer:
- bwang
- Date:
- 2018-02-10
- Revision:
- 196:7172e6e28867
- Parent:
- 191:66861311bdcd
- Child:
- 232:47f6cf4f9126
File content as of revision 196:7172e6e28867:
#include <math.h> #include "BREMSStructs.h" #include "Calibration.h" #include "MathHelpers.h" #include "Transforms.h" #include "hardware.h" #include "derived.h" #include "prefs.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) { io->pc->printf("%s\n", "Starting calibration procedure"); const int n = (int) (128); const int n2 = 10; float delta = 2 * PI / (n * n2); float error_f[n] = {0}; float error_b[n] = {0}; float theta_ref = 0.0f; float theta_actual = 0.0f; float theta_last = 0.0f; float rollover = 0.0f; float vd = 0.5f; 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 * LINEAR_MODULATION_MAX); set_dtc(io->b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX); set_dtc(io->c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX); wait_us(100); } theta_last = io->pos->GetElecPosition(); 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 * LINEAR_MODULATION_MAX); set_dtc(io->b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX); set_dtc(io->c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX); wait_us(3000); } theta_actual = io->pos->GetElecPosition(); //compensate for position rollover if (theta_actual - theta_last < -PI) rollover += 2 * PI; if (theta_actual - theta_last > PI) rollover -= 2 * PI; io->pc->printf("%f,%f\n", theta_actual + rollover, theta_ref); theta_last = theta_actual; error_f[i] = theta_ref - theta_actual - rollover; } 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 * LINEAR_MODULATION_MAX); set_dtc(io->b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX); set_dtc(io->c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX); wait_us(3000); } theta_actual = io->pos->GetElecPosition(); if (theta_actual - theta_last < -PI) rollover += 2 * PI; if (theta_actual - theta_last > PI) rollover -= 2 * PI; io->pc->printf("%f,%f\n", theta_actual + rollover, theta_ref); theta_last = theta_actual; error_b[i] = theta_ref - theta_actual- rollover; } 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, 2 * PI); io->pc->printf("Offset: %f\n", offset); }