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@229:90c6892f4d3b, 2018-11-10 (annotated)

Committer:
bwang
Date:
Sat Nov 10 06:27:07 2018 +0000
Revision:
229:90c6892f4d3b
Parent:
226:0e3fbbfea464
Child:
237:1a4591a0dd2d
11/10/2018 01:26 - renamed ia_supp_offset, ib_supp_offset to ad1, ad2_supp_offset to reflect the fact that they correspond to ADC channel offsets and not currents (which can be renamed in elsewhere)

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 223:b986e7cee521 9 #include "LedBlinker.h"
bwang 42:030e0ec4eac5 10
bwang 196:7172e6e28867 11 #include "hardware.h"
bwang 185:5c102874b490 12 #include "derived.h"
bwang 181:d3510c8beab6 13 #include "prefs.h"
bwang 193:3abadeecf908 14 #include "errors.h"
bwang 181:d3510c8beab6 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 189:760cd81a7633 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 226:0e3fbbfea464 52 //PA_4 as analog
bwang 42:030e0ec4eac5 53 GPIOA->MODER |= (1 << 8);
bwang 42:030e0ec4eac5 54 GPIOA->MODER |= (1 << 9);
bwang 42:030e0ec4eac5 55
bwang 226:0e3fbbfea464 56 //PB_0 as analog
bwang 42:030e0ec4eac5 57 GPIOB->MODER |= (1 << 0);
bwang 42:030e0ec4eac5 58 GPIOB->MODER |= (1 << 1);
bwang 42:030e0ec4eac5 59
bwang 42:030e0ec4eac5 60 //DAC setup
bwang 42:030e0ec4eac5 61 RCC->APB1ENR |= 0x20000000;
bwang 42:030e0ec4eac5 62 DAC->CR |= DAC_CR_EN2;
bwang 42:030e0ec4eac5 63
bwang 226:0e3fbbfea464 64 //PA_5 as analog
bwang 42:030e0ec4eac5 65 GPIOA->MODER |= (1 << 10);
bwang 42:030e0ec4eac5 66 GPIOA->MODER |= (1 << 11);
bwang 47:1c9868e226d0 67
bwang 47:1c9868e226d0 68 set_dtc(io->a, 0.0f);
bwang 47:1c9868e226d0 69 set_dtc(io->b, 0.0f);
bwang 47:1c9868e226d0 70 set_dtc(io->c, 0.0f);
bwang 42:030e0ec4eac5 71 }
bwang 42:030e0ec4eac5 72
bwang 42:030e0ec4eac5 73 void BREMSZeroCurrent(ReadDataStruct *read) {
bwang 42:030e0ec4eac5 74 for (int i = 0; i < 1000; i++){
bwang 229:90c6892f4d3b 75 read->ad1_supp_offset += (float) (ADC1->DR);
bwang 229:90c6892f4d3b 76 read->ad2_supp_offset += (float) (ADC2->DR);
bwang 42:030e0ec4eac5 77 ADC1->CR2 |= 0x40000000;
bwang 42:030e0ec4eac5 78 wait_us(100);
bwang 42:030e0ec4eac5 79 }
bwang 229:90c6892f4d3b 80 read->ad1_supp_offset /= 1000.0f;
bwang 229:90c6892f4d3b 81 read->ad2_supp_offset /= 1000.0f;
bwang 229:90c6892f4d3b 82 read->ad1_supp_offset = read->ad1_supp_offset / 4096.0f * AVDD - I_OFFSET;
bwang 229:90c6892f4d3b 83 read->ad2_supp_offset = read->ad2_supp_offset / 4096.0f * AVDD - I_OFFSET;
bwang 42:030e0ec4eac5 84 }
bwang 42:030e0ec4eac5 85
bwang 42:030e0ec4eac5 86 void BREMSStartupMsg(ReadDataStruct *read, Serial *pc) {
bwang 181:d3510c8beab6 87 pc->printf("%s\n", "FOC'ed in the Bot Rev A.");
bwang 42:030e0ec4eac5 88 }
bwang 42:030e0ec4eac5 89
bwang 165:2463dbe52eee 90 void BREMSInit(IOStruct *io, ReadDataStruct *read, FOCStruct *foc, ControlStruct *control, bool tune) {
bwang 42:030e0ec4eac5 91 io->en = new DigitalOut(EN);
bwang 42:030e0ec4eac5 92 io->en->write(0);
bwang 42:030e0ec4eac5 93
bwang 42:030e0ec4eac5 94 io->pc = new Serial(USBTX, USBRX);
bwang 181:d3510c8beab6 95 io->pc->baud(115200);
bwang 206:5c848ea69028 96 NVIC_SetPriority(USART2_IRQn, 2);
bwang 165:2463dbe52eee 97
bwang 193:3abadeecf908 98 init_masks();
bwang 193:3abadeecf908 99
bwang 217:1171ed0f6145 100 wait_ms(50);
bwang 216:198ebadc005c 101 DigitalOut resolver_reset_out(RESOLVER_RESET);
bwang 216:198ebadc005c 102 resolver_reset_out = 0;
bwang 216:198ebadc005c 103 wait_ms(10);
bwang 216:198ebadc005c 104 DigitalIn resolver_reset_in(RESOLVER_RESET);
bwang 216:198ebadc005c 105
bwang 181:d3510c8beab6 106 BREMSStartupMsg(read, io->pc);
bwang 199:c160a2c03781 107
bwang 199:c160a2c03781 108 BREMS_mode = MODE_CFG;
bwang 188:43f50a4cc040 109 io->pref = new PreferenceWriter(6);
bwang 181:d3510c8beab6 110 cmd_reload(io->pc, io->pref);
bwang 182:5ed20e4ce158 111 if (_PREFS_VALID != 1) {
bwang 181:d3510c8beab6 112 io->pc->printf("%s\n", "Stored config invalid");
bwang 199:c160a2c03781 113 BREMS_mode = MODE_CFG;
bwang 181:d3510c8beab6 114 cmd_defaults(io->pc);
bwang 210:f9b722cf8a2c 115 BREMS_op = OP_TORQUE;
bwang 210:f9b722cf8a2c 116 BREMS_src = CMD_SRC_TERMINAL;
bwang 181:d3510c8beab6 117 io->pc->printf("%s\n", "You should probably at least set throttle and current limits!");
bwang 181:d3510c8beab6 118 }
bwang 188:43f50a4cc040 119
bwang 223:b986e7cee521 120 io->blink = new LedBlinker(STATUS_LED, _F_SW, 1.0);
bwang 223:b986e7cee521 121 io->blink->set_code(0x0f);
bwang 223:b986e7cee521 122
bwang 197:68fd01b73537 123 wait_ms(750);
bwang 197:68fd01b73537 124
bwang 189:760cd81a7633 125 io->pos = new PositionSensorEncoder(_CPR, 0);
bwang 189:760cd81a7633 126 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 197:68fd01b73537 127
bwang 189:760cd81a7633 128 io->throttle_in = new PwmIn(TH_PIN, _TH_LIMIT_LOW, _TH_LIMIT_HIGH);
bwang 42:030e0ec4eac5 129
bwang 189:760cd81a7633 130 control->throttle_filter = new MedianFilter(_THROTTLE_FILTER_WINDOW);
bwang 189:760cd81a7633 131 control->velocity_filter = new MedianFilter(_W_FILTER_WINDOW);
bwang 154:0a22dcf91577 132
bwang 189:760cd81a7633 133 read->vbus = _BUS_VOLTAGE;
bwang 42:030e0ec4eac5 134 read->w = 0.0f;
bwang 229:90c6892f4d3b 135 read->ad1_supp_offset = 0.0f;
bwang 229:90c6892f4d3b 136 read->ad2_supp_offset = 0.0f;
bwang 42:030e0ec4eac5 137 read->p_mech = io->pos->GetMechPosition();
bwang 42:030e0ec4eac5 138
bwang 52:fd3d8df99287 139 BREMSConfigRegisters(io);
bwang 52:fd3d8df99287 140 wait_ms(250);
bwang 52:fd3d8df99287 141 BREMSZeroCurrent(read);
bwang 181:d3510c8beab6 142 io->pc->printf("%s", ">");
bwang 52:fd3d8df99287 143
bwang 42:030e0ec4eac5 144 control->d_integral = 0.0f;
bwang 42:030e0ec4eac5 145 control->q_integral = 0.0f;
bwang 42:030e0ec4eac5 146 control->d_filtered = 0.0f;
bwang 42:030e0ec4eac5 147 control->q_filtered = 0.0f;
bwang 42:030e0ec4eac5 148 control->last_d = 0.0f;
bwang 42:030e0ec4eac5 149 control->last_q = 0.0f;
bwang 42:030e0ec4eac5 150 control->d_ref = 0.0f;
bwang 42:030e0ec4eac5 151 control->q_ref = 0.0f;
bwang 70:5e39beeb4a21 152 control->torque_percent = 0.0f;
bwang 187:523cf8c962e4 153 control->enabled = false;
bwang 46:748aba7d111d 154
bwang 42:030e0ec4eac5 155 io->en->write(1);
bwang 42:030e0ec4eac5 156 }