Bayley Wang
/
qonly_controller
derp
Diff: main.cpp
- Revision:
- 1:7b61790f6be9
- Parent:
- 0:bac9c3a3a6ca
- Child:
- 2:eabe8feaaabb
--- a/main.cpp Wed Mar 09 06:44:51 2016 +0000 +++ b/main.cpp Wed Mar 09 17:21:01 2016 +0000 @@ -5,12 +5,12 @@ #include "Transforms.h" #include "config.h" -FastPWM a(PWMA); -FastPWM b(PWMB); -FastPWM c(PWMC); +FastPWM *a; +FastPWM *b; +FastPWM *c; DigitalOut en(EN); +DigitalOut toggle(PC_10); -AnalogOut test(TEST_DAC); AnalogIn Ia(IA); AnalogIn Ib(IB); @@ -18,18 +18,103 @@ Serial pc(USBTX, USBRX); -using namespace Transforms; +int state = 0; +int adval1, adval2; +float ia, ib; +float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV) + +extern "C" void TIM1_UP_TIM10_IRQHandler(void) { + if (TIM1->SR & TIM_SR_UIF ) { + adval1 = ADC1->DR; + adval2 = ADC2->DR; + ADC1->CR2 |= 0x40000000; + } + TIM1->SR = 0x00; + toggle = state; + state = !state; +} + +void zero_current(){ + for (int i = 0; i < 1000; i++){ + ia_supp_offset += (float) (ADC1->DR); + ib_supp_offset += (float) (ADC2->DR); + ADC1->CR2 |= 0x40000000; + wait_us(100); + } + ia_supp_offset /= 1000.0f; + ib_supp_offset /= 1000.0f; + ia_supp_offset = ia_supp_offset / 4096.0f * AVDD - I_OFFSET; + ib_supp_offset = ib_supp_offset / 4096.0f * AVDD - I_OFFSET; +} void config_globals() { pc.baud(115200); - a.period_us(200); - b.period_us(200); - c.period_us(200); - a = 1.0f; - b = 1.0f; - c = 1.0f; + //Enable clocks for GPIOs + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; + + RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock + + a = new FastPWM(PWMA); + b = new FastPWM(PWMB); + c = new FastPWM(PWMC); + + NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ + + TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt + TIM1->CR1 = 0x40; //CMS = 10, interrupt only when counting up + TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on, + TIM1->RCR |= 0x01; //update event once per up/down count of tim1 + TIM1->EGR |= TIM_EGR_UG; + + TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock + TIM1->ARR = 0x4650; //5 Khz + TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on. + TIM1->CR1 |= TIM_CR1_CEN; + + //ADC Setup + RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1 + RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2 + + ADC->CCR = 0x00000006; //Regular simultaneous mode, 3 channels + + ADC1->CR2 |= ADC_CR2_ADON; //ADC1 on + ADC1->SQR3 = 0x0000004; //PA_4 as ADC1, sequence 0 + ADC2->CR2 |= ADC_CR2_ADON; //ADC2 ON + ADC2->SQR3 = 0x00000008; //PB_0 as ADC2, sequence 1 + + GPIOA->MODER |= (1 << 8); + GPIOA->MODER |= (1 << 9); + + GPIOA->MODER |= (1 << 2); + GPIOA->MODER |= (1 << 3); + + GPIOA->MODER |= (1 << 0); + GPIOA->MODER |= (1 << 1); + + GPIOB->MODER |= (1 << 0); + GPIOB->MODER |= (1 << 1); + + GPIOC->MODER |= (1 << 2); + GPIOC->MODER |= (1 << 3); + + //DAC setup + RCC->APB1ENR |= 0x20000000; + DAC->CR |= DAC_CR_EN2; + + GPIOA->MODER |= (1 << 10); + GPIOA->MODER |= (1 << 11); + + //Zero duty cycles + set_dtc(a, 0.0f); + set_dtc(b, 0.0f); + set_dtc(c, 0.0f); + + wait_ms(250); + zero_current(); en = 1; } @@ -41,6 +126,8 @@ pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE); pc.printf("Loop KP: %f\n\r", KP); pc.printf("Loop KI: %f\n\r", KI); + pc.printf("Ia offset: %f mV\n\r", ia_supp_offset); + pc.printf("Ib offset: %f mV\n\r", ib_supp_offset); pc.printf("\n\r"); } @@ -49,14 +136,10 @@ if (p < 0) p += 2 * PI; float pos_dac = 0.85f * p / (2 * PI) + 0.05f; - - float v_ia = Ia.read() * AVDD; - float v_ib = Ib.read() * AVDD; + DAC->DHR12R2 = (unsigned int) (pos_dac * 4096); - float ia = (v_ia - I_OFFSET) / I_SCALE; - float ib = (v_ia - I_OFFSET) / I_SCALE; - - test = pos_dac; + ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE; + ib = ((float) adval2 / 4096.0f * AVDD - I_OFFSET - ib_supp_offset) / I_SCALE; set_dtc(a, 0.5f + 0.5f * cosf(p)); set_dtc(b, 0.5f + 0.5f * cosf(p + 2 * PI / 3));