forked from Candas1/Arduino-FOC
-
Notifications
You must be signed in to change notification settings - Fork 1
Expand file tree
/
Copy pathFOCMotor.cpp
More file actions
159 lines (142 loc) · 4.9 KB
/
Copy pathFOCMotor.cpp
File metadata and controls
159 lines (142 loc) · 4.9 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
#include "FOCMotor.h"
#include "../../communication/SimpleFOCDebug.h"
/**
* Default constructor - setting all variabels to default values
*/
FOCMotor::FOCMotor()
{
// maximum angular velocity to be used for positioning
velocity_limit = DEF_VEL_LIM;
// maximum voltage to be set to the motor
voltage_limit = DEF_POWER_SUPPLY;
// not set on the begining
current_limit = DEF_CURRENT_LIM;
// index search velocity
velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY;
// sensor and motor align voltage
voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN;
// default modulation is SinePWM
foc_modulation = FOCModulationType::SinePWM;
// default target value
target = 0;
voltage.d = 0;
voltage.q = 0;
// current target values
current_sp = 0;
current.q = 0;
current.d = 0;
// voltage bemf
voltage_bemf = 0;
// Initialize phase voltages U alpha and U beta used for inverse Park and Clarke transform
Ualpha = 0;
Ubeta = 0;
//monitor_port
monitor_port = nullptr;
//sensor
sensor_offset = 0.0f;
sensor = nullptr;
//current sensor
current_sense = nullptr;
}
/**
Sensor linking method
*/
void FOCMotor::linkSensor(Sensor* _sensor) {
sensor = _sensor;
}
/**
CurrentSense linking method
*/
void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) {
current_sense = _current_sense;
}
// shaft angle calculation
float FOCMotor::shaftAngle() {
// if no sensor linked return previous value ( for open loop )
if(!sensor) return shaft_angle;
return sensor_direction*LPF_angle(sensor->getAngle()) - sensor_offset;
}
// shaft velocity calculation
float FOCMotor::shaftVelocity() {
// if no sensor linked return previous value ( for open loop )
if(!sensor) return shaft_velocity;
return sensor_direction*LPF_velocity(sensor->getVelocity());
}
float FOCMotor::electricalAngle(){
// if no sensor linked return previous value ( for open loop )
if(!sensor) return electrical_angle;
return _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - zero_electric_angle );
}
/**
* Monitoring functions
*/
// function implementing the monitor_port setter
void FOCMotor::useMonitoring(Print &print){
monitor_port = &print; //operate on the address of print
#ifndef SIMPLEFOC_DISABLE_DEBUG
SimpleFOCDebug::enable(&print);
SIMPLEFOC_DEBUG("MOT: Monitor enabled!");
#endif
}
// utility function intended to be used with serial plotter to monitor motor variables
// significantly slowing the execution down!!!!
void FOCMotor::monitor() {
if( !monitor_downsample || monitor_cnt++ < (monitor_downsample-1) ) return;
monitor_cnt = 0;
if(!monitor_port) return;
bool printed = 0;
if(monitor_variables & _MON_TARGET){
if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
monitor_port->print(target,monitor_decimals);
printed= true;
}
if(monitor_variables & _MON_VOLT_Q) {
if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
else if(printed) monitor_port->print(monitor_separator);
monitor_port->print(voltage.q,monitor_decimals);
printed= true;
}
if(monitor_variables & _MON_VOLT_D) {
if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
else if(printed) monitor_port->print(monitor_separator);
monitor_port->print(voltage.d,monitor_decimals);
printed= true;
}
// read currents if possible - even in voltage mode (if current_sense available)
if(monitor_variables & _MON_CURR_Q || monitor_variables & _MON_CURR_D) {
DQCurrent_s c = current;
if( current_sense && torque_controller != TorqueControlType::foc_current ){
c = current_sense->getFOCCurrents(electrical_angle);
c.q = LPF_current_q(c.q);
c.d = LPF_current_d(c.d);
}
if(monitor_variables & _MON_CURR_Q) {
if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
else if(printed) monitor_port->print(monitor_separator);
monitor_port->print(c.q*1000, monitor_decimals); // mAmps
printed= true;
}
if(monitor_variables & _MON_CURR_D) {
if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
else if(printed) monitor_port->print(monitor_separator);
monitor_port->print(c.d*1000, monitor_decimals); // mAmps
printed= true;
}
}
if(monitor_variables & _MON_VEL) {
if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
else if(printed) monitor_port->print(monitor_separator);
monitor_port->print(shaft_velocity,monitor_decimals);
printed= true;
}
if(monitor_variables & _MON_ANGLE) {
if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
else if(printed) monitor_port->print(monitor_separator);
monitor_port->print(shaft_angle,monitor_decimals);
printed= true;
}
if(printed){
if(monitor_end_char) monitor_port->println(monitor_end_char);
else monitor_port->println("");
}
}