Bitcraze Forums

Code: Select all

/*
 * motorsDriver.c
 *
 *  Created on: Sep 20, 2018
 *      Author: bitcraze
 */
#include <stdbool.h>
#include "stm32fxxx.h"
#include "motorsDriver.h"
// FreeRTOS includes
#include "FreeRTOS.h"
#include "task.h"

#define BASE_FREQ (328125)

static bool isInit=false;

// initialize output
void GPIOInit(){
	GPIO_InitTypeDef GPIO_Struct;
	// clocl setup
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
	// TIMER SETUP
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM5);
	//
	GPIO_Struct.GPIO_Mode=GPIO_Mode_AF;
	GPIO_Struct.GPIO_OType=GPIO_OType_PP;
	GPIO_Struct.GPIO_Pin=GPIO_Pin_2;
	GPIO_Struct.GPIO_PuPd=GPIO_PuPd_DOWN;
	GPIO_Struct.GPIO_Speed=GPIO_Speed_2MHz;
	// initialize GPIOA with setup config
	GPIO_Init(GPIOA, &GPIO_Struct);
}
void timerInit(){
	// Struct to initialize/configure TIM5
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct;
	//clock
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
	// Configure TIM_TimeBaseStruct
	TIM_TimeBaseStruct.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseStruct.TIM_Prescaler=0;
	TIM_TimeBaseStruct.TIM_ClockDivision=0;
	TIM_TimeBaseStruct.TIM_Period=255;
	TIM_TimeBaseStruct.TIM_RepetitionCounter=0;
	//initialize timer
	TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStruct);
	//enable timer5
	TIM_CtrlPWMOutputs(TIM5, ENABLE);
	//set compare register values
	TIM_SetCompare3(TIM5,0x00);
	// start timer
	TIM_Cmd(TIM5, ENABLE);
}
void pwmInit(){
	TIM_OCInitTypeDef TIM_OCStruct;
	// configure TIM_OCStruct
	TIM_OCStruct.TIM_OCMode=TIM_OCMode_PWM1;
	TIM_OCStruct.TIM_OutputState=TIM_OutputState_Enable;
	TIM_OCStruct.TIM_OCPolarity=TIM_OCPolarity_High;
	TIM_OCStruct.TIM_Pulse=255;
	//initialize on channel 3
	TIM_OC3Init(TIM5, &TIM_OCStruct);
	//enable preload
	TIM_OC3PreloadConfig(TIM5, TIM_OCPreload_Enable);
}
void motorDriverInit(){
	if(isInit)
		return;
	GPIOInit();
	timerInit();
	pwmInit();

	isInit=true;
}

void motorSetRatio(uint8_t ratio){
	TIM_SetCompare3(TIM5,ratio);
}

void motorSetFreq(uint16_t freq){
	TIM_PrescalerConfig(TIM5, BASE_FREQ/freq, TIM_PSCReloadMode_Update);
}

Code: Select all

/*
 * motorsDriver.h
 *
 *  Created on: Sep 20, 2018
 *      Author: bitcraze
 */
#ifndef SRC_DRIVERS_INTERFACE_MOTORSDRIVE_H_
#define SRC_DRIVERS_INTERFACE_MOTORSDRIVE_H_

#include <stdint.h>
#include <stdbool.h>
#include "config.h"

void GPIOInit();
void timerInit();
void pwmInit();

// motor initilization
void motorDriverInit();

void motorSetRatio(uint8_t ratio);

void motorSetFreq(uint16_t freq);

#endif /*SRC DRIVERS INTERFACE MOTORSDRIVER_H_*/

Code: Select all

/*
 * pwmMotors.c
 *
 *  Created on: Sep 21, 2018
 *      Author: bitcraze
 */
#include <stdint.h>
#include <string.h>
#include <unistd.h>

#include "stm32fxxx.h"
#include "config.h"
#include "deck.h"
#include "param.h"

#include "FreeRTOS.h"
#include "task.h"
#include "system.h"

#include "motorsDriver.h"

static bool isInit;
static int prevDir=-1;
static uint8_t prevSpeed=255;
static uint8_t speed;
static uint16_t PERIOD_SLEEP_TIME_IN_MS=100;

typedef enum{
     stop=0,
	 cw=1,
	 ccw=2
} Mode;
static Mode mode;

void pwmMotors(void* arg);

void pwmWrite(uint8_t ratio){
   motorSetRatio(ratio);
}

void stopMotor(){
   motorSetRatio(0);
}

static void pwmMotorsInit(DeckInfo* info){
   if (isInit)
       return;
	motorDriverInit();

	pinMode(DECK_GPIO_IO1, OUTPUT);
	pinMode(DECK_GPIO_IO2, OUTPUT);
	mode=1;
	speed=255;
	motorSetFreq(10000);

	xTaskCreate(pwmMotors, "pmwMotors", configMINIMAL_STACK_SIZE,NULL, /*PRIORITY*/ 1, NULL);
	isInit=true;
}

void pwmMotors(void* arg){
    systemWaitStart();
	TickType_t xLastWakeTime;
	while(1){
	  xLastWakeTime=xTaskGetTickCount();
	if(prevDir == mode&& prevSpeed == speed){
	  vTaskDelayUntil(&xLastWakeTime, M2T(PERIOD_SLEEP_TIME_IN_MS));
	  continue;
	}
	switch(mode){
	    case stop:
		     digitalWrite(DECK_GPIO_IO1,0);
			 digitalWrite(DECK_GPIO_IO2,0);
			 stopMotor();
			 break;
		case cw:
		     stopMotor();
		     digitalWrite(DECK_GPIO_IO1,1);
			 digitalWrite(DECK_GPIO_IO2,0);
			 pwmWrite(speed);
			 break;
		case ccw:
		     stopMotor();
		     digitalWrite(DECK_GPIO_IO1,0);
			 digitalWrite(DECK_GPIO_IO2,1);
			 pwmWrite(speed);
			 break;
        default:
             break;
	}
	 prevDir=mode;
	 prevSpeed=speed;

	 vTaskDelayUntil(&xLastWakeTime, M2T(PERIOD_SLEEP_TIME_IN_MS));
	}
	}

	PARAM_GROUP_START(xMotorPWM)
	PARAM_ADD(PARAM_UINT8, mode, &mode)
	PARAM_ADD(PARAM_UINT8, speed, &speed)
	PARAM_GROUP_STOP(xMotorPWM)

	static const DeckDriver pwmMotors_deck={
	   .vid=0,
	   .pid=0,
	   .name="pwmMotors",
	   .usedPeriph=DECK_USING_TIMER5,
	   .usedGpio=DECK_USING_PB5 | DECK_USING_PB8 | DECK_USING_RX2,
	   .init=pwmMotorsInit
	};

	DECK_DRIVER(pwmMotors_deck);