-
Notifications
You must be signed in to change notification settings - Fork 1.5k
/
controller.hpp
136 lines (111 loc) · 5.38 KB
/
controller.hpp
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
#ifndef __CONTROLLER_HPP
#define __CONTROLLER_HPP
class Controller : public ODriveIntf::ControllerIntf {
public:
struct Anticogging_t {
uint32_t index = 0;
float cogging_map[3600];
bool pre_calibrated = false;
bool calib_anticogging = false;
float calib_pos_threshold = 1.0f;
float calib_vel_threshold = 1.0f;
float cogging_ratio = 1.0f;
bool anticogging_enabled = true;
};
struct Autotuning_t {
float frequency = 0.0f;
float pos_amplitude = 0.0f;
float vel_amplitude = 0.0f;
float torque_amplitude = 0.0f;
};
struct Config_t {
ControlMode control_mode = CONTROL_MODE_POSITION_CONTROL; //see: ControlMode_t
InputMode input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t
float pos_gain = 20.0f; // [(turn/s) / turn]
float vel_gain = 1.0f / 6.0f; // [Nm/(turn/s)]
// float vel_gain = 0.2f / 200.0f, // [Nm/(rad/s)] <sensorless example>
float vel_integrator_gain = 2.0f / 6.0f; // [Nm/(turn/s * s)]
float vel_limit = 2.0f; // [turn/s] Infinity to disable.
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable.
float vel_integrator_limit = INFINITY; // Vel. integrator clamping value. Infinity to disable.
float vel_ramp_rate = 1.0f; // [(turn/s) / s]
float torque_ramp_rate = 0.01f; // Nm / sec
bool circular_setpoints = false;
float circular_setpoint_range = 1.0f; // Circular range when circular_setpoints is true. [turn]
uint32_t steps_per_circular_range = 1024;
float inertia = 0.0f; // [Nm/(turn/s^2)]
float input_filter_bandwidth = 2.0f; // [1/s]
float homing_speed = 0.25f; // [turn/s]
Anticogging_t anticogging;
float gain_scheduling_width = 10.0f;
bool enable_gain_scheduling = false;
bool enable_vel_limit = true;
bool enable_overspeed_error = true;
bool enable_torque_mode_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
uint8_t axis_to_mirror = -1;
float mirror_ratio = 1.0f;
float torque_mirror_ratio = 0.0f;
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration(). Set to -1 to select sensorless estimator.
float mechanical_power_bandwidth = 20.0f; // [rad/s] filter cutoff for mechanical power for spinout detction
float electrical_power_bandwidth = 20.0f; // [rad/s] filter cutoff for electrical power for spinout detection
float spinout_electrical_power_threshold = 10.0f; // [W] electrical power threshold for spinout detection
float spinout_mechanical_power_threshold = -10.0f; // [W] mechanical power threshold for spinout detection
// custom setters
Controller* parent;
void set_input_filter_bandwidth(float value) { input_filter_bandwidth = value; parent->update_filter_gains(); }
void set_steps_per_circular_range(uint32_t value) { steps_per_circular_range = value > 0 ? value : steps_per_circular_range; }
void set_control_mode(ControlMode value) { control_mode = value; parent->control_mode_updated(); }
};
bool apply_config();
void reset();
void set_error(Error error);
constexpr void input_pos_updated() {
input_pos_updated_ = true;
}
bool control_mode_updated();
void set_input_pos_and_steps(float pos);
bool select_encoder(size_t encoder_num);
// Trajectory-Planned control
void move_to_pos(float goal_point);
void move_incremental(float displacement, bool from_goal_point);
// TODO: make this more similar to other calibration loops
void start_anticogging_calibration();
float remove_anticogging_bias();
bool anticogging_calibration(float pos_estimate, float vel_estimate);
float get_anticogging_value(uint32_t index) {
return (index < 3600) ? config_.anticogging.cogging_map[index] : 0.0f;
}
void update_filter_gains();
bool update();
Config_t config_;
Axis* axis_ = nullptr; // set by Axis constructor
Error error_ = ERROR_NONE;
float last_error_time_ = 0.0f;
// Inputs
InputPort<float> pos_estimate_linear_src_;
InputPort<float> pos_estimate_circular_src_;
InputPort<float> vel_estimate_src_;
InputPort<float> pos_wrap_src_;
float pos_setpoint_ = 0.0f; // [turns]
float vel_setpoint_ = 0.0f; // [turn/s]
// float vel_setpoint = 800.0f; <sensorless example>
float vel_integrator_torque_ = 0.0f; // [Nm]
float torque_setpoint_ = 0.0f; // [Nm]
float input_pos_ = 0.0f; // [turns]
float input_vel_ = 0.0f; // [turn/s]
float input_torque_ = 0.0f; // [Nm]
float input_filter_kp_ = 0.0f;
float input_filter_ki_ = 0.0f;
Autotuning_t autotuning_;
float autotuning_phase_ = 0.0f;
bool input_pos_updated_ = false;
bool trajectory_done_ = true;
bool anticogging_valid_ = false;
float mechanical_power_ = 0.0f; // [W]
float electrical_power_ = 0.0f; // [W]
// Outputs
OutputPort<float> torque_output_ = 0.0f;
// custom setters
void set_input_pos(float value) { set_input_pos_and_steps(value); input_pos_updated(); }
};
#endif // __CONTROLLER_HPP