/**
* @brief Starts the PWM output.
* @note The pwmStart() function used in this code is not
* the original ChibiOS HAL function, but modified
* one with STM32_TIM_CR1_CEN flag removed.
* @return none.
*/
void pwmOutputStart(void) {
#if STM32_PWM_USE_ADVANCED
/* Get dead-time generator value for TIM1. */
uint32_t bdtr_dt = pwmOutputGetBDTRDeadTime(g_pwmOutput[PWM_OUT_ROLL].dt_cmd_id & PWM_OUT_DT_ID_MASK);
pwmcfg_d1_d8.bdtr |= bdtr_dt;
#endif
pwmStart(&PWMD1, &pwmcfg_d1_d8);
#if STM32_PWM_USE_ADVANCED
/* Clear bdtr_dt value from previous calculation. */
pwmcfg_d1_d8.bdtr &= ~bdtr_dt;
/* Get dead-time generator value for TIM8. */
bdtr_dt = pwmOutputGetBDTRDeadTime(g_pwmOutput[PWM_OUT_PITCH].dt_cmd_id & PWM_OUT_DT_ID_MASK);
pwmcfg_d1_d8.bdtr |= bdtr_dt;
#endif
/* Configure TIM8 as master timer: */
pwmcfg_d1_d8.cr2 = STM32_TIM_CR2_MMS(1); // Master mode set to Enable;
pwmStart(&PWMD8, &pwmcfg_d1_d8);
switch (g_pwmOutput[PWM_OUT_YAW].dt_cmd_id & PWM_OUT_DT_ID_MASK) {
case PWM_OUT_DT750NS:
pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_US7;
break;
case PWM_OUT_DT1000NS:
pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_1US;
break;
case PWM_OUT_DT2000NS:
pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_2US;
break;
case PWM_OUT_DT3000NS:
pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_3US;
break;
case PWM_OUT_DT4000NS:
pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_4US;
break;
case PWM_OUT_DT5000NS:
pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_5US;
break;
default:
pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_5US;
}
pwmStart(&PWMD4, &pwmcfg_d4);
pwmStart(&PWMD5, &pwmcfg_d5);
/* Configure TIM5 as slave timer: */
PWMD5.tim->SMCR =
STM32_TIM_SMCR_SMS(6) | // Trigger Mode;
STM32_TIM_SMCR_TS(3); // Trigger event comes from TIM8;
/* Configure TIM4 as slave timer: */
PWMD4.tim->SMCR =
STM32_TIM_SMCR_SMS(6) | // Trigger Mode;
STM32_TIM_SMCR_TS(3); // Trigger event comes from TIM8;
/* Switch to center-aligned mode 1 and start timers. */
PWMD5.tim->CR1 |= (STM32_TIM_CR1_CMS(1)); // This is a slave timer - do not start;
PWMD4.tim->CR1 |= (STM32_TIM_CR1_CMS(1)); // This is a slave timer - do not start;
PWMD1.tim->CR1 |= (STM32_TIM_CR1_CMS(1) | STM32_TIM_CR1_CEN);
PWMD8.tim->CR1 |= (STM32_TIM_CR1_CMS(1) | STM32_TIM_CR1_CEN); // Start TIM8, TIM4 and TIM5 simultaneously;
}
void InitPWM(void) {
/*
* Initializes the PWM drivers
*/
pwmStart(&PWMD3, &pwmcfg);
pwmStart(&PWMD4, &pwmcfg);
pwmStart(&PWMD5, &pwmcfg);
// pwmStart(&PWMD8, &pwmcfg);
}
/* This function will start the PWM generator.
*/
extern void bldcInit(void) {
bldc.scheme = &pwmScheme;
bldc.state = 0; //Default to first state
bldc.nextState = 0;
bldc.directionFwd = TRUE;
bldc.stateChangeInterval = MS2RTT(20);
bldc.prevStateChange = halGetCounterValue();
bldc.nextStateChange = bldc.prevStateChange + bldc.stateChangeInterval;
bldc.pwmOutT0 = 0;
bldc.pwmOutT1 = 0;
bldc.stateCount = sizeof(pwmScheme)/3;
bldc.dutyCycle = 1800;
palWriteGroup (PWM_OUT_PORT, PWM_OUT_PORT_MASK, PWM_OUT_OFFSET, PWM_OFF);
palSetGroupMode (PWM_OUT_PORT, PWM_OUT_PORT_MASK, PWM_OUT_OFFSET, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode (GPIOA, GPIOA_PIN1, PAL_MODE_INPUT_ANALOG);
pwmStart(&PWMD1, &pwmcfg);
// ADC trigger channel. This will trigger the ADC read at 95% of the cycle,
// when all the PWM outputs are set to 0
pwmEnableChannel (&PWMD1, PWM_ADCTRIG_CH, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, PWM_MAX_DUTY_CYCLE));
// Start the ADC
adcStart(&ADCD1, NULL);
}
void goToPos(int32_t new_pos){
if(new_pos == position)
return;
config.frequency = clk_freq;
config.period = clk_freq / initial_freq;
config.callback = cb;
current_freq = initial_freq;
config.channels[0] = {PWM_OUTPUT_DISABLED, NULL};
config.channels[1] = {PWM_OUTPUT_DISABLED, NULL};
config.channels[2] = {PWM_OUTPUT_DISABLED, NULL};
config.channels[3] = {PWM_OUTPUT_DISABLED, NULL};
config.channels[channel] = {PWM_OUTPUT_ACTIVE_HIGH, NULL};
config.cr2 = 0;
config.dier = 0;
dir_positive = new_pos > position;
target_position = new_pos;
dir_pin.assign(dir_positive);
ramp_length = (new_pos > position) ?
((max_pos_freq - initial_freq) / ramp) :
((max_neg_freq - initial_freq) / ramp);
pwmStart(driver, &config);
pwmEnableChannel(driver, channel, 10);
pwmEnablePeriodicNotificationI(driver);
}
int main(void)
{
halInit();
chSysInit();
palSetPadMode(GPIOB, 0, PAL_MODE_ALTERNATE(2)); // alt mode is PWM-enabled TIM3
palSetPadMode(GPIOB, INAPIN, PAL_MODE_OUTPUT_PUSHPULL); // set INA to be an output
palSetPadMode(GPIOB, INBPIN, PAL_MODE_OUTPUT_PUSHPULL); // set INB to be an output
pwmStart(&PWMD3, &pwmcfg); // TIM3 for motors
while (TRUE) {
// one direction (possibly forward)
pwmEnableChannel(&PWMD3, 3, 500);
palSetPad(GPIOB, INAPIN);
palClearPad(GPIOB, INBPIN);
chThdSleepMilliseconds(3000);
// the other direction (possibly backward)
pwmEnableChannel(&PWMD3, 3, 500);
palSetPad(GPIOB, INBPIN);
palClearPad(GPIOB, INAPIN);
chThdSleepMilliseconds(3000);
}
}
void PwmBacklight::Init()
{
palSetPadMode(BACKLIGHT_PORT, BACKLIGHT_PIN, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
pwmStart(&PWMD4, &pwmconfig);
FadeIn();
}
void convStart( void )
{
/*
pwmStart( &PWMD2, &pwmCfg );
palSetPadMode( GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL );
palSetPadMode( GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL );
//palSetPadMode( GPIOA, 2, PAL_MODE_STM32_ALTERNATE_PUSHPULL );
pwmEnableChannel(&PWMD2, 0, PWM_PERCENTAGE_TO_WIDTH( &PWMD2, 9500 ) );
pwmEnableChannel(&PWMD2, 1, PWM_PERCENTAGE_TO_WIDTH( &PWMD2, 3030 ) );
//pwmEnableChannel(&PWMD2, 2, PWM_PERCENTAGE_TO_WIDTH( &PWMD2, 3030 ) );
*/
// Start PWM peripherial.
pwmStart( &CONV_PWM, &pwmCfg );
// Init PWM pins.
palSetPadMode( CONV_PORT, CONV_BUCK_PIN, PAL_MODE_STM32_ALTERNATE_PUSHPULL );
palSetPadMode( CONV_PORT, CONV_BOOST_PIN, PAL_MODE_STM32_ALTERNATE_PUSHPULL );
// Set zero active period.
pwmEnableChannel(&CONV_PWM, PWM_BOOST_CHAN, PWM_PERCENTAGE_TO_WIDTH( &CONV_PWM, 0000 ) );
pwmEnableChannel(&CONV_PWM, PWM_BUCK_CHAN, PWM_PERCENTAGE_TO_WIDTH( &CONV_PWM, 0000 ) );
// Init ADC.
palSetGroupMode(CONV_ADC_PORT, PAL_PORT_BIT( CONV_BUCK_FB_PIN ) |
PAL_PORT_BIT( CONV_BOOST_FB_PIN ) |
PAL_PORT_BIT( CONV_INPUT_FB_PIN ),
0, PAL_MODE_INPUT_ANALOG);
adcStart( &ADCD1, NULL );
adcStartConversion( &ADCD1, &adcGroup, adcSamples, ADC_BUF_DEPTH );
}
/**
* @brief Put bus in active mode.
*/
static void ow_bus_active(onewireDriver *owp) {
pwmStart(owp->config->pwmd, owp->config->pwmcfg);
#if defined(STM32F1XX)
palSetPadMode(owp->config->port, owp->config->pad,
owp->config->pad_mode_active);
#endif
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
pwmStart(&PWMD1, &pwmcfg);
/*
* Activates the serial driver 2 using the driver default configuration.
*/
palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(2));
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state.
*/
while (true) {
if (TRUE) //(!palReadPad(GPIOC, GPIOC_BUTTON))
//TestThread(&SD1);
chThdSleepMilliseconds(5000);
}
}
void PwmBacklight::Init()
{
palSetPadMode(GPIOB, 6, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
pwmStart(&PWMD4, &pwmconfig);
FadeIn();
}
PWMPlatform::PWMPlatform() {
pwmStart(&PWMD1, &MOTOR_PWM_CONFIG);
palSetPadMode(GPIOC, 6, PAL_MODE_ALTERNATE(4));
palSetPadMode(GPIOC, 7, PAL_MODE_ALTERNATE(4));
palSetPadMode(GPIOC, 8, PAL_MODE_ALTERNATE(4));
palSetPadMode(GPIOC, 9, PAL_MODE_ALTERNATE(4));
}
msg_t pwm2sub_node(void * arg) {
Node node("pwm2sub");
Subscriber<PWM2Msg, 5> sub(callback);
(void) arg;
chRegSetThreadName("pwm2sub");
palSetPad(DRIVER_GPIO, DRIVER_RESET);
chThdSleepMilliseconds(500);
pwmStart(&PWM_DRIVER, &pwmcfg);
node.subscribe(sub, "pwm2");
for (;;) {
if (!node.spin(Time::ms(100))) {
// Stop motor if no messages for 100 ms
chSysLock()
;
pwm_lld_enable_channel(&PWM_DRIVER, 0, 0);
pwm_lld_enable_channel(&PWM_DRIVER, 0, 0);
chSysUnlock();
}
}
return CH_SUCCESS;
}
void demo_led_init(void)
{
/*
* PWM configuration structure.
* Cyclic callback enabled, channels 1 and 4 enabled without callbacks,
* the active state is a logic one.
*/
static const PWMConfig pwmcfg = {
100000, /* 100kHz PWM clock frequency. */
128, /* PWM period is 128 cycles. */
NULL,
{
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL}
},
/* HW dependent part.*/
0,
0
};
/*
* Initializes the PWM driver 4, routes the TIM4 outputs to the board LEDs.
*/
pwmStart(&PWMD4, &pwmcfg);
palSetPadMode(GPIOD, GPIOD_LED4, PAL_MODE_ALTERNATE(2)); /* Green. */
palSetPadMode(GPIOD, GPIOD_LED3, PAL_MODE_ALTERNATE(2)); /* Orange. */
palSetPadMode(GPIOD, GPIOD_LED5, PAL_MODE_ALTERNATE(2)); /* Red. */
palSetPadMode(GPIOD, GPIOD_LED6, PAL_MODE_ALTERNATE(2)); /* Blue. */
}
void motor_init(void) {
pwmStart(&PWMD2, &pwmcfg);
pwmEnableChannel(&PWMD2, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 500)); // 1mS Pulse
pwmEnableChannel(&PWMD2, 1, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 500)); // 1mS Pulse
pwmEnableChannel(&PWMD2, 2, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 500)); // 1mS Pulse
pwmEnableChannel(&PWMD2, 3, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 500)); // 1mS Pulse
}
bool Pwm_chibios::init(void)
{
pwmStop(driver_);
pwmStart(driver_, &config_);
pwmEnablePeriodicNotification(driver_);
pwmEnableChannelNotification(driver_, channel_);
return true;
}
/*
* Application entry point.
*/
int main(void) {
Thread *shelltp = NULL;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SERIAL_DRIVER, NULL);
/*
* Initializes the PWM driver.
*/
pwmStart(&PWM_DRIVER, &pwmcfg);
/*
* Initializes the GPT driver.
*/
gptStart(&GPT_DRIVER, &gptcfg);
/*
* Initializes the EXT driver.
*/
extStart(&EXT_DRIVER, &extcfg);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state.
*/
while (TRUE) {
if (!shelltp)
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp);
shelltp = NULL;
}
chprintf(&SERIAL_DRIVER, "M: %6umm T1: %6u T2: %6u\r\n", measure / 2, tmp1, tmp2);
chThdSleepMilliseconds(200);
}
}
void MotorsInit(void){
pwmStart(&PWMD8, &pwmcfg);
palSetPadMode(GPIOC, 6, PAL_MODE_ALTERNATE(3));
palSetPadMode(GPIOC, 7, PAL_MODE_ALTERNATE(3));
palSetPadMode(GPIOC, 8, PAL_MODE_ALTERNATE(3));
palSetPadMode(GPIOC, 9, PAL_MODE_ALTERNATE(3));
unsigned i;
for (i=0; i<NUM_MOTORS; i++){
MotorsSetSpeed(i, 0);
}
}
static __attribute__((noreturn)) msg_t PWMThread2(void *arg){
(void)arg;
chRegSetThreadName("PWM Thread 2");
uint16_t pwmVal2 = 1;
palSetPadMode(GPIOA, GPIOA_PA1, PAL_MODE_OUTPUT_PUSHPULL);
pwmStart(&PWMD2,&pwmcfg2);
while(TRUE){
if(pwmVal2 >= 1024){ pwmVal2 = 1; }
else { pwmVal2 <<= 1; }
pwmEnableChannel(&PWMD2,1,pwmVal2);
chThdSleepMilliseconds(100);
}
}
PWMPlatform::PWMPlatform() {
// TIM1
pwmStart(&PWMD1, &PWMD1_CONFIG);
palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(1));
palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(1));
palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(1));
palSetPadMode(GPIOA, 11, PAL_MODE_ALTERNATE(1));
// TIM3
pwmStart(&PWMD3, &PWMD3_CONFIG);
palSetPadMode(GPIOA, 6, PAL_MODE_ALTERNATE(2));
palSetPadMode(GPIOA, 7, PAL_MODE_ALTERNATE(2));
palSetPadMode(GPIOB, 0, PAL_MODE_ALTERNATE(2));
palSetPadMode(GPIOB, 1, PAL_MODE_ALTERNATE(2));
// TIM4
pwmStart(&PWMD4, &PWMD4_CONFIG);
palSetPadMode(GPIOB, 6, PAL_MODE_ALTERNATE(2));
palSetPadMode(GPIOB, 7, PAL_MODE_ALTERNATE(2));
palSetPadMode(GPIOB, 8, PAL_MODE_ALTERNATE(2));
palSetPadMode(GPIOB, 9, PAL_MODE_ALTERNATE(2));
}
int main(void) {
halInit();
chSysInit();
chThdSleepMilliseconds(200);
// input capture & high-res timer
const ICUConfig icuConfig = { ICU_INPUT_ACTIVE_HIGH, 1000000, nullptr, nullptr, nullptr, nullptr, nullptr };
icuStart(&TIMING_ICU, &icuConfig);
icuEnable(&TIMING_ICU);
// serial setup
const SerialConfig btSerialConfig = { 921600, 0, USART_CR2_STOP1_BITS, USART_CR3_CTSE | USART_CR3_RTSE };
sdStart(&BT_SERIAL, &btSerialConfig);
// PWM setup
const PWMConfig mPWMConfig = { STM32_TIMCLK1, PWM_PERIOD, nullptr, {
{ PWM_OUTPUT_DISABLED, nullptr },
{ PWM_OUTPUT_DISABLED, nullptr },
{ PWM_OUTPUT_ACTIVE_HIGH, nullptr },
{ PWM_OUTPUT_ACTIVE_HIGH, nullptr } }, 0, };
pwmStart(&M1_PWM, &mPWMConfig);
// SPI setup
// speed = pclk/8 = 5.25MHz
const SPIConfig m1SPIConfig = { NULL, GPIOC, GPIOC_M1_NSS, SPI_CR1_DFF | SPI_CR1_BR_1 };
const SPIConfig m2SPIConfig = { NULL, GPIOD, GPIOD_M2_NSS, SPI_CR1_DFF | SPI_CR1_BR_1 };
const SPIConfig adcSPIConfig = { NULL, GPIOA, GPIOA_ADC_NSS, SPI_CR1_BR_2 | SPI_CR1_CPHA };
spiStart(&M1_SPI, &m1SPIConfig);
spiStart(&M2_SPI, &m2SPIConfig);
spiStart(&ADC_SPI, &adcSPIConfig);
// motor setup
A4960 m1(&M1_SPI, &M1_PWM, M1_PWM_CHAN);
A4960 m2(&M2_SPI, &M2_PWM, M2_PWM_CHAN);
// ADC setup
ADS1259 adc(&ADC_SPI);
// initialize control structure
Tortilla tortilla(m1, m2, adc, &TIMING_ICU, &BT_SERIAL);
// start slave threads
// chThdCreateStatic(waHeartbeat, sizeof(waHeartbeat), IDLEPRIO, tfunc_t(threadHeartbeat), nullptr);
chThdCreateStatic(waIO, sizeof(waIO), LOWPRIO, tfunc_t(threadIO), &tortilla);
// done with setup
palClearPad(GPIOC, GPIOC_LEDB);
palClearPad(GPIOB, GPIOB_LED2);
tortilla.fastLoop();
}
/*
* Application entry point.
*/
int main(void)
{
#if 0
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler
};
#endif
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
crypto_init();
buttons_init();
chHeapInit(fast_heap, FAST_HEAP_ADDR, FAST_HEAP_SIZE);
sdStart(&SD2, NULL);
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
lcd_init();
setvbuf(stdin, NULL, _IONBF, 0);
mmcObjectInit(&MMCD1);
mmcStart(&MMCD1, &mmccfg);
mount_card();
pwmStart(&PWMD1, &lcd_pwmcfg);
palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(1));
// XXX moveme
palSetPadMode(GPIOB, 6, PAL_MODE_INPUT_PULLUP);
palSetPadMode(GPIOB, 15, PAL_MODE_INPUT_PULLUP);
palSetPadMode(GPIOB, 14, PAL_MODE_INPUT_PULLUP);
palSetPadMode(GPIOC, 10, PAL_MODE_INPUT_PULLUP);
palSetPadMode(GPIOC, 11, PAL_MODE_INPUT_PULLUP);
fiprintf(lcd_stdout, "HIHI");
while (TRUE) {
console_cmd_loop();
//chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
}
return 0;
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Initializes the PWM driver 1.
* GPIOC1 is the PWM output.
*/
palSetPadMode(IOPORT3, 1, PAL_MODE_ALTERNATIVE_4);
pwmStart(&PWMD1, &pwmcfg);
pwmEnablePeriodicNotification(&PWMD1);
/*
* Starts the PWM channel 0 using 25% duty cycle.
*/
pwmEnableChannel(&PWMD1, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 2500));
pwmEnableChannel(&PWMD1, 1, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, 2500));
pwmEnableChannelNotification(&PWMD1, 1); // MUST be before EnableChannel...
chThdSleepMilliseconds(2500);
/*
* Changes PWM period to 500µs the duty cycle becomes 50%
* implicitly.
*/
pwmChangePeriod(&PWMD1, 18000);
chThdSleepMilliseconds(2500);
//~ pwmEnablePeriodicNotification(&PWMD1);
/*
* Disables channel 0 and stops the drivers.
*/
//~ pwmDisableChannel(&PWMD1, 0);
//~ pwmStop(&PWMD1);
/*
* Normal main() thread activity, in this demo it does nothing.
*/
while (TRUE) {
palTogglePad(TEENSY_PIN13_IOPORT, TEENSY_PIN13);
chThdSleepMilliseconds(5);
}
return 0;
}
void initPWM(void){
/*
*Start PWM1 function associated with Timer 1.
*/
#if STM32_PWM_USE_TIM1
pwmStart(&PWMD1, &pwmcfg1) ;
/* palSetPadMode(SERVO2_PORT, SERVO2_PIN, PAL_MODE_ALTERNATE(1)) ; */
chThdSleepMilliseconds(10) ;
#endif /*STM32_PWM_USE_TIM1 */
/*
* Start PWM2 function associated with Timer 2.
*/
#if STM32_PWM_USE_TIM2
pwmStart(&PWMD2, &pwmcfg2) ;
/* palSetPadMode(SERVO4_PORT, SERVO4_PIN, PAL_MODE_ALTERNATE(1)) ; */
chThdSleepMilliseconds(10) ;
#endif /*STM32_PWM_USE_TIM2 */
/*
* Start PWM3 function associated with Timer 3.
*/
#if STM32_PWM_USE_TIM3
pwmStart(&PWMD3, &pwmcfg3) ;
/* palSetPadMode(SERVO1_PORT, SERVO1_PIN, PAL_MODE_ALTERNATE(2)) ; */
/* palSetPadMode(SERVO5_PORT, SERVO5_PIN, PAL_MODE_ALTERNATE(2)) ; */
/* palSetPadMode(SERVO6_PORT, SERVO6_PIN, PAL_MODE_ALTERNATE(2)) ; */
chThdSleepMilliseconds(10) ;
#endif /*STM32_PWM_USE_TIM3 */
/*
* Start PWM8 function associated with Timer 8.
*/
#if STM32_PWM_USE_TIM8
pwmStart(&PWMD8, &pwmcfg8) ;
/* palSetPadMode(SERVO3_PORT, SERVO3_PIN, PAL_MODE_ALTERNATE(3)) ; */
chThdSleepMilliseconds(10) ;
#endif /*STM32_PWM_USE_TIM1 */
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Turn off the RGB LED.
*/
palSetLine(LINE_LED_RED); /* red */
palSetLine(LINE_LED_GREEN); /* green */
palSetLine(LINE_LED_BLUE); /* blue */
/*
* Create the button check thread.
*/
chThdCreateStatic(waButtonThread, sizeof(waButtonThread), NORMALPRIO, ButtonThread, NULL);
/*
* Start the PWM driver, route TPM0 output to PTE29, PTE31, PTD5.
* Enable channels now to avoid a blink later.
*/
pwmStart(&PWM_DRIVER, &pwmcfg);
palSetLineMode(LINE_LED_RED, PAL_MODE_ALTERNATIVE_3);
palSetLineMode(LINE_LED_GREEN, PAL_MODE_ALTERNATIVE_3);
palSetLineMode(LINE_LED_BLUE, PAL_MODE_ALTERNATIVE_4);
pwmEnableChannel(&PWM_DRIVER, 2, 0);
pwmEnableChannel(&PWM_DRIVER, 4, 0);
pwmEnableChannel(&PWM_DRIVER, 5, 0);
/*
* Create the breathe thread.
*/
chThdCreateStatic(waBreatheThread, sizeof(waBreatheThread), NORMALPRIO, BreatheThread, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state, when the button is
* pressed ... nothing happens.
*/
while(true) {
chThdSleepMilliseconds(500);
}
}
void servoInit(Servo *servo) {
/* create the channel configuration */
PWMChannelConfig chcfg = { PWM_OUTPUT_ACTIVE_HIGH, NULL };
pwmcfg.channels[servo->pwm_channel] = chcfg;
/* configure PAL */
#if defined(STM32F1XX)
palSetPadMode(servo->port, servo->pin, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
#else
#error "PAL configuration for this device not implemented yet - Please modify servoInit()"
#endif
/* start the PWM unit */
pwmStart(servo->pwm_driver, &pwmcfg);
}
int main (void)
{
halInit();
chSysInit();
sdStart (&SD2, NULL);
pwmStart (&PWMD3, &pwmCfg);
adcStart (&ADCD1, &adcGrpCfg);
palSetPadMode (GPIOA, 6, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode (GPIOA, 7, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode (GPIOB, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode (GPIOB, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode (GPIOC, 2, PAL_MODE_INPUT_ANALOG);
chThdCreateStatic (heartWrkArea, sizeof (heartWrkArea), NORMALPRIO, heartBeat, NULL);
chThdCreateStatic (sensorWrkArea, sizeof (sensorWrkArea), NORMALPRIO, sensorRead, NULL);
}
void motorInit(void) {
//Pin selection PC8 PC9
palSetPadMode(GPIOC, 8, PAL_MODE_ALTERNATE(2));
palSetPadMode(GPIOC, 9, PAL_MODE_ALTERNATE(2));//650
// hardware confg
pwmStart(&PWMD3, &pwmcfg);
//drive motor enable
pwmEnableChannel(&PWMD3, 2, drive_pulse);
//wait until it enables drive motor
chThdSleepMilliseconds(500);
//start threed
chThdCreateStatic(waThread3, sizeof(waThread3),NORMALPRIO + 10, Thread3, NULL);
chThdCreateStatic(waThread4, sizeof(waThread4),NORMALPRIO + 10, Thread4, NULL);
}
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;
}
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;
}
THD_TABLE_END
#endif
int main(void) {
halInit();
/*
* NOTE: when compiling for NIL, after the chSysInit() call, nothing
* more can be done in this thread so we first initialize PWM subsystem.
*/
static PWMConfig pwm3cfg = {
1023, /* Not real clock */
1023, /* Maximum PWM count */
NULL,
{
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
},
};
/* PE3-5 are timer 3 pwm channel outputs */
palSetPadMode(IOPORT5, 3, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(IOPORT5, 4, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(IOPORT5, 5, PAL_MODE_OUTPUT_PUSHPULL);
pwmStart(&PWMD3, &pwm3cfg);
/* channel 0 with 50% duty cycle, 1 with 25% and 2 with 75% */
pwmEnableChannel(&PWMD3, 0, 511);
pwmEnableChannel(&PWMD3, 1, 255);
pwmEnableChannel(&PWMD3, 2, 767);
chSysInit();
while (1) {}
}