Motor-Controller/shared/devices/motor_controller.c

#include "motor_controller.h"
#include "putil.h"
#include "stm32l4xx_hal_tim.h"
#include <math.h>

static TIM_HandleTypeDef *_motor_timer = NULL;
void mc_init(TIM_HandleTypeDef *tim)
{
	_motor_timer = tim;
}

// degrees is anything from 0-359
// speed is anything from 0-99
void mc_service(uint16_t degrees, uint8_t speed)
{
	float right_pwm = cos(((float)degrees * (3.1415 / 180.0f)));
	float left_pwm = right_pwm;
	// PDEBUG("| DEGREES | COS    | SIN    | \n");
	// PDEBUG("| %7d | %-5.3f | %-5.3f | \n",
	// 	(int)degrees,
	// 	right_pwm,
	// 	right_pwm);

	HAL_GPIO_WritePin(m1_dir_GPIO_Port, m1_dir_Pin, right_pwm >= 0 ? 0 : 1);
	HAL_GPIO_WritePin(m2_dir_GPIO_Port, m2_dir_Pin, left_pwm >= 0 ? 0 : 1);
	setPWM(_motor_timer, TIM_CHANNEL_2, abs((int)(right_pwm * 100.0)));
	setPWM(_motor_timer, TIM_CHANNEL_4, abs((int)(left_pwm * 100.0)));
}