예제 #1
0
파일: pwm.c 프로젝트: TheShed/ChibiOS
/**
 * @brief   Disables a PWM channel.
 * @pre     The PWM unit must have been activated using @p pwmStart().
 * @post    The channel is disabled and its output line returned to the
 *          idle state.
 * @note    Depending on the hardware implementation this function has
 *          effect starting on the next cycle (recommended implementation)
 *          or immediately (fallback implementation).
 *
 * @param[in] pwmp      pointer to a @p PWMDriver object
 * @param[in] channel   PWM channel identifier (0...PWM_CHANNELS-1)
 *
 * @api
 */
void pwmDisableChannel(PWMDriver *pwmp, pwmchannel_t channel) {

  osalDbgCheck((pwmp != NULL) && (channel < PWM_CHANNELS));

  osalSysLock();
  osalDbgAssert(pwmp->state == PWM_READY, "not ready");
  pwm_lld_disable_channel(pwmp, channel);
  osalSysUnlock();
}
예제 #2
0
파일: pwm.c 프로젝트: Kreyl/Quetta
/**
 * @brief   Disables a PWM channel.
 * @pre     The PWM unit must have been activated using @p pwmStart().
 * @post    The channel is disabled and its output line returned to the
 *          idle state.
 * @note    Depending on the hardware implementation this function has
 *          effect starting on the next cycle (recommended implementation)
 *          or immediately (fallback implementation).
 *
 * @param[in] pwmp      pointer to a @p PWMDriver object
 * @param[in] channel   PWM channel identifier (0...PWM_CHANNELS-1)
 *
 * @api
 */
void pwmDisableChannel(PWMDriver *pwmp, pwmchannel_t channel) {

  chDbgCheck((pwmp != NULL) && (channel < PWM_CHANNELS),
             "pwmEnableChannel");

  chSysLock();
  chDbgAssert(pwmp->state == PWM_READY,
              "pwmDisableChannel(), #1", "not ready");
  pwm_lld_disable_channel(pwmp, channel);
  chSysUnlock();
}
예제 #3
0
msg_t pwm_node(void * arg) {
	uint8_t index = *(reinterpret_cast<uint8_t *>(arg));
	r2p::Node node("pwm2sub");
	r2p::Subscriber<r2p::PWM2Msg, 5> pwm_sub;
	r2p::PWM2Msg * msgp;

	(void) arg;

	chRegSetThreadName("pwm_node");

	/* Enable the h-bridge. */
	palSetPad(GPIOB, GPIOB_MOTOR_ENABLE); palClearPad(GPIOA, GPIOA_MOTOR_D1);
	chThdSleepMilliseconds(500);
	pwmStart(&PWM_DRIVER, &pwmcfg);

	node.subscribe(pwm_sub, "pwm");

	for (;;) {
		if (node.spin(r2p::Time::ms(1000))) {
			if (pwm_sub.fetch(msgp)) {
				pwm = msgp->value[index];
				chSysLock()
				;
				if (pwm >= 0) {
					pwm_lld_enable_channel(&PWMD1, 0, msgp->value[index]);
					pwm_lld_enable_channel(&PWMD1, 1, 0);
				} else {
					pwm_lld_enable_channel(&PWMD1, 0, 0);
					pwm_lld_enable_channel(&PWMD1, 1, -msgp->value[index]);
				}
				chSysUnlock();
				pwm_sub.release(*msgp);
			}
		} 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;
}
예제 #4
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;
}
예제 #5
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;
}