Bayley Wang / priustroller

Prius IPM controller

Dependencies:   mbed

Fork of analoghalls5_5 by N K

Committer:
bwang
Date:
Mon Mar 16 15:40:07 2015 +0000
Revision:
35:83cf9564bd0c
Parent:
31:86b87913d8e1
Set for 3A Iq, 0A Id, Ki = 1e-3, Kp = Kd = 0, loop still oscillates but is passable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang11:dccbaa9274c5 1#include "includes.h"
bwang11:dccbaa9274c5 2#include "core.h"
bwang11:dccbaa9274c5 3#include "meta.h"
bwang11:dccbaa9274c5 4#include "sensors.h"
bwang11:dccbaa9274c5 5#include "filters.h"
bwang11:dccbaa9274c5 6#include "context.h"
bwang24:f1ff9c7256b5 7#include "debug.h"
bwang11:dccbaa9274c5 8
bwang24:f1ff9c7256b5 9float debug_registers[32];
bwang15:c10d0aa3c0e1 10
bwang11:dccbaa9274c5 11Context::Context() {
bwang11:dccbaa9274c5 12 _index = 0;
bwang11:dccbaa9274c5 13 _time_upd_ticker = new Ticker();
bwang11:dccbaa9274c5 14 _time_upd_ticker->attach_us(this, &Context::upd_function, 50);
bwang11:dccbaa9274c5 15}
bwang11:dccbaa9274c5 16
bwang11:dccbaa9274c5 17void Context::ConfigureOutputs(PinName oa, PinName ob, PinName oc, PinName en) {
bwang11:dccbaa9274c5 18 _oa = oa;
bwang11:dccbaa9274c5 19 _ob = ob;
bwang11:dccbaa9274c5 20 _oc = oc;
bwang11:dccbaa9274c5 21 _en = en;
bwang11:dccbaa9274c5 22}
bwang11:dccbaa9274c5 23
bwang11:dccbaa9274c5 24void Context::ConfigureCurrentSensors(PinName ib_pin, PinName ic_pin, float scale, float filter_strength) {
bwang11:dccbaa9274c5 25 _ib_pin = ib_pin;
bwang11:dccbaa9274c5 26 _ic_pin = ic_pin;
bwang11:dccbaa9274c5 27 _scale = scale;
bwang11:dccbaa9274c5 28 _filter_strength = filter_strength;
bwang11:dccbaa9274c5 29}
bwang11:dccbaa9274c5 30
bwang26:d00561c7bf43 31void Context::ConfigureIdPidController(float ki, float kp, float kd, float pidmax, float pidmin) {
bwang26:d00561c7bf43 32 _dki = ki;
bwang26:d00561c7bf43 33 _dkp = kp;
bwang26:d00561c7bf43 34 _dkd = kd;
bwang26:d00561c7bf43 35 _dpidmax = pidmax;
bwang26:d00561c7bf43 36 _dpidmin = pidmin;
bwang26:d00561c7bf43 37}
bwang26:d00561c7bf43 38
bwang26:d00561c7bf43 39
bwang26:d00561c7bf43 40void Context::ConfigureIqPidController(float ki, float kp, float kd, float pidmax, float pidmin) {
bwang26:d00561c7bf43 41 _qki = ki;
bwang26:d00561c7bf43 42 _qkp = kp;
bwang26:d00561c7bf43 43 _qkd = kd;
bwang26:d00561c7bf43 44 _qpidmax = pidmax;
bwang26:d00561c7bf43 45 _qpidmin = pidmin;
bwang11:dccbaa9274c5 46}
bwang11:dccbaa9274c5 47
bwang11:dccbaa9274c5 48void Context::ConfigureThrottle(PinName throttle_pin, float min, float max) {
bwang11:dccbaa9274c5 49 _throttle_pin = throttle_pin;
bwang11:dccbaa9274c5 50 _min = min;
bwang11:dccbaa9274c5 51 _max = max;
bwang11:dccbaa9274c5 52}
bwang11:dccbaa9274c5 53
bwang11:dccbaa9274c5 54void Context::ConfigurePositionSensor(PinName pos_a_pin, PinName pos_b_pin, float cal1_a, float cal2_a, float cal1_b, float cal2_b, float offset) {
bwang11:dccbaa9274c5 55 _pos_a_pin = pos_a_pin;
bwang11:dccbaa9274c5 56 _pos_b_pin = pos_b_pin;
bwang11:dccbaa9274c5 57 _cal1_a = cal1_a;
bwang13:79e247e54d78 58 _cal2_a = cal2_a;
bwang11:dccbaa9274c5 59 _cal1_b = cal1_b;
bwang11:dccbaa9274c5 60 _cal2_b = cal2_b;
bwang11:dccbaa9274c5 61 _offset = offset;
bwang11:dccbaa9274c5 62}
bwang11:dccbaa9274c5 63
bwang11:dccbaa9274c5 64void Context::ConfigureReference(float max_current) {
bwang11:dccbaa9274c5 65 _max_current = max_current;
bwang11:dccbaa9274c5 66}
bwang11:dccbaa9274c5 67
bwang11:dccbaa9274c5 68void Context::AttachCallBack(void (*f)(Context *), int freq) {
bwang11:dccbaa9274c5 69 _call_times[_index] = 0;
bwang11:dccbaa9274c5 70 _call_periods[_index] = 1000000 / freq;
bwang11:dccbaa9274c5 71 _callbacks[_index++] = f;
bwang11:dccbaa9274c5 72}
bwang11:dccbaa9274c5 73
nki33:e7b132029bae 74void Context::Start() {
nki33:e7b132029bae 75 InitData();
nki33:e7b132029bae 76
bwang11:dccbaa9274c5 77 for (;;) {
bwang11:dccbaa9274c5 78 for (int i = 0; i < _index; i++) {
bwang11:dccbaa9274c5 79 if (_time - _call_times[i] >= _call_periods[i]) {
bwang11:dccbaa9274c5 80 (*_callbacks[i])(this);
bwang11:dccbaa9274c5 81 _call_times[i] = _time;
bwang11:dccbaa9274c5 82 }
bwang11:dccbaa9274c5 83 }
bwang11:dccbaa9274c5 84 }
bwang11:dccbaa9274c5 85}
bwang11:dccbaa9274c5 86
bwang24:f1ff9c7256b5 87void Context::ConfigureDebugger(int debugger_channels, int debugger_size) {
bwang24:f1ff9c7256b5 88 _debugger_channels = debugger_channels;
bwang24:f1ff9c7256b5 89 _debugger_size = debugger_size;
bwang24:f1ff9c7256b5 90}
bwang24:f1ff9c7256b5 91
bwang11:dccbaa9274c5 92void Context::InitData() {
bwang13:79e247e54d78 93 sense_p = new AnalogHallPositionSensor(_pos_a_pin, _pos_b_pin, _cal1_a, _cal2_a, _cal1_b, _cal2_b, _offset);
nki33:e7b132029bae 94 sense_ib = new AnalogCurrentSensor(_ib_pin, _scale);
nki33:e7b132029bae 95 sense_ic = new AnalogCurrentSensor(_ic_pin, _scale);
bwang11:dccbaa9274c5 96 throttle = new Throttle(_throttle_pin, _min, _max);
bwang11:dccbaa9274c5 97 sense_bus = new VoltageSensor();
bwang11:dccbaa9274c5 98 sense_t_motor = new TempSensor();
bwang11:dccbaa9274c5 99 sense_t_inverter = new TempSensor();
bwang11:dccbaa9274c5 100
bwang26:d00561c7bf43 101 pid_d = new PidController(_dki, _dkp, _dkd, _dpidmax, _dpidmin);
bwang26:d00561c7bf43 102 pid_q = new PidController(_qki, _qkp, _qkd, _qpidmax, _qpidmin);
bwang11:dccbaa9274c5 103
nki33:e7b132029bae 104 motor = new Motor(sense_ic, sense_ib, sense_p, sense_t_motor);
bwang11:dccbaa9274c5 105 inverter = new Inverter(_oa, _ob, _oc, _en, sense_bus, sense_t_inverter);
bwang11:dccbaa9274c5 106 user = new User(throttle);
bwang19:280c62c71cf8 107 modulator = new SvmModulator(inverter);
bwang11:dccbaa9274c5 108 reference = new SynchronousReferenceSynthesizer(_max_current);
bwang11:dccbaa9274c5 109 filter_d = new MeanFilter(_filter_strength);
bwang11:dccbaa9274c5 110 filter_q = new MeanFilter(_filter_strength);
bwang16:fd6771a6685a 111
nki33:e7b132029bae 112 serial = new Serial(USBTX, USBRX);
nki33:e7b132029bae 113 serial->baud(115200);
nki33:e7b132029bae 114 serial->printf("%s\n\r", "Init Serial Communications");
bwang24:f1ff9c7256b5 115
bwang24:f1ff9c7256b5 116 debugger = new BufferedDebugger(this, _debugger_channels, _debugger_size);
bwang11:dccbaa9274c5 117}
bwang11:dccbaa9274c5 118
bwang11:dccbaa9274c5 119void Context::upd_function() {
bwang11:dccbaa9274c5 120 _time += 50;
bwang11:dccbaa9274c5 121}