diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 566e8dc5..5150d704 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,3 +1,8 @@ +## UNRELEASED + +### Added +* Gimbal motor mode + ## [0.3.1] - 2018-01-18 ### Added diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 55373716..89caf961 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -49,6 +49,10 @@ const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CP // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public + +// NOTE: for gimbal motors, all units of A are instead V. +// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] +// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. Motor_t motors[] = { { // M0 @@ -92,6 +96,8 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup + .motor_type = MOTOR_TYPE_HIGH_CURRENT, + // .motor_type = MOTOR_TYPE_GIMBAL, .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { @@ -189,6 +195,7 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup + .motor_type = MOTOR_TYPE_HIGH_CURRENT, .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { @@ -804,17 +811,23 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { bool motor_calibration(Motor_t* motor) { motor->error = ERROR_NO_ERROR; - // #warning(hardcoded values for SK3-5065-280kv!) - // float R = 0.0332548246f; - // float L = 7.97315806e-06f; + float calibration_voltage = 0.0f; + if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (!measure_phase_resistance(motor, motor->calibration_current, 1.0f)) + return false; + calibration_voltage = motor->calibration_current * motor->phase_resistance; - if (!measure_phase_resistance(motor, motor->calibration_current, 1.0f)) - return false; - if (!measure_phase_inductance(motor, -1.0f, 1.0f)) + if (!measure_phase_inductance(motor, -1.0f, 1.0f)) + return false; + } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { + calibration_voltage = motor->calibration_current; + } else { return false; + } + if (motor->rotor_mode == ROTOR_MODE_ENCODER || motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) + if (!calib_enc_offset(motor, calibration_voltage)) return false; } @@ -898,19 +911,7 @@ __attribute__((unused)) void FOC_voltage_loop(Motor_t* motor, float v_d, float v osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); update_rotor(motor); - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float v_alpha = c * v_d - s * v_q; - float v_beta = c * v_q + s * v_d; - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_FOC_VOLTAGE_TIMING; - return; - } + FOC_voltage(motor, v_d, v_q); } } @@ -1176,6 +1177,22 @@ void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { queue_modulation_timings(motor, mod_alpha, mod_beta); } +bool FOC_voltage(Motor_t* motor, float v_d, float v_q) { + float phase = get_rotor_phase(motor); + float c = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float v_alpha = c*v_d - s*v_q; + float v_beta = c*v_q + s*v_d; + queue_voltage_timings(motor, v_alpha, v_beta); + + // Check we meet deadlines after queueing + if (!(check_timing(motor) < motor->control_deadline)) { + motor->error = ERROR_FOC_VOLTAGE_TIMING; + return false; + } + return true; +} + bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { Current_control_t* ictrl = &motor->current_control; @@ -1329,8 +1346,18 @@ void control_motor_loop(Motor_t* motor) { motor->current_control.Iq = Iq; // Execute current command - if (!FOC_current(motor, 0.0f, Iq)) { - break; // in case of error exit loop, motor->error has been set by FOC_current + if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if(!FOC_current(motor, 0.0f, Iq)){ + break; // in case of error exit loop, motor->error has been set by FOC_current + } + } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { + //In gimbal motor mode, current is reinterptreted as voltage. + if(!FOC_voltage(motor, 0.0f, Iq)){ + break; // in case of error exit loop, motor->error has been set by FOC_voltage + } + } else { + motor->error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + break; } update_brake_current(); diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index c9caccdc..34d88c8d 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -49,6 +49,7 @@ typedef enum { ERROR_POS_CTRL_DURING_SENSORLESS, ERROR_SPIN_UP_TIMEOUT, ERROR_DRV_FAULT, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, } Error_t; // Note: these should be sorted from lowest level of control to @@ -60,6 +61,12 @@ typedef enum { CTRL_MODE_POSITION_CONTROL } Motor_control_mode_t; +typedef enum { + MOTOR_TYPE_HIGH_CURRENT, + // MOTOR_TYPE_LOW_CURRENT, //Not yet implemented + MOTOR_TYPE_GIMBAL +} Motor_type_t; + typedef struct { float phB; float phC; @@ -150,6 +157,7 @@ typedef struct { Iph_BC_t DC_calib; DRV8301_Obj gate_driver; DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers + Motor_type_t motor_type; float shunt_conductance; float phase_current_rev_gain; //Reverse gain for ADC to Amps Current_control_t current_control; @@ -239,6 +247,7 @@ void update_brake_current(); void set_brake_current(float brake_current); void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta); void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta); +bool FOC_voltage(Motor_t* motor, float v_d, float v_q); bool FOC_current(Motor_t* motor, float Id_des, float Iq_des); void control_motor_loop(Motor_t* motor); diff --git a/Firmware/README.md b/Firmware/README.md index a72a6eb6..07b88e66 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -70,6 +70,17 @@ You must set: * `ENCODER_CPR`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. * `POLE_PAIRS`: This is the number of magnet poles in the rotor, divided by two. You can simply count the number of magnets in the rotor, if you can see them. * `brake_resistance`: This is the resistance of the brake resistor. If you are not using it, you may set it to 0.0f. +* `motor_type`: This is the type of motor being used. Currently two types of motors are supported -- High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). + +### Motor Modes +The firwmare currently supports two different types of motors, high-current motors, and Gimbal motors. If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For high-torque gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. + +**Further detail:** + +If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`. +If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (omega * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`. + +If 100's of mA current noise is "large" for you, and you intend to spin the motor fast, then you need to replace the shunt resistors on the ODrive. ### Tuning parameters The most important parameters are the limits: