/*
 * Stepper_Control.c
 *
 *  Created on: May 5, 2022
 *      Author: Jano
 */
//includes
#include <stdio.h>
#include "main.h"
#include "stm32l4xx.h"
#include "LCD.h"
#include "Stepper_Control.h"

extern uint8_t len_meas;
extern uint8_t pulse_meas;

extern TIM_HandleTypeDef htim1;

//init motor registers
void Motor_INIT(){
	HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 1);	//Disable X motor driver
	HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 1);	//Disable Z motor driver
	HAL_Delay(1);

	//Enable timer update interrupt
	__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
	//Set PWM to 50%
	TIM1->CCR4 = 4;
	TIM1->CCR1 = 4;

}

void Motor_CALIBRATE(){
	lcd_clear();
	lcd_set_line_col(1, 3);
	lcd_send_string("CALIBRATING");

	//Calibrate Z
	HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 0);	//Enable Z motor driver
	HAL_Delay(1);

	HAL_GPIO_WritePin(DIR_Z_GPIO_Port, DIR_Z_Pin, 0); //Dir = UP
	HAL_Delay(1);

	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); //Motor PWM Start

	while(HAL_GPIO_ReadPin(CAL_SW_Z_GPIO_Port, CAL_SW_Z_Pin)){	//While switch is off - inverted logic

		if(!HAL_GPIO_ReadPin(CAL_SW_Z_GPIO_Port, CAL_SW_Z_Pin)){	//If switch is on
			HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);	//Stop motor PWM
			HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 1);	//Disable Z motor driver
		}

	}

	HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1); //Stop motor PWM
	HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 1); //Disable Z motor driver

	len_meas = 0;
	pulse_meas = 0;

	//Move Z 1mm from switch
	HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 0);
	HAL_Delay(1);

	HAL_GPIO_WritePin(DIR_Z_GPIO_Port, DIR_Z_Pin, 1); //Dir = D
	HAL_Delay(1);

	HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_1);

	while(len_meas < 1){

	}

	HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_1);

	HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 1);
	HAL_Delay(1);

	len_meas = 0;
	pulse_meas = 0;


	//Calibrate X
	HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 0);	//Enable X motor driver
	HAL_Delay(1);

	HAL_GPIO_WritePin(DIR_X_GPIO_Port, DIR_X_Pin, 0);	//Dir = L
	HAL_Delay(1);

	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); //Motor PWM Start

	while((HAL_GPIO_ReadPin(CAL_SW_X_GPIO_Port, CAL_SW_X_Pin))){	//While switch is off - inverted logic

		if(!HAL_GPIO_ReadPin(CAL_SW_X_GPIO_Port, CAL_SW_X_Pin)){	//If switch is on
			HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4);	//Stop motor PWM
			HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 1);	//Disable X motor driver

		}
	}
	HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4);	//Stop motor PWM
	HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 1);	//Disable X motor driver

	//Zero measurement variables
	len_meas = 0;
	pulse_meas = 0;

	// Move X 1mm from switch
	HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 0);
	HAL_Delay(5);

	HAL_GPIO_WritePin(DIR_X_GPIO_Port, DIR_X_Pin, 1); // Dir = L
	HAL_Delay(5);

	HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_4);

	while(len_meas < 1){

	}

	HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_4);

	HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 1);
	HAL_Delay(1);

	len_meas = 0;
	pulse_meas = 0;

	lcd_clear();
}


//set direction and length to move in mm
void Motor_MOVE_X(char dir_set, uint16_t len_set){
	if(HAL_GPIO_ReadPin(CAL_SW_X_GPIO_Port, CAL_SW_X_Pin)){
		HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 0);
		HAL_Delay(1);

		if(dir_set == 'R'){
			HAL_GPIO_WritePin(DIR_X_GPIO_Port, DIR_X_Pin, 0);
		}

		if(dir_set == 'L'){
			HAL_GPIO_WritePin(DIR_X_GPIO_Port, DIR_X_Pin, 1);
		}
		HAL_Delay(1);

		HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_4);

		while(len_meas < len_set){

		}

		HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_4);

		HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 1);
		HAL_Delay(1);
		len_meas = 0;
		pulse_meas = 0;
	}

}

void Motor_MOVE_Z(char dir_set, uint16_t len_set){
	if(HAL_GPIO_ReadPin(CAL_SW_Z_GPIO_Port, CAL_SW_Z_Pin)){
		HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 0);
		HAL_Delay(1);

		if(dir_set == 'U'){
			HAL_GPIO_WritePin(DIR_Z_GPIO_Port, DIR_Z_Pin, 0);
		}

		if(dir_set == 'D'){
			HAL_GPIO_WritePin(DIR_Z_GPIO_Port, DIR_Z_Pin, 1);
		}
		HAL_Delay(1);

		HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_1);

		while(len_meas < len_set){

		}

		HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_1);

		HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 1);
		HAL_Delay(1);
		len_meas = 0;
		pulse_meas = 0;
	}
}

void Motor_POSITION_INIT(){
	//Calibrate Z
	HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 0);
	HAL_Delay(1);

	HAL_GPIO_WritePin(DIR_Z_GPIO_Port, DIR_Z_Pin, 0);
	HAL_Delay(1);

	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);

	while(HAL_GPIO_ReadPin(CAL_SW_Z_GPIO_Port, CAL_SW_Z_Pin)){

		if(!HAL_GPIO_ReadPin(CAL_SW_Z_GPIO_Port, CAL_SW_Z_Pin)){
			HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
			HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 1);
		}
	}

	HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
	HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 1);

	len_meas = 0;
	pulse_meas = 0;

	//Move Z 1mm from switch
	HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 0);
	HAL_Delay(1);

	HAL_GPIO_WritePin(DIR_Z_GPIO_Port, DIR_Z_Pin, 1); //Dir = D
	HAL_Delay(1);


	HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_1);

	while(len_meas < 1){

	}

	HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_1);

	HAL_GPIO_WritePin(ENA_Z_GPIO_Port, ENA_Z_Pin, 1);
	HAL_Delay(1);

	len_meas = 0;
	pulse_meas = 0;

	//calibrate X
	HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 0);
	HAL_Delay(1);

	HAL_GPIO_WritePin(DIR_X_GPIO_Port, DIR_X_Pin, 0);
	HAL_Delay(1);

	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);

	while((HAL_GPIO_ReadPin(CAL_SW_X_GPIO_Port, CAL_SW_X_Pin))){

		if(!HAL_GPIO_ReadPin(CAL_SW_X_GPIO_Port, CAL_SW_X_Pin)){
			HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4);
			HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 1);

		}
	}
	HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4);
	HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 1);

	len_meas = 0;
	pulse_meas = 0;

	// Move X 1mm from switch
	HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 0);
	HAL_Delay(1);

	HAL_GPIO_WritePin(DIR_X_GPIO_Port, DIR_X_Pin, 1); // Dir = L

	HAL_Delay(1);

	HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_4);

	while(len_meas < 1){

	}

	HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_4);

	HAL_GPIO_WritePin(ENA_X_GPIO_Port, ENA_X_Pin, 1);
	HAL_Delay(1);

	len_meas = 0;
	pulse_meas = 0;
}






