msg_t velocity_node(void *arg) { r2p::Node node("velocity"); r2p::Subscriber<r2p::Velocity3Msg, 5> vel_sub; r2p::Velocity3Msg *velp; r2p::Subscriber<r2p::QEIMsg, 5> qei1_sub(qei1_callback); r2p::Subscriber<r2p::QEIMsg, 5> qei2_sub(qei2_callback); r2p::Publisher<r2p::Velocity3Msg> odometry_pub; float v; float w; (void) arg; chRegSetThreadName("velocity"); node.subscribe(qei1_sub, "qei1"); node.subscribe(qei2_sub, "qei2"); node.subscribe(vel_sub, "velocity"); if (!node.advertise(odometry_pub, "odometry")) while(1); vel_pid.config(1.0, 3, 0, 0.05, -5.0, 5.0); // vel_pid.config(2.0, 2, -0.1, 0.05, -5.0, 5.0); // vel_pid.config(5.0, 1.0, 0.00, 0.05, -5.0, 5.0); // vel_pid.config(5.0, 1.0, 0.05, 0.05, -5.0, 5.0); vel_pid.set(0); for (;;) { if (!node.spin(r2p::Time::ms(500))) { angle_setpoint = 0; continue; } v = (left_speed + right_speed) / 2; w = (right_speed - left_speed) / 0.425; angle_setpoint = vel_pid.update(v); // 20hz r2p::Velocity3Msg *msgp; if (odometry_pub.alloc(msgp)) { msgp->x = v; msgp->y = 0; msgp->w = w; odometry_pub.publish(*msgp); } while (vel_sub.fetch(velp)) { palTogglePad(LED2_GPIO, LED2); vel_setpoint = velp->x; w_setpoint = velp->w; vel_sub.release(*velp); } vel_pid.set(vel_setpoint); } return CH_SUCCESS; }
/* * Balance control node */ msg_t balance_node(void *arg) { r2p::Node node("balance"); r2p::Subscriber<r2p::TiltMsg, 2> tilt_sub; r2p::TiltMsg *tiltp; r2p::Publisher<r2p::PWM2Msg> pwm2_pub; r2p::PWM2Msg *pwmp; int32_t pwm = 0; (void) arg; chRegSetThreadName("balance"); node.advertise(pwm2_pub, "pwm2"); node.subscribe(tilt_sub, "tilt"); PID pid; //pid.config(800, 0.35, 0.09, 0.02, -4000, 4000); pid.config(800, 0.35, 0.01, 0.02, -4000, 4000); //pid.config(550, 0.6, 0, 0.02, -4000, 4000); //pid.config(500, 990.35, 0, 0.02, -4000, 4000); // pid.config(600, 0.35, 0, 0.02, -4000, 4000); // // // pid.set(angle_setpoint); for (;;) { pid.set(angle_setpoint); while (!tilt_sub.fetch(tiltp)) { r2p::Thread::sleep(r2p::Time::ms(1)); } pwm = pid.update(tiltp->angle); //rad2grad tilt_sub.release(*tiltp); if (pwm2_pub.alloc(pwmp)) { pwmp->value[0] = (pwm - w_setpoint * 100); //FIXME pwmp->value[1] = -(pwm + w_setpoint * 100); //FIXME pwm2_pub.publish(*pwmp); palTogglePad(LED3_GPIO, LED3); palSetPad(LED4_GPIO, LED4); } else { palClearPad(LED4_GPIO, LED4); } } 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; }