Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

BREMS/BREMSConfig.cpp@79:d0b1bb3dcf68, 2017-02-28 (annotated)

Committer:
bwang
Date:
Tue Feb 28 03:32:21 2017 +0000
Revision:
79:d0b1bb3dcf68
Parent:
78:b8df106126a7
Child:
80:64732542010a
Child:
82:5e741c5ffd9f
added FlashWriter class

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