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