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