Diff: main.cpp

Revision:

11:1a68e17c0279

Parent:

10:6829abb438fc

--- a/main.cpp	Tue May 17 09:58:58 2016 +0000
+++ b/main.cpp	Thu May 19 14:32:31 2016 +0000
@@ -4,23 +4,20 @@
 #include "FastPWM.h"
 #include "Transforms.h"
 #include "config.h"
+#include <numeric>
 
 FastPWM *a;
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
 DigitalOut toggle(PC_10);
-
-PositionSensorEncoder pos(CPR, 0);
-
+//PositionSensorEncoder pos(CPR, 0);
 Serial pc(USBTX, USBRX);
 
 int state = 0;
-int adval1, adval2;
+float adval1, adval2;
 float ia, ib, ic, alpha, beta, d, q, vd, vq, p;
-
 float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV)
-
 float d_integral = 0.0f, q_integral = 0.0f;
 float last_d = 0.0f, last_q = 0.0f;
 float d_ref = -0.0f, q_ref = -50.0f;
@@ -30,36 +27,143 @@
 void config_globals();
 void startup_msg();
 
-extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
+ADC_HandleTypeDef g_AdcHandle;
+uint32_t currentA_ADCValue, currentB_ADCValue;
+int g_MeasurementNumber;
+
+enum{ ADC_BUFFER_LENGTH = 200 };  //originally 8192
+uint32_t currentA_ADCBuffer[ADC_BUFFER_LENGTH];  //current_A ADC buffer
+uint32_t currentB_ADCBuffer[ADC_BUFFER_LENGTH];  //current_B ADC buffer
+
+uint32_t debug_int = 0;
+
+
+void ConfigureADC()
+{
+    GPIO_InitTypeDef currentA_gpioInit, currentB_gpioInit;
+
+    __GPIOA_CLK_ENABLE();
+    __GPIOB_CLK_ENABLE();
+    __ADC1_CLK_ENABLE();
+    
+    //Initialize PA_4 as an analog input
+    currentA_gpioInit.Pin = GPIO_PIN_4;
+    currentA_gpioInit.Mode = GPIO_MODE_ANALOG;
+    currentA_gpioInit.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOA, &currentA_gpioInit);
+    //Initialize PB_0 as an analog input
+    currentB_gpioInit.Pin = GPIO_PIN_0;
+    currentB_gpioInit.Mode = GPIO_MODE_ANALOG;
+    currentB_gpioInit.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOB, &currentB_gpioInit);
+
+    HAL_NVIC_SetPriority(ADC_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(ADC_IRQn);
+
+    ADC_ChannelConfTypeDef adcChannel_currentA;
+    ADC_ChannelConfTypeDef adcChannel_currentB;
+
+    g_AdcHandle.Instance = ADC1;
+
+    g_AdcHandle.Init.ClockPrescaler = ADC_CLOCKPRESCALER_PCLK_DIV2;
+    g_AdcHandle.Init.Resolution = ADC_RESOLUTION_12B;
+    g_AdcHandle.Init.ScanConvMode = ENABLE;  //Enabled to convert multiple channels.  Changed from DISABLE
+    g_AdcHandle.Init.ContinuousConvMode = ENABLE;
+    g_AdcHandle.Init.DiscontinuousConvMode = DISABLE;
+    g_AdcHandle.Init.NbrOfDiscConversion = 0;
+    g_AdcHandle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+    g_AdcHandle.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1;
+    g_AdcHandle.Init.DataAlign = ADC_DATAALIGN_RIGHT;
+    g_AdcHandle.Init.NbrOfConversion = 2;
+    g_AdcHandle.Init.DMAContinuousRequests = ENABLE;
+    g_AdcHandle.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+    HAL_ADC_Init(&g_AdcHandle);
+    
+    //ADC_CHANNEL_4 corresponds to PA_4
+    adcChannel_currentA.Channel = ADC_CHANNEL_4;
+    adcChannel_currentA.Rank = 1;
+    adcChannel_currentA.SamplingTime = ADC_SAMPLETIME_480CYCLES;  //was 28  
+    adcChannel_currentA.Offset = 0;
+    if (HAL_ADC_ConfigChannel(&g_AdcHandle, &adcChannel_currentA) != HAL_OK) {
+        asm("bkpt 255");
+    }
+    //duplicated the previous 7 lines here with the ADC channel changed to 8, corresponding to PB_0
+    adcChannel_currentB.Channel = ADC_CHANNEL_8;
+    adcChannel_currentB.Rank = 2;
+    adcChannel_currentB.SamplingTime = ADC_SAMPLETIME_480CYCLES;
+    adcChannel_currentB.Offset = 0; 
+    if (HAL_ADC_ConfigChannel(&g_AdcHandle, &adcChannel_currentB) != HAL_OK) {
+        asm("bkpt 255");
+    }
+}
+
+DMA_HandleTypeDef  currentAB_DmaHandle;
+
+void ConfigureDMA()
+{
+    __DMA2_CLK_ENABLE();
+    
+    currentAB_DmaHandle.Instance = DMA2_Stream4;
+    currentAB_DmaHandle.Init.Channel = DMA_CHANNEL_0;
+    currentAB_DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
+    currentAB_DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE;
+    currentAB_DmaHandle.Init.MemInc = DMA_MINC_ENABLE;
+    currentAB_DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
+    currentAB_DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
+    currentAB_DmaHandle.Init.Mode = DMA_CIRCULAR;
+    currentAB_DmaHandle.Init.Priority = DMA_PRIORITY_HIGH;
+    currentAB_DmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+    currentAB_DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_HALFFULL;
+    currentAB_DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE;
+    currentAB_DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
+    HAL_DMA_Init(&currentAB_DmaHandle);
+    
+    __HAL_LINKDMA(&g_AdcHandle, DMA_Handle, currentAB_DmaHandle);  //linking ADC1 to DMA2_Stream4
+    HAL_NVIC_SetPriority(DMA2_Stream4_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(DMA2_Stream4_IRQn);
+}
+
+static void SystemClock_Config(void)
+{
+    RCC_ClkInitTypeDef RCC_ClkInitStruct;
+    RCC_OscInitTypeDef RCC_OscInitStruct;
+
+    __HAL_RCC_PWR_CLK_ENABLE();
+    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);
+
+    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
+    RCC_OscInitStruct.HSEState = RCC_HSE_ON;
+    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
+    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
+    RCC_OscInitStruct.PLL.PLLM = 8;
+    RCC_OscInitStruct.PLL.PLLN = 288;
+    RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
+    RCC_OscInitStruct.PLL.PLLQ = 6;
+    HAL_RCC_OscConfig(&RCC_OscInitStruct);
+
+    RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2);
+    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
+    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
+    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
+    HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4);
+    SystemCoreClockUpdate();
+
+    if (HAL_GetREVID() == 0x1001)
+        __HAL_FLASH_PREFETCH_BUFFER_ENABLE();
+}
+
+extern "C" void TIM1_UP_TIM10_IRQHandler(void) {  //this samples current at the 000 state
     if (TIM1->SR & TIM_SR_UIF ) {
-        toggle = 1;
-        ADC1->CR2  |= 0x40000000; 
-        volatile int delay;
-        for (delay = 0; delay < 35; delay++);
-        toggle = 0;
-        adval1 = ADC1->DR;
-        adval2 = ADC2->DR;
-        commutate();
+
     }
     TIM1->SR = 0x00;
 }
 
 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);
