Ben Katz
/
Hobbyking_Cheetah_V1
FOC Implementation for putting multirotor motors in robots
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?
User | Revision | Line number | New 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 |