Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Embed:
(wiki syntax)
Show/hide line numbers
Calibration.cpp
00001 #include <math.h> 00002 00003 #include "BREMSStructs.h" 00004 #include "Calibration.h" 00005 #include "MathHelpers.h" 00006 #include "Transforms.h" 00007 00008 #include "hardware.h" 00009 #include "derived.h" 00010 #include "prefs.h" 00011 00012 #include "errors.h" 00013 00014 //output is in modulation depth 00015 void abc(float theta, float vd, float vq, float *a, float *b, float *c) { 00016 float valpha, vbeta; 00017 float va, vb, vc, voff; 00018 00019 invpark(vd, vq, sinf(theta), cosf(theta), &valpha, &vbeta); 00020 invclarke(valpha, vbeta, &va, &vb); 00021 vc = -va - vb; 00022 00023 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it 00024 va = va - voff; 00025 vb = vb - voff; 00026 vc = vc - voff; 00027 00028 *a = va; 00029 *b = vb; 00030 *c = vc; 00031 } 00032 00033 void calibrate_position(IOStruct *io) { 00034 if (!checks_passed()) { 00035 io->pc->printf("%s\n", "Errors present, exiting"); 00036 return; 00037 } 00038 io->pc->putc(127); 00039 io->pc->printf("%s\n", "Calibrating..."); 00040 00041 const int n = (int) (128); 00042 const int n2 = 10; 00043 00044 float delta = 2 * PI / (n * n2); 00045 00046 float error_f[n] = {0}; 00047 float error_b[n] = {0}; 00048 00049 float theta_ref = 0.0f; 00050 float theta_actual = 0.0f; 00051 float theta_last = 0.0f; 00052 float rollover = 0.0f; 00053 00054 float vd = 0.5f; 00055 float vq = 0.0f; 00056 float va, vb, vc = 0.0f; 00057 00058 abc(theta_ref, vd, vq, &va, &vb, &vc); 00059 00060 for (int i = 0; i < 40000; i++) { 00061 set_dtc(io->a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX); 00062 set_dtc(io->b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX); 00063 set_dtc(io->c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX); 00064 wait_us(100); 00065 } 00066 00067 theta_last = io->pos->GetElecPosition(); 00068 for (int i = 0; i < n; i++) { 00069 for (int j = 0; j < n2; j++) { 00070 theta_ref += delta; 00071 abc(theta_ref, vd, vq, &va, &vb, &vc); 00072 00073 set_dtc(io->a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX); 00074 set_dtc(io->b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX); 00075 set_dtc(io->c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX); 00076 wait_us(3000); 00077 } 00078 00079 theta_actual = io->pos->GetElecPosition(); 00080 00081 //compensate for position rollover 00082 if (theta_actual - theta_last < -PI) rollover += 2 * PI; 00083 if (theta_actual - theta_last > PI) rollover -= 2 * PI; 00084 00085 theta_last = theta_actual; 00086 00087 error_f[i] = theta_ref - theta_actual - rollover; 00088 } 00089 00090 for (int i = 0; i < n; i++) { 00091 for (int j = 0; j < n2; j++) { 00092 theta_ref -= delta; 00093 abc(theta_ref, vd, vq, &va, &vb, &vc); 00094 00095 set_dtc(io->a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX); 00096 set_dtc(io->b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX); 00097 set_dtc(io->c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX); 00098 wait_us(3000); 00099 } 00100 00101 theta_actual = io->pos->GetElecPosition(); 00102 00103 if (theta_actual - theta_last < -PI) rollover += 2 * PI; 00104 if (theta_actual - theta_last > PI) rollover -= 2 * PI; 00105 00106 theta_last = theta_actual; 00107 00108 error_b[i] = theta_ref - theta_actual- rollover; 00109 } 00110 00111 float offset = 0.0f; 00112 for (int i = 0; i < n; i++) { 00113 offset += (error_f[i] + error_b[n - 1 - i]) / (2.0f * n); 00114 } 00115 offset = fmodf(offset, 2 * PI); 00116 io->pc->printf("Done!\n"); 00117 io->pc->printf("Offset = %f\n", -offset); 00118 io->pc->printf("Use 'flush' to save this value to flash\n"); 00119 _POS_OFFSET = -offset; 00120 }
Generated on Tue Jul 12 2022 17:58:39 by 1.7.2