Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FastPWM3 mbed

BREMS/BREMSConfig.cpp@184:633119bb0b77, 2018-02-09 (annotated)

Committer:
bwang
Date:
Fri Feb 09 21:02:14 2018 +0000
Revision:
184:633119bb0b77
Parent:
182:5ed20e4ce158
Child:
185:5c102874b490
02/09/2018 16:01 - moved pinouts to layout.h

Who changed what in which revision?

UserRevisionLine numberNew 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 180:a783a972a867 5 #include "BufferedLogger.h"
bwang 181:d3510c8beab6 6 #include "CommandProcessor.h"
bwang 181:d3510c8beab6 7 #include "PreferenceWriter.h"
bwang 154:0a22dcf91577 8 #include "Filter.h"
bwang 42:030e0ec4eac5 9
bwang 184:633119bb0b77 10 #include "layout.h"
bwang 184:633119bb0b77 11
bwang 42:030e0ec4eac5 12 #include "config_pins.h"
bwang 42:030e0ec4eac5 13 #include "config_inverter.h"
bwang 42:030e0ec4eac5 14 #include "config_motor.h"
bwang 42:030e0ec4eac5 15 #include "config_loop.h"
bwang 180:a783a972a867 16 #include "config_logging.h"
bwang 42:030e0ec4eac5 17
bwang 181:d3510c8beab6 18 #include "prefs.h"
bwang 181:d3510c8beab6 19
bwang 42:030e0ec4eac5 20 void BREMSConfigRegisters(IOStruct *io) {
bwang 42:030e0ec4eac5 21 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
bwang 42:030e0ec4eac5 22 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
bwang 42:030e0ec4eac5 23 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
bwang 42:030e0ec4eac5 24
bwang 42:030e0ec4eac5 25 RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock
bwang 42:030e0ec4eac5 26
bwang 42:030e0ec4eac5 27 io->a = new FastPWM(PWMA);
bwang 42:030e0ec4eac5 28 io->b = new FastPWM(PWMB);
bwang 42:030e0ec4eac5 29 io->c = new FastPWM(PWMC);
bwang 42:030e0ec4eac5 30
bwang 42:030e0ec4eac5 31 NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
bwang 42:030e0ec4eac5 32
bwang 42:030e0ec4eac5 33 TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt
bwang 91:f58472ac3fae 34 TIM1->CR1 = 0x00; //CMS = 10, interrupt only when counting up
bwang 42:030e0ec4eac5 35 TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on,
bwang 91:f58472ac3fae 36 TIM1->RCR |= 0x00; //update event once per up/down count of tim1
bwang 42:030e0ec4eac5 37 TIM1->EGR |= TIM_EGR_UG;
bwang 42:030e0ec4eac5 38
bwang 42:030e0ec4eac5 39 TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock
bwang 91:f58472ac3fae 40 TIM1->ARR = (int) (2 * (float) 9e7 / F_SW);
bwang 90:2ef53b1a22de 41 TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
bwang 42:030e0ec4eac5 42 TIM1->CR1 |= TIM_CR1_CEN;
bwang 42:030e0ec4eac5 43
bwang 42:030e0ec4eac5 44 //ADC Setup
bwang 42:030e0ec4eac5 45 RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
bwang 42:030e0ec4eac5 46 RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
bwang 42:030e0ec4eac5 47
bwang 42:030e0ec4eac5 48 ADC->CCR = 0x00000006; //Regular simultaneous mode, 3 channels
bwang 42:030e0ec4eac5 49
bwang 42:030e0ec4eac5 50 ADC1->CR2 |= ADC_CR2_ADON; //ADC1 on
bwang 42:030e0ec4eac5 51 ADC1->SQR3 = 0x0000004; //PA_4 as ADC1, sequence 0
bwang 42:030e0ec4eac5 52
bwang 42:030e0ec4eac5 53 ADC2->CR2 |= ADC_CR2_ADON; //ADC2 ON
bwang 42:030e0ec4eac5 54 ADC2->SQR3 = 0x00000008; //PB_0 as ADC2, sequence 1
bwang 42:030e0ec4eac5 55
bwang 42:030e0ec4eac5 56 GPIOA->MODER |= (1 << 8);
bwang 42:030e0ec4eac5 57 GPIOA->MODER |= (1 << 9);
bwang 42:030e0ec4eac5 58
bwang 42:030e0ec4eac5 59 GPIOA->MODER |= (1 << 2);
bwang 42:030e0ec4eac5 60 GPIOA->MODER |= (1 << 3);
bwang 42:030e0ec4eac5 61
bwang 42:030e0ec4eac5 62 GPIOA->MODER |= (1 << 0);
bwang 42:030e0ec4eac5 63 GPIOA->MODER |= (1 << 1);
bwang 42:030e0ec4eac5 64
bwang 42:030e0ec4eac5 65 GPIOB->MODER |= (1 << 0);
bwang 42:030e0ec4eac5 66 GPIOB->MODER |= (1 << 1);
bwang 42:030e0ec4eac5 67
bwang 42:030e0ec4eac5 68 GPIOC->MODER |= (1 << 2);
bwang 42:030e0ec4eac5 69 GPIOC->MODER |= (1 << 3);
bwang 42:030e0ec4eac5 70
bwang 42:030e0ec4eac5 71 //DAC setup
bwang 42:030e0ec4eac5 72 RCC->APB1ENR |= 0x20000000;
bwang 42:030e0ec4eac5 73 DAC->CR |= DAC_CR_EN2;
bwang 42:030e0ec4eac5 74
bwang 42:030e0ec4eac5 75 GPIOA->MODER |= (1 << 10);
bwang 42:030e0ec4eac5 76 GPIOA->MODER |= (1 << 11);
bwang 47:1c9868e226d0 77
bwang 47:1c9868e226d0 78 set_dtc(io->a, 0.0f);
bwang 47:1c9868e226d0 79 set_dtc(io->b, 0.0f);
bwang 47:1c9868e226d0 80 set_dtc(io->c, 0.0f);
bwang 42:030e0ec4eac5 81 }
bwang 42:030e0ec4eac5 82
bwang 42:030e0ec4eac5 83 void BREMSZeroCurrent(ReadDataStruct *read) {
bwang 42:030e0ec4eac5 84 for (int i = 0; i < 1000; i++){
bwang 42:030e0ec4eac5 85 read->ia_supp_offset += (float) (ADC1->DR);
bwang 42:030e0ec4eac5 86 read->ib_supp_offset += (float) (ADC2->DR);
bwang 42:030e0ec4eac5 87 ADC1->CR2 |= 0x40000000;
bwang 42:030e0ec4eac5 88 wait_us(100);
bwang 42:030e0ec4eac5 89 }
bwang 42:030e0ec4eac5 90 read->ia_supp_offset /= 1000.0f;
bwang 42:030e0ec4eac5 91 read->ib_supp_offset /= 1000.0f;
bwang 42:030e0ec4eac5 92 read->ia_supp_offset = read->ia_supp_offset / 4096.0f * AVDD - I_OFFSET;
bwang 42:030e0ec4eac5 93 read->ib_supp_offset = read->ib_supp_offset / 4096.0f * AVDD - I_OFFSET;
bwang 42:030e0ec4eac5 94 }
bwang 42:030e0ec4eac5 95
bwang 42:030e0ec4eac5 96 void BREMSStartupMsg(ReadDataStruct *read, Serial *pc) {
bwang 181:d3510c8beab6 97 pc->printf("%s\n", "FOC'ed in the Bot Rev A.");
bwang 42:030e0ec4eac5 98 }
bwang 42:030e0ec4eac5 99
bwang 165:2463dbe52eee 100 void BREMSInit(IOStruct *io, ReadDataStruct *read, FOCStruct *foc, ControlStruct *control, bool tune) {
bwang 42:030e0ec4eac5 101 io->en = new DigitalOut(EN);
bwang 42:030e0ec4eac5 102 io->en->write(0);
bwang 42:030e0ec4eac5 103
bwang 42:030e0ec4eac5 104 io->pc = new Serial(USBTX, USBRX);
bwang 181:d3510c8beab6 105 io->pc->baud(115200);
bwang 165:2463dbe52eee 106
bwang 165:2463dbe52eee 107 io->pos = new PositionSensorEncoder(CPR, 0);
bwang 180:a783a972a867 108 io->logger = new BufferedLogger(LOG_PACKET_SIZE, (LOG_PAGE_SIZE-LOG_HEADER_SIZE)/(LOG_PACKET_SIZE+1), LOG_TX, LOG_RX, LOG_BAUD_RATE);
bwang 181:d3510c8beab6 109 io->pref = new PreferenceWriter(6);
bwang 180:a783a972a867 110
bwang 165:2463dbe52eee 111 wait_ms(750);
bwang 181:d3510c8beab6 112
bwang 181:d3510c8beab6 113 BREMSStartupMsg(read, io->pc);
bwang 181:d3510c8beab6 114
bwang 181:d3510c8beab6 115 cmd_reload(io->pc, io->pref);
bwang 182:5ed20e4ce158 116 if (_PREFS_VALID != 1) {
bwang 181:d3510c8beab6 117 io->pc->printf("%s\n", "Stored config invalid");
bwang 181:d3510c8beab6 118 cmd_defaults(io->pc);
bwang 181:d3510c8beab6 119 io->pc->printf("%s\n", "You should probably at least set throttle and current limits!");
bwang 181:d3510c8beab6 120 }
bwang 165:2463dbe52eee 121
bwang 151:5bbb15351798 122 io->throttle_in = new PwmIn(TH_PIN, TH_LIMIT_LOW, TH_LIMIT_HIGH);
bwang 42:030e0ec4eac5 123
bwang 154:0a22dcf91577 124 control->throttle_filter = new MedianFilter(THROTTLE_FILTER_WINDOW);
bwang 154:0a22dcf91577 125 control->velocity_filter = new MedianFilter(W_FILTER_WINDOW);
bwang 154:0a22dcf91577 126
bwang 42:030e0ec4eac5 127 read->vbus = BUS_VOLTAGE;
bwang 42:030e0ec4eac5 128 read->w = 0.0f;
bwang 42:030e0ec4eac5 129 read->ia_supp_offset = 0.0f;
bwang 42:030e0ec4eac5 130 read->ib_supp_offset = 0.0f;
bwang 42:030e0ec4eac5 131 read->p_mech = io->pos->GetMechPosition();
bwang 42:030e0ec4eac5 132
bwang 52:fd3d8df99287 133 BREMSConfigRegisters(io);
bwang 52:fd3d8df99287 134 wait_ms(250);
bwang 52:fd3d8df99287 135 BREMSZeroCurrent(read);
bwang 181:d3510c8beab6 136 io->pc->printf("%s", ">");
bwang 52:fd3d8df99287 137
bwang 42:030e0ec4eac5 138 control->d_integral = 0.0f;
bwang 42:030e0ec4eac5 139 control->q_integral = 0.0f;
bwang 42:030e0ec4eac5 140 control->d_filtered = 0.0f;
bwang 42:030e0ec4eac5 141 control->q_filtered = 0.0f;
bwang 42:030e0ec4eac5 142 control->last_d = 0.0f;
bwang 42:030e0ec4eac5 143 control->last_q = 0.0f;
bwang 42:030e0ec4eac5 144 control->d_ref = 0.0f;
bwang 42:030e0ec4eac5 145 control->q_ref = 0.0f;
bwang 70:5e39beeb4a21 146 control->torque_percent = 0.0f;
bwang 46:748aba7d111d 147
bwang 42:030e0ec4eac5 148 io->en->write(1);
bwang 42:030e0ec4eac5 149 }