Bayley Wang
/
foc-ed_in_the_bot_compact
robot
BREMS/BREMSConfig.cpp@179:935f9d78d936, 2018-02-08 (annotated)
- Committer:
- bwang
- Date:
- Thu Feb 08 02:42:16 2018 +0000
- Revision:
- 179:935f9d78d936
- Parent:
- 165:2463dbe52eee
02/07/2017 21:39 - updated BREMS*.h/cpp to include PreferenceWriter functionality, updated CommandProcessor to match current config variables
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 | 154:0a22dcf91577 | 5 | #include "Filter.h" |
bwang | 42:030e0ec4eac5 | 6 | |
bwang | 179:935f9d78d936 | 7 | #include "CommandProcessor.h" |
bwang | 179:935f9d78d936 | 8 | #include "PreferenceWriter.h" |
bwang | 179:935f9d78d936 | 9 | |
bwang | 42:030e0ec4eac5 | 10 | #include "config_pins.h" |
bwang | 42:030e0ec4eac5 | 11 | #include "config_inverter.h" |
bwang | 42:030e0ec4eac5 | 12 | #include "config_motor.h" |
bwang | 42:030e0ec4eac5 | 13 | #include "config_loop.h" |
bwang | 179:935f9d78d936 | 14 | #include "layout.h" |
bwang | 42:030e0ec4eac5 | 15 | |
bwang | 42:030e0ec4eac5 | 16 | void BREMSConfigRegisters(IOStruct *io) { |
bwang | 42:030e0ec4eac5 | 17 | RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; |
bwang | 42:030e0ec4eac5 | 18 | RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; |
bwang | 42:030e0ec4eac5 | 19 | RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; |
bwang | 42:030e0ec4eac5 | 20 | |
bwang | 42:030e0ec4eac5 | 21 | RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock |
bwang | 42:030e0ec4eac5 | 22 | |
bwang | 42:030e0ec4eac5 | 23 | io->a = new FastPWM(PWMA); |
bwang | 42:030e0ec4eac5 | 24 | io->b = new FastPWM(PWMB); |
bwang | 42:030e0ec4eac5 | 25 | io->c = new FastPWM(PWMC); |
bwang | 42:030e0ec4eac5 | 26 | |
bwang | 42:030e0ec4eac5 | 27 | NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ |
bwang | 42:030e0ec4eac5 | 28 | |
bwang | 42:030e0ec4eac5 | 29 | TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt |
bwang | 91:f58472ac3fae | 30 | TIM1->CR1 = 0x00; //CMS = 10, interrupt only when counting up |
bwang | 42:030e0ec4eac5 | 31 | TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on, |
bwang | 91:f58472ac3fae | 32 | TIM1->RCR |= 0x00; //update event once per up/down count of tim1 |
bwang | 42:030e0ec4eac5 | 33 | TIM1->EGR |= TIM_EGR_UG; |
bwang | 42:030e0ec4eac5 | 34 | |
bwang | 42:030e0ec4eac5 | 35 | TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock |
bwang | 91:f58472ac3fae | 36 | TIM1->ARR = (int) (2 * (float) 9e7 / F_SW); |
bwang | 90:2ef53b1a22de | 37 | TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on. |
bwang | 42:030e0ec4eac5 | 38 | TIM1->CR1 |= TIM_CR1_CEN; |
bwang | 42:030e0ec4eac5 | 39 | |
bwang | 42:030e0ec4eac5 | 40 | //ADC Setup |
bwang | 42:030e0ec4eac5 | 41 | RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1 |
bwang | 42:030e0ec4eac5 | 42 | RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2 |
bwang | 42:030e0ec4eac5 | 43 | |
bwang | 42:030e0ec4eac5 | 44 | ADC->CCR = 0x00000006; //Regular simultaneous mode, 3 channels |
bwang | 42:030e0ec4eac5 | 45 | |
bwang | 42:030e0ec4eac5 | 46 | ADC1->CR2 |= ADC_CR2_ADON; //ADC1 on |
bwang | 42:030e0ec4eac5 | 47 | ADC1->SQR3 = 0x0000004; //PA_4 as ADC1, sequence 0 |
bwang | 42:030e0ec4eac5 | 48 | |
bwang | 42:030e0ec4eac5 | 49 | ADC2->CR2 |= ADC_CR2_ADON; //ADC2 ON |
bwang | 42:030e0ec4eac5 | 50 | ADC2->SQR3 = 0x00000008; //PB_0 as ADC2, sequence 1 |
bwang | 42:030e0ec4eac5 | 51 | |
bwang | 42:030e0ec4eac5 | 52 | GPIOA->MODER |= (1 << 8); |
bwang | 42:030e0ec4eac5 | 53 | GPIOA->MODER |= (1 << 9); |
bwang | 42:030e0ec4eac5 | 54 | |
bwang | 42:030e0ec4eac5 | 55 | GPIOA->MODER |= (1 << 2); |
bwang | 42:030e0ec4eac5 | 56 | GPIOA->MODER |= (1 << 3); |
bwang | 42:030e0ec4eac5 | 57 | |
bwang | 42:030e0ec4eac5 | 58 | GPIOA->MODER |= (1 << 0); |
bwang | 42:030e0ec4eac5 | 59 | GPIOA->MODER |= (1 << 1); |
bwang | 42:030e0ec4eac5 | 60 | |
bwang | 42:030e0ec4eac5 | 61 | GPIOB->MODER |= (1 << 0); |
bwang | 42:030e0ec4eac5 | 62 | GPIOB->MODER |= (1 << 1); |
bwang | 42:030e0ec4eac5 | 63 | |
bwang | 42:030e0ec4eac5 | 64 | GPIOC->MODER |= (1 << 2); |
bwang | 42:030e0ec4eac5 | 65 | GPIOC->MODER |= (1 << 3); |
bwang | 42:030e0ec4eac5 | 66 | |
bwang | 42:030e0ec4eac5 | 67 | //DAC setup |
bwang | 42:030e0ec4eac5 | 68 | RCC->APB1ENR |= 0x20000000; |
bwang | 42:030e0ec4eac5 | 69 | DAC->CR |= DAC_CR_EN2; |
bwang | 42:030e0ec4eac5 | 70 | |
bwang | 42:030e0ec4eac5 | 71 | GPIOA->MODER |= (1 << 10); |
bwang | 42:030e0ec4eac5 | 72 | GPIOA->MODER |= (1 << 11); |
bwang | 47:1c9868e226d0 | 73 | |
bwang | 47:1c9868e226d0 | 74 | set_dtc(io->a, 0.0f); |
bwang | 47:1c9868e226d0 | 75 | set_dtc(io->b, 0.0f); |
bwang | 47:1c9868e226d0 | 76 | set_dtc(io->c, 0.0f); |
bwang | 42:030e0ec4eac5 | 77 | } |
bwang | 42:030e0ec4eac5 | 78 | |
bwang | 42:030e0ec4eac5 | 79 | void BREMSZeroCurrent(ReadDataStruct *read) { |
bwang | 42:030e0ec4eac5 | 80 | for (int i = 0; i < 1000; i++){ |
bwang | 42:030e0ec4eac5 | 81 | read->ia_supp_offset += (float) (ADC1->DR); |
bwang | 42:030e0ec4eac5 | 82 | read->ib_supp_offset += (float) (ADC2->DR); |
bwang | 42:030e0ec4eac5 | 83 | ADC1->CR2 |= 0x40000000; |
bwang | 42:030e0ec4eac5 | 84 | wait_us(100); |
bwang | 42:030e0ec4eac5 | 85 | } |
bwang | 42:030e0ec4eac5 | 86 | read->ia_supp_offset /= 1000.0f; |
bwang | 42:030e0ec4eac5 | 87 | read->ib_supp_offset /= 1000.0f; |
bwang | 42:030e0ec4eac5 | 88 | read->ia_supp_offset = read->ia_supp_offset / 4096.0f * AVDD - I_OFFSET; |
bwang | 42:030e0ec4eac5 | 89 | read->ib_supp_offset = read->ib_supp_offset / 4096.0f * AVDD - I_OFFSET; |
bwang | 42:030e0ec4eac5 | 90 | } |
bwang | 42:030e0ec4eac5 | 91 | |
bwang | 42:030e0ec4eac5 | 92 | void BREMSStartupMsg(ReadDataStruct *read, Serial *pc) { |
bwang | 42:030e0ec4eac5 | 93 | pc->printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A."); |
bwang | 42:030e0ec4eac5 | 94 | } |
bwang | 42:030e0ec4eac5 | 95 | |
bwang | 165:2463dbe52eee | 96 | void BREMSInit(IOStruct *io, ReadDataStruct *read, FOCStruct *foc, ControlStruct *control, bool tune) { |
bwang | 42:030e0ec4eac5 | 97 | io->en = new DigitalOut(EN); |
bwang | 42:030e0ec4eac5 | 98 | io->en->write(0); |
bwang | 42:030e0ec4eac5 | 99 | |
bwang | 42:030e0ec4eac5 | 100 | io->pc = new Serial(USBTX, USBRX); |
bwang | 179:935f9d78d936 | 101 | io->pc->baud(115200); |
bwang | 179:935f9d78d936 | 102 | |
bwang | 179:935f9d78d936 | 103 | io->pref = new PreferenceWriter(6); |
bwang | 179:935f9d78d936 | 104 | BREMSStartupMsg(read, io->pc); |
bwang | 179:935f9d78d936 | 105 | cmd_reload(io->pc, io->pref); |
bwang | 179:935f9d78d936 | 106 | io->pc->printf("%s", ">"); |
bwang | 165:2463dbe52eee | 107 | |
bwang | 165:2463dbe52eee | 108 | io->pos = new PositionSensorEncoder(CPR, 0); |
bwang | 165:2463dbe52eee | 109 | |
bwang | 165:2463dbe52eee | 110 | wait_ms(750); |
bwang | 165:2463dbe52eee | 111 | |
bwang | 151:5bbb15351798 | 112 | io->throttle_in = new PwmIn(TH_PIN, TH_LIMIT_LOW, TH_LIMIT_HIGH); |
bwang | 42:030e0ec4eac5 | 113 | |
bwang | 154:0a22dcf91577 | 114 | control->throttle_filter = new MedianFilter(THROTTLE_FILTER_WINDOW); |
bwang | 154:0a22dcf91577 | 115 | control->velocity_filter = new MedianFilter(W_FILTER_WINDOW); |
bwang | 154:0a22dcf91577 | 116 | |
bwang | 42:030e0ec4eac5 | 117 | read->vbus = BUS_VOLTAGE; |
bwang | 42:030e0ec4eac5 | 118 | read->w = 0.0f; |
bwang | 42:030e0ec4eac5 | 119 | read->ia_supp_offset = 0.0f; |
bwang | 42:030e0ec4eac5 | 120 | read->ib_supp_offset = 0.0f; |
bwang | 42:030e0ec4eac5 | 121 | read->p_mech = io->pos->GetMechPosition(); |
bwang | 42:030e0ec4eac5 | 122 | |
bwang | 52:fd3d8df99287 | 123 | BREMSConfigRegisters(io); |
bwang | 52:fd3d8df99287 | 124 | wait_ms(250); |
bwang | 52:fd3d8df99287 | 125 | BREMSZeroCurrent(read); |
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 | } |