File content as of revision 6:4960629abb90:

#include "isr.h"
#include "mbed.h"
#include "shared.h"
#include "constants.h"
#include "util.h"
#include "math.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->dtc;
    motor->dtc_b = sinetab[(int) (ib)] * motor->dtc;
    motor->dtc_c = sinetab[(int) (ic)] * motor->dtc;
#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.256f)/(0.484f-0.256f)-0.5f);
    float bscaled = 2*(((float)analogb-0.254f)/(0.474f-0.254f)-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(ascaled<0){
           if(bscaled<0) motor->angle = 90 - arctan[index];
           if(bscaled>0) motor->angle = 90 + arctan[index];
   } 
       
   if(ascaled>0){
           if(bscaled>0) motor->angle = 270 - arctan[index];
           if(bscaled<0) motor->angle = 270 + arctan[index];
   }
    
    if(motor->angle > 360.0f) motor->angle = 360.0f;
    if(motor->angle < 0) motor->angle = 0;
    
    //calculate rotational rate
    
    float speed_raw = motor->angle - motor->lastangle;
    //these IFs check for theta = 0 crossings
    if(speed_raw < -180.0f){
        speed_raw = motor->angle - motor->lastangle + 360.0f;
    }
    if(speed_raw > 180.0f){
        speed_raw = motor->angle - motor->lastangle - 360.0f;
    }
    motor->lastangle = motor->angle;
    
    motor->speed = (1.0f - SPEED_LPF) * speed_raw + SPEED_LPF * motor->speed;
    
#ifdef __DEBUG
    skipidx++;
    if (skipidx == SKIP) {
        skipidx = 0;
        if (!motor->halt) {
            fbuffer[bufidx] = motor->angle;
            bufidx++;
        }
    }
    if (bufidx == DBG_BUF_SZ) {
        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;
    motor->command = motor->throttle;
    if (motor->throttle < 0.05f & abs(motor->speed) < 1.0f) {
        motor->halt = 1;
    } else {
        motor->halt = 0;
    }
}

void isense_update() {
        
    float ia = ia_read;
    float ib = ib_read;
    
    if(motor->halt == 1){ //zero the current sensors
        motor->iazero = ia;
        motor->ibzero = ib;
    }
    
    ia = ia-motor->iazero;
    ib = ib-motor->ibzero;
    
    float irms = ia*ia + ib*ib + (ia+ib)*(ia+ib);
    
    motor->current = (1.0f - CURRENT_LPF) * irms + CURRENT_LPF * motor->current;
    
    float iset = IMAX * motor->command; 
    motor->iP = iset - motor->current;
//    motor->iI = motor->iIlast + motor->iP;
//    motor->iD = motor->iP - motor->ilast;
    motor->dtc = Kp_i*motor->iP; //+ Ki_i*motor->iI + Kd_i*motor->iD;
    
    if(motor->dtc < 0.0f){
        motor->dtc = 0.0f;
    }
    if(motor->dtc > 1.0f){
        motor->dtc = 1.0f;
    }

    
    
    
    //float error = irms - iset  
}