Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

BREMS/BREMSConfig.cpp@238:cf54cb197c8f, 2018-11-10 (annotated)

Committer:
bwang
Date:
Sat Nov 10 11:20:38 2018 +0000
Revision:
238:cf54cb197c8f
Parent:
237:1a4591a0dd2d
Child:
240:2aaffa217627
11/10/2018 06:20 - added cmd_clear() before startup message in BREMSInit()

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 237:1a4591a0dd2d 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 238:cf54cb197c8f 106 cmd_clear(io->pc);
bwang 181:d3510c8beab6 107 BREMSStartupMsg(read, io->pc);
bwang 199:c160a2c03781 108
bwang 199:c160a2c03781 109 BREMS_mode = MODE_CFG;
bwang 188:43f50a4cc040 110 io->pref = new PreferenceWriter(6);
bwang 181:d3510c8beab6 111 cmd_reload(io->pc, io->pref);
bwang 182:5ed20e4ce158 112 if (_PREFS_VALID != 1) {
bwang 181:d3510c8beab6 113 io->pc->printf("%s\n", "Stored config invalid");
bwang 199:c160a2c03781 114 BREMS_mode = MODE_CFG;
bwang 181:d3510c8beab6 115 cmd_defaults(io->pc);
bwang 210:f9b722cf8a2c 116 BREMS_op = OP_TORQUE;
bwang 210:f9b722cf8a2c 117 BREMS_src = CMD_SRC_TERMINAL;
bwang 181:d3510c8beab6 118 io->pc->printf("%s\n", "You should probably at least set throttle and current limits!");
bwang 181:d3510c8beab6 119 }
bwang 188:43f50a4cc040 120
bwang 223:b986e7cee521 121 io->blink = new LedBlinker(STATUS_LED, _F_SW, 1.0);
bwang 223:b986e7cee521 122 io->blink->set_code(0x0f);
bwang 223:b986e7cee521 123
bwang 197:68fd01b73537 124 wait_ms(750);
bwang 197:68fd01b73537 125
bwang 189:760cd81a7633 126 io->pos = new PositionSensorEncoder(_CPR, 0);
bwang 189:760cd81a7633 127 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 128
bwang 189:760cd81a7633 129 io->throttle_in = new PwmIn(TH_PIN, _TH_LIMIT_LOW, _TH_LIMIT_HIGH);
bwang 42:030e0ec4eac5 130
bwang 189:760cd81a7633 131 control->throttle_filter = new MedianFilter(_THROTTLE_FILTER_WINDOW);
bwang 189:760cd81a7633 132 control->velocity_filter = new MedianFilter(_W_FILTER_WINDOW);
bwang 154:0a22dcf91577 133
bwang 189:760cd81a7633 134 read->vbus = _BUS_VOLTAGE;
bwang 42:030e0ec4eac5 135 read->w = 0.0f;
bwang 229:90c6892f4d3b 136 read->ad1_supp_offset = 0.0f;
bwang 229:90c6892f4d3b 137 read->ad2_supp_offset = 0.0f;
bwang 42:030e0ec4eac5 138 read->p_mech = io->pos->GetMechPosition();
bwang 42:030e0ec4eac5 139
bwang 52:fd3d8df99287 140 BREMSConfigRegisters(io);
bwang 52:fd3d8df99287 141 wait_ms(250);
bwang 52:fd3d8df99287 142 BREMSZeroCurrent(read);
bwang 181:d3510c8beab6 143 io->pc->printf("%s", ">");
bwang 52:fd3d8df99287 144
bwang 42:030e0ec4eac5 145 control->d_integral = 0.0f;
bwang 42:030e0ec4eac5 146 control->q_integral = 0.0f;
bwang 42:030e0ec4eac5 147 control->d_filtered = 0.0f;
bwang 42:030e0ec4eac5 148 control->q_filtered = 0.0f;
bwang 42:030e0ec4eac5 149 control->last_d = 0.0f;
bwang 42:030e0ec4eac5 150 control->last_q = 0.0f;
bwang 42:030e0ec4eac5 151 control->d_ref = 0.0f;
bwang 42:030e0ec4eac5 152 control->q_ref = 0.0f;
bwang 70:5e39beeb4a21 153 control->torque_percent = 0.0f;
bwang 187:523cf8c962e4 154 control->enabled = false;
bwang 46:748aba7d111d 155
bwang 42:030e0ec4eac5 156 io->en->write(1);
bwang 42:030e0ec4eac5 157 }