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 analogWrite(uint8 pin, int val) { if (pin >= BOARD_NR_GPIO_PINS) return; timer_dev *dev = PIN_MAP[pin].timer_device; if (dev == NULL || dev->type == TIMER_BASIC) return; // configure as PWM if not if ( TIMER_OC_MODE_PWM_1 != timer_oc_get_mode(dev, PIN_MAP[pin].timer_channel) ) { pinMode(pin, PWM); } if (val > 255) val = 255; // map compare according to the current reload uint16 compare = map(val, 0, 255, 0, timer_get_reload(dev)); timer_set_compare(dev, PIN_MAP[pin].timer_channel, compare); }
// Adafruit Modification to change PWM Period/Frequency void pwmPeriod(uint8 pin, uint32 us) { if (pin >= BOARD_NR_GPIO_PINS) return; timer_dev *dev = PIN_MAP[pin].timer_device; if (dev == NULL || dev->type == TIMER_BASIC) return; // Get timer's max speed in hz uint32 max_speed = rcc_dev_timer_clk_speed(dev->clk_id); // period in cpu cycles uint32 cycle = us * (max_speed / 1000000UL); uint16 prescaler = (uint16) (cycle / TIMER_MAX_RELOAD); uint16 reload = (uint16) round(cycle / (prescaler+1)); // Re-map compare to preserve duty cycle uint16 compare = timer_get_compare(dev, PIN_MAP[pin].timer_channel); compare = map(compare, 0, timer_get_reload(dev), 0, reload); timer_set_prescaler(dev, prescaler); timer_set_reload(dev, reload); timer_set_compare(dev, PIN_MAP[pin].timer_channel, compare); }
uint16 HardwareTimer::getOverflow() { return timer_get_reload(this->dev); }
uint16 speaker_getOverflow(void) { return timer_get_reload(SPEAKER_TIMER); }