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@180:a783a972a867, 2018-02-09 (annotated)

Committer:
bwang
Date:
Fri Feb 09 00:48:26 2018 +0000
Revision:
180:a783a972a867
Parent:
165:2463dbe52eee
Child:
181:d3510c8beab6
Added BufferedLogger object to IOStruct, logging seems to work (insofar as data is sent over serial and the motor still seems to spin)

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