You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
29 lines
831 B
C
29 lines
831 B
C
#include "motor_controller.h"
|
|
#include <math.h>
|
|
#include "putil.h"
|
|
#include "stm32l4xx_hal_tim.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)));
|
|
}
|