robot

Dependencies:   FastPWM3 mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Calibration.cpp Source File

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 }