Skip to content
Permalink
Browse files

Intial brushed motor control working.

  • Loading branch information
tlalexander committed Jul 5, 2019
1 parent 7e0977a commit 28431bf5b605034d52de5ad723a061dd8f307af9
@@ -274,6 +274,21 @@ bool Axis::run_closed_loop_control_loop() {
set_step_dir_active(false);
return check_for_errors();
}
bool Axis::run_brushed_voltage_control_loop() {
run_control_loop([this](){
if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL)
return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;

// Note that all estimators are updated in the loop prefix in run_control_loop
float current_setpoint;
if (!controller_.update(0, 0, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, 0, 0))
return false; // set_error should update axis.error_
return true;
});
return check_for_errors();
}

bool Axis::run_idle_loop() {
// run_control_loop ignores missed modulation timing updates
@@ -324,6 +339,8 @@ void Axis::run_state_machine_loop() {
task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
task_chain_[pos++] = AXIS_STATE_IDLE;
} else if (requested_state_ == AXIS_STATE_BRUSHED_VOLTAGE_CONTROL) {
task_chain_[pos++] = AXIS_STATE_BRUSHED_VOLTAGE_CONTROL;
} else if (requested_state_ != AXIS_STATE_UNDEFINED) {
task_chain_[pos++] = requested_state_;
task_chain_[pos++] = AXIS_STATE_IDLE;
@@ -392,6 +409,18 @@ void Axis::run_state_machine_loop() {
status = run_closed_loop_control_loop();
} break;

case AXIS_STATE_BRUSHED_CURRENT_CONTROL: {
// Not implemented.
goto invalid_state_label;

} break;

case AXIS_STATE_BRUSHED_VOLTAGE_CONTROL: {
if (motor_.config_.direction==0)
goto invalid_state_label;
status = run_brushed_voltage_control_loop();
} break;

case AXIS_STATE_IDLE: {
run_idle_loop();
status = motor_.arm(); // done with idling - try to arm the motor
@@ -35,6 +35,8 @@ class Axis {
AXIS_STATE_CLOSED_LOOP_CONTROL = 8, //<! run closed loop control
AXIS_STATE_LOCKIN_SPIN = 9, //<! run lockin spin
AXIS_STATE_ENCODER_DIR_FIND = 10,
AXIS_STATE_BRUSHED_CURRENT_CONTROL = 11, //<! Not implemented.
AXIS_STATE_BRUSHED_VOLTAGE_CONTROL = 12, //<! run open loop brushed voltage control
};

struct LockinConfig_t {
@@ -179,6 +181,7 @@ class Axis {
bool run_lockin_spin();
bool run_sensorless_control_loop();
bool run_closed_loop_control_loop();
bool run_brushed_voltage_control_loop();
bool run_idle_loop();

void run_state_machine_loop();
@@ -291,6 +291,10 @@ bool Motor::run_calibration() {
return false;
} else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
// no calibration needed
} else if (config_.motor_type == MOTOR_TYPE_BRUSHED_CURRENT) {
// no calibration needed
} else if (config_.motor_type == MOTOR_TYPE_BRUSHED_VOLTAGE) {
// no calibration needed
} else {
return false;
}
@@ -322,6 +326,28 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
return true;
}

bool Motor::enqueue_brushed_voltage_timings(float voltage_setpoint) {

// Clamp voltage_setpoint to just shy of vbus voltage.
if((-vbus_voltage * BRUSHED_VOLTAGE_MAX_RATIO) > voltage_setpoint)
voltage_setpoint = -vbus_voltage * BRUSHED_VOLTAGE_MAX_RATIO;
if((vbus_voltage * BRUSHED_VOLTAGE_MAX_RATIO) < voltage_setpoint)
voltage_setpoint = vbus_voltage * BRUSHED_VOLTAGE_MAX_RATIO;

float drive_ratio = voltage_setpoint/vbus_voltage;

float tA, tB, tC;

tA = 0.5f;
tB = (drive_ratio + 1.0f)/2.0f;
tC = 1.0f - tB;

next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_valid_ = true;
return true;
}
// We should probably make FOC Current call FOC Voltage to avoid duplication.
bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) {
float c = our_arm_cos_f32(pwm_phase);
@@ -423,6 +449,11 @@ bool Motor::update(float current_setpoint, float phase, float phase_vel) {
//In gimbal motor mode, current is reinterptreted as voltage.
if(!FOC_voltage(0.0f, current_setpoint, pwm_phase))
return false;
} else if (config_.motor_type == MOTOR_TYPE_BRUSHED_CURRENT) {
//TBD
} else if (config_.motor_type == MOTOR_TYPE_BRUSHED_VOLTAGE) {
//In brushed motor mode, current is reinterptreted as voltage.
enqueue_brushed_voltage_timings(current_setpoint);
} else {
set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE);
return false;
@@ -28,7 +28,9 @@ class Motor {
enum MotorType_t {
MOTOR_TYPE_HIGH_CURRENT = 0,
// MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented
MOTOR_TYPE_GIMBAL = 2
MOTOR_TYPE_GIMBAL = 2,
MOTOR_TYPE_BRUSHED_CURRENT = 3,
MOTOR_TYPE_BRUSHED_VOLTAGE = 4
};

struct Iph_BC_t {
@@ -121,6 +123,7 @@ class Motor {
bool run_calibration();
bool enqueue_modulation_timings(float mod_alpha, float mod_beta);
bool enqueue_voltage_timings(float v_alpha, float v_beta);
bool enqueue_brushed_voltage_timings(float voltage_setpoint);
bool FOC_voltage(float v_d, float v_q, float pwm_phase);
bool FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase);
bool update(float current_setpoint, float phase, float phase_vel);
@@ -98,6 +98,9 @@ extern Axis *axes[AXIS_COUNT];
extern float oscilloscope[OSCILLOSCOPE_SIZE];
extern size_t oscilloscope_pos;

// TODO: See why this is needed. I saw DRV errors above this sometimes.
#define BRUSHED_VOLTAGE_MAX_RATIO 0.997f

// TODO: move
// this is technically not thread-safe but practically it might be
#define DEFINE_ENUM_FLAG_OPERATORS(ENUMTYPE) \
@@ -12,6 +12,8 @@
AXIS_STATE_CLOSED_LOOP_CONTROL = 8
AXIS_STATE_LOCKIN_SPIN = 9
AXIS_STATE_ENCODER_DIR_FIND = 10
AXIS_STATE_BRUSHED_CURRENT_CONTROL = 11
AXIS_STATE_BRUSHED_VOLTAGE_CONTROL = 12

class errors:
class axis:
@@ -59,6 +61,8 @@ class controller:
MOTOR_TYPE_HIGH_CURRENT = 0
#MOTOR_TYPE_LOW_CURRENT = 1
MOTOR_TYPE_GIMBAL = 2
MOTOR_TYPE_BRUSHED_CURRENT = 3
MOTOR_TYPE_BRUSHED_VOLTAGE = 4

CTRL_MODE_VOLTAGE_CONTROL = 0
CTRL_MODE_CURRENT_CONTROL = 1

0 comments on commit 28431bf

Please sign in to comment.
You can’t perform that action at this time.