bool callback(const PWM2Msg &msg) { int16_t pwm; switch (stm32_id8()) { case M1: pwm = msg.pwm1; break; case M2: pwm = msg.pwm2; break; default: pwm = 0; return true; } chSysLock() ; if (pwm > 0) { pwm_lld_enable_channel(&PWM_DRIVER, 0, pwm); pwm_lld_enable_channel(&PWM_DRIVER, 1, 0); } else { pwm_lld_enable_channel(&PWM_DRIVER, 0, 0); pwm_lld_enable_channel(&PWM_DRIVER, 1, -pwm); } chSysUnlock(); return true; }
msg_t pwm2sub_node(void * arg) { Node node("pwm2sub"); Subscriber<PWM2Msg, 5> sub(callback); (void) arg; chRegSetThreadName("pwm2sub"); palSetPad(DRIVER_GPIO, DRIVER_RESET); chThdSleepMilliseconds(500); pwmStart(&PWM_DRIVER, &pwmcfg); node.subscribe(sub, "pwm2"); for (;;) { if (!node.spin(Time::ms(100))) { // Stop motor if no messages for 100 ms chSysLock() ; pwm_lld_enable_channel(&PWM_DRIVER, 0, 0); pwm_lld_enable_channel(&PWM_DRIVER, 0, 0); chSysUnlock(); } } return CH_SUCCESS; }
/* * PWM cyclic callback. */ static void pwmcb(PWMDriver *pwmp) { (void) pwmp; chSysLockFromIsr() ; if (pwm >= 0) { pwm_lld_enable_channel(&PWMD1, 0, pwm); pwm_lld_enable_channel(&PWMD1, 1, 0); } else { pwm_lld_enable_channel(&PWMD1, 0, 0); pwm_lld_enable_channel(&PWMD1, 1, -pwm); } chSysUnlockFromIsr(); }
bool enc_callback(const r2p::EncoderMsg &msg) { pwm = speed_pid.update(msg.delta); pwm = 4096; chSysLock() ; if (pwm > 0) { pwm_lld_enable_channel(&PWM_DRIVER, 1, pwm); pwm_lld_enable_channel(&PWM_DRIVER, 0, 0); } else { pwm_lld_enable_channel(&PWM_DRIVER, 1, 0); pwm_lld_enable_channel(&PWM_DRIVER, 0, -pwm); } chSysUnlock(); return true; }
bool callback(const PWM2Msg &msg) { int16_t pwm; pwm = msg.value[MOTOR_ID]; chSysLock(); if (pwm > 0) { pwm_lld_enable_channel(&PWM_DRIVER, 0, pwm); pwm_lld_enable_channel(&PWM_DRIVER, 1, 0); } else { pwm_lld_enable_channel(&PWM_DRIVER, 0, 0); pwm_lld_enable_channel(&PWM_DRIVER, 1, -pwm); } chSysUnlock(); return true; }
/** * @brief Enables a PWM channel. * @pre The PWM unit must have been activated using @p pwmStart(). * @post The channel is active using the specified configuration. * @note Depending on the hardware implementation this function has * effect starting on the next cycle (recommended implementation) * or immediately (fallback implementation). * * @param[in] pwmp pointer to a @p PWMDriver object * @param[in] channel PWM channel identifier (0...PWM_CHANNELS-1) * @param[in] width PWM pulse width as clock pulses number * * @api */ void pwmEnableChannel(PWMDriver *pwmp, pwmchannel_t channel, pwmcnt_t width) { osalDbgCheck((pwmp != NULL) && (channel < PWM_CHANNELS)); osalSysLock(); osalDbgAssert(pwmp->state == PWM_READY, "not ready"); pwm_lld_enable_channel(pwmp, channel, width); osalSysUnlock(); }
msg_t pwm_node(void * arg) { uint8_t index = *(reinterpret_cast<uint8_t *>(arg)); r2p::Node node("pwm2sub"); r2p::Subscriber<r2p::PWM2Msg, 5> pwm_sub; r2p::PWM2Msg * msgp; (void) arg; chRegSetThreadName("pwm_node"); /* Enable the h-bridge. */ palSetPad(GPIOB, GPIOB_MOTOR_ENABLE); palClearPad(GPIOA, GPIOA_MOTOR_D1); chThdSleepMilliseconds(500); pwmStart(&PWM_DRIVER, &pwmcfg); node.subscribe(pwm_sub, "pwm"); for (;;) { if (node.spin(r2p::Time::ms(1000))) { if (pwm_sub.fetch(msgp)) { pwm = msgp->value[index]; chSysLock() ; if (pwm >= 0) { pwm_lld_enable_channel(&PWMD1, 0, msgp->value[index]); pwm_lld_enable_channel(&PWMD1, 1, 0); } else { pwm_lld_enable_channel(&PWMD1, 0, 0); pwm_lld_enable_channel(&PWMD1, 1, -msgp->value[index]); } chSysUnlock(); pwm_sub.release(*msgp); } } else { // Stop motor if no messages for 1000 ms pwm_lld_disable_channel(&PWM_DRIVER, 0); pwm_lld_disable_channel(&PWM_DRIVER, 1); } } return CH_SUCCESS; }
/** * @brief Enables a PWM channel. * @details Programs (or reprograms) a PWM channel. * * @param[in] pwmp pointer to a @p PWMDriver object * @param[in] channel PWM channel identifier (0...PWM_CHANNELS-1) * @param[in] width PWM pulse width as clock pulses number * * @api */ void pwmEnableChannel(PWMDriver *pwmp, pwmchannel_t channel, pwmcnt_t width) { chDbgCheck((pwmp != NULL) && (channel < PWM_CHANNELS), "pwmEnableChannel"); chSysLock(); chDbgAssert(pwmp->pd_state == PWM_READY, "pwmEnableChannel(), #1", "not ready"); pwm_lld_enable_channel(pwmp, channel, width); chSysUnlock(); }
msg_t motor_calibration_node(void * arg) { //Configure current node calibration_pub_node_conf* conf; if (arg != NULL) conf = (calibration_pub_node_conf *) arg; else conf = &defaultPubConf; Node node(conf->name); Publisher<FloatMsg> current_pub; FloatMsg * msgp; chRegSetThreadName(conf->name); node.advertise(current_pub, conf->topic); // Start the ADC driver and conversion adcStart(&ADC_DRIVER, NULL); adcStartConversion(&ADC_DRIVER, &adcgrpcfg, adc_samples, ADC_BUF_DEPTH); // Init motor driver palSetPad(DRIVER_GPIO, DRIVER_RESET); chThdSleepMilliseconds(500); pwmStart(&PWM_DRIVER, &pwmcfg); // wait some time chThdSleepMilliseconds(500); // start pwm float voltage = 24.0; const float pwm_res = 4096.0f / 24.0f; pwm = static_cast<int>(voltage * pwm_res); if (pwm > 0) { pwm_lld_enable_channel(&PWM_DRIVER, 1, pwm); pwm_lld_enable_channel(&PWM_DRIVER, 0, 0); pwm_lld_enable_channel(&PWM_DRIVER, 2, pwm/2); } else { pwm_lld_enable_channel(&PWM_DRIVER, 1, 0); pwm_lld_enable_channel(&PWM_DRIVER, 0, -pwm); pwm_lld_enable_channel(&PWM_DRIVER, 2, -pwm/2); } // Start publishing current measures for (;;) { // Wait for interrupt chSysLock() ; tp_motor = chThdSelf(); chSchGoSleepS(THD_STATE_SUSPENDED); chSysUnlock(); // publish current if (current_pub.alloc(msgp)) { msgp->value = meanLevel; current_pub.publish(*msgp); } } return CH_SUCCESS; }