Bayley Wang
/
priustroller_3
temp repo
Fork of priustroller_2 by
context.cpp@31:86b87913d8e1, 2015-03-15 (annotated)
- Committer:
- bwang
- Date:
- Sun Mar 15 02:21:32 2015 +0000
- Revision:
- 31:86b87913d8e1
- Parent:
- 30:2b6d426f3bfc
- Child:
- 33:e7b132029bae
Current sensor zeroing about as good as I can get it;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 11:dccbaa9274c5 | 1 | #include "includes.h" |
bwang | 11:dccbaa9274c5 | 2 | #include "core.h" |
bwang | 11:dccbaa9274c5 | 3 | #include "meta.h" |
bwang | 11:dccbaa9274c5 | 4 | #include "sensors.h" |
bwang | 11:dccbaa9274c5 | 5 | #include "filters.h" |
bwang | 11:dccbaa9274c5 | 6 | #include "context.h" |
bwang | 24:f1ff9c7256b5 | 7 | #include "debug.h" |
bwang | 11:dccbaa9274c5 | 8 | |
bwang | 24:f1ff9c7256b5 | 9 | float debug_registers[32]; |
bwang | 16:c10d0aa3c0e1 | 10 | |
bwang | 11:dccbaa9274c5 | 11 | Context::Context() { |
bwang | 11:dccbaa9274c5 | 12 | _index = 0; |
bwang | 11:dccbaa9274c5 | 13 | _time_upd_ticker = new Ticker(); |
bwang | 11:dccbaa9274c5 | 14 | _time_upd_ticker->attach_us(this, &Context::upd_function, 50); |
bwang | 11:dccbaa9274c5 | 15 | } |
bwang | 11:dccbaa9274c5 | 16 | |
bwang | 11:dccbaa9274c5 | 17 | void Context::ConfigureOutputs(PinName oa, PinName ob, PinName oc, PinName en) { |
bwang | 11:dccbaa9274c5 | 18 | _oa = oa; |
bwang | 11:dccbaa9274c5 | 19 | _ob = ob; |
bwang | 11:dccbaa9274c5 | 20 | _oc = oc; |
bwang | 11:dccbaa9274c5 | 21 | _en = en; |
bwang | 11:dccbaa9274c5 | 22 | } |
bwang | 11:dccbaa9274c5 | 23 | |
bwang | 11:dccbaa9274c5 | 24 | void Context::ConfigureCurrentSensors(PinName ib_pin, PinName ic_pin, float scale, float filter_strength) { |
bwang | 11:dccbaa9274c5 | 25 | _ib_pin = ib_pin; |
bwang | 11:dccbaa9274c5 | 26 | _ic_pin = ic_pin; |
bwang | 11:dccbaa9274c5 | 27 | _scale = scale; |
bwang | 11:dccbaa9274c5 | 28 | _filter_strength = filter_strength; |
bwang | 11:dccbaa9274c5 | 29 | } |
bwang | 11:dccbaa9274c5 | 30 | |
bwang | 26:d00561c7bf43 | 31 | void Context::ConfigureIdPidController(float ki, float kp, float kd, float pidmax, float pidmin) { |
bwang | 26:d00561c7bf43 | 32 | _dki = ki; |
bwang | 26:d00561c7bf43 | 33 | _dkp = kp; |
bwang | 26:d00561c7bf43 | 34 | _dkd = kd; |
bwang | 26:d00561c7bf43 | 35 | _dpidmax = pidmax; |
bwang | 26:d00561c7bf43 | 36 | _dpidmin = pidmin; |
bwang | 26:d00561c7bf43 | 37 | } |
bwang | 26:d00561c7bf43 | 38 | |
bwang | 26:d00561c7bf43 | 39 | |
bwang | 26:d00561c7bf43 | 40 | void Context::ConfigureIqPidController(float ki, float kp, float kd, float pidmax, float pidmin) { |
bwang | 26:d00561c7bf43 | 41 | _qki = ki; |
bwang | 26:d00561c7bf43 | 42 | _qkp = kp; |
bwang | 26:d00561c7bf43 | 43 | _qkd = kd; |
bwang | 26:d00561c7bf43 | 44 | _qpidmax = pidmax; |
bwang | 26:d00561c7bf43 | 45 | _qpidmin = pidmin; |
bwang | 11:dccbaa9274c5 | 46 | } |
bwang | 11:dccbaa9274c5 | 47 | |
bwang | 11:dccbaa9274c5 | 48 | void Context::ConfigureThrottle(PinName throttle_pin, float min, float max) { |
bwang | 11:dccbaa9274c5 | 49 | _throttle_pin = throttle_pin; |
bwang | 11:dccbaa9274c5 | 50 | _min = min; |
bwang | 11:dccbaa9274c5 | 51 | _max = max; |
bwang | 11:dccbaa9274c5 | 52 | } |
bwang | 11:dccbaa9274c5 | 53 | |
bwang | 11:dccbaa9274c5 | 54 | 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) { |
bwang | 11:dccbaa9274c5 | 55 | _pos_a_pin = pos_a_pin; |
bwang | 11:dccbaa9274c5 | 56 | _pos_b_pin = pos_b_pin; |
bwang | 11:dccbaa9274c5 | 57 | _cal1_a = cal1_a; |
bwang | 13:79e247e54d78 | 58 | _cal2_a = cal2_a; |
bwang | 11:dccbaa9274c5 | 59 | _cal1_b = cal1_b; |
bwang | 11:dccbaa9274c5 | 60 | _cal2_b = cal2_b; |
bwang | 11:dccbaa9274c5 | 61 | _offset = offset; |
bwang | 11:dccbaa9274c5 | 62 | } |
bwang | 11:dccbaa9274c5 | 63 | |
bwang | 11:dccbaa9274c5 | 64 | void Context::ConfigureReference(float max_current) { |
bwang | 11:dccbaa9274c5 | 65 | _max_current = max_current; |
bwang | 11:dccbaa9274c5 | 66 | } |
bwang | 11:dccbaa9274c5 | 67 | |
bwang | 11:dccbaa9274c5 | 68 | void Context::AttachCallBack(void (*f)(Context *), int freq) { |
bwang | 11:dccbaa9274c5 | 69 | _call_times[_index] = 0; |
bwang | 11:dccbaa9274c5 | 70 | _call_periods[_index] = 1000000 / freq; |
bwang | 11:dccbaa9274c5 | 71 | _callbacks[_index++] = f; |
bwang | 11:dccbaa9274c5 | 72 | } |
bwang | 11:dccbaa9274c5 | 73 | |
nki | 29:cb03760ba9ea | 74 | void Context::Start() { |
bwang | 11:dccbaa9274c5 | 75 | for (;;) { |
bwang | 11:dccbaa9274c5 | 76 | for (int i = 0; i < _index; i++) { |
bwang | 11:dccbaa9274c5 | 77 | if (_time - _call_times[i] >= _call_periods[i]) { |
bwang | 11:dccbaa9274c5 | 78 | (*_callbacks[i])(this); |
bwang | 11:dccbaa9274c5 | 79 | _call_times[i] = _time; |
bwang | 11:dccbaa9274c5 | 80 | } |
bwang | 11:dccbaa9274c5 | 81 | } |
bwang | 11:dccbaa9274c5 | 82 | } |
bwang | 11:dccbaa9274c5 | 83 | } |
bwang | 11:dccbaa9274c5 | 84 | |
bwang | 24:f1ff9c7256b5 | 85 | void Context::ConfigureDebugger(int debugger_channels, int debugger_size) { |
bwang | 24:f1ff9c7256b5 | 86 | _debugger_channels = debugger_channels; |
bwang | 24:f1ff9c7256b5 | 87 | _debugger_size = debugger_size; |
bwang | 24:f1ff9c7256b5 | 88 | } |
bwang | 24:f1ff9c7256b5 | 89 | |
bwang | 11:dccbaa9274c5 | 90 | void Context::InitData() { |
nki | 30:2b6d426f3bfc | 91 | serial = new Serial(USBTX, USBRX); |
nki | 30:2b6d426f3bfc | 92 | serial->baud(115200); |
nki | 30:2b6d426f3bfc | 93 | serial->printf("%s\n\r", "Init Serial Communications"); |
nki | 30:2b6d426f3bfc | 94 | |
bwang | 13:79e247e54d78 | 95 | sense_p = new AnalogHallPositionSensor(_pos_a_pin, _pos_b_pin, _cal1_a, _cal2_a, _cal1_b, _cal2_b, _offset); |
nki | 29:cb03760ba9ea | 96 | channel_ib = NativeAnalogIn::PinToAdcChannel(_ib_pin); |
nki | 29:cb03760ba9ea | 97 | channel_ic = NativeAnalogIn::PinToAdcChannel(_ic_pin); |
nki | 30:2b6d426f3bfc | 98 | AnalogIn dummyb = AnalogIn(_ib_pin); |
nki | 30:2b6d426f3bfc | 99 | AnalogIn dummyc = AnalogIn(_ic_pin); |
nki | 30:2b6d426f3bfc | 100 | _dummyb = dummyb; |
nki | 30:2b6d426f3bfc | 101 | _dummyc = dummyc; |
bwang | 11:dccbaa9274c5 | 102 | throttle = new Throttle(_throttle_pin, _min, _max); |
bwang | 11:dccbaa9274c5 | 103 | sense_bus = new VoltageSensor(); |
bwang | 11:dccbaa9274c5 | 104 | sense_t_motor = new TempSensor(); |
bwang | 11:dccbaa9274c5 | 105 | sense_t_inverter = new TempSensor(); |
bwang | 11:dccbaa9274c5 | 106 | |
bwang | 26:d00561c7bf43 | 107 | pid_d = new PidController(_dki, _dkp, _dkd, _dpidmax, _dpidmin); |
bwang | 26:d00561c7bf43 | 108 | pid_q = new PidController(_qki, _qkp, _qkd, _qpidmax, _qpidmin); |
bwang | 11:dccbaa9274c5 | 109 | |
nki | 29:cb03760ba9ea | 110 | motor = new Motor(sense_p, sense_t_motor); |
bwang | 11:dccbaa9274c5 | 111 | inverter = new Inverter(_oa, _ob, _oc, _en, sense_bus, sense_t_inverter); |
bwang | 11:dccbaa9274c5 | 112 | user = new User(throttle); |
bwang | 19:280c62c71cf8 | 113 | modulator = new SvmModulator(inverter); |
bwang | 11:dccbaa9274c5 | 114 | reference = new SynchronousReferenceSynthesizer(_max_current); |
bwang | 11:dccbaa9274c5 | 115 | filter_d = new MeanFilter(_filter_strength); |
bwang | 11:dccbaa9274c5 | 116 | filter_q = new MeanFilter(_filter_strength); |
bwang | 17:fd6771a6685a | 117 | |
bwang | 31:86b87913d8e1 | 118 | wait(1); |
nki | 29:cb03760ba9ea | 119 | |
bwang | 31:86b87913d8e1 | 120 | double ib_mean, ic_mean; |
bwang | 31:86b87913d8e1 | 121 | TIM2->DIER = 0; |
bwang | 31:86b87913d8e1 | 122 | for (int i = 0; i < 100; i++) { |
nki | 30:2b6d426f3bfc | 123 | ADC1->SQR3 = 0; |
nki | 30:2b6d426f3bfc | 124 | ADC1->SQR3 |= channel_ib; |
nki | 30:2b6d426f3bfc | 125 | ADC1->CR2 |= ADC_CR2_SWSTART; |
nki | 30:2b6d426f3bfc | 126 | while ((ADC1->SR & 2) == 0){} |
bwang | 31:86b87913d8e1 | 127 | ib_mean += (float) ADC1->DR; |
nki | 30:2b6d426f3bfc | 128 | |
nki | 30:2b6d426f3bfc | 129 | ADC1->SQR3 = 0; |
nki | 30:2b6d426f3bfc | 130 | ADC1->SQR3 |= channel_ic; |
nki | 30:2b6d426f3bfc | 131 | ADC1->CR2 |= ADC_CR2_SWSTART; |
nki | 30:2b6d426f3bfc | 132 | while ((ADC1->SR & 2) == 0){} |
bwang | 31:86b87913d8e1 | 133 | ic_mean += (float) ADC1->DR; |
nki | 30:2b6d426f3bfc | 134 | } |
bwang | 31:86b87913d8e1 | 135 | TIM2->DIER = 1; |
bwang | 31:86b87913d8e1 | 136 | ib_mean /= 100.0f; |
bwang | 31:86b87913d8e1 | 137 | ib_mean /= 4095.0f; |
bwang | 31:86b87913d8e1 | 138 | ic_mean /= 100.0f; |
bwang | 31:86b87913d8e1 | 139 | ic_mean /= 4095.0f; |
nki | 30:2b6d426f3bfc | 140 | |
bwang | 31:86b87913d8e1 | 141 | motor->InitSensors((float) ib_mean, (float) ic_mean, _scale); |
bwang | 24:f1ff9c7256b5 | 142 | |
bwang | 24:f1ff9c7256b5 | 143 | debugger = new BufferedDebugger(this, _debugger_channels, _debugger_size); |
bwang | 11:dccbaa9274c5 | 144 | } |
bwang | 11:dccbaa9274c5 | 145 | |
bwang | 11:dccbaa9274c5 | 146 | void Context::upd_function() { |
bwang | 11:dccbaa9274c5 | 147 | _time += 50; |
bwang | 11:dccbaa9274c5 | 148 | } |