File content as of revision 155:7c6005933d4c:

#include <math.h>

#include "BREMSStructs.h"
#include "Calibration.h"
#include "MathHelpers.h"
#include "Transforms.h"
#include "config_pins.h"
#include "config_motor.h"
#include "config_inverter.h"

//output is in modulation depth
void abc(float theta, float vd, float vq, float *a, float *b, float *c) {
    float valpha, vbeta;
    float va, vb, vc, voff;
    
    invpark(vd, vq, sinf(theta), cosf(theta), &valpha, &vbeta);
    invclarke(valpha, vbeta, &va, &vb);
    vc = -va - vb;
    
    voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
    va = va - voff;
    vb = vb - voff;
    vc = vc - voff;
    
    *a = va;
    *b = vb;
    *c = vc;
}

void calibrate_position(IOStruct *io) {
    io->pc->printf("%s\n", "Starting calibration procedure");
    
    const int n = (int) (128);
    const int n2 = 10;
    
    float delta = 2 * PI / (n * n2);
    
    float error_f[n] = {0};
    float error_b[n] = {0};
    
    float theta_ref = 0.0f;
    float theta_actual = 0.0f;
    float theta_last = 0.0f;
    float rollover = 0.0f;
    
    float vd = 0.5f;
    float vq = 0.0f;
    float va, vb, vc = 0.0f;
    
    abc(theta_ref, vd, vq, &va, &vb, &vc);
    
    for (int i = 0; i < 40000; i++) {
        set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX);
        set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX);
        set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
        wait_us(100);
    }
    
    theta_last = io->pos->GetElecPosition();
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n2; j++) {
            theta_ref += delta;
            abc(theta_ref, vd, vq, &va, &vb, &vc);
            
            set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX);
            set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX);
            set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
            wait_us(3000);
        }
        
        theta_actual = io->pos->GetElecPosition();
        
        //compensate for position rollover
        if (theta_actual - theta_last < -PI) rollover += 2 * PI;
        if (theta_actual - theta_last > PI) rollover -= 2 * PI;
        io->pc->printf("%f,%f\n", theta_actual + rollover, theta_ref);
        
        theta_last = theta_actual;
        
        error_f[i] = theta_ref - theta_actual - rollover;
    }
    
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n2; j++) {
            theta_ref -= delta;
            abc(theta_ref, vd, vq, &va, &vb, &vc);
            
            set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX);
            set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX);
            set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
            wait_us(3000);
        }
        
        theta_actual = io->pos->GetElecPosition();
        
        if (theta_actual - theta_last < -PI) rollover += 2 * PI;
        if (theta_actual - theta_last > PI) rollover -= 2 * PI;
        io->pc->printf("%f,%f\n", theta_actual + rollover, theta_ref);
        
        theta_last = theta_actual;
        
        error_b[i] = theta_ref - theta_actual- rollover;
    }
    
    float offset = 0.0f;
    for (int i = 0; i < n; i++) {
        offset += (error_f[i] + error_b[n - 1 - i]) / (2.0f * n);
    }
    offset = fmodf(offset, 2 * PI);
    io->pc->printf("Offset: %f\n", offset);
}