Skip to content

Commit

Permalink
try a simple slip based algorithm instead of FOC, it seems to work be…
Browse files Browse the repository at this point in the history 
…tter
  • Loading branch information
@catphish
catphish committed Jul 29, 2018
1 parent 8f0fb59 commit 157c7ee
Showing 1 changed file with 53 additions and 96 deletions.
149 changes: 53 additions & 96 deletions main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,117 +7,74 @@
#define nop() __asm__ __volatile__ ("nop" ::)
#define PI 3.14159265f

// Performance metrics
int calculation_cycles;

// Debug data
int id_debug, iq_debug;
int vd_debug, vq_debug;
// Globals for data that is either persistent or used for debuging
int i1, i2, i3; // Phase currents
int total_current; // Total instantaneous current
int angle; // The angle of the stator voltage
int voltage; // The magnitude of the stator voltage
int encoder_position_previous; // The previous encoder value

// Analog to digital converter DMA data
uint16_t adc_data[4];
uint16_t adc_data[4]; // [i1, i2, i3, throttle]

// Motor parameters
const float loop_frequency = 9765.625f; // I use 8192 cycles of 80MHz MCU
const float loop_period = 0.0001024f; // 1/9765.625
const float time_constant = 0.06f; // Black magic and guesswork
const float motor_poles = 4.0f; // 4 Pole motor
const float encoder_resolution = 2400.0f; // Encoder produces 1200PPR, we use both lines
const float id_target = 3000; // Arbitrary choice of magnetising current
const float gain = 0.05f; // Gain for corrective voltage changes

// For flux estimator / FOC
float imag; // Stores current magnetizin current
float flux_angle; // The all important flux angle
uint32_t encoder_position_previous; // Does what it says it does
float vd, vq; // Persistent outputs voltages
#define MOTOR_POLES 4
#define ENCODER_RESOLUTION 2400
#define MAGNETIZING_CURRENT 3000
#define MAX_TORQUE_CURRENT 7000
#define GAIN 2

