static void conf_motor_pins() { /* Set pins to output */ DRV_PWM_DDR |= _BV(DRV1_PWM_PIN); DRV_DIR_DDR |= _BV(DRV1_DIR_PIN); DRV_PWM_DDR |= _BV(DRV2_PWM_PIN); DRV_DIR_DDR |= _BV(DRV2_DIR_PIN); DRV_EN_DDR |= _BV(DRV_EN_PIN1); DRV_EN_DDR |= _BV(DRV_EN_PIN2); /* Set 0 to all outputs */ drv1_forward(); drv2_forward(); drv1_turn_off(); drv2_turn_off(); drv_disable(); }
void inline motors_pwm() { if (!pwm_cnt) { if (speed1) drv1_turn_on(); if (speed2) drv2_turn_on(); } if (pwm_cnt == speed1) drv1_turn_off(); if (pwm_cnt == speed2) drv2_turn_off(); pwm_cnt++; if (pwm_cnt > PWM_CNT_MAX) pwm_cnt = 0; }
void inline motors_pwm() { if (!m_pwm_cnt) { if (m_speed1) drv1_turn_on(); if (m_speed2) drv2_turn_on(); } if (m_pwm_cnt == m_speed1) drv1_turn_off(); if (m_pwm_cnt == m_speed2) drv2_turn_off(); m_pwm_cnt++; if (m_pwm_cnt > PWM_CNT_MAX) m_pwm_cnt = 0; }