File content as of revision 1:70eed554399b:

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

void dtc_update() { 
    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;
    
    if (motor->halt) {
        pha = 0.0f;
        phb = 1.0f;
        phc = 0.0f;
    } else {
        pha = DB_COEFF * motor->dtc_a + DB_OFFSET;
        phb = DB_COEFF * (1.0f - motor->dtc_b) + DB_OFFSET;
        phc = 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;
}

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;
    }
}