diff --git a/shared/common.h b/shared/common.h index 69f21810..1014d1d9 100644 --- a/shared/common.h +++ b/shared/common.h @@ -11,12 +11,10 @@ #define DATABAUD 2250000 //baudrate used for communication -//fixed point calculations +//fixed point calculations signed bit, 9 bit predecimal, 6 bit decimal #define TOFIXED(a) ((int16_t)((a) * 64)) #define TOFLOAT(a) ((float)((a) / 64.0)) -#define TOSFLOAT(a) (ABS(a) >= 4.0) ? (((int16_t)((a) * 64)) | 0x4000) : (((int16_t)((a) * 4096.0)) & 0xBFFFF) -#define TOFLOATS(a) ((a) & 0x4000) ? ((float)(((a) & 0xBFFFF) / 64.0)) : ((float)(((a) & 0xBFFFF) / 4096.0)) #define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x))) #define MIN(a, b) (((a) < (b)) ? (a) : (b)) diff --git a/src/comps/netbob.comp b/src/comps/netbob.comp new file mode 100644 index 00000000..d6bee162 --- /dev/null +++ b/src/comps/netbob.comp @@ -0,0 +1,239 @@ +/* +* This file is part of the stmbl project. +* +* Copyright (C) 2016 Rene Hopf +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program. If not, see . +*/ + +HAL_COMP(netbob); + +// pins +HAL_PIN(dump_pd_vals) = 0.0; +HAL_PIN(error) = 0.0;//counts unknown commands +HAL_PIN(crc_error) = 0.0;//counts crc errors +HAL_PIN(connected) = 0.0;//connection status TODO: not stable during startup, needs link to pd +HAL_PIN(timeout) = 100.0;// 20khz / 1khz * 2 reads = 40 +HAL_PIN(rxfoo) = 0.0; + +HAL_PIN(pos_cmd) = 0.0; +HAL_PIN(pos_cmd_d) = 0.0; +HAL_PIN(pos_fb) = 0.0; + +HAL_PIN(in0) = 0.0; +HAL_PIN(in1) = 0.0; +HAL_PIN(in2) = 0.0; +HAL_PIN(in3) = 0.0; +HAL_PIN(fault) = 0.0; + +HAL_PIN(out0) = 0.0; +HAL_PIN(out1) = 0.0; +HAL_PIN(out2) = 0.0; +HAL_PIN(out3) = 0.0; +HAL_PIN(enable) = 0.0; + +#define DRV_COUNT 6 + +typedef struct{ + uint8_t sink; + uint8_t addr; + uint8_t data; + struct { + uint8_t enable : 1; + uint8_t brake : 1; + uint8_t tmp : 2; + uint8_t io : 4; + } flags; + float pos; + float vel; +} p_data_t; + +typedef struct{ + uint8_t packed_type; + uint8_t length; + uint8_t packed_cnt; + uint8_t source; + p_data_t drive[DRV_COUNT]; + uint32_t crc; +} packet_t; + +MEM(volatile uint8_t rxbuf[128]); +MEM(volatile uint8_t txbuf[20]); + +MEM(uint16_t address);//current address pointer +MEM(int rxpos); +MEM(uint32_t timeout); +MEM(int bufferpos); +MEM(int available); +MEM(uint8_t cnt) = 0; + +//pb13 txen +//pc12 UART5 tx +//pd2 UART5 rx +void init_hardware() { + GPIO_InitTypeDef GPIO_InitStruct; + USART_InitTypeDef USART_InitStruct; + RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); + //USART TX + GPIO_PinAFConfig(GPIOC, GPIO_PinSource12, GPIO_AF_UART5); + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP ; + GPIO_Init(GPIOC, &GPIO_InitStruct); + + //USART RX + GPIO_PinAFConfig(GPIOD, GPIO_PinSource2, GPIO_AF_UART5); + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_2; + GPIO_Init(GPIOD, &GPIO_InitStruct); + + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13; + GPIO_Init(GPIOB, &GPIO_InitStructure); + + USART_InitStruct.USART_BaudRate = 5000000; + USART_InitStruct.USART_WordLength = USART_WordLength_8b; + USART_InitStruct.USART_StopBits = USART_StopBits_1; + USART_InitStruct.USART_Parity = USART_Parity_No; + USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + USART_InitStruct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; + USART_OverSampling8Cmd(UART5,ENABLE);//16 bit: 2.625, 8bit:5.25 + USART_Init(UART5, &USART_InitStruct); + + USART_Cmd(UART5, ENABLE); + + //RX DMA + + DMA_Cmd(DMA1_Stream0, DISABLE); + DMA_DeInit(DMA1_Stream0); + + // DMA1-Config + DMA_InitStructure.DMA_Channel = DMA_Channel_4; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(UART5->DR); + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&rxbuf; + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; + DMA_InitStructure.DMA_BufferSize = sizeof(rxbuf); + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; + DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(DMA1_Stream0, &DMA_InitStructure); + + DMA_Cmd(DMA1_Stream0, ENABLE); + + USART_DMACmd(UART5, USART_DMAReq_Rx, ENABLE); + + //TX DMA + + DMA_Cmd(DMA1_Stream7, DISABLE); + DMA_DeInit(DMA1_Stream7); + + // DMA2-Config + DMA_InitStructure.DMA_Channel = DMA_Channel_4; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(UART5->DR); + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&txbuf; + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; + DMA_InitStructure.DMA_BufferSize = sizeof(txbuf); + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Priority_Low; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; + DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(DMA1_Stream7, &DMA_InitStructure); + + //DMA_Cmd(DMA1_Stream7, ENABLE); + + USART_DMACmd(UART5, USART_DMAReq_Tx, ENABLE); + + //tx enable + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13; + GPIO_Init(GPIOB, &GPIO_InitStructure); + + GPIO_ResetBits(GPIOB, GPIO_Pin_13); +} + + +RT_INIT( + init_hardware(); + rxpos = 0; + timeout = 1000;//make sure we start in timeout +); + + +RT ( + packet_t packet; + //next received packet will be written to bufferpos + bufferpos = sizeof(rxbuf) - DMA_GetCurrDataCounter(DMA1_Stream0); + //how many packets we have the the rx buffer for processing + available = (bufferpos - rxpos + sizeof(rxbuf)) % sizeof(rxbuf); + if(!EDGE(available)){ + + if(available == sizeof(packet)){ + //copy packet from dma buffer to struct. TODO: zero copy by writing directly to struct. + for(int i = 0;i acc velerr = velcmd - velfb; + velerr_p = velerr; if(ABS(velerr) < PIN(vel_min)){ - velerr = 0; + velerr_p = 0; } - acccmd = velerr / period * velp + PIN(acc_ff) * accextcmd; // acc = p * vel_error / period + ff2 + acccmd = velerr_p / period * velp + PIN(acc_ff) * accextcmd; // acc = p * vel_error / period + ff2 acccmd = acccmd * vellp + (1.0 - vellp) * old_acccmd; // lowpass accsat = SAT2(acccmd, accmin * accp, accmax * accp); acccmd = CLAMP(acccmd, accmin * accp, accmax * accp); // min/max clamping