Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Embed:
(wiki syntax)
Show/hide line numbers
BREMSConfig.cpp
00001 #include "mbed.h" 00002 00003 #include "BREMSConfig.h" 00004 #include "BREMSStructs.h" 00005 #include "BufferedLogger.h" 00006 #include "CommandProcessor.h" 00007 #include "PreferenceWriter.h" 00008 #include "Filter.h" 00009 #include "LedBlinker.h" 00010 00011 #include "hardware.h" 00012 #include "derived.h" 00013 #include "prefs.h" 00014 #include "errors.h" 00015 00016 void BREMSConfigRegisters(IOStruct *io) { 00017 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; 00018 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; 00019 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; 00020 00021 RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock 00022 00023 io->a = new FastPWM(PWMA); 00024 io->b = new FastPWM(PWMB); 00025 io->c = new FastPWM(PWMC); 00026 00027 NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ 00028 00029 TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt 00030 TIM1->CR1 = 0x00; //CMS = 10, interrupt only when counting up 00031 TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on, 00032 TIM1->RCR |= 0x00; //update event once per up/down count of tim1 00033 TIM1->EGR |= TIM_EGR_UG; 00034 00035 TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock 00036 TIM1->ARR = (int) (2 * (float) 9e7 / _F_SW); 00037 TIM1->CCER |= (TIM_CCER_CC1NP); //rising edge aligned, non-inverting 00038 TIM1->CR1 |= TIM_CR1_CEN; 00039 00040 //ADC Setup 00041 RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1 00042 RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2 00043 00044 ADC->CCR = 0x00000006; //Regular simultaneous mode, 3 channels 00045 00046 ADC1->CR2 |= ADC_CR2_ADON; //ADC1 on 00047 ADC1->SQR3 = 0x0000004; //PA_4 as ADC1, sequence 0 00048 00049 ADC2->CR2 |= ADC_CR2_ADON; //ADC2 ON 00050 ADC2->SQR3 = 0x00000008; //PB_0 as ADC2, sequence 1 00051 00052 //PA_4 as analog 00053 GPIOA->MODER |= (1 << 8); 00054 GPIOA->MODER |= (1 << 9); 00055 00056 //PB_0 as analog 00057 GPIOB->MODER |= (1 << 0); 00058 GPIOB->MODER |= (1 << 1); 00059 00060 //DAC setup 00061 RCC->APB1ENR |= 0x20000000; 00062 DAC->CR |= DAC_CR_EN2; 00063 00064 //PA_5 as analog 00065 GPIOA->MODER |= (1 << 10); 00066 GPIOA->MODER |= (1 << 11); 00067 00068 set_dtc(io->a, 0.0f); 00069 set_dtc(io->b, 0.0f); 00070 set_dtc(io->c, 0.0f); 00071 } 00072 00073 void BREMSZeroCurrent(ReadDataStruct *read) { 00074 for (int i = 0; i < 1000; i++){ 00075 read->ad1_supp_offset += (float) (ADC1->DR); 00076 read->ad2_supp_offset += (float) (ADC2->DR); 00077 ADC1->CR2 |= 0x40000000; 00078 wait_us(100); 00079 } 00080 read->ad1_supp_offset /= 1000.0f; 00081 read->ad2_supp_offset /= 1000.0f; 00082 read->ad1_supp_offset = read->ad1_supp_offset / 4096.0f * AVDD - I_OFFSET; 00083 read->ad2_supp_offset = read->ad2_supp_offset / 4096.0f * AVDD - I_OFFSET; 00084 } 00085 00086 void BREMSStartupMsg(ReadDataStruct *read, Serial *pc) { 00087 pc->printf("%s\n", "FOC'ed in the Bot Rev A."); 00088 } 00089 00090 void BREMSInit(IOStruct *io, ReadDataStruct *read, FOCStruct *foc, ControlStruct *control, bool tune) { 00091 io->en = new DigitalOut(EN); 00092 io->en->write(0); 00093 00094 io->pc = new Serial(USBTX, USBRX); 00095 io->pc->baud(115200); 00096 NVIC_SetPriority(USART2_IRQn, 2); 00097 00098 init_masks(); 00099 00100 wait_ms(50); 00101 DigitalOut resolver_reset_out(RESOLVER_RESET); 00102 resolver_reset_out = 0; 00103 wait_ms(10); 00104 DigitalIn resolver_reset_in(RESOLVER_RESET); 00105 00106 cmd_clear(io->pc); 00107 BREMSStartupMsg(read, io->pc); 00108 00109 BREMS_mode = MODE_CFG; 00110 io->pref = new PreferenceWriter(6); 00111 cmd_reload(io->pc, io->pref); 00112 if (_PREFS_VALID != 1) { 00113 io->pc->printf("%s\n", "Stored config invalid"); 00114 BREMS_mode = MODE_CFG; 00115 cmd_defaults(io->pc); 00116 BREMS_op = OP_TORQUE; 00117 BREMS_src = CMD_SRC_TERMINAL; 00118 io->pc->printf("%s\n", "You should probably at least set throttle and current limits!"); 00119 } 00120 00121 io->blink = new LedBlinker(STATUS_LED, _F_SW, 1.0); 00122 io->blink->set_code(0x0f); 00123 00124 wait_ms(750); 00125 00126 io->pos = new PositionSensorEncoder(_CPR, 0); 00127 io->logger = new BufferedLogger(_LOG_PACKET_SIZE, (_LOG_PAGE_SIZE-_LOG_HEADER_SIZE)/(_LOG_PACKET_SIZE+1), LOG_TX, LOG_RX, _LOG_BAUD_RATE); 00128 io->throttle_in = new PwmIn(TH_PIN, _TH_LIMIT_LOW, _TH_LIMIT_HIGH); 00129 00130 io->cmd_busy = false; 00131 00132 control->throttle_filter = new MedianFilter(_THROTTLE_FILTER_WINDOW); 00133 control->velocity_filter = new MedianFilter(_W_FILTER_WINDOW); 00134 00135 read->vbus = _BUS_VOLTAGE; 00136 read->w = 0.0f; 00137 read->ad1_supp_offset = 0.0f; 00138 read->ad2_supp_offset = 0.0f; 00139 read->p_mech = io->pos->GetMechPosition(); 00140 00141 BREMSConfigRegisters(io); 00142 wait_ms(250); 00143 BREMSZeroCurrent(read); 00144 io->pc->printf("%s", ">"); 00145 00146 control->d_integral = 0.0f; 00147 control->q_integral = 0.0f; 00148 control->d_filtered = 0.0f; 00149 control->q_filtered = 0.0f; 00150 control->last_d = 0.0f; 00151 control->last_q = 0.0f; 00152 control->d_ref = 0.0f; 00153 control->q_ref = 0.0f; 00154 control->torque_percent = 0.0f; 00155 control->w_integral = 0.0f; 00156 control->enabled = false; 00157 00158 io->en->write(1); 00159 }
Generated on Tue Jul 12 2022 17:58:38 by 1.7.2