Bayley Wang
/
foc-ed_in_the_bot_compact
robot
BREMS/BREMSConfig.cpp@92:a9dac72d8cac, 2017-04-06 (annotated)
- Committer:
- bwang
- Date:
- Thu Apr 06 17:33:47 2017 +0000
- Revision:
- 92:a9dac72d8cac
- Parent:
- 91:f58472ac3fae
- Child:
- 120:57b6f3b1356b
--PwmIn now checks lower bounds for sanity and fall without rise; --switched to edge aligned pwm to work around prius module propagation delays
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 42:030e0ec4eac5 | 1 | #include "mbed.h" |
bwang | 42:030e0ec4eac5 | 2 | |
bwang | 42:030e0ec4eac5 | 3 | #include "BREMSConfig.h" |
bwang | 42:030e0ec4eac5 | 4 | #include "BREMSStructs.h" |
bwang | 42:030e0ec4eac5 | 5 | |
bwang | 42:030e0ec4eac5 | 6 | #include "config_pins.h" |
bwang | 42:030e0ec4eac5 | 7 | #include "config_inverter.h" |
bwang | 42:030e0ec4eac5 | 8 | #include "config_motor.h" |
bwang | 42:030e0ec4eac5 | 9 | #include "config_loop.h" |
bwang | 42:030e0ec4eac5 | 10 | |
bwang | 42:030e0ec4eac5 | 11 | void BREMSConfigRegisters(IOStruct *io) { |
bwang | 42:030e0ec4eac5 | 12 | RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; |
bwang | 42:030e0ec4eac5 | 13 | RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; |
bwang | 42:030e0ec4eac5 | 14 | RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; |
bwang | 42:030e0ec4eac5 | 15 | |
bwang | 42:030e0ec4eac5 | 16 | RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock |
bwang | 42:030e0ec4eac5 | 17 | |
bwang | 42:030e0ec4eac5 | 18 | io->a = new FastPWM(PWMA); |
bwang | 42:030e0ec4eac5 | 19 | io->b = new FastPWM(PWMB); |
bwang | 42:030e0ec4eac5 | 20 | io->c = new FastPWM(PWMC); |
bwang | 42:030e0ec4eac5 | 21 | |
bwang | 42:030e0ec4eac5 | 22 | NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ |
bwang | 42:030e0ec4eac5 | 23 | |
bwang | 42:030e0ec4eac5 | 24 | TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt |
bwang | 91:f58472ac3fae | 25 | TIM1->CR1 = 0x00; //CMS = 10, interrupt only when counting up |
bwang | 42:030e0ec4eac5 | 26 | TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on, |
bwang | 91:f58472ac3fae | 27 | TIM1->RCR |= 0x00; //update event once per up/down count of tim1 |
bwang | 42:030e0ec4eac5 | 28 | TIM1->EGR |= TIM_EGR_UG; |
bwang | 42:030e0ec4eac5 | 29 | |
bwang | 42:030e0ec4eac5 | 30 | TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock |
bwang | 91:f58472ac3fae | 31 | TIM1->ARR = (int) (2 * (float) 9e7 / F_SW); |
bwang | 90:2ef53b1a22de | 32 | TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on. |
bwang | 42:030e0ec4eac5 | 33 | TIM1->CR1 |= TIM_CR1_CEN; |
bwang | 42:030e0ec4eac5 | 34 | |
bwang | 42:030e0ec4eac5 | 35 | //ADC Setup |
bwang | 42:030e0ec4eac5 | 36 | RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1 |
bwang | 42:030e0ec4eac5 | 37 | RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2 |
bwang | 42:030e0ec4eac5 | 38 | |
bwang | 42:030e0ec4eac5 | 39 | ADC->CCR = 0x00000006; //Regular simultaneous mode, 3 channels |
bwang | 42:030e0ec4eac5 | 40 | |
bwang | 42:030e0ec4eac5 | 41 | ADC1->CR2 |= ADC_CR2_ADON; //ADC1 on |
bwang | 42:030e0ec4eac5 | 42 | ADC1->SQR3 = 0x0000004; //PA_4 as ADC1, sequence 0 |
bwang | 42:030e0ec4eac5 | 43 | |
bwang | 42:030e0ec4eac5 | 44 | ADC2->CR2 |= ADC_CR2_ADON; //ADC2 ON |
bwang | 42:030e0ec4eac5 | 45 | ADC2->SQR3 = 0x00000008; //PB_0 as ADC2, sequence 1 |
bwang | 42:030e0ec4eac5 | 46 | |
bwang | 42:030e0ec4eac5 | 47 | GPIOA->MODER |= (1 << 8); |
bwang | 42:030e0ec4eac5 | 48 | GPIOA->MODER |= (1 << 9); |
bwang | 42:030e0ec4eac5 | 49 | |
bwang | 42:030e0ec4eac5 | 50 | GPIOA->MODER |= (1 << 2); |
bwang | 42:030e0ec4eac5 | 51 | GPIOA->MODER |= (1 << 3); |
bwang | 42:030e0ec4eac5 | 52 | |
bwang | 42:030e0ec4eac5 | 53 | GPIOA->MODER |= (1 << 0); |
bwang | 42:030e0ec4eac5 | 54 | GPIOA->MODER |= (1 << 1); |
bwang | 42:030e0ec4eac5 | 55 | |
bwang | 42:030e0ec4eac5 | 56 | GPIOB->MODER |= (1 << 0); |
bwang | 42:030e0ec4eac5 | 57 | GPIOB->MODER |= (1 << 1); |
bwang | 42:030e0ec4eac5 | 58 | |
bwang | 42:030e0ec4eac5 | 59 | GPIOC->MODER |= (1 << 2); |
bwang | 42:030e0ec4eac5 | 60 | GPIOC->MODER |= (1 << 3); |
bwang | 42:030e0ec4eac5 | 61 | |
bwang | 42:030e0ec4eac5 | 62 | //DAC setup |
bwang | 42:030e0ec4eac5 | 63 | RCC->APB1ENR |= 0x20000000; |
bwang | 42:030e0ec4eac5 | 64 | DAC->CR |= DAC_CR_EN2; |
bwang | 42:030e0ec4eac5 | 65 | |
bwang | 42:030e0ec4eac5 | 66 | GPIOA->MODER |= (1 << 10); |
bwang | 42:030e0ec4eac5 | 67 | GPIOA->MODER |= (1 << 11); |
bwang | 47:1c9868e226d0 | 68 | |
bwang | 47:1c9868e226d0 | 69 | set_dtc(io->a, 0.0f); |
bwang | 47:1c9868e226d0 | 70 | set_dtc(io->b, 0.0f); |
bwang | 47:1c9868e226d0 | 71 | set_dtc(io->c, 0.0f); |
bwang | 42:030e0ec4eac5 | 72 | } |
bwang | 42:030e0ec4eac5 | 73 | |
bwang | 42:030e0ec4eac5 | 74 | void BREMSZeroCurrent(ReadDataStruct *read) { |
bwang | 42:030e0ec4eac5 | 75 | for (int i = 0; i < 1000; i++){ |
bwang | 42:030e0ec4eac5 | 76 | read->ia_supp_offset += (float) (ADC1->DR); |
bwang | 42:030e0ec4eac5 | 77 | read->ib_supp_offset += (float) (ADC2->DR); |
bwang | 42:030e0ec4eac5 | 78 | ADC1->CR2 |= 0x40000000; |
bwang | 42:030e0ec4eac5 | 79 | wait_us(100); |
bwang | 42:030e0ec4eac5 | 80 | } |
bwang | 42:030e0ec4eac5 | 81 | read->ia_supp_offset /= 1000.0f; |
bwang | 42:030e0ec4eac5 | 82 | read->ib_supp_offset /= 1000.0f; |
bwang | 42:030e0ec4eac5 | 83 | read->ia_supp_offset = read->ia_supp_offset / 4096.0f * AVDD - I_OFFSET; |
bwang | 42:030e0ec4eac5 | 84 | read->ib_supp_offset = read->ib_supp_offset / 4096.0f * AVDD - I_OFFSET; |
bwang | 42:030e0ec4eac5 | 85 | } |
bwang | 42:030e0ec4eac5 | 86 | |
bwang | 42:030e0ec4eac5 | 87 | void BREMSStartupMsg(ReadDataStruct *read, Serial *pc) { |
bwang | 42:030e0ec4eac5 | 88 | pc->printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A."); |
bwang | 42:030e0ec4eac5 | 89 | pc->printf("%s\n\r", "====Config Data===="); |
bwang | 42:030e0ec4eac5 | 90 | pc->printf("Current Sensor Offset: %f mV\n\r", I_OFFSET); |
bwang | 42:030e0ec4eac5 | 91 | pc->printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE); |
bwang | 42:030e0ec4eac5 | 92 | pc->printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE); |
bwang | 42:030e0ec4eac5 | 93 | pc->printf("Switching Frequency: %f KHz \n\r", F_SW / 1000.0f); |
bwang | 44:3fd6a43b91f0 | 94 | pc->printf("Polling Frequency: %f Hz \n\r", F_SLOW_LOOP); |
bwang | 42:030e0ec4eac5 | 95 | pc->printf("Pole pairs: %d\n\r", (int) POLE_PAIRS); |
bwang | 42:030e0ec4eac5 | 96 | pc->printf("Resolver lobes: %d\n\r", (int) RESOLVER_LOBES); |
bwang | 59:0416da6c3060 | 97 | pc->printf("Loop KP_D: %f\n\r", KP_D); |
bwang | 59:0416da6c3060 | 98 | pc->printf("Loop KI_D: %f\n\r", KI_D); |
bwang | 59:0416da6c3060 | 99 | pc->printf("Loop KP_Q: %f\n\r", KP_Q); |
bwang | 59:0416da6c3060 | 100 | pc->printf("Loop KI_Q: %f\n\r", KI_Q); |
bwang | 42:030e0ec4eac5 | 101 | pc->printf("Ia offset: %f mV\n\r", read->ia_supp_offset); |
bwang | 42:030e0ec4eac5 | 102 | pc->printf("Ib offset: %f mV\n\r", read->ib_supp_offset); |
bwang | 42:030e0ec4eac5 | 103 | pc->printf("\n\r"); |
bwang | 42:030e0ec4eac5 | 104 | } |
bwang | 42:030e0ec4eac5 | 105 | |
bwang | 42:030e0ec4eac5 | 106 | void BREMSInit(IOStruct *io, ReadDataStruct *read, FOCStruct *foc, ControlStruct *control, bool tune) { |
bwang | 42:030e0ec4eac5 | 107 | io->en = new DigitalOut(EN); |
bwang | 42:030e0ec4eac5 | 108 | io->en->write(0); |
bwang | 42:030e0ec4eac5 | 109 | |
bwang | 42:030e0ec4eac5 | 110 | io->pc = new Serial(USBTX, USBRX); |
bwang | 61:85a31897e719 | 111 | io->pc->baud(921600); |
bwang | 46:748aba7d111d | 112 | |
bwang | 92:a9dac72d8cac | 113 | io->throttle_in = new PwmIn(TH_PIN, TH_LIMIT_LOW, TH_LIMIT_HIGH, TH_LIMIT_CRAZY_LOW, TH_LIMIT_CRAZY_HI); |
bwang | 42:030e0ec4eac5 | 114 | io->pos = new PositionSensorEncoder(CPR, 0); |
bwang | 42:030e0ec4eac5 | 115 | |
bwang | 42:030e0ec4eac5 | 116 | read->vbus = BUS_VOLTAGE; |
bwang | 42:030e0ec4eac5 | 117 | read->w = 0.0f; |
bwang | 42:030e0ec4eac5 | 118 | read->ia_supp_offset = 0.0f; |
bwang | 42:030e0ec4eac5 | 119 | read->ib_supp_offset = 0.0f; |
bwang | 42:030e0ec4eac5 | 120 | read->p_mech = io->pos->GetMechPosition(); |
bwang | 42:030e0ec4eac5 | 121 | |
bwang | 52:fd3d8df99287 | 122 | BREMSConfigRegisters(io); |
bwang | 52:fd3d8df99287 | 123 | wait_ms(250); |
bwang | 52:fd3d8df99287 | 124 | BREMSZeroCurrent(read); |
bwang | 52:fd3d8df99287 | 125 | BREMSStartupMsg(read, io->pc); |
bwang | 52:fd3d8df99287 | 126 | |
bwang | 42:030e0ec4eac5 | 127 | control->d_integral = 0.0f; |
bwang | 42:030e0ec4eac5 | 128 | control->q_integral = 0.0f; |
bwang | 42:030e0ec4eac5 | 129 | control->d_filtered = 0.0f; |
bwang | 42:030e0ec4eac5 | 130 | control->q_filtered = 0.0f; |
bwang | 42:030e0ec4eac5 | 131 | control->last_d = 0.0f; |
bwang | 42:030e0ec4eac5 | 132 | control->last_q = 0.0f; |
bwang | 42:030e0ec4eac5 | 133 | control->d_ref = 0.0f; |
bwang | 42:030e0ec4eac5 | 134 | control->q_ref = 0.0f; |
bwang | 70:5e39beeb4a21 | 135 | control->torque_percent = 0.0f; |
bwang | 46:748aba7d111d | 136 | |
bwang | 42:030e0ec4eac5 | 137 | io->en->write(1); |
bwang | 42:030e0ec4eac5 | 138 | } |