main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "PositionSensor.h"
00004 #include "FastPWM.h"
00005 #include "Transforms.h"
00006 #include "config.h"
00007 #include "pwm_in.h"
00008 
00009 FastPWM *a;
00010 FastPWM *b;
00011 FastPWM *c;
00012 DigitalOut en(EN);
00013 DigitalOut toggle(PC_10);
00014 PWM_IN p_in(PB_8, 1100, 1900);
00015 bool control_enabled = false;
00016 PositionSensorEncoder pos(CPR, 0);
00017 
00018 Serial pc(USBTX, USBRX);
00019 
00020 int state = 0;
00021 int adval1, adval2;
00022 float ia, ib, ic, alpha, beta, d, q, vd, vq, p;
00023 
00024 float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV)
00025 
00026 float d_integral = 0.0f, q_integral = 0.0f;
00027 float last_d = 0.0f, last_q = 0.0f;
00028 float d_ref = 0.0f, q_ref = 50.0f;
00029 
00030 
00031 void commutate();
00032 void zero_current();
00033 void config_globals();
00034 void startup_msg();
00035 
00036 void go_enabled()
00037 {
00038     d_integral = 0.0f;
00039     q_integral = 0.0f;
00040     control_enabled = true;
00041     en = 1;
00042 }
00043 
00044 void go_disabled()
00045 {
00046     control_enabled = false;
00047     en = 0;
00048 }
00049 
00050 float fminf(float a, float b)
00051 {
00052     if(a < b) return a;
00053     return b;
00054 }
00055 
00056 float fmaxf(float a, float b)
00057 {
00058     if(a > b) return a;
00059     return b;
00060 }
00061 
00062 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
00063     if (TIM1->SR & TIM_SR_UIF ) {
00064         toggle = 1;
00065         ADC1->CR2  |= 0x40000000; 
00066         volatile int delay;
00067         for (delay = 0; delay < 35; delay++);
00068         toggle = 0;
00069         adval1 = ADC1->DR;
00070         adval2 = ADC2->DR;
00071         commutate();
00072     }
00073     TIM1->SR = 0x00;
00074 }
00075 
00076 void zero_current(){
00077     for (int i = 0; i < 1000; i++){
00078         ia_supp_offset += (float) (ADC1->DR);
00079         ib_supp_offset += (float) (ADC2->DR);
00080         ADC1->CR2  |= 0x40000000;
00081         wait_us(100); 
00082     }
00083     ia_supp_offset /= 1000.0f;
00084     ib_supp_offset /= 1000.0f;
00085     ia_supp_offset = ia_supp_offset / 4096.0f * AVDD - I_OFFSET;
00086     ib_supp_offset = ib_supp_offset / 4096.0f * AVDD - I_OFFSET;
00087 }
00088 
00089 void config_globals() {
00090     pc.baud(115200);
00091     
00092     //Enable clocks for GPIOs
00093     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
00094     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
00095     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
00096     
00097     RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock
00098     
00099     a = new FastPWM(PWMA);
00100     b = new FastPWM(PWMB);
00101     c = new FastPWM(PWMC);
00102     
00103     NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
00104 
00105     TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt
00106     TIM1->CR1 = 0x40; //CMS = 10, interrupt only when counting up
00107     TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on, 
00108     TIM1->RCR |= 0x01; //update event once per up/down count of tim1 
00109     TIM1->EGR |= TIM_EGR_UG;
00110     
00111     TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock
00112     TIM1->ARR = 0x4650; //5 Khz
00113     TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
00114     TIM1->CR1 |= TIM_CR1_CEN;
00115     
00116     //ADC Setup
00117     RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
00118     RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
00119     
00120     ADC->CCR = 0x00000006; //Regular simultaneous mode, 3 channels
00121     
00122     ADC1->CR2 |= ADC_CR2_ADON; //ADC1 on
00123     ADC1->SQR3 = 0x0000004; //PA_4 as ADC1, sequence 0
00124     
00125     ADC2->CR2 |= ADC_CR2_ADON; //ADC2 ON
00126     ADC2->SQR3 = 0x00000008; //PB_0 as ADC2, sequence 1
00127     
00128     GPIOA->MODER |= (1 << 8);
00129     GPIOA->MODER |= (1 << 9);
00130     
00131     GPIOA->MODER |= (1 << 2);
00132     GPIOA->MODER |= (1 << 3);
00133     
00134     GPIOA->MODER |= (1 << 0);
00135     GPIOA->MODER |= (1 << 1);
00136     
00137     GPIOB->MODER |= (1 << 0);
00138     GPIOB->MODER |= (1 << 1);
00139     
00140     GPIOC->MODER |= (1 << 2);
00141     GPIOC->MODER |= (1 << 3);
00142     
00143     //DAC setup
00144     RCC->APB1ENR |= 0x20000000;
00145     DAC->CR |= DAC_CR_EN2;
00146     
00147     GPIOA->MODER |= (1 << 10);
00148     GPIOA->MODER |= (1 << 11);
00149     
00150     //Zero duty cycles
00151     set_dtc(a, 0.0f);
00152     set_dtc(b, 0.0f);
00153     set_dtc(c, 0.0f);
00154     
00155     wait_ms(250);
00156     zero_current();
00157     en = 1;
00158 }
00159 
00160 void startup_msg() {
00161     pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A.");
00162     pc.printf("%s\n\r", "====Config Data====");
00163     pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET);
00164     pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE);
00165     pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE);
00166     pc.printf("Pole pairs: %d\n\r", (int) POLE_PAIRS);
00167     pc.printf("Resolver lobes: %d\n\r", (int) RESOLVER_LOBES);
00168     pc.printf("Loop KP: %f\n\r", KP);
00169     pc.printf("Loop KI: %f\n\r", KI);
00170     pc.printf("Ia offset: %f mV\n\r", ia_supp_offset);
00171     pc.printf("Ib offset: %f mV\n\r", ib_supp_offset);
00172     pc.printf("\n\r");
00173 }    
00174 
00175 void commutate() {
00176     if(control_enabled && !p_in.get_enabled()) go_disabled();
00177     if(!control_enabled && p_in.get_enabled()) go_enabled();
00178     q_ref = p_in.get_throttle() * Q_MAX;
00179     p = pos.GetElecPosition() - POS_OFFSET;
00180     if (p < 0) p += 2 * PI;
00181     
00182     float sin_p = sinf(p);
00183     float cos_p = cosf(p);
00184     
00185     //float pos_dac = 0.85f * p / (2 * PI) + 0.05f;
00186     //DAC->DHR12R2 = (unsigned int) (pos_dac * 4096);
00187     
00188     ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE;
00189     ib = ((float) adval2 / 4096.0f * AVDD - I_OFFSET - ib_supp_offset) / I_SCALE;
00190     ic = -ia - ib;
00191     
00192     float u = CURRENT_U;
00193     float v = CURRENT_V;
00194     
00195     alpha = u;
00196     beta = 1 / sqrtf(3.0f) * u + 2 / sqrtf(3.0f) * v;
00197     
00198     d = alpha * cos_p - beta * sin_p;
00199     q = -alpha * sin_p - beta * cos_p;
00200     
00201     float d_err = d_ref - d;
00202     float q_err = q_ref - q;
00203     
00204     d_integral += d_err * KI;
00205     q_integral += q_err * KI;
00206     
00207     if (q_integral > INTEGRAL_MAX) q_integral = INTEGRAL_MAX;
00208     if (d_integral > INTEGRAL_MAX) d_integral = INTEGRAL_MAX;
00209     if (q_integral < -INTEGRAL_MAX) q_integral = -INTEGRAL_MAX;
00210     if (d_integral < -INTEGRAL_MAX) d_integral = -INTEGRAL_MAX;
00211     
00212     if(control_enabled)
00213     {
00214         vd = KP * d_err + d_integral;
00215         vq = KP * q_err + q_integral;
00216     }
00217     else
00218     {
00219         vd = 0;
00220         vq = 0;
00221     }
00222     
00223     if (vd < -1.0f) vd = -1.0f;
00224     if (vd > 1.0f) vd = 1.0f;
00225     if (vq < -1.0f) vq = -1.0f;
00226     if (vq > 1.0f) vq = 1.0f;
00227     
00228     DAC->DHR12R2 = (unsigned int) (-q * 20 + 2048);
00229     //DAC->DHR12R2 = (unsigned int) (-vd * 2000 + 2048);
00230     
00231     //vd = 0.0f;
00232     //vq = 1.0f;
00233     
00234     float valpha = vd * cos_p - vq * sin_p;
00235     float vbeta = vd * sin_p + vq * cos_p;
00236     
00237     float va = valpha;
00238     float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta;
00239     float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta;
00240     
00241     float voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;
00242     va = va - voff;
00243     vb = vb - voff;
00244     vc = vc - voff;
00245     
00246     set_dtc(a, 0.5f + 0.5f * va);
00247     set_dtc(b, 0.5f + 0.5f * vb);
00248     set_dtc(c, 0.5f + 0.5f * vc);
00249 }
00250     
00251 int main() {
00252     config_globals();
00253     startup_msg();
00254     
00255     for (;;) {
00256         //pc.printf("%f\n\r", p);
00257         //wait(0.1);
00258     }
00259 }