Jens Frank Jensen / Mbed 2 deprecated foc-ed_in_the_bot_compact

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FastPWM3

Calibration/Calibration.cpp@185:5c102874b490, 2018-02-09 (annotated)

Committer:
bwang
Date:
Fri Feb 09 22:19:52 2018 +0000
Revision:
185:5c102874b490
Parent:
184:633119bb0b77
Child:
186:c18db1e31da6
moved all the hard-coded values to defaults.h

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 184:633119bb0b77 7
bwang 184:633119bb0b77 8 #include "layout.h"
bwang 185:5c102874b490 9 #include "defaults.h"
bwang 185:5c102874b490 10 #include "derived.h"
bwang 185:5c102874b490 11 /*
bwang 117:97da9eb4300e 12 #include "config_motor.h"
bwang 117:97da9eb4300e 13 #include "config_inverter.h"
bwang 185:5c102874b490 14 */
bwang 117:97da9eb4300e 15
bwang 117:97da9eb4300e 16 //output is in modulation depth
bwang 117:97da9eb4300e 17 void abc(float theta, float vd, float vq, float *a, float *b, float *c) {
bwang 117:97da9eb4300e 18 float valpha, vbeta;
bwang 117:97da9eb4300e 19 float va, vb, vc, voff;
bwang 117:97da9eb4300e 20
bwang 117:97da9eb4300e 21 invpark(vd, vq, sinf(theta), cosf(theta), &valpha, &vbeta);
bwang 117:97da9eb4300e 22 invclarke(valpha, vbeta, &va, &vb);
bwang 117:97da9eb4300e 23 vc = -va - vb;
bwang 117:97da9eb4300e 24
bwang 117:97da9eb4300e 25 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang 117:97da9eb4300e 26 va = va - voff;
bwang 117:97da9eb4300e 27 vb = vb - voff;
bwang 117:97da9eb4300e 28 vc = vc - voff;
bwang 117:97da9eb4300e 29
bwang 117:97da9eb4300e 30 *a = va;
bwang 117:97da9eb4300e 31 *b = vb;
bwang 117:97da9eb4300e 32 *c = vc;
bwang 117:97da9eb4300e 33 }
bwang 117:97da9eb4300e 34
bwang 118:2b6dab10b69d 35 void calibrate_position(IOStruct *io) {
bwang 117:97da9eb4300e 36 io->pc->printf("%s\n", "Starting calibration procedure");
bwang 117:97da9eb4300e 37
bwang 119:ad7a6af6fba3 38 const int n = (int) (128);
bwang 117:97da9eb4300e 39 const int n2 = 10;
bwang 117:97da9eb4300e 40
bwang 119:ad7a6af6fba3 41 float delta = 2 * PI / (n * n2);
bwang 117:97da9eb4300e 42
bwang 117:97da9eb4300e 43 float error_f[n] = {0};
bwang 117:97da9eb4300e 44 float error_b[n] = {0};
bwang 117:97da9eb4300e 45
bwang 117:97da9eb4300e 46 float theta_ref = 0.0f;
bwang 117:97da9eb4300e 47 float theta_actual = 0.0f;
bwang 119:ad7a6af6fba3 48 float theta_last = 0.0f;
bwang 119:ad7a6af6fba3 49 float rollover = 0.0f;
bwang 117:97da9eb4300e 50
bwang 118:2b6dab10b69d 51 float vd = 0.5f;
bwang 117:97da9eb4300e 52 float vq = 0.0f;
bwang 117:97da9eb4300e 53 float va, vb, vc = 0.0f;
bwang 117:97da9eb4300e 54
bwang 117:97da9eb4300e 55 abc(theta_ref, vd, vq, &va, &vb, &vc);
bwang 117:97da9eb4300e 56
bwang 117:97da9eb4300e 57 for (int i = 0; i < 40000; i++) {
bwang 157:a9b2002994d5 58 set_dtc(io->a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX);
bwang 157:a9b2002994d5 59 set_dtc(io->b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX);
bwang 157:a9b2002994d5 60 set_dtc(io->c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX);
bwang 117:97da9eb4300e 61 wait_us(100);
bwang 117:97da9eb4300e 62 }
bwang 117:97da9eb4300e 63
bwang 119:ad7a6af6fba3 64 theta_last = io->pos->GetElecPosition();
bwang 117:97da9eb4300e 65 for (int i = 0; i < n; i++) {
bwang 117:97da9eb4300e 66 for (int j = 0; j < n2; j++) {
bwang 117:97da9eb4300e 67 theta_ref += delta;
bwang 117:97da9eb4300e 68 abc(theta_ref, vd, vq, &va, &vb, &vc);
bwang 117:97da9eb4300e 69
bwang 157:a9b2002994d5 70 set_dtc(io->a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX);
bwang 157:a9b2002994d5 71 set_dtc(io->b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX);
bwang 157:a9b2002994d5 72 set_dtc(io->c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX);
bwang 118:2b6dab10b69d 73 wait_us(3000);
bwang 117:97da9eb4300e 74 }
bwang 119:ad7a6af6fba3 75
bwang 119:ad7a6af6fba3 76 theta_actual = io->pos->GetElecPosition();
bwang 119:ad7a6af6fba3 77
bwang 119:ad7a6af6fba3 78 //compensate for position rollover
bwang 119:ad7a6af6fba3 79 if (theta_actual - theta_last < -PI) rollover += 2 * PI;
bwang 119:ad7a6af6fba3 80 if (theta_actual - theta_last > PI) rollover -= 2 * PI;
bwang 119:ad7a6af6fba3 81 io->pc->printf("%f,%f\n", theta_actual + rollover, theta_ref);
bwang 119:ad7a6af6fba3 82
bwang 119:ad7a6af6fba3 83 theta_last = theta_actual;
bwang 119:ad7a6af6fba3 84
bwang 119:ad7a6af6fba3 85 error_f[i] = theta_ref - theta_actual - rollover;
bwang 117:97da9eb4300e 86 }
bwang 117:97da9eb4300e 87
bwang 117:97da9eb4300e 88 for (int i = 0; i < n; i++) {
bwang 117:97da9eb4300e 89 for (int j = 0; j < n2; j++) {
bwang 117:97da9eb4300e 90 theta_ref -= delta;
bwang 117:97da9eb4300e 91 abc(theta_ref, vd, vq, &va, &vb, &vc);
bwang 117:97da9eb4300e 92
bwang 157:a9b2002994d5 93 set_dtc(io->a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX);
bwang 157:a9b2002994d5 94 set_dtc(io->b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX);
bwang 157:a9b2002994d5 95 set_dtc(io->c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX);
bwang 118:2b6dab10b69d 96 wait_us(3000);
bwang 117:97da9eb4300e 97 }
bwang 119:ad7a6af6fba3 98
bwang 119:ad7a6af6fba3 99 theta_actual = io->pos->GetElecPosition();
bwang 119:ad7a6af6fba3 100
bwang 119:ad7a6af6fba3 101 if (theta_actual - theta_last < -PI) rollover += 2 * PI;
bwang 119:ad7a6af6fba3 102 if (theta_actual - theta_last > PI) rollover -= 2 * PI;
bwang 119:ad7a6af6fba3 103 io->pc->printf("%f,%f\n", theta_actual + rollover, theta_ref);
bwang 119:ad7a6af6fba3 104
bwang 119:ad7a6af6fba3 105 theta_last = theta_actual;
bwang 119:ad7a6af6fba3 106
bwang 119:ad7a6af6fba3 107 error_b[i] = theta_ref - theta_actual- rollover;
bwang 117:97da9eb4300e 108 }
bwang 117:97da9eb4300e 109
bwang 117:97da9eb4300e 110 float offset = 0.0f;
bwang 117:97da9eb4300e 111 for (int i = 0; i < n; i++) {
bwang 117:97da9eb4300e 112 offset += (error_f[i] + error_b[n - 1 - i]) / (2.0f * n);
bwang 117:97da9eb4300e 113 }
bwang 119:ad7a6af6fba3 114 offset = fmodf(offset, 2 * PI);
bwang 117:97da9eb4300e 115 io->pc->printf("Offset: %f\n", offset);
bwang 117:97da9eb4300e 116 }