Ben Katz / Mbed 2 deprecated Hobbyking_Cheetah_V1

FOC Implementation for putting multirotor motors in robots

Dependencies:   FastPWM3 mbed

Inverter/Inverter.cpp@14:80ce59119d93, 2016-10-31 (annotated)

Committer:
benkatz
Date:
Mon Oct 31 16:48:16 2016 +0000
Revision:
14:80ce59119d93
Parent:
10:370851e6e132 
Misc. changes.  Finally fixed transforms (turns out B and C current measurements were accidentally swapped)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:4e1c4df6aabd 1 #include "mbed.h"
benkatz 0:4e1c4df6aabd 2 #include "FastPWM.h"
benkatz 0:4e1c4df6aabd 3 #include "Inverter.h"
benkatz 0:4e1c4df6aabd 4
benkatz 0:4e1c4df6aabd 5 Inverter::Inverter(PinName PinA, PinName PinB, PinName PinC, PinName PinEnable, float I_Scale, float Period){
benkatz 0:4e1c4df6aabd 6
benkatz 0:4e1c4df6aabd 7 _I_Scale = I_Scale;
benkatz 0:4e1c4df6aabd 8
benkatz 0:4e1c4df6aabd 9
benkatz 0:4e1c4df6aabd 10 Enable = new DigitalOut(PinEnable);
benkatz 0:4e1c4df6aabd 11
benkatz 2:8724412ad628 12 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // enable the clock to GPIOC
benkatz 2:8724412ad628 13 //RCC->APB1ENR |= 0x00000001; // enable TIM2 clock
benkatz 2:8724412ad628 14 RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock
benkatz 2:8724412ad628 15
benkatz 0:4e1c4df6aabd 16 GPIOC->MODER = (1 << 8); // set pin 4 to be general purpose output
benkatz 0:4e1c4df6aabd 17
benkatz 0:4e1c4df6aabd 18 PWM_A = new FastPWM(PinA);
benkatz 0:4e1c4df6aabd 19 PWM_B = new FastPWM(PinB);
benkatz 0:4e1c4df6aabd 20 PWM_C = new FastPWM(PinC);
benkatz 0:4e1c4df6aabd 21
benkatz 2:8724412ad628 22 //TIM2->CR1 &= ~(TIM_CR1_CEN);
benkatz 2:8724412ad628 23 //TIM2->CR1 |= TIM_CR1_CMS;
benkatz 2:8724412ad628 24 //TIM2->CR1 |= TIM_CR1_CEN;
benkatz 2:8724412ad628 25
benkatz 0:4e1c4df6aabd 26 //PWM_A->period(Period);
benkatz 0:4e1c4df6aabd 27
benkatz 2:8724412ad628 28 //ISR Setup
benkatz 2:8724412ad628 29 NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
benkatz 2:8724412ad628 30
benkatz 2:8724412ad628 31 TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
benkatz 2:8724412ad628 32 TIM1->CR1 = 0x40;//CMS = 10, interrupt only when counting up
benkatz 2:8724412ad628 33 TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on,
benkatz 2:8724412ad628 34 TIM1->RCR |= 0x001; // update event once per up/down count of tim1
benkatz 2:8724412ad628 35 TIM1->EGR |= TIM_EGR_UG;
benkatz 2:8724412ad628 36
benkatz 0:4e1c4df6aabd 37 //PWM Setup
benkatz 0:4e1c4df6aabd 38
benkatz 2:8724412ad628 39 TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
benkatz 14:80ce59119d93 40 //TIM1->ARR = 0x1194; // 20 Khz
benkatz 14:80ce59119d93 41 TIM1->ARR = 0x8CA;
benkatz 2:8724412ad628 42 TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
benkatz 3:6a0015d88d06 43 TIM1->CR1 |= TIM_CR1_CEN;
benkatz 2:8724412ad628 44
benkatz 0:4e1c4df6aabd 45 // ADC Setup
benkatz 0:4e1c4df6aabd 46 RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
benkatz 0:4e1c4df6aabd 47 RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
benkatz 0:4e1c4df6aabd 48 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;//0x0000002; // Enable clock for GPIOC
benkatz 0:4e1c4df6aabd 49
benkatz 0:4e1c4df6aabd 50 ADC->CCR = 0x00000006; // Regular simultaneous mode only
benkatz 0:4e1c4df6aabd 51 ADC1->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON
benkatz 0:4e1c4df6aabd 52 ADC1->SQR3 = 0x000000A; // use PC_0 as input
benkatz 0:4e1c4df6aabd 53 ADC2->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON
benkatz 0:4e1c4df6aabd 54 ADC2->SQR3 = 0x0000000B; // use PC_1 as input
benkatz 0:4e1c4df6aabd 55 GPIOC->MODER |= 0x0000000f; // PC_0, PC_1 are analog inputs
benkatz 0:4e1c4df6aabd 56
benkatz 0:4e1c4df6aabd 57 // DAC set-up
benkatz 0:4e1c4df6aabd 58 RCC->APB1ENR |= 0x20000000; // Enable clock for DAC
benkatz 0:4e1c4df6aabd 59 DAC->CR |= 0x00000001; // DAC control reg, both channels ON
benkatz 1:b8bceb4daed5 60 GPIOA->MODER |= 0x00000300; // PA04 as analog output
benkatz 0:4e1c4df6aabd 61
benkatz 10:370851e6e132 62
benkatz 10:370851e6e132 63 //Enabled pin must be on for current sensors to turn on
benkatz 0:4e1c4df6aabd 64 EnableInverter();
benkatz 0:4e1c4df6aabd 65 SetDTC(0.0f, 0.0f, 0.0f);
benkatz 0:4e1c4df6aabd 66 wait(.2);
benkatz 0:4e1c4df6aabd 67 ZeroCurrent();
benkatz 9:d7eb815cb057 68 wait(.1);
benkatz 9:d7eb815cb057 69 DisableInverter();
benkatz 0:4e1c4df6aabd 70 }
benkatz 0:4e1c4df6aabd 71
benkatz 0:4e1c4df6aabd 72 void Inverter::SetDTC(float DTC_A, float DTC_B, float DTC_C){
benkatz 2:8724412ad628 73 PWM_A->write(1.0f-DTC_A);
benkatz 2:8724412ad628 74 PWM_B->write(1.0f-DTC_B);
benkatz 2:8724412ad628 75 PWM_C->write(1.0f-DTC_C);
benkatz 0:4e1c4df6aabd 76 }
benkatz 0:4e1c4df6aabd 77
benkatz 0:4e1c4df6aabd 78 void Inverter::EnableInverter(){
benkatz 0:4e1c4df6aabd 79 Enable->write(1);
benkatz 0:4e1c4df6aabd 80 }
benkatz 0:4e1c4df6aabd 81
benkatz 0:4e1c4df6aabd 82 void Inverter::DisableInverter(){
benkatz 0:4e1c4df6aabd 83 Enable->write(0);
benkatz 0:4e1c4df6aabd 84 }
benkatz 0:4e1c4df6aabd 85
benkatz 0:4e1c4df6aabd 86 void Inverter::ZeroCurrent(){
benkatz 0:4e1c4df6aabd 87 I_B_Offset = 0;
benkatz 0:4e1c4df6aabd 88 I_C_Offset = 0;
benkatz 0:4e1c4df6aabd 89 for (int i=0; i < 1000; i++){
benkatz 14:80ce59119d93 90 I_B_Offset += ADC2->DR;
benkatz 14:80ce59119d93 91 I_C_Offset += ADC1->DR;
benkatz 0:4e1c4df6aabd 92 ADC1->CR2 |= 0x40000000;
benkatz 9:d7eb815cb057 93 wait(.0001);
benkatz 0:4e1c4df6aabd 94 }
benkatz 0:4e1c4df6aabd 95 I_B_Offset = I_B_Offset/1000.0f;
benkatz 0:4e1c4df6aabd 96 I_C_Offset = I_C_Offset/1000.0f;
benkatz 9:d7eb815cb057 97 //printf("B_Offset: %f C_Offset: %f\n\r", I_B_Offset, I_C_Offset);
benkatz 0:4e1c4df6aabd 98 }
benkatz 0:4e1c4df6aabd 99
benkatz 0:4e1c4df6aabd 100 void Inverter::GetCurrent(float *A, float *B, float *C){
benkatz 0:4e1c4df6aabd 101 *A = I_A;
benkatz 0:4e1c4df6aabd 102 *B = I_B;
benkatz 0:4e1c4df6aabd 103 *C = I_C;
benkatz 14:80ce59119d93 104 //printf("I_A: %f I_B: %f I_C: %f\n\r", I_A, I_B, I_C);
benkatz 0:4e1c4df6aabd 105 }
benkatz 0:4e1c4df6aabd 106
benkatz 0:4e1c4df6aabd 107 void Inverter::SampleCurrent(void){
benkatz 0:4e1c4df6aabd 108 // Dbg->write(1);
benkatz 7:dc5f27756e02 109 GPIOC->ODR ^= (1 << 4); //Toggle pin for debugging
benkatz 14:80ce59119d93 110 I_B = _I_Scale*((float) (ADC2->DR) - I_B_Offset);
benkatz 14:80ce59119d93 111 I_C = _I_Scale*((float) (ADC1->DR)- I_C_Offset);
benkatz 0:4e1c4df6aabd 112 I_A = -I_B - I_C;
benkatz 0:4e1c4df6aabd 113 //DAC->DHR12R1 = ADC2->DR;
benkatz 0:4e1c4df6aabd 114 //DAC->DHR12R1 = TIM3->CNT>>2;//ADC2->DR; // pass ADC -> DAC, also clears EOC flag
benkatz 0:4e1c4df6aabd 115 ADC1->CR2 |= 0x40000000;
benkatz 7:dc5f27756e02 116 GPIOC->ODR ^= (1 << 4); //toggle pin for debugging
benkatz 0:4e1c4df6aabd 117 }
benkatz 0:4e1c4df6aabd 118