context.cpp

00001 #include "includes.h"
00002 #include "core.h"
00003 #include "meta.h"
00004 #include "sensors.h"
00005 #include "filters.h"
00006 #include "context.h"
00007 #include "debug.h"
00008 
00009 float debug_registers[32];
00010 
00011 Context::Context() {
00012     _index = 0;
00013     _time_upd_ticker = new Ticker();
00014     _time_upd_ticker->attach_us(this, &Context::upd_function, 50);
00015 }
00016 
00017 void Context::ConfigureOutputs(PinName oa, PinName ob, PinName oc, PinName en) {
00018     _oa = oa;
00019     _ob = ob;
00020     _oc = oc;
00021     _en = en;
00022 }
00023 
00024 void Context::ConfigureCurrentSensors(PinName ib_pin, PinName ic_pin, float scale, float filter_strength) {
00025     _ib_pin = ib_pin;
00026     _ic_pin = ic_pin;
00027     _scale = scale;
00028     _filter_strength = filter_strength;
00029 }
00030 
00031 void Context::ConfigureIdPidController(float ki, float kp, float kd, float pidmax, float pidmin) {
00032     _dki = ki;
00033     _dkp = kp;
00034     _dkd = kd;
00035     _dpidmax = pidmax;
00036     _dpidmin = pidmin;
00037 }
00038 
00039 
00040 void Context::ConfigureIqPidController(float ki, float kp, float kd, float pidmax, float pidmin) {
00041     _qki = ki;
00042     _qkp = kp;
00043     _qkd = kd;
00044     _qpidmax = pidmax;
00045     _qpidmin = pidmin;
00046 }
00047 
00048 void Context::ConfigureThrottle(PinName throttle_pin, float min, float max, float th_filter_strength) {
00049     _throttle_pin = throttle_pin;
00050     _min = min;
00051     _max = max;
00052     _th_filter_strength = th_filter_strength;
00053 }
00054 
00055 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) {
00056     _pos_a_pin = pos_a_pin;
00057     _pos_b_pin = pos_b_pin;
00058     _cal1_a = cal1_a;
00059     _cal2_a = cal2_a;
00060     _cal1_b = cal1_b;
00061     _cal2_b = cal2_b;
00062     _offset = offset;
00063 }
00064 
00065 void Context::ConfigureReference(float max_current) {
00066     _max_current = max_current;
00067 }
00068 
00069 void Context::AttachCallBack(void (*f)(Context *), int freq) {
00070     _call_times[_index] = 0;
00071     _call_periods[_index] = 1000000 / freq;
00072     _callbacks[_index++] = f;
00073 }
00074 
00075 void Context::Start() {
00076     InitData();
00077         
00078     for (;;) {
00079         for (int i = 0; i < _index; i++) {
00080             if (_time - _call_times[i] >= _call_periods[i]) {
00081                 (*_callbacks[i])(this);
00082                 _call_times[i] = _time;
00083             }
00084         }
00085     }
00086 }
00087 
00088 void Context::ConfigureDebugger(int debugger_channels, int debugger_size) {
00089     _debugger_channels = debugger_channels;
00090     _debugger_size = debugger_size;
00091 }
00092 
00093 void Context::InitData() {
00094     sense_p = new AnalogHallPositionSensor(_pos_a_pin, _pos_b_pin, _cal1_a, _cal2_a, _cal1_b, _cal2_b, _offset);
00095     sense_ib = new AnalogCurrentSensor(_ib_pin, _scale);
00096     sense_ic = new AnalogCurrentSensor(_ic_pin, _scale);
00097     throttle = new Throttle(_throttle_pin, _min, _max);
00098     sense_bus = new VoltageSensor();
00099     sense_t_motor = new TempSensor();
00100     sense_t_inverter = new TempSensor();
00101     
00102     pid_d = new PidController(_dki, _dkp, _dkd, _dpidmax, _dpidmin);
00103     pid_q = new PidController(_qki, _qkp, _qkd, _qpidmax, _qpidmin);
00104     
00105     motor = new Motor(sense_ic, sense_ib, sense_p, sense_t_motor);
00106     inverter = new Inverter(_oa, _ob, _oc, _en, sense_bus, sense_t_inverter);
00107     user = new User(throttle);
00108     modulator = new SvmModulator(inverter);
00109     reference = new SynchronousReferenceSynthesizer(_max_current);
00110     filter_d = new MeanFilter(_filter_strength);
00111     filter_q = new MeanFilter(_filter_strength);
00112     filter_th = new MeanFilter(_th_filter_strength);
00113     filter_sp = new MeanFilter(0.999);
00114     
00115     
00116     serial = new Serial(USBTX, USBRX);
00117     serial->baud(115200);
00118     serial->printf("%s\n\r", "Init Serial Communications");
00119     
00120     debugger = new BufferedDebugger(this, _debugger_channels, _debugger_size);
00121 }
00122 
00123 void Context::upd_function() {
00124     _time += 50;
00125 }