// This runs at the PWM frequency: 9765.625Hz
// It runs 0.000048s after it's triggered (approx half the PWM period)
// This means data is collected during the first half of each
// pwm cycle ready for calculations in the second half.
void DMA1_Channel1_IRQHandler(void) {
int start_time = TIM1 -> CNT;

// Fetch the phase 1 and 2 currents
int i1 = adc_data[0] - 32840; // Offset to 0A
int i2 = adc_data[1] - 32840; // Offset to 0A

// Clarke
float ia = i1;
float ib = 0.577350269f * i1 + 1.154700538f * i2;

// Park
float id = ia * cosf(flux_angle) + ib * sinf(flux_angle);
float iq = ib * cosf(flux_angle) - ia * sinf(flux_angle);
// Fetch the phase currents
i1 = adc_data[0] - 32840; // Offset to 0A
i2 = adc_data[1] - 32840; // Offset to 0A
i3 = adc_data[2] - 32840; // Offset to 0A
// Calculate the total current (FPU operation)
total_current = sqrtf((float)i1*(float)i1 + (float)i2*(float)i2 + (float)i3*(float)i3);

// Calculate encoder ticks change since the last iteration
uint32_t encoder_position_current = TIM2->CNT; // Atomic as possible
int encoder_position_change = encoder_position_current - encoder_position_previous; // Nasty casting to signed here
encoder_position_previous = encoder_position_current;

// Convert encoder pulse count to (electrical) radians/second
float rotor_velocity = encoder_position_change * loop_frequency / encoder_resolution * PI * motor_poles;

/////////////////////////
// Flux angle estimator//
/////////////////////////
// This is based largely on the algorithm described here:
// http://ww1.microchip.com/downloads/en/AppNotes/ACIM%20Vector%20Control%2000908a.pdf

imag = imag + (loop_period / time_constant) * (id - imag);
if(imag < 0) imag = -imag; // Magnetizing current is always positive, this avoids randomly switching pole

if(imag != 0.0f) { // Avoid divide by zero
float flux_slip_speed = (1.0f / time_constant) * (iq / imag);
float flux_speed = rotor_velocity + flux_slip_speed;
flux_angle = flux_angle + loop_period * flux_speed;
}
// Wrap the angle to maintain precision
if(flux_angle > PI) flux_angle -= PI*2.0f;
if(flux_angle < -PI) flux_angle += PI*2.0f;

// Calculate current errors
float iq_target = (adc_data[3]-32768) / 8; // Torque current target from throttle +/-
float id_error = id - id_target; // Torque current error
float iq_error = iq - iq_target; // Flux current error
vd -= id_error * gain; // Apply errors to ouutput voltages according to gain
vq -= iq_error * gain; // Apply errors to ouutput voltages according to gain

// Limits - This is very primative, we do not support field weakening yet
// We limit to 4000 (98% of the 12 bit inverter)
if(vd > 4000.0f) vd = 4000.0f; // Limit flux current directly
if(vd < -4000.0f) vd = -4000.0f; // Limit flux current directly
// If the total exceeds 4000, we weaken the torque
if(sqrtf(vd*vd+vq*vq) > 4000.0f) {
// Absolute value of torque to bring us up to 4000
float vqtmp = sqrtf(16000000.0f - vd * vd);
// Use the new value according to the original direction
if(vq > 0) vq = vqtmp; else vq = -vqtmp;
// 1 Revolution = 67108864 (2**26) increments
// 1 RPM = 6872 increments per loop (2**26 / 9765.625Hz)
// Encoder at 1 RPM produces 0.24576 pulses per loop (2400 / 9765.625Hz)

// Determine slip from the throttle
// By happy coincidence this gives +/- 32768 (+/- 5Hz)
int slip = adc_data[3] - 32768;
// Increment the angle by the requested slip
angle += slip;
// Increment the angle by the encoder measurement
angle += encoder_position_change * 67108864 / ENCODER_RESOLUTION * MOTOR_POLES / 2;

// Wrap angle
angle &= 0x3FFFFFF;

// Calculate the target current.
// This starts at MAGNETIZING_CURRENT and increases wih slip
// up to a maximum of MAGNETIZING_CURRENT + MAX_TORQUE_CURRENT.
// We use floating point operations here to avoid overflows
int target_current;
if(slip >= 0) {
target_current = (float)MAGNETIZING_CURRENT + (float)MAX_TORQUE_CURRENT * (float)slip / 32768.0f;
} else {
target_current = (float)MAGNETIZING_CURRENT + (float)MAX_TORQUE_CURRENT * -(float)slip / 32768.0f;
}

// Inverse Park
float va = vd * cosf(flux_angle) - vq * sinf(flux_angle);
float vb = vd * sinf(flux_angle) + vq * cosf(flux_angle);

// Instead of inverse clark, we convert to an angle and amplitude
float vangle = atan2f(vb, va);
int angle = ((vangle / PI) + 1.0f) * 4095;
int voltage = sqrtf(va * va + vb * vb);
// Apply current error to voltage output
if(total_current < target_current && voltage < 4000) voltage += GAIN;
if(total_current > target_current && voltage > 0) voltage -= GAIN;

// Update PWM outputs from SVM style lookup table
TIM1->CCR3 = (voltage * table1[angle]) >> 15;
TIM1->CCR2 = (voltage * table2[angle]) >> 15;
TIM1->CCR1 = (voltage * table3[angle]) >> 15;

// Debug output
id_debug = id;
iq_debug = iq;
vd_debug = vd;
vq_debug = vq;

// Store the number of cycles it took to do the math
calculation_cycles = start_time - TIM1 -> CNT;
TIM1->CCR3 = (voltage * table1[angle >> 13]) >> 15;
TIM1->CCR2 = (voltage * table2[angle >> 13]) >> 15;
TIM1->CCR1 = (voltage * table3[angle >> 13]) >> 15;

// Clear interrupt
DMA1->IFCR = 0xFFFFFFFF;
Expand All @@ -134,15 +91,15 @@ int main() {
while(1) {
// A handy loop for debugging
// All the real work is interrupt driven
uart_write_int(id_debug);
uart_write_int(i1);
uart_write_char(',');
uart_write_int(iq_debug);
uart_write_int(i2);
uart_write_char(',');
uart_write_int(vd_debug);
uart_write_int(i3);
uart_write_char(',');
uart_write_int(vq_debug);
uart_write_int(total_current);
uart_write_char(',');
uart_write_int(calculation_cycles);
uart_write_int(voltage);
uart_write_nl();
int n; for(n=0;n<1000;n++) nop();
}
Expand Down

0 comments on commit 157c7ee

Please sign in to comment.