Esempio n. 1
0
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;
}
Esempio n. 2
0
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;
}