Jared DiCarlo / Mbed 2 deprecated george

motor controller

Dependencies:   mbed plotter

foc.cpp@2:7312ac02785d, 2016-10-30 (annotated)

Committer:
dicarloj
Date:
Sun Oct 30 19:37:46 2016 +0000
Revision:
2:7312ac02785d
Parent:
1:2acd7dfc4b1b
Child:
3:08746709f023
working, no safety

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dicarloj 0:f899a5183b5e 1 #include "foc.h"
dicarloj 1:2acd7dfc4b1b 2 #include "io.h"
dicarloj 0:f899a5183b5e 3 #include "config.h"
dicarloj 0:f899a5183b5e 4 #include "mbed.h"
dicarloj 2:7312ac02785d 5 #include "plotter.h"
dicarloj 2:7312ac02785d 6 #include "vehicle_controller.h"
dicarloj 0:f899a5183b5e 7
dicarloj 2:7312ac02785d 8 //*************FOC VARIABLES***************//
dicarloj 2:7312ac02785d 9 float q_command = 0.0f; //q current setpoint, amps
dicarloj 2:7312ac02785d 10 float d_command = 0.0f; //d current setpoint, amps
dicarloj 2:7312ac02785d 11 float K_P = K_P_CURRENT; //proportional gain, q and d axis current PI controller
dicarloj 2:7312ac02785d 12 float K_I = K_I_CURRENT; //integral gain, q and d axis current PI controller
dicarloj 2:7312ac02785d 13 float integral_max = K_CURRENT_INT_MAX; //maximum integral.
dicarloj 2:7312ac02785d 14 float axis_v_max = K_AXIS_V_MAX; //maximum alpha/beta axis voltage
dicarloj 2:7312ac02785d 15 float d_int = 0.0f; //integral of d axis current error
dicarloj 2:7312ac02785d 16 float q_int = 0.0f; //integral of q axis current error
dicarloj 2:7312ac02785d 17 float SQRT_3 = sqrtf(3.0f); //math constant
dicarloj 2:7312ac02785d 18 float theta_e = 0.0f; //electrical motor angle
dicarloj 2:7312ac02785d 19 float ia_supp_offset = 0.0f; //calculated current sensor offset
dicarloj 2:7312ac02785d 20 float ib_supp_offset = 0.0f; //calculated current sensor offset
dicarloj 0:f899a5183b5e 21
dicarloj 0:f899a5183b5e 22
dicarloj 2:7312ac02785d 23 //********MATH FUNCTIONS********/
dicarloj 0:f899a5183b5e 24 float fminf(float a, float b)
dicarloj 0:f899a5183b5e 25 {
dicarloj 0:f899a5183b5e 26 if(a < b) return a;
dicarloj 0:f899a5183b5e 27 return b;
dicarloj 0:f899a5183b5e 28 }
dicarloj 0:f899a5183b5e 29
dicarloj 0:f899a5183b5e 30 float fmaxf(float a, float b)
dicarloj 0:f899a5183b5e 31 {
dicarloj 0:f899a5183b5e 32 if(a > b) return a;
dicarloj 0:f899a5183b5e 33 return b;
dicarloj 0:f899a5183b5e 34 }
dicarloj 0:f899a5183b5e 35
dicarloj 2:7312ac02785d 36 //******PUBLIC FUNCTIONS********/
dicarloj 2:7312ac02785d 37
dicarloj 2:7312ac02785d 38 //initialize motor controller
dicarloj 2:7312ac02785d 39 void init_control()
dicarloj 0:f899a5183b5e 40 {
dicarloj 2:7312ac02785d 41 zero_current();
dicarloj 0:f899a5183b5e 42 }
dicarloj 0:f899a5183b5e 43
dicarloj 2:7312ac02785d 44 //run motor control loop
dicarloj 2:7312ac02785d 45 void motor_control()
dicarloj 0:f899a5183b5e 46 {
dicarloj 2:7312ac02785d 47 ADC1->CR2 |= 0x40000000;
dicarloj 2:7312ac02785d 48 volatile int delay;
dicarloj 2:7312ac02785d 49 for (delay = 0; delay < 35; delay++);
dicarloj 2:7312ac02785d 50 foc(ADC1->DR, ADC2->DR);
dicarloj 0:f899a5183b5e 51 }
dicarloj 0:f899a5183b5e 52
dicarloj 2:7312ac02785d 53 //calculate zero current offset
dicarloj 2:7312ac02785d 54 void zero_current(){
dicarloj 2:7312ac02785d 55 for (int i = 0; i < 1000; i++){
dicarloj 2:7312ac02785d 56 ia_supp_offset += (float) (ADC1->DR);
dicarloj 2:7312ac02785d 57 ib_supp_offset += (float) (ADC2->DR);
dicarloj 2:7312ac02785d 58 ADC1->CR2 |= 0x40000000;
dicarloj 2:7312ac02785d 59 wait_us(100);
dicarloj 2:7312ac02785d 60 }
dicarloj 2:7312ac02785d 61 ia_supp_offset /= 1000.0f;
dicarloj 2:7312ac02785d 62 ib_supp_offset /= 1000.0f;
dicarloj 2:7312ac02785d 63 ia_supp_offset = ia_supp_offset / 4096.0f * AVDD - I_OFFSET;
dicarloj 2:7312ac02785d 64 ib_supp_offset = ib_supp_offset / 4096.0f * AVDD - I_OFFSET;
dicarloj 2:7312ac02785d 65 }
dicarloj 0:f899a5183b5e 66
dicarloj 2:7312ac02785d 67
dicarloj 2:7312ac02785d 68 //*****************PRIVATE*****************//
dicarloj 0:f899a5183b5e 69
dicarloj 2:7312ac02785d 70 void foc(float ia, float ib)
dicarloj 2:7312ac02785d 71 {
dicarloj 2:7312ac02785d 72 //get q current command from vehicle_controller and throttle value
dicarloj 2:7312ac02785d 73 q_command = velocity_control(get_throttle());
dicarloj 2:7312ac02785d 74
dicarloj 2:7312ac02785d 75 //scale and offset a and b current values
dicarloj 2:7312ac02785d 76 float i_a = ((float) ia / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE;
dicarloj 2:7312ac02785d 77 float i_b = ((float) ib / 4096.0f * AVDD - I_OFFSET - ib_supp_offset) / I_SCALE;
dicarloj 2:7312ac02785d 78
dicarloj 2:7312ac02785d 79 //scale and offset electrical position
dicarloj 2:7312ac02785d 80 theta_e = get_position() + K_POS_OFFSET;
dicarloj 2:7312ac02785d 81
dicarloj 2:7312ac02785d 82 //set theta to a positive angle
dicarloj 2:7312ac02785d 83 if(theta_e < 0) theta_e += 2 * M_PI;
dicarloj 0:f899a5183b5e 84
dicarloj 2:7312ac02785d 85 //compute sin/cos ahead of time
dicarloj 0:f899a5183b5e 86 float sin_theta_e = sinf(theta_e);
dicarloj 0:f899a5183b5e 87 float cos_theta_e = cosf(theta_e);
dicarloj 0:f899a5183b5e 88
dicarloj 0:f899a5183b5e 89 //calculate third phase current
dicarloj 0:f899a5183b5e 90 float i_c = -i_a - i_b;
dicarloj 0:f899a5183b5e 91
dicarloj 0:f899a5183b5e 92 //convert from a, b, c to alpha, beta
dicarloj 0:f899a5183b5e 93 float alpha_i = i_a;
dicarloj 0:f899a5183b5e 94 float beta_i = (i_c - i_b)/SQRT_3;
dicarloj 0:f899a5183b5e 95
dicarloj 0:f899a5183b5e 96 //convert from alpha, beta to q,d
dicarloj 0:f899a5183b5e 97 float d_i = alpha_i * cos_theta_e - beta_i * sin_theta_e;
dicarloj 0:f899a5183b5e 98 float q_i = alpha_i * sin_theta_e + beta_i * cos_theta_e;
dicarloj 0:f899a5183b5e 99
dicarloj 2:7312ac02785d 100 //calculate error integral
dicarloj 0:f899a5183b5e 101 q_int += (q_command - q_i) * K_I;
dicarloj 0:f899a5183b5e 102 d_int += (d_command - d_i) * K_I;
dicarloj 0:f899a5183b5e 103
dicarloj 0:f899a5183b5e 104 //prevent integral windup
dicarloj 0:f899a5183b5e 105 q_int = fabs(q_int) > integral_max ? integral_max * (q_int > 0 ? 1 : -1) : q_int;
dicarloj 0:f899a5183b5e 106 d_int = fabs(d_int) > integral_max ? integral_max * (d_int > 0 ? 1 : -1) : d_int;
dicarloj 0:f899a5183b5e 107
dicarloj 0:f899a5183b5e 108 //calculate q, d axis voltages
dicarloj 2:7312ac02785d 109 float q_v = K_P * (q_command - q_i) + q_int;
dicarloj 2:7312ac02785d 110 float d_v = K_P * (d_command - d_i) + d_int;
dicarloj 0:f899a5183b5e 111
dicarloj 0:f899a5183b5e 112 //limit axis voltages
dicarloj 0:f899a5183b5e 113 q_v = fabs(q_v) > axis_v_max ? axis_v_max * (q_v > 0 ? 1 : -1) : q_v;
dicarloj 0:f899a5183b5e 114 d_v = fabs(d_v) > axis_v_max ? axis_v_max * (d_v > 0 ? 1 : -1) : d_v;
dicarloj 0:f899a5183b5e 115
dicarloj 0:f899a5183b5e 116 //convert back to alpha, beta
dicarloj 2:7312ac02785d 117 float alpha_v = d_v * cos_theta_e + q_v * sin_theta_e;
dicarloj 0:f899a5183b5e 118 float beta_v = -d_v * sin_theta_e + q_v * cos_theta_e;
dicarloj 0:f899a5183b5e 119
dicarloj 0:f899a5183b5e 120 //convert to a, b, c voltages
dicarloj 0:f899a5183b5e 121 float a_v = alpha_v;
dicarloj 0:f899a5183b5e 122 float b_v = -alpha_v/2.0f - SQRT_3 * beta_v / 2.0f;
dicarloj 0:f899a5183b5e 123 float c_v = -alpha_v/2.0f + SQRT_3 * beta_v / 2.0f;
dicarloj 0:f899a5183b5e 124
dicarloj 2:7312ac02785d 125 //SVPWM
dicarloj 2:7312ac02785d 126 float offset_voltage = (fminf(a_v, fminf(b_v, c_v)) + fmaxf(a_v, fmaxf(b_v, c_v)))/2.0f;
dicarloj 2:7312ac02785d 127 a_v = a_v - offset_voltage;
dicarloj 2:7312ac02785d 128 b_v = b_v - offset_voltage;
dicarloj 2:7312ac02785d 129 c_v = c_v - offset_voltage;
dicarloj 0:f899a5183b5e 130
dicarloj 2:7312ac02785d 131 set_inverter(a_v, b_v, c_v);
dicarloj 0:f899a5183b5e 132 }
dicarloj 0:f899a5183b5e 133
dicarloj 0:f899a5183b5e 134
dicarloj 0:f899a5183b5e 135