/**
* @brief Disables a PWM channel.
* @pre The PWM unit must have been activated using @p pwmStart().
* @post The channel is disabled and its output line returned to the
* idle state.
* @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)
*
* @api
*/
void pwmDisableChannel(PWMDriver *pwmp, pwmchannel_t channel) {
osalDbgCheck((pwmp != NULL) && (channel < PWM_CHANNELS));
osalSysLock();
osalDbgAssert(pwmp->state == PWM_READY, "not ready");
pwm_lld_disable_channel(pwmp, channel);
osalSysUnlock();
}
/**
* @brief Disables a PWM channel.
* @pre The PWM unit must have been activated using @p pwmStart().
* @post The channel is disabled and its output line returned to the
* idle state.
* @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)
*
* @api
*/
void pwmDisableChannel(PWMDriver *pwmp, pwmchannel_t channel) {
chDbgCheck((pwmp != NULL) && (channel < PWM_CHANNELS),
"pwmEnableChannel");
chSysLock();
chDbgAssert(pwmp->state == PWM_READY,
"pwmDisableChannel(), #1", "not ready");
pwm_lld_disable_channel(pwmp, channel);
chSysUnlock();
}
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;
}
msg_t pid_node(void * arg) {
pid_conf * conf = reinterpret_cast<pid_conf *>(arg);
r2p::Node node("pid");
r2p::Subscriber<r2p::SpeedMsg, 5> speed_sub;
r2p::Subscriber<r2p::EncoderMsg, 5> enc_sub(enc_callback);
r2p::SpeedMsg * msgp;
r2p::Time last_setpoint(0);
(void) arg;
chRegSetThreadName("pid");
speed_pid.config(conf->k, conf->ti, conf->td, conf->ts, -4095.0, 4095.0);
/* Enable the h-bridge. */
palSetPad(GPIOB, GPIOB_MOTOR_ENABLE); palClearPad(GPIOA, GPIOA_MOTOR_D1);
chThdSleepMilliseconds(500);
pwmStart(&PWM_DRIVER, &pwmcfg);
node.subscribe(speed_sub, "speed");
node.subscribe(enc_sub, "encoder");
for (;;) {
if (node.spin(r2p::Time::ms(1000))) {
if (speed_sub.fetch(msgp)) {
speed_pid.set(msgp->value);
last_setpoint = r2p::Time::now();
speed_sub.release(*msgp);
} else if (r2p::Time::now() - last_setpoint > r2p::Time::ms(1000)) {
speed_pid.set(0);
}
} 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;
}
msg_t pid3_node(void * arg) {
Node node("pid");
Subscriber<Speed3Msg, 5> speed_sub;
Subscriber<tEncoderMsg, 5> enc_sub(enc_callback);
Speed3Msg * msgp;
Time last_setpoint(0);
(void) arg;
chRegSetThreadName("pid");
// speed_pid.config(450.0, 0.125, 0.0, 0.01, -4095.0, 4095.0); /* Robocom */
speed_pid.config(250.0, 0.0, 0.0, 0.01, -4095.0, 4095.0); /* Triskar */
switch (stm32_id8()) {
case M1:
index = 0;
break;
case M2:
index = 1;
break;
case M3:
index = 2;
break;
default:
break;
}
// Init motor driver
palSetPad(DRIVER_GPIO, DRIVER_RESET);
chThdSleepMilliseconds(500);
pwmStart(&PWM_DRIVER, &pwmcfg);
node.subscribe(speed_sub, "speed3");
switch (stm32_id8()) {
case M1:
node.subscribe(enc_sub, "encoder1");
break;
case M2:
node.subscribe(enc_sub, "encoder2");
break;
case M3:
node.subscribe(enc_sub, "encoder3");
break;
default:
node.subscribe(enc_sub, "encoder");
break;
}
for (;;) {
if (node.spin(Time::ms(100))) {
if (speed_sub.fetch(msgp)) {
speed_pid.set(msgp->value[index]);
last_setpoint = Time::now();
speed_sub.release(*msgp);
palTogglePad(LED3_GPIO, LED3);
} else if (Time::now() - last_setpoint > Time::ms(100)) {
speed_pid.set(0);
}
} else {
// Stop motor if no messages for 100 ms
pwm_lld_disable_channel(&PWM_DRIVER, 0);
pwm_lld_disable_channel(&PWM_DRIVER, 1);
palTogglePad(LED4_GPIO, LED4);
}
}
return CH_SUCCESS;
}
