Bayley Wang
/
foc-ed_in_the_bot_compact
robot
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?
User | Revision | Line number | New 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 | } |