void Dynamixel::begin(int baud) {
//TxDString("[DXL]start begin\r\n");
afio_remap(AFIO_REMAP_USART1);//USART1 -> DXL
afio_cfg_debug_ports(AFIO_DEBUG_FULL_SWJ_NO_NJRST);
#ifdef BOARD_CM900 //Engineering version case
gpio_set_mode(PORT_ENABLE_TXD, PIN_ENABLE_TXD, GPIO_OUTPUT_PP);
gpio_set_mode(PORT_ENABLE_RXD, PIN_ENABLE_RXD, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_ENABLE_TXD, PIN_ENABLE_TXD, 0 );// TX Disable
gpio_write_bit(PORT_ENABLE_RXD, PIN_ENABLE_RXD, 1 );// RX Enable
#else
gpio_set_mode(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, 0 );// RX Enable
#endif
timer_set_mode(TIMER2, TIMER_CH1, TIMER_OUTPUT_COMPARE);
timer_pause(TIMER2);
uint16 ovf = timer_get_reload(TIMER2);
timer_set_count(TIMER2, min(0, ovf));
timer_set_reload(TIMER2, 30000);//set overflow
ovf = timer_get_reload(TIMER2);
timer_set_compare(TIMER2, TIMER_CH1, min(1000, ovf));
timer_attach_interrupt(TIMER2, TIMER_CH1, TIM2_IRQHandler);
timer_generate_update(TIMER2);
timer_resume(TIMER2);
dxl_initialize(0, baud);
}
void HardwareI2C::begin()
{
/* set as master */
if (this->i2c_d == _I2C1) afio_remap(AFIO_MAPR_I2C1_REMAP);
i2cInit(this->i2c_d, I2C_400KHz_SPEED);
delay(I2CDELAY);
}
void Dynamixel::begin(int baud) {
//TxDString("[DXL]start begin\r\n");
afio_remap(AFIO_REMAP_USART1);//USART1 -> DXL
afio_cfg_debug_ports(AFIO_DEBUG_FULL_SWJ_NO_NJRST);
#ifdef BOARD_CM900 //Engineering version case
gpio_set_mode(PORT_ENABLE_TXD, PIN_ENABLE_TXD, GPIO_OUTPUT_PP);
gpio_set_mode(PORT_ENABLE_RXD, PIN_ENABLE_RXD, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_ENABLE_TXD, PIN_ENABLE_TXD, 0 );// TX Disable
gpio_write_bit(PORT_ENABLE_RXD, PIN_ENABLE_RXD, 1 );// RX Enable
#else
gpio_set_mode(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, 0 );// RX Enable
#endif
/* timer_set_mode(TIMER2, TIMER_CH1, TIMER_OUTPUT_COMPARE);
timer_pause(TIMER2);
uint16 ovf = timer_get_reload(TIMER2);
timer_set_count(TIMER2, min(0, ovf));
timer_set_reload(TIMER2, 10000);//set overflow
ovf = timer_get_reload(TIMER2);
timer_set_compare(TIMER2, TIMER_CH1, min(1000, ovf));
timer_attach_interrupt(TIMER2, TIMER_CH1, TIM2_IRQHandler);
timer_generate_update(TIMER2);
//timer_resume(TIMER2);*/
/*
* Timer Configuation for Dynamixel bus
* 2013-04-03 ROBOTIS Changed it as the below codes
* Dynamixel bus used timer 2(channel 1) to check timeout for receiving data from the bus.
* So, don't use time 2(channel 1) in other parts.
* */
// Pause the timer while we're configuring it
timer.pause();
// Set up period
timer.setPeriod(1); // in microseconds
// Set up an interrupt on channel 1
timer.setMode(TIMER_CH1,TIMER_OUTPUT_COMPARE);
timer.setCompare(TIMER_CH1, 1); // Interrupt 1 count after each update
timer.attachInterrupt(TIMER_CH1,TIM2_IRQHandler);
// Refresh the timer's count, prescale, and overflow
timer.refresh();
// Start the timer counting
//timer.resume();
dxl_initialize(0, baud);
}
extern void uart_enable(UART_CLASS port, UART_CB *cb, void *param, int priority)
{
if (port < UARTS_COUNT)
{
UART_HW* uart = (UART_HW*)sys_alloc(sizeof(UART_HW));
if (uart)
{
_uart_handlers[port] = uart;
uart->cb = cb;
uart->param = param;
uart->read_buf = NULL;
uart->write_buf = NULL;
uart->read_size = 0;
uart->write_size = 0;
//setup pins
#if defined(STM32F1)
if ((USART_TX_DISABLE_MASK & (1 << port)) == 0)
gpio_enable_afio(UART_TX_PINS[port], AFIO_MODE_PUSH_PULL);
if ((USART_RX_DISABLE_MASK & (1 << port)) == 0)
gpio_enable(UART_RX_PINS[port]);
#if (USART_REMAP_MASK)
if ((1 << port) & USART_REMAP_MASK)
afio_remap();
#endif //USART_REMAP_MASK
#elif defined(STM32F2)
if ((USART_TX_DISABLE_MASK & (1 << port)) == 0)
gpio_enable_afio(UART_TX_PINS[port], port < UART_4 ? AFIO_MODE_USART1_2_3 : AFIO_MODE_UART_4_5_USART_6);
if ((USART_RX_DISABLE_MASK & (1 << port)) == 0)
gpio_enable_afio(UART_RX_PINS[port], port < UART_4 ? AFIO_MODE_USART1_2_3 : AFIO_MODE_UART_4_5_USART_6);
#endif
//power up
if (port == UART_1 || port == UART_6)
RCC->APB2ENR |= RCC_UART[port];
else
RCC->APB1ENR |= RCC_UART[port];
//enable interrupts
NVIC_EnableIRQ(UART_IRQ_VECTORS[port]);
NVIC_SetPriority(UART_IRQ_VECTORS[port], priority);
USART[port]->CR1 |= USART_CR1_UE;
}
else
error_dev(ERROR_MEM_OUT_OF_SYSTEM_MEMORY, DEV_UART, port);
}
else
error_dev(ERROR_DEVICE_INDEX_OUT_OF_RANGE, DEV_UART, port);
}
void Dynamixel::begin(int baud){
uint32 Baudrate = 0;
if(mDxlUsart == USART1)
afio_remap(AFIO_REMAP_USART1);
#ifdef BOARD_CM900 //Engineering version case
gpio_set_mode(PORT_ENABLE_TXD, PIN_ENABLE_TXD, GPIO_OUTPUT_PP);
gpio_set_mode(PORT_ENABLE_RXD, PIN_ENABLE_RXD, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_ENABLE_TXD, PIN_ENABLE_TXD, 0 );// TX Disable
gpio_write_bit(PORT_ENABLE_RXD, PIN_ENABLE_RXD, 1 );// RX Enable
#else
gpio_set_mode(mDirPort, mDirPin, GPIO_OUTPUT_PP);
gpio_write_bit(mDirPort, mDirPin, 0 );// RX Enable
//gpio_set_mode(GPIOB, 5, GPIO_OUTPUT_PP);
// gpio_write_bit(GPIOB, 5, 0 );// RX Enable
#endif
// initialize GPIO D20(PB6), D21(PB7) as DXL TX, RX respectively
gpio_set_mode(mTxPort, mTxPin, GPIO_AF_OUTPUT_PP);
gpio_set_mode(mRxPort, mRxPin, GPIO_INPUT_FLOATING);
//Initialize USART 1 device
usart_init(mDxlUsart);
//Calculate baudrate, refer to ROBOTIS support page.
Baudrate = 2000000 / (baud + 1);
if(mDxlUsart == USART1)
usart_set_baud_rate(mDxlUsart, STM32_PCLK2, Baudrate);
else
usart_set_baud_rate(mDxlUsart, STM32_PCLK1, Baudrate);
nvic_irq_set_priority(mDxlUsart->irq_num, 0);//[ROBOTIS][ADD] 2013-04-10 set to priority 0
usart_attach_interrupt(mDxlUsart, mDxlDevice->handlers);
usart_enable(mDxlUsart);
delay(80);
mDXLtxrxStatus = 0;
mBusUsed = 0;// only 1 when tx/rx is operated
//gbIsDynmixelUsed = 1; //[ROBOTIS]2012-12-13 to notify end of using dynamixel SDK to uart.c
this->clearBuffer();
this->setLibStatusReturnLevel(2);
this->setLibNumberTxRxAttempts(1);
}
void LCD_IO_Init(uint8_t cs, uint8_t rs) {
uint32_t controllerAddress;
if (fsmcInit) return;
fsmcInit = 1;
switch (cs) {
case FSMC_CS_NE1: controllerAddress = (uint32_t)FSMC_NOR_PSRAM_REGION1; break;
#ifdef STM32_XL_DENSITY
case FSMC_CS_NE2: controllerAddress = (uint32_t)FSMC_NOR_PSRAM_REGION2; break;
case FSMC_CS_NE3: controllerAddress = (uint32_t)FSMC_NOR_PSRAM_REGION3; break;
case FSMC_CS_NE4: controllerAddress = (uint32_t)FSMC_NOR_PSRAM_REGION4; break;
#endif
default: return;
}
#define _ORADDR(N) controllerAddress |= (_BV32(N) - 2)
switch (rs) {
#ifdef STM32_XL_DENSITY
case FSMC_RS_A0: _ORADDR( 1); break;
case FSMC_RS_A1: _ORADDR( 2); break;
case FSMC_RS_A2: _ORADDR( 3); break;
case FSMC_RS_A3: _ORADDR( 4); break;
case FSMC_RS_A4: _ORADDR( 5); break;
case FSMC_RS_A5: _ORADDR( 6); break;
case FSMC_RS_A6: _ORADDR( 7); break;
case FSMC_RS_A7: _ORADDR( 8); break;
case FSMC_RS_A8: _ORADDR( 9); break;
case FSMC_RS_A9: _ORADDR(10); break;
case FSMC_RS_A10: _ORADDR(11); break;
case FSMC_RS_A11: _ORADDR(12); break;
case FSMC_RS_A12: _ORADDR(13); break;
case FSMC_RS_A13: _ORADDR(14); break;
case FSMC_RS_A14: _ORADDR(15); break;
case FSMC_RS_A15: _ORADDR(16); break;
#endif
case FSMC_RS_A16: _ORADDR(17); break;
case FSMC_RS_A17: _ORADDR(18); break;
case FSMC_RS_A18: _ORADDR(19); break;
case FSMC_RS_A19: _ORADDR(20); break;
case FSMC_RS_A20: _ORADDR(21); break;
case FSMC_RS_A21: _ORADDR(22); break;
case FSMC_RS_A22: _ORADDR(23); break;
case FSMC_RS_A23: _ORADDR(24); break;
#ifdef STM32_XL_DENSITY
case FSMC_RS_A24: _ORADDR(25); break;
case FSMC_RS_A25: _ORADDR(26); break;
#endif
default: return;
}
rcc_clk_enable(RCC_FSMC);
gpio_set_mode(GPIOD, 14, GPIO_AF_OUTPUT_PP); // FSMC_D00
gpio_set_mode(GPIOD, 15, GPIO_AF_OUTPUT_PP); // FSMC_D01
gpio_set_mode(GPIOD, 0, GPIO_AF_OUTPUT_PP); // FSMC_D02
gpio_set_mode(GPIOD, 1, GPIO_AF_OUTPUT_PP); // FSMC_D03
gpio_set_mode(GPIOE, 7, GPIO_AF_OUTPUT_PP); // FSMC_D04
gpio_set_mode(GPIOE, 8, GPIO_AF_OUTPUT_PP); // FSMC_D05
gpio_set_mode(GPIOE, 9, GPIO_AF_OUTPUT_PP); // FSMC_D06
gpio_set_mode(GPIOE, 10, GPIO_AF_OUTPUT_PP); // FSMC_D07
gpio_set_mode(GPIOE, 11, GPIO_AF_OUTPUT_PP); // FSMC_D08
gpio_set_mode(GPIOE, 12, GPIO_AF_OUTPUT_PP); // FSMC_D09
gpio_set_mode(GPIOE, 13, GPIO_AF_OUTPUT_PP); // FSMC_D10
gpio_set_mode(GPIOE, 14, GPIO_AF_OUTPUT_PP); // FSMC_D11
gpio_set_mode(GPIOE, 15, GPIO_AF_OUTPUT_PP); // FSMC_D12
gpio_set_mode(GPIOD, 8, GPIO_AF_OUTPUT_PP); // FSMC_D13
gpio_set_mode(GPIOD, 9, GPIO_AF_OUTPUT_PP); // FSMC_D14
gpio_set_mode(GPIOD, 10, GPIO_AF_OUTPUT_PP); // FSMC_D15
gpio_set_mode(GPIOD, 4, GPIO_AF_OUTPUT_PP); // FSMC_NOE
gpio_set_mode(GPIOD, 5, GPIO_AF_OUTPUT_PP); // FSMC_NWE
gpio_set_mode(PIN_MAP[cs].gpio_device, PIN_MAP[cs].gpio_bit, GPIO_AF_OUTPUT_PP); //FSMC_CS_NEx
gpio_set_mode(PIN_MAP[rs].gpio_device, PIN_MAP[rs].gpio_bit, GPIO_AF_OUTPUT_PP); //FSMC_RS_Ax
FSMC_NOR_PSRAM4_BASE->BCR = FSMC_BCR_WREN | FSMC_BCR_MTYP_SRAM | FSMC_BCR_MWID_16BITS | FSMC_BCR_MBKEN;
FSMC_NOR_PSRAM4_BASE->BTR = (FSMC_DATA_SETUP_TIME << 8) | FSMC_ADDRESS_SETUP_TIME;
afio_remap(AFIO_REMAP_FSMC_NADV);
LCD = (LCD_CONTROLLER_TypeDef*)controllerAddress;
}
/**
* @brief Initialize an I2C device as bus master
* @param dev Device to enable
* @param flags Bitwise or of the following I2C options:
* I2C_FAST_MODE: 400 khz operation,
* I2C_DUTY_16_9: 16/9 Tlow/Thigh duty cycle (only applicable for
* fast mode),
* I2C_BUS_RESET: Reset the bus and clock out any hung slaves on
* initialization,
* I2C_10BIT_ADDRESSING: Enable 10-bit addressing,
* I2C_REMAP: Remap I2C1 to SCL/PB8 SDA/PB9.
*/
void i2c_master_enable(i2c_dev *dev, uint32 flags) {
#define I2C_CLK (STM32_PCLK1/1000000)
uint32 ccr = 0;
uint32 trise = 0;
/* PE must be disabled to configure the device */
ASSERT(!(dev->regs->CR1 & I2C_CR1_PE));
if ((dev == I2C1) && (flags & I2C_REMAP)) {
afio_remap(AFIO_REMAP_I2C1);
I2C1->sda_pin = 9;
I2C1->scl_pin = 8;
}
/* Reset the bus. Clock out any hung slaves. */
if (flags & I2C_BUS_RESET) {
i2c_bus_reset(dev);
}
/* Turn on clock and set GPIO modes */
i2c_init(dev);
gpio_set_mode(dev->gpio_port, dev->sda_pin, GPIO_AF_OUTPUT_OD);
gpio_set_mode(dev->gpio_port, dev->scl_pin, GPIO_AF_OUTPUT_OD);
/* I2C1 and I2C2 are fed from APB1, clocked at 36MHz */
i2c_set_input_clk(dev, I2C_CLK);
if (flags & I2C_FAST_MODE) {
ccr |= I2C_CCR_FS;
if (flags & I2C_DUTY_16_9) {
/* Tlow/Thigh = 16/9 */
ccr |= I2C_CCR_DUTY;
ccr |= STM32_PCLK1/(400000 * 25);
} else {
/* Tlow/Thigh = 2 */
ccr |= STM32_PCLK1/(400000 * 3);
}
trise = (300 * (I2C_CLK)/1000) + 1;
} else {
/* Tlow/Thigh = 1 */
ccr = STM32_PCLK1/(100000 * 2);
trise = I2C_CLK + 1;
}
/* Set minimum required value if CCR < 1*/
if ((ccr & I2C_CCR_CCR) == 0) {
ccr |= 0x1;
}
i2c_set_clk_control(dev, ccr);
i2c_set_trise(dev, trise);
/* Enable event and buffer interrupts */
nvic_irq_enable(dev->ev_nvic_line);
nvic_irq_enable(dev->er_nvic_line);
i2c_enable_irq(dev, I2C_IRQ_EVENT | I2C_IRQ_BUFFER | I2C_IRQ_ERROR);
/*
* Important STM32 Errata:
*
* See STM32F10xx8 and STM32F10xxB Errata sheet (Doc ID 14574 Rev 8),
* Section 2.11.1, 2.11.2.
*
* 2.11.1:
* When the EV7, EV7_1, EV6_1, EV6_3, EV2, EV8, and EV3 events are not
* managed before the current byte is being transferred, problems may be
* encountered such as receiving an extra byte, reading the same data twice
* or missing data.
*
* 2.11.2:
* In Master Receiver mode, when closing the communication using
* method 2, the content of the last read data can be corrupted.
*
* If the user software is not able to read the data N-1 before the STOP
* condition is generated on the bus, the content of the shift register
* (data N) will be corrupted. (data N is shifted 1-bit to the left).
*
* ----------------------------------------------------------------------
*
* In order to ensure that events are not missed, the i2c interrupt must
* not be preempted. We set the i2c interrupt priority to be the highest
* interrupt in the system (priority level 0). All other interrupts have
* been initialized to priority level 16. See nvic_init().
*/
nvic_irq_set_priority(dev->ev_nvic_line, 0);
nvic_irq_set_priority(dev->er_nvic_line, 0);
/* Make it go! */
i2c_peripheral_enable(dev);
dev->state = I2C_STATE_IDLE;
}
// boardInit(): NUCLEO rely on some remapping
void boardInit(void) {
afio_remap(AFIO_REMAP_TIM2_FULL);
afio_remap(AFIO_REMAP_TIM3_PARTIAL);
}
//Dynamixel::~Dynamixel() {
// // TODO Auto-generated destructor stub
//}
void Dynamixel::begin(int baud) {
uint32 Baudrate = 0;
mPacketType = DXL_PACKET_TYPE1; //2014-04-02 default packet type is 1.0 -> set as 1
if(mDxlUsart == USART1)
afio_remap(AFIO_REMAP_USART1);
#ifdef BOARD_CM900 //Engineering version case
gpio_set_mode(PORT_ENABLE_TXD, PIN_ENABLE_TXD, GPIO_OUTPUT_PP);
gpio_set_mode(PORT_ENABLE_RXD, PIN_ENABLE_RXD, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_ENABLE_TXD, PIN_ENABLE_TXD, 0 );// TX Disable
gpio_write_bit(PORT_ENABLE_RXD, PIN_ENABLE_RXD, 1 );// RX Enable
#else
if(mDirPort != 0) {
gpio_set_mode(mDirPort, mDirPin, GPIO_OUTPUT_PP);
gpio_write_bit(mDirPort, mDirPin, 0 );// RX Enable
}
//gpio_set_mode(GPIOB, 5, GPIO_OUTPUT_PP);
// gpio_write_bit(GPIOB, 5, 0 );// RX Enable
#endif
// initialize GPIO D20(PB6), D21(PB7) as DXL TX, RX respectively
gpio_set_mode(mTxPort, mTxPin, GPIO_AF_OUTPUT_PP);
gpio_set_mode(mRxPort, mRxPin, GPIO_INPUT_FLOATING);
//Initialize USART 1 device
usart_init(mDxlUsart);
//Calculate baudrate, refer to ROBOTIS support page.
//Baudrate = dxl_get_baudrate(baud); //Dxl 2.0
if(baud == 3)
baud = 1;
else if(baud == 2)
baud = 16;
else if(baud == 1)
baud = 34;
else if(baud == 0)
baud = 207;
Baudrate = 2000000 / (baud + 1);
if(mDxlUsart == USART1)
usart_set_baud_rate(mDxlUsart, STM32_PCLK2, Baudrate);
else
usart_set_baud_rate(mDxlUsart, STM32_PCLK1, Baudrate);
nvic_irq_set_priority(mDxlUsart->irq_num, 0);//[ROBOTIS][ADD] 2013-04-10 set to priority 0
usart_attach_interrupt(mDxlUsart, mDxlDevice->handlers);
usart_enable(mDxlUsart);
delay(100);
mDXLtxrxStatus = 0;
mBusUsed = 0;// only 1 when tx/rx is operated
//gbIsDynmixelUsed = 1; //[ROBOTIS]2012-12-13 to notify end of using dynamixel SDK to uart.c
this->setLibStatusReturnLevel(2);
this->setLibNumberTxRxAttempts(2);
this->clearBuffer();
if(this->checkPacketType()) { // Dxl 2.0
this->setPacketType(DXL_PACKET_TYPE2);
} else { // Dxl 1.0
this->setPacketType(DXL_PACKET_TYPE1);
}
this->setLibNumberTxRxAttempts(1);
this->clearBuffer();
if(mDxlUsart == USART2) { //140508 shin
SmartDelayFlag = 1;
this->setPacketType(DXL_PACKET_TYPE2);
}
}