Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Calibration/Calibration.cpp@252:38644631ed97, 2018-11-13 (annotated)

Committer:
bwang
Date:
Tue Nov 13 17:46:23 2018 +0000
Revision:
252:38644631ed97
Parent:
244:1c6c0af8508e 
11/13/2018 12:45 - hitting <return> on empty line prints "\r>", so that hitting enter after intially connecting to the controller generates a prompt

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