//
#include "main.h"
#include "math.h"
#include "stdlib.h"
#include "globals.h"
#include "MISC.h"
//*****************

			//		SetCurrents( 0,  1);DelayUS(DEL);
			//		SetCurrents( 1,  1);DelayUS(DEL);
			//		SetCurrents( 1,  0);DelayUS(DEL);
			//		SetCurrents( 0,  0);DelayUS(DEL);
			//		SetCurrents( 0, -1);DelayUS(DEL);
			//		SetCurrents(-1, -1);DelayUS(DEL);
			//		SetCurrents(-1,  0);DelayUS(DEL);
			//		SetCurrents( 0,  0);DelayUS(DEL);

			//		SetCurrents(-1,  0);DelayUS(DEL);
			//		SetCurrents( 0,  1);DelayUS(DEL);
			//		SetCurrents( 1,  0);DelayUS(DEL);
			//		SetCurrents( 0, -1);DelayUS(DEL);
//*****************
void DelayUS(uint32_t us) {
   uint32_t del;
   del = us<<6;
    TIM2->CNT = 0;

    while (TIM2->CNT  < del);
}

void SetCurrents(int8_t W1, int8_t W2) {
	switch(W1){
	case 1:{
		HAL_GPIO_WritePin( IN1A_GPIO_Port , IN1A_Pin,   SET);
		HAL_GPIO_WritePin( IN2A_GPIO_Port , IN2A_Pin, RESET);
		break;
	}
	case 0:{
		HAL_GPIO_WritePin( IN1A_GPIO_Port , IN1A_Pin, RESET);
		HAL_GPIO_WritePin( IN2A_GPIO_Port , IN2A_Pin, RESET);
		break;
	}
	case -1:{
		HAL_GPIO_WritePin( IN1A_GPIO_Port , IN1A_Pin, RESET);
		HAL_GPIO_WritePin( IN2A_GPIO_Port , IN2A_Pin,   SET);
		break;
	}


	}
	switch(W2){
	case 1:{
		HAL_GPIO_WritePin( IN1B_GPIO_Port , IN1B_Pin,   SET);
		HAL_GPIO_WritePin( IN2B_GPIO_Port , IN2B_Pin, RESET);
		break;
	}
	case 0:{
		HAL_GPIO_WritePin( IN1B_GPIO_Port , IN1B_Pin, RESET);
		HAL_GPIO_WritePin( IN2B_GPIO_Port , IN2B_Pin, RESET);
		break;
	}
	case -1:{
		HAL_GPIO_WritePin( IN1B_GPIO_Port , IN1B_Pin, RESET);
		HAL_GPIO_WritePin( IN2B_GPIO_Port , IN2B_Pin,   SET);
		break;
	}


	}
}

void Enable_MB(uint8_t onoff){
	//Enable motor A when onoff=1; disable when onoff =0
	if(onoff ) HAL_GPIO_WritePin( ENB_GPIO_Port , ENB_Pin, SET);
	else HAL_GPIO_WritePin( ENB_GPIO_Port , ENB_Pin, RESET);
}
void Enable_MA(uint8_t onoff){
	//Enable motor B when onoff=1; disable when onoff =0
	if(onoff ) HAL_GPIO_WritePin(ENA_GPIO_Port, ENA_Pin, SET);
	else HAL_GPIO_WritePin( ENA_GPIO_Port , ENA_Pin, RESET);
}




