File content as of revision 29:cb03760ba9ea:

#include "includes.h"
#include "core.h"
#include "meta.h"
#include "sensors.h"
#include "filters.h"
#include "context.h"
#include "debug.h"

float debug_registers[32];

Context::Context() {
    _index = 0;
    _time_upd_ticker = new Ticker();
    _time_upd_ticker->attach_us(this, &Context::upd_function, 50);
}

void Context::ConfigureOutputs(PinName oa, PinName ob, PinName oc, PinName en) {
    _oa = oa;
    _ob = ob;
    _oc = oc;
    _en = en;
}

void Context::ConfigureCurrentSensors(PinName ib_pin, PinName ic_pin, float scale, float filter_strength) {
    _ib_pin = ib_pin;
    _ic_pin = ic_pin;
    _scale = scale;
    _filter_strength = filter_strength;
}

void Context::ConfigureIdPidController(float ki, float kp, float kd, float pidmax, float pidmin) {
    _dki = ki;
    _dkp = kp;
    _dkd = kd;
    _dpidmax = pidmax;
    _dpidmin = pidmin;
}


void Context::ConfigureIqPidController(float ki, float kp, float kd, float pidmax, float pidmin) {
    _qki = ki;
    _qkp = kp;
    _qkd = kd;
    _qpidmax = pidmax;
    _qpidmin = pidmin;
}

void Context::ConfigureThrottle(PinName throttle_pin, float min, float max) {
    _throttle_pin = throttle_pin;
    _min = min;
    _max = max;
}

void Context::ConfigurePositionSensor(PinName pos_a_pin, PinName pos_b_pin, float cal1_a, float cal2_a, float cal1_b, float cal2_b, float offset) {
    _pos_a_pin = pos_a_pin;
    _pos_b_pin = pos_b_pin;
    _cal1_a = cal1_a;
    _cal2_a = cal2_a;
    _cal1_b = cal1_b;
    _cal2_b = cal2_b;
    _offset = offset;
}

void Context::ConfigureReference(float max_current) {
    _max_current = max_current;
}

void Context::AttachCallBack(void (*f)(Context *), int freq) {
    _call_times[_index] = 0;
    _call_periods[_index] = 1000000 / freq;
    _callbacks[_index++] = f;
}

void Context::Start() {    
    for (;;) {
        for (int i = 0; i < _index; i++) {
            if (_time - _call_times[i] >= _call_periods[i]) {
                (*_callbacks[i])(this);
                _call_times[i] = _time;
            }
        }
    }
}

void Context::ConfigureDebugger(int debugger_channels, int debugger_size) {
    _debugger_channels = debugger_channels;
    _debugger_size = debugger_size;
}

void Context::InitData() {
    sense_p = new AnalogHallPositionSensor(_pos_a_pin, _pos_b_pin, _cal1_a, _cal2_a, _cal1_b, _cal2_b, _offset);
    channel_ib = NativeAnalogIn::PinToAdcChannel(_ib_pin);
    channel_ic = NativeAnalogIn::PinToAdcChannel(_ic_pin);
    throttle = new Throttle(_throttle_pin, _min, _max);
    sense_bus = new VoltageSensor();
    sense_t_motor = new TempSensor();
    sense_t_inverter = new TempSensor();
    
    pid_d = new PidController(_dki, _dkp, _dkd, _dpidmax, _dpidmin);
    pid_q = new PidController(_qki, _qkp, _qkd, _qpidmax, _qpidmin);
    
    motor = new Motor(sense_p, sense_t_motor);
    inverter = new Inverter(_oa, _ob, _oc, _en, sense_bus, sense_t_inverter);
    user = new User(throttle);
    modulator = new SvmModulator(inverter);
    reference = new SynchronousReferenceSynthesizer(_max_current);
    filter_d = new MeanFilter(_filter_strength);
    filter_q = new MeanFilter(_filter_strength);
    
    
    serial = new Serial(USBTX, USBRX);
    serial->baud(115200);
    serial->printf("%s\n\r", "Init Serial Communications");
    
    debugger = new BufferedDebugger(this, _debugger_channels, _debugger_size);
}

void Context::upd_function() {
    _time += 50;
}