robot

Dependencies:   FastPWM3 mbed

DQMapper/DQMapper.cpp

Committer:
bwang
Date:
2017-07-01
Revision:
160:6948bb7bcabd
Parent:
124:e70ca81676fc
Child:
161:19eac809c727

File content as of revision 160:6948bb7bcabd:

#include "math.h"
#include "DQMapper.h"

#include "config_motor.h"
#include "config_table.h"

#include "ndtab.h"
#include "nqtab.h"
#include "pdtab.h"
#include "pqtab.h"
#include "zdtab.h"
#include "zqtab.h"

void LutMapper::map(float torque_percent, float w, float *d, float *q) {
    w *= POLE_PAIRS;
    bool neg = false;
    
    if (w < 0.f) {
        w = -w;
        torque_percent = -torque_percent;
        neg = true;
    }
    if (fabs(torque_percent) < 0.02f) {
        int index = (int) (w / W_STEP);
        if (index >= ROWS) index = ROWS - 1;
        
        *d = (float) zdtab[index] / 128.f;
        *q = (float) zqtab[index] / 128.f;
    } else if (torque_percent >= 0.02f) {
        int row = (int) (w / W_STEP);
        int col = (int) ((torque_percent - 0.02f) * COLUMNS);
        
        if (row >= ROWS) row = ROWS - 1;
        if (col >= COLUMNS) col = COLUMNS - 1;
        
        *d = (float) pdtab[row][col] / 128.f;
        *q = (float) pqtab[row][col] / 128.f;
    } else {
        int row = (int) (w / W_STEP);
        int col = (int) ((-torque_percent - 0.02f) * COLUMNS);
        
        if (row >= ROWS) row = ROWS - 1;
        if (col >= COLUMNS) col = COLUMNS - 1;
        
        *d = (float) ndtab[row][col] / 128.f;
        *q = (float) nqtab[row][col] / 128.f;
    }
    
    if (neg) {
        *q = -*q;
    }
}

void LinearNoFWMapper::map(float torque_percent, float w, float *d, float *q) {
    float is = torque_percent * _tmax / _kt;
    *d = (-_lambda + sqrtf(_lambda * _lambda + 8 * (Ld - Lq) * (Ld - Lq) * is * is)) / (4.f * (Ld - Lq));
    *q = sqrtf(is * is - *d * *d);
}

void AngleMapper::map(float torque_percent, float w, float *d, float *q) {
    *q = _is * torque_percent * sinf(_theta);
    *d = _is * torque_percent * cosf(_theta);
}

void DirectMapper::map(float torque_percent, float w, float *d, float *q) {
    *d = _id * torque_percent;
    *q = _iq * torque_percent;
}

void SwapMapper::map(float torque_percent, float w, float *d, float *q) {
    if (torque_percent < 0.5f) {
        *d = 0.0f;
        *q = _iq;
    } else {
        *d = _id;
        *q = 0.0f;
    }
}

void AutoMapper::map(float torque_percent, float w, float *d, float *q) {
    if (torque_percent > 0.99f) {
        _theta += (_phase_high - _phase_low) / _steps;
        torque_percent = 0.0f;
    }
    if (_theta >= _phase_high) {
        *q = 0.0f;
        *d = 0.0f;
        return;
    }
    
    *q = _is * torque_percent * sinf(_theta);
    *d = _is * torque_percent * cosf(_theta);
}