Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Calibration/Calibration.cpp@117:97da9eb4300e, 2017-04-23 (annotated)

Committer:
bwang
Date:
Sun Apr 23 07:17:48 2017 +0000
Revision:
117:97da9eb4300e
Child:
118:2b6dab10b69d
(untested) position sensor autocalibration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 117:97da9eb4300e 1 #include <math.h>
bwang 117:97da9eb4300e 2
bwang 117:97da9eb4300e 3 #include "BREMSStructs.h"
bwang 117:97da9eb4300e 4 #include "Calibration.h"
bwang 117:97da9eb4300e 5 #include "MathHelpers.h"
bwang 117:97da9eb4300e 6 #include "Transforms.h"
bwang 117:97da9eb4300e 7 #include "config_pins.h"
bwang 117:97da9eb4300e 8 #include "config_motor.h"
bwang 117:97da9eb4300e 9 #include "config_inverter.h"
bwang 117:97da9eb4300e 10
bwang 117:97da9eb4300e 11 //output is in modulation depth
bwang 117:97da9eb4300e 12 void abc(float theta, float vd, float vq, float *a, float *b, float *c) {
bwang 117:97da9eb4300e 13 float valpha, vbeta;
bwang 117:97da9eb4300e 14 float va, vb, vc, voff;
bwang 117:97da9eb4300e 15
bwang 117:97da9eb4300e 16 invpark(vd, vq, sinf(theta), cosf(theta), &valpha, &vbeta);
bwang 117:97da9eb4300e 17 invclarke(valpha, vbeta, &va, &vb);
bwang 117:97da9eb4300e 18 vc = -va - vb;
bwang 117:97da9eb4300e 19
bwang 117:97da9eb4300e 20 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang 117:97da9eb4300e 21 va = va - voff;
bwang 117:97da9eb4300e 22 vb = vb - voff;
bwang 117:97da9eb4300e 23 vc = vc - voff;
bwang 117:97da9eb4300e 24
bwang 117:97da9eb4300e 25 *a = va;
bwang 117:97da9eb4300e 26 *b = vb;
bwang 117:97da9eb4300e 27 *c = vc;
bwang 117:97da9eb4300e 28 }
bwang 117:97da9eb4300e 29
bwang 117:97da9eb4300e 30 void calibrate_position(IOStruct *io, ControlStruct *control) {
bwang 117:97da9eb4300e 31 io->pc->printf("%s\n", "Starting calibration procedure");
bwang 117:97da9eb4300e 32
bwang 117:97da9eb4300e 33 const int n = (int) (128 * POLE_PAIRS);
bwang 117:97da9eb4300e 34 const int n2 = 10;
bwang 117:97da9eb4300e 35
bwang 117:97da9eb4300e 36 float delta = 2 * PI * POLE_PAIRS / (n * n2);
bwang 117:97da9eb4300e 37
bwang 117:97da9eb4300e 38 float error_f[n] = {0};
bwang 117:97da9eb4300e 39 float error_b[n] = {0};
bwang 117:97da9eb4300e 40
bwang 117:97da9eb4300e 41 float theta_ref = 0.0f;
bwang 117:97da9eb4300e 42 float theta_actual = 0.0f;
bwang 117:97da9eb4300e 43
bwang 117:97da9eb4300e 44 float vd = 0.1f;
bwang 117:97da9eb4300e 45 float vq = 0.0f;
bwang 117:97da9eb4300e 46 float va, vb, vc = 0.0f;
bwang 117:97da9eb4300e 47
bwang 117:97da9eb4300e 48 abc(theta_ref, vd, vq, &va, &vb, &vc);
bwang 117:97da9eb4300e 49
bwang 117:97da9eb4300e 50 for (int i = 0; i < 40000; i++) {
bwang 117:97da9eb4300e 51 set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX);
bwang 117:97da9eb4300e 52 set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX);
bwang 117:97da9eb4300e 53 set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
bwang 117:97da9eb4300e 54 wait_us(100);
bwang 117:97da9eb4300e 55 }
bwang 117:97da9eb4300e 56
bwang 117:97da9eb4300e 57 for (int i = 0; i < n; i++) {
bwang 117:97da9eb4300e 58 for (int j = 0; j < n2; j++) {
bwang 117:97da9eb4300e 59 theta_ref += delta;
bwang 117:97da9eb4300e 60 abc(theta_ref, vd, vq, &va, &vb, &vc);
bwang 117:97da9eb4300e 61
bwang 117:97da9eb4300e 62 set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX);
bwang 117:97da9eb4300e 63 set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX);
bwang 117:97da9eb4300e 64 set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
bwang 117:97da9eb4300e 65 wait_us(100);
bwang 117:97da9eb4300e 66 }
bwang 117:97da9eb4300e 67 theta_actual = io->pos->GetMechPosition();
bwang 117:97da9eb4300e 68 error_f[i] = theta_ref / POLE_PAIRS - theta_actual;
bwang 117:97da9eb4300e 69 }
bwang 117:97da9eb4300e 70
bwang 117:97da9eb4300e 71 for (int i = 0; i < n; i++) {
bwang 117:97da9eb4300e 72 for (int j = 0; j < n2; j++) {
bwang 117:97da9eb4300e 73 theta_ref -= delta;
bwang 117:97da9eb4300e 74 abc(theta_ref, vd, vq, &va, &vb, &vc);
bwang 117:97da9eb4300e 75
bwang 117:97da9eb4300e 76 set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX);
bwang 117:97da9eb4300e 77 set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX);
bwang 117:97da9eb4300e 78 set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
bwang 117:97da9eb4300e 79 wait_us(100);
bwang 117:97da9eb4300e 80 }
bwang 117:97da9eb4300e 81 theta_actual = io->pos->GetMechPosition();
bwang 117:97da9eb4300e 82 error_b[i] = theta_ref / POLE_PAIRS - theta_actual;
bwang 117:97da9eb4300e 83 }
bwang 117:97da9eb4300e 84
bwang 117:97da9eb4300e 85 float offset = 0.0f;
bwang 117:97da9eb4300e 86 for (int i = 0; i < n; i++) {
bwang 117:97da9eb4300e 87 offset += (error_f[i] + error_b[n - 1 - i]) / (2.0f * n);
bwang 117:97da9eb4300e 88 }
bwang 117:97da9eb4300e 89 offset = fmodf(offset * POLE_PAIRS, 2 * PI);
bwang 117:97da9eb4300e 90 io->pc->printf("Offset: %f\n", offset);
bwang 117:97da9eb4300e 91 }