Drivers

/*
  * motorCommand.h
  *
  *  Created on: 27 mars 2017
  *      Author: kerhoas
  */
 
 #ifndef INC_MOTORCOMMAND_H_
 #define INC_MOTORCOMMAND_H_
 
 #include "main.h"
 
 
 void motorCommand_Init(void);
 void motorCommand_SetDuty(int);
 
 
 #endif /* INC_MOTORCOMMAND_H_ */
 

/*
 * motorCommand.c
 *
 *  Created on: 27 mars 2017
 *      Author: kerhoas
 */



/*
 * MotorCommand.c
 */

#include "motorCommand.h"

static TIM_HandleTypeDef    TimHandle;
static TIM_OC_InitTypeDef   sConfigOC;

//=================================================================
//			PWM INIT
// TIMER 3 (PWM)  : CH1
// ENABLE : Sortie Logique (GPIO) PB5
//=================================================================

void motorCommand_Init(void)
{
	unsigned int uwPrescalerValue = 0;

	/* Compute the prescaler value to have TIM4 counter clock equal to 10MHz */
	  uwPrescalerValue = (unsigned int) ((SystemCoreClock / 10000000) - 1);
	  TimHandle.Instance = TIM3;
	  TimHandle.Init.Period = PWM_VAL_MAX - 1;				 // 100MHz/200=50kHz
	  TimHandle.Init.Prescaler = 0 ;//uwPrescalerValue;
	  TimHandle.Init.ClockDivision = 0;
	  TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;

	  HAL_TIM_Base_Init(&TimHandle);

	  sConfigOC.OCMode = TIM_OCMODE_PWM1;
	  sConfigOC.Pulse = 0x5;// Specifies the pulse value to be loaded into the Capture Compare Register. This parameter can be a number between Min_Data = 0x0000 and Max_Data = 0xFFFF */

	  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
	  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;

	 HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfigOC, TIM_CHANNEL_1);

	 // CHANGEMENT DU RAPPORT CYCLIQUE
	 __HAL_TIM_SetCompare(&TimHandle, TIM_CHANNEL_1, PWM_VAL_MAX/2);	// 100 : moteurs au repos

	  HAL_TIM_PWM_Start(&TimHandle, TIM_CHANNEL_1);	// MOTOR RIGHT

	  // ENABLE MOTEUR (SI INVERSEUR)
	  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, 0);
}

//=================================================================
//			SET DUTY CYCLE
//=================================================================
void motorCommand_SetDuty(int duty)
{
	__HAL_TIM_SetCompare(&TimHandle, TIM_CHANNEL_1, (PWM_VAL_MAX/2)+duty);
}

/*
 * quadEncoder.h
 *
 *  Created on: 30 juin 2016
 *      Author: kerhoas
 */

#ifndef INC_QUADENCODER_H_
#define INC_QUADENCODER_H_



#include "main.h"

void quadEncoder_Init(void);
uint16_t quadEncoder_GetPos16(void);
int32_t quadEncoder_GetPos32(void);
int16_t quadEncoder_GetSpeed(void);
void quadEncoder_CallbackIndexZ(void);
void quadEncoder_PosCalc(int*);


#endif /* INC_QUADENCODER_H_ */

/*
 * QuadEncoder.c
 */
#include "quadEncoder.h"

#define TE_ms	5	// Période d'échantillonage en ms
#define USE_QUAD_ENCODER_1250_CPR 1


#if USE_QUAD_ENCODER_1250_CPR
#define COUNT_PER_ROUND 1250
#define MAX_CNT_PER_REV (COUNT_PER_ROUND * 4 - 1)
#define MAX_COUNT (int)(((unsigned long)MAX_CNT_PER_REV*6555)/1000)
#define HALF_MAX_COUNT (MAX_COUNT>>1)
#define COEFF	6555
#endif

#if USE_QUAD_ENCODER_1000_CPR
#define COUNT_PER_ROUND 1000
#define MAX_CNT_PER_REV (COUNT_PER_ROUND * 4 - 1)
#define MAX_COUNT (int)(((unsigned long)MAX_CNT_PER_REV*8192)/1000)
#define HALF_MAX_COUNT (MAX_COUNT>>1)
#define COEFF 8192
#endif

#if USE_QUAD_ENCODER_500_CPR
#define COUNT_PER_ROUND 500
#define MAX_CNT_PER_REV (COUNT_PER_ROUND * 4 - 1)
#define MAX_COUNT (int)(((unsigned long)MAX_CNT_PER_REV*16392)/1000)
#define HALF_MAX_COUNT (MAX_COUNT>>1)
#define COEFF 16392
#endif

