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;
}
