#include "motor.h" #include #include #include "parameters.h" volatile fix16_t motor_speed; volatile fix16_t servo_position; #define MOTOR_PWM_MAX 600 const PWMConfig pwmconfig_tim4 = { 12000000, // 12 MHz timer, 600 ticks per period => 20kHz PWM MOTOR_PWM_MAX, NULL, {{1, NULL}, {1, NULL}} }; const PWMConfig pwmconfig_tim3 = { 1000000, // 1MHz timer, 10000 ticks per period => 100 Hz servo pulse 10000, NULL, {{1, NULL}} }; void motor_start() { pwmStart(&PWMD4, &pwmconfig_tim4); pwmEnableChannel(&PWMD4, 0, MOTOR_PWM_MAX); pwmEnableChannel(&PWMD4, 1, MOTOR_PWM_MAX); pwmStart(&PWMD3, &pwmconfig_tim3); pwmEnableChannel(&PWMD3, 0, params.servo_center); palSetPad(GPIOB, GPIOB_SERVO_PWR); } void set_motor_speed(fix16_t speed) { if (speed > fix16_one) speed = fix16_one; if (speed < -fix16_one) speed = -fix16_one; if (speed > 0) { pwmEnableChannel(&PWMD4, 0, MOTOR_PWM_MAX); pwmEnableChannel(&PWMD4, 1, MOTOR_PWM_MAX - fix16_mul(MOTOR_PWM_MAX, speed)); } else { pwmEnableChannel(&PWMD4, 0, MOTOR_PWM_MAX - fix16_mul(MOTOR_PWM_MAX, -speed)); pwmEnableChannel(&PWMD4, 1, MOTOR_PWM_MAX); } chSysLock(); motor_speed = speed; chSysUnlock(); } void set_servo_position(fix16_t position) { if (position > fix16_one) position = fix16_one; if (position < -fix16_one) position = -fix16_one; int pos = params.servo_center + fix16_mul(params.servo_delta, position); pwmEnableChannel(&PWMD3, 0, pos); chSysLock(); servo_position = position; chSysUnlock(); } void set_servo_power(bool on) { if (on) palSetPad(GPIOB, GPIOB_SERVO_PWR); else palClearPad(GPIOB, GPIOB_SERVO_PWR); }