File content as of revision 48:a1a09c83d42c:

#include "mbed.h"
#include "math.h"

#include "PositionSensor.h"
#include "FastPWM.h"
#include "PwmIn.h"
#include "MathHelpers.h"
#include "Transforms.h"
#include "DQMapper.h"
#include "ThrottleMapper.h"

#include "BREMSStructs.h"
#include "BREMSConfig.h"

#include "config_motor.h"
#include "config_loop.h"
#include "config_pins.h"
#include "config_inverter.h"
#include "config_driving.h"

#include "main.h"

IOStruct io;
ReadDataStruct read;
FOCStruct foc;
ControlStruct control;

DQMapper *dq;
ThrottleMapper *th;

int loop_counter = 0;
bool control_enabled = false;

void update_velocity() {
    read.last_p_mech = read.p_mech;
    read.p_mech = io.pos->GetMechPosition();
    
    float dp_mech = read.p_mech - read.last_p_mech;
    if (dp_mech < -PI) dp_mech += 2 * PI;
    if (dp_mech >  PI) dp_mech -= 2 * PI;
    
    float w_raw = dp_mech * F_SW; //rad/s
    if (w_raw > W_CRAZY) w_raw = read.w; //with this limiting scheme noise < 0
    if (w_raw < -W_CRAZY) w_raw = read.w; //so we need to throw out the large deltas first
    
    read.w = update_filter(read.w, w_raw, W_FILTER_STRENGTH);
}

void commutate() {
    /*safety checks*/
    if (control_enabled && !io.throttle_in->get_enabled()) go_disabled();
    if (control_enabled && !io.pos->IsValid()) go_disabled();
    if (!control_enabled && io.throttle_in->get_enabled()) go_enabled();
    
    /*update velocity, references*/
    update_velocity();
    if (loop_counter % SLOW_LOOP_COUNTER == 0) {
        loop_counter = 0;
        slow_loop();
    }
    loop_counter++;
    
    /*update position, sin, cos*/
    foc.p = io.pos->GetElecPosition() - POS_OFFSET;
    float sin_p = sinf(foc.p);
    float cos_p = cosf(foc.p);
    
    /*scale and offset currents (adval1, 2 are updated in ISR*/    
    foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ia_supp_offset) / I_SCALE;
    foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ib_supp_offset) / I_SCALE;
    
    /*compute d, q*/
    clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
    park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
    
    /*PI controller*/
    control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
    control.q_filtered = update_filter(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
        
    float d_err = control.d_ref - control.d_filtered;
    float q_err = control.q_ref - control.q_filtered;
    
    control.d_integral += d_err * KI;
    control.q_integral += q_err * KI;
    
    control.q_integral = constrain(control.q_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
    control.d_integral = constrain(control.d_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
    
    foc.vd = KP * d_err + control.d_integral;// - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE;
    foc.vq = KP * q_err + control.q_integral;// + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE;
    
    foc.vd = constrain(foc.vd, -1.0f, 1.0f);
    foc.vq = constrain(foc.vq, -1.0f, 1.0f);
    
    if (!control_enabled) {
        foc.vd = 0.0f;
        foc.vq = 0.0f;
    }
    
    /*inverse transforms*/
    invpark(foc.vd, foc.vq, sin_p, cos_p, &foc.valpha, &foc.vbeta);
    
    float va, vb, vc, voff;
    
    invclarke(foc.valpha, foc.vbeta, &va, &vb);
    vc = -va - vb;
    
    /*SVPWM*/
    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;
    
    /*output to timers*/
    set_dtc(io.a, 0.5f + 0.5f * va);
    set_dtc(io.b, 0.5f + 0.5f * vb);
    set_dtc(io.c, 0.5f + 0.5f * vc);
}

void slow_loop() {
    float torque_percent = th->map(io.throttle_in->get_throttle(), read.w);
    dq->map(torque_percent, read.w, &control.d_ref, &control.q_ref);
}

void go_enabled() {
    control.d_integral = 0.0f;
    control.q_integral = 0.0f;
    control_enabled = true;
    io.en->write(1);
}

void go_disabled() {
    control_enabled = false;
    io.en->write(0);
}

float update_filter(float old, float x, float str) {
    return str * old + (1.0f - str) * x;
}

extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
    if (TIM1->SR & TIM_SR_UIF ) {
        ADC1->CR2  |= 0x40000000; 
        volatile int delay;
        for (delay = 0; delay < 35; delay++);
        read.adval1 = ADC1->DR;
        read.adval2 = ADC2->DR;
        commutate();
    }
    TIM1->SR = 0x00;
}

int main() {
    dq = new LutMapper();//LinearNoFWMapper(KT, TORQUE_MAX, FLUX_LINKAGE);
    th = new NullThrottleMapper();
    BREMSInit(&io, &read, &foc, &control, false);
    
    for (;;) {
    }
}