#if USE_QUAD_ENCODER_250_CPR
#define COUNT_PER_ROUND 250
#define MAX_CNT_PER_REV (COUNT_PER_ROUND * 4 - 1)
#define MAX_COUNT (int)(((unsigned long)MAX_CNT_PER_REV*32768)/1000)
#define HALF_MAX_COUNT (MAX_COUNT>>1)
#define COEFF 32768
#endif



TIM_HandleTypeDef    TimEncoderHandle;

static int indexZ=-1;

int first_index_reached = 0;

//================================================================
//		TIMER INIT
//================================================================

void quadEncoder_Init(void)
{
	TIM_Encoder_InitTypeDef sConfig;

	TimEncoderHandle.Instance = TIM1;
	TimEncoderHandle.Init.Prescaler = 0;
	TimEncoderHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
	TimEncoderHandle.Init.Period = COUNT_PER_ROUND*4;
	TimEncoderHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;

	sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
	sConfig.IC1Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
	sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
	sConfig.IC1Prescaler = TIM_ICPSC_DIV4;
	sConfig.IC1Filter = 0x0F;
	sConfig.IC2Polarity = TIM_INPUTCHANNELPOLARITY_RISING;
	sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;//TIM_ICSELECTION_DIRECTTI; //TIM_TI1SELECTION_XORCOMBINATION
	sConfig.IC2Prescaler = TIM_ICPSC_DIV4;
	sConfig.IC2Filter = 0x0F;

	HAL_TIM_Encoder_Init(&TimEncoderHandle, &sConfig);
	HAL_TIM_Encoder_Start(&TimEncoderHandle,TIM_CHANNEL_1);
	HAL_TIM_Encoder_Start(&TimEncoderHandle,TIM_CHANNEL_2);
}
//================================================================
//		POSITION LEFT CALC
//================================================================
void quadEncoder_PosCalc(int* AngPos)
{
	int POSCNTcopy = 0;
	POSCNTcopy = (int)TIM1->CNT;
	AngPos[1] = AngPos[0];
	AngPos[0] = (unsigned int)(((unsigned long)POSCNTcopy * COEFF)/1000); // 0 <= POSCNT <= 4999 to 0 <= AngPos <= 32767
}
//================================================================
//		POSITION LEFT 16 BITS
//================================================================
uint16_t quadEncoder_GetPos16(void)
{
	uint16_t Pos = 0;
	Pos=TIM1->CNT;
	return Pos;
}
//================================================================
//		POSITION LEFT 32 BITS	(pos 16 bits + nombre de tours)
//================================================================
int32_t quadEncoder_GetPos32(void)
{
	int32_t  Pos = 0;
	Pos=indexZ*4*COUNT_PER_ROUND + (int32_t) quadEncoder_GetPos16();
	return Pos;
}
//================================================================
//		SPEED LEFT
//--> get pos  ; must be called every Te
//================================================================
int16_t quadEncoder_GetSpeed(void)
{
	static int AngPos[2] = {0,0};
	static int16_t Speed=0;
	int speed_rpm;


	quadEncoder_PosCalc(AngPos);
	Speed = AngPos[0] - AngPos[1];
	if (Speed >= 0)
	{
		if (Speed >= HALF_MAX_COUNT)
			{
			Speed = Speed - MAX_COUNT;
			}
	}
	else
	{
		if (Speed < -HALF_MAX_COUNT)
			{
			Speed = Speed + MAX_COUNT;
			}
	}

	//***********************************
	// CONVERT RPM
	// 1 tour = 32767
	// Nbre de Tours pendant Te: DELTA_pos/32767
	// Nbre de Tours pendant 1s (Te en ms) : (DELTA_pos/32767)*(1000/Te)
	// Nbre de Tours par minute : : (DELTA_pos/32767)*((60*1000)/Te)

	speed_rpm=(Speed*60*1000)/(32767*TE_ms);


	//************************************

	return speed_rpm;
}
//================================================================
//		MAJ indexZ Left
//================================================================
void quadEncoder_CallbackIndexZ()
{
		if (__HAL_TIM_DIRECTION_STATUS(&TimEncoderHandle)==1)
		{
			indexZ--;
		}
		else
		{
			indexZ++;
		}
		__HAL_TIM_SetCounter(&TimEncoderHandle, 0);		// RAZ Counter
		HAL_TIM_Encoder_Start(&TimEncoderHandle,TIM_CHANNEL_1);
		HAL_TIM_Encoder_Start(&TimEncoderHandle,TIM_CHANNEL_2);

		first_index_reached = 1;
}
//================================================================