File content as of revision 3:86ccde39f61b:

#include "isr.h"
#include "mbed.h"
#include "shared.h"
#include "constants.h"
#include "util.h"

void dtc_update() { 
#ifndef __SVM
    float ia = motor->angle - motor->sensor_phase;
    if (ia < 0) ia += 360.0f;
    if (ia >= 360.0f) ia -= 360.0f;    
    float ib = ia - 120.0f;
    if (ib < 0) ib += 360.0f;
    float ic = ia + 120.0f;
    if (ic >= 360.0f) ic -= 360.0f;
      
    motor->dtc_a = sinetab[(int) (ia)] * motor->throttle;
    motor->dtc_b = sinetab[(int) (ib)] * motor->throttle;
    motor->dtc_c = sinetab[(int) (ic)] * motor->throttle;
#else
    float ix = 120.0f - motor->angle;
    if (ix < 0) ix += 360.0f;
    if (ix >= 360.0f) ix -= 360.0f;
    float dx = sinetab[(int) ix];
    float iy = motor->angle;
    if (iy < 0) iy += 360.0f;
    if (iy >= 360.0f) iy -= 360.0f;
    float dy = sinetab[(int) iy];
    int sector = (int) (iy / 60.0f) + 1;
    
    switch(sector) {
    case 1:
        motor->dtc_a = 1.0f - dx - dy;
        motor->dtc_b = 1.0f + dx - dy;
        motor->dtc_c = 1.0f + dx + dy;
        break;
    case 2:
        motor->dtc_a = 1.0f - dx + dy;
        motor->dtc_b = 1.0f - dx - dy;
        motor->dtc_c = 1.0f + dx + dy;
        break;
    case 3:
        motor->dtc_a = 1.0f + dx + dy;
        motor->dtc_b = 1.0f - dx - dy;
        motor->dtc_c = 1.0f + dx - dy;
        break;
    case 4:
        motor->dtc_a = 1.0f + dx + dy;
        motor->dtc_b = 1.0f - dx + dy;
        motor->dtc_c = 1.0f - dx + dy; //suspect
        break;
    case 5:
        motor->dtc_a = 1.0f + dx - dy;
        motor->dtc_b = 1.0f + dx + dy;
        motor->dtc_c = 1.0f - dx - dy;
        break;
    case 6:
        motor->dtc_a = 1.0f - dx - dy;
        motor->dtc_b = 1.0f + dx + dy;
        motor->dtc_c = 1.0f - dx + dy;
        break;
    default:
        break;
    }   
#endif    
    if (motor->halt || motor->debug_stop) {
        setDtcA(0);
        setDtcB(1.0f);
        setDtcC(0);
    } else {
        setDtcA(DB_COEFF * motor->dtc_a + DB_OFFSET);
        setDtcB(DB_COEFF * (1.0f - motor->dtc_b) + DB_OFFSET);
        setDtcC(DB_COEFF * motor->dtc_c + DB_OFFSET);
    }
}

void pos_update() {
    float ascaled = 2 *(((float) analoga - 0.143f)/(0.618f - 0.143f) - 0.5f);
    float bscaled = 2 *(((float) analogb - 0.202f)/(0.542f - 0.202f) - 0.5f);
   
    float x = bscaled / ascaled;
    
    unsigned int index = (abs(x) / ATAN_UPPER_BOUND)* ATAN_TABLE_SIZE;
    
    if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
    
    if(bscaled < 0){
        if(ascaled < 0) motor->angle = arctan[index];
        if(ascaled > 0) motor->angle = 180.0f - arctan[index];
    } 
    
    if(bscaled > 0){
        if(ascaled > 0) motor->angle = 180.0f + arctan[index];
        if(ascaled < 0) motor->angle = 360.0f - arctan[index];
    }
    
    if(motor->angle > 360.0f) motor->angle = 360.0f;
    if(motor->angle < 0) motor->angle = 0;
    
#ifdef __DEBUG
    if (!motor->halt) {
        fbuffer[bufidx] = motor->angle;
        bufidx++;
    }
    if (bufidx == 10000) {
        motor->debug_stop = 1;
    }
#endif
}

void throttle_update() {
    float throttle_raw = throttle_read;
    throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
    if (throttle_raw < 0.0f) throttle_raw = 0.0f;
    motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
    if (motor->throttle < 0.05f) {
        motor->halt = 1;
    } else {
        motor->halt = 0;
    }
}