forked from Candas1/Arduino-FOC
-
Notifications
You must be signed in to change notification settings - Fork 1
Expand file tree
/
Copy pathGenericCurrentSense.cpp
More file actions
163 lines (153 loc) · 5.59 KB
/
Copy pathGenericCurrentSense.cpp
File metadata and controls
163 lines (153 loc) · 5.59 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
#include "GenericCurrentSense.h"
// GenericCurrentSense constructor
GenericCurrentSense::GenericCurrentSense(PhaseCurrent_s (*readCallback)(), void (*initCallback)()){
// if function provided add it to the
if(readCallback != nullptr) this->readCallback = readCallback;
if(initCallback != nullptr) this->initCallback = initCallback;
}
// Inline sensor init function
int GenericCurrentSense::init(){
// configure ADC variables
if(initCallback != nullptr) initCallback();
// calibrate zero offsets
calibrateOffsets();
// set the initialized flag
initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
// return success
return 1;
}
// Function finding zero offsets of the ADC
void GenericCurrentSense::calibrateOffsets(){
const int calibration_rounds = 1000;
// find adc offset = zero current voltage
offset_ia = 0;
offset_ib = 0;
offset_ic = 0;
// read the adc voltage 1000 times ( arbitrary number )
for (int i = 0; i < calibration_rounds; i++) {
PhaseCurrent_s current = readCallback();
offset_ia += current.a;
offset_ib += current.b;
offset_ic += current.c;
_delay(1);
}
// calculate the mean offsets
offset_ia = offset_ia / calibration_rounds;
offset_ib = offset_ib / calibration_rounds;
offset_ic = offset_ic / calibration_rounds;
}
// read all three phase currents (if possible 2 or 3)
PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){
PhaseCurrent_s current = readCallback();
current.a = (current.a - offset_ia); // amps
current.b = (current.b - offset_ib); // amps
current.c = (current.c - offset_ic); // amps
return current;
}
// Function aligning the current sense with motor driver
// if all pins are connected well none of this is really necessary! - can be avoided
// returns flag
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
int GenericCurrentSense::driverAlign(float voltage){
_UNUSED(voltage) ; // remove unused parameter warning
int exit_flag = 1;
if(skip_align) return exit_flag;
if (!initialized) return 0;
// // set phase A active and phases B and C down
// driver->setPwm(voltage, 0, 0);
// _delay(200);
// PhaseCurrent_s c = getPhaseCurrents();
// // read the current 100 times ( arbitrary number )
// for (int i = 0; i < 100; i++) {
// PhaseCurrent_s c1 = getPhaseCurrents();
// c.a = c.a*0.6f + 0.4f*c1.a;
// c.b = c.b*0.6f + 0.4f*c1.b;
// c.c = c.c*0.6f + 0.4f*c1.c;
// _delay(3);
// }
// driver->setPwm(0, 0, 0);
// // align phase A
// float ab_ratio = fabs(c.a / c.b);
// float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
// if( ab_ratio > 1.5f ){ // should be ~2
// gain_a *= _sign(c.a);
// }else if( ab_ratio < 0.7f ){ // should be ~0.5
// // switch phase A and B
// int tmp_pinA = pinA;
// pinA = pinB;
// pinB = tmp_pinA;
// gain_a *= _sign(c.b);
// exit_flag = 2; // signal that pins have been switched
// }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
// // switch phase A and C
// int tmp_pinA = pinA;
// pinA = pinC;
// pinC= tmp_pinA;
// gain_a *= _sign(c.c);
// exit_flag = 2;// signal that pins have been switched
// }else{
// // error in current sense - phase either not measured or bad connection
// return 0;
// }
// // set phase B active and phases A and C down
// driver->setPwm(0, voltage, 0);
// _delay(200);
// c = getPhaseCurrents();
// // read the current 50 times
// for (int i = 0; i < 100; i++) {
// PhaseCurrent_s c1 = getPhaseCurrents();
// c.a = c.a*0.6f + 0.4f*c1.a;
// c.b = c.b*0.6f + 0.4f*c1.b;
// c.c = c.c*0.6f + 0.4f*c1.c;
// _delay(3);
// }
// driver->setPwm(0, 0, 0);
// float ba_ratio = fabs(c.b/c.a);
// float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
// if( ba_ratio > 1.5f ){ // should be ~2
// gain_b *= _sign(c.b);
// }else if( ba_ratio < 0.7f ){ // it should be ~0.5
// // switch phase A and B
// int tmp_pinB = pinB;
// pinB = pinA;
// pinA = tmp_pinB;
// gain_b *= _sign(c.a);
// exit_flag = 2; // signal that pins have been switched
// }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
// // switch phase A and C
// int tmp_pinB = pinB;
// pinB = pinC;
// pinC = tmp_pinB;
// gain_b *= _sign(c.c);
// exit_flag = 2; // signal that pins have been switched
// }else{
// // error in current sense - phase either not measured or bad connection
// return 0;
// }
// // if phase C measured
// if(_isset(pinC)){
// // set phase B active and phases A and C down
// driver->setPwm(0, 0, voltage);
// _delay(200);
// c = getPhaseCurrents();
// // read the adc voltage 500 times ( arbitrary number )
// for (int i = 0; i < 50; i++) {
// PhaseCurrent_s c1 = getPhaseCurrents();
// c.c = (c.c+c1.c)/50.0f;
// }
// driver->setPwm(0, 0, 0);
// gain_c *= _sign(c.c);
// }
// if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
// exit flag is either
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
return exit_flag;
}