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