-    
+void config_globals() {    
     //Enable clocks for GPIOs
     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
@@ -84,32 +188,9 @@
     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
+//ADCs PA_4 and PB_0 for current sensing
+// PA_4 PA_2 PA_0 PB_0 PC_1 analog
     
-    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;
@@ -129,22 +210,12 @@
 }
 
 void startup_msg() {
-    pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A.");
-    pc.printf("%s\n\r", "====Config Data====");
-    pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET);
-    pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE);
-    pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE);
-    pc.printf("Pole pairs: %d\n\r", (int) POLE_PAIRS);
-    pc.printf("Resolver lobes: %d\n\r", (int) RESOLVER_LOBES);
-    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");
+    pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev xA.");
 }    
 
 void commutate() {
-    p = pos.GetElecPosition() - POS_OFFSET;
+    //p = pos.GetElecPosition() - POS_OFFSET;
+    p = 0.0f;
     if (p < 0) p += 2 * PI;
     
     float sin_p = sinf(p);
@@ -204,11 +275,62 @@
 }
     
 int main() {
+    pc.baud(115200);
     config_globals();
     startup_msg();
     
+    HAL_Init();
+    SystemClock_Config();
+    ConfigureADC();
+            ADC1->CR2 |= (1<<0); //A/D on
+    //        ADC1->CR2 |= (1<<8); //DMA mode
+    //ConfigureDMA();
+    HAL_ADC_Start_IT(&g_AdcHandle);
+    //HAL_ADC_Start_DMA(&g_AdcHandle, g_ADCBuffer, ADC_BUFFER_LENGTH);  //this enables the overrun interrupt
+    
+    //HAL_DMAEx_MultiBufferStart(..) wants uint32_t that are the destination addresses of the buffers.
+    uint32_t currentA_ADCBufferAddress = (uint32_t)&currentA_ADCBuffer[0];
+    uint32_t currentB_ADCBufferAddress = (uint32_t)&currentB_ADCBuffer[0];
+    
+    //HAL_DMAEx_MultiBufferStart(&currentAB_DmaHandle, ADC1->DR, currentA_ADCBufferAddress, currentB_ADCBufferAddress, ADC_BUFFER_LENGTH); //DMA_Handle, SrcAddress, DstAddress, DstAddress2, DataLength
+     
     for (;;) {
-        //pc.printf("%f\r\n", p);
-        //wait_ms(100);
+        //ADC1->CR1 |= 0x4000000;
+        //pc.printf("%d \n\r", ADC1->DR);
+        //pc.printf("%d \n\r", ADC1->SR);
+        pc.printf("%d %d \n\r", currentA_ADCValue, currentB_ADCValue);
+        //pc.printf("%d \n\r", currentB_ADCValue);
+        //pc.printf("%d \n\r", debug_int);
+        //pc.printf("%s %x %s %x\n\r", "CR1:" , ADC1->CR1, "CR2:" , ADC1->CR2);
     }
 }
+
+extern "C"  //legacy C code goes here
+{
+    void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* AdcHandle)
+    {
+        //currentA_ADCValue = HAL_ADC_GetValue(AdcHandle)
+        toggle = state;
+        state = !state;
+        if(state){currentA_ADCValue = ADC1->DR;}
+        else{currentB_ADCValue = ADC1->DR;}
+        toggle = state;
+    
+    /*
+        currentA_ADCValue = std::accumulate(currentA_ADCBuffer, currentA_ADCBuffer + ADC_BUFFER_LENGTH, 0) / ADC_BUFFER_LENGTH;
+        currentB_ADCValue = std::accumulate(currentB_ADCBuffer, currentB_ADCBuffer + ADC_BUFFER_LENGTH, 0) / ADC_BUFFER_LENGTH;
+        g_MeasurementNumber += ADC_BUFFER_LENGTH;
+    */
+    }
+
+    void DMA2_Stream4_IRQHandler()
+    {
+        HAL_DMA_IRQHandler(&currentAB_DmaHandle);
+    }
+
+    void ADC_IRQHandler()
+    {
+        HAL_ADC_IRQHandler(&g_AdcHandle);
+
+    }
+}