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 encoder_node(void *arg) {
Node node("encoder");
Publisher<tEncoderMsg> enc_pub;
systime_t time;
tEncoderMsg *msgp;
(void) arg;
chRegSetThreadName("encoder");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
switch (stm32_id8()) {
case M1:
node.advertise(enc_pub, "encoder1");
break;
case M2:
node.advertise(enc_pub, "encoder2");
break;
case M3:
node.advertise(enc_pub, "encoder3");
break;
default:
node.advertise(enc_pub, "encoder");
break;
}
for (;;) {
time = chTimeNow();
if (enc_pub.alloc(msgp)) {
msgp->timestamp.sec = chTimeNow();
msgp->timestamp.nsec = chTimeNow();
msgp->delta = T2R(qeiUpdate(&QEI_DRIVER) * 100);
enc_pub.publish(*msgp);
}
time += MS2ST(10);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
msg_t qeipub_node(void *arg) {
Node node("qeipub");
Publisher<tQEIMsg> qei_pub;
systime_t time;
tQEIMsg *msgp;
(void) arg;
chRegSetThreadName("qeipub");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
switch (stm32_id8()) {
case M1:
node.advertise(qei_pub, "qei1");
break;
case M2:
node.advertise(qei_pub, "qei2");
break;
case M3:
node.advertise(qei_pub, "qei3");
break;
default:
node.advertise(qei_pub, "qei");
break;
}
for (;;) {
time = chTimeNow();
int16_t delta = qeiUpdate(&QEI_DRIVER);
if (qei_pub.alloc(msgp)) {
msgp->timestamp.sec = 0;
msgp->timestamp.nsec = 0;
msgp->delta = delta;
qei_pub.publish(*msgp);
}
time += MS2ST(50);
chThdSleepUntil(time);
}
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;
}
static void cmd_id(BaseSequentialStream *chp, int argc, char *argv[]) {
(void) argc;
(void) argv;
chprintf(chp, "UID: %d\r\n", stm32_id8());
}
