118 lines
3.0 KiB
C
118 lines
3.0 KiB
C
|
|
|
|
#include "motor_speed.h"
|
|
#include "motor_direction.h"
|
|
#include "motor_pid.h"
|
|
|
|
void
|
|
motor_control_task(void *params)
|
|
{
|
|
car_struct_t *car_struct = (car_struct_t *)params;
|
|
for (;;)
|
|
{
|
|
set_wheel_direction(DIRECTION_FORWARD);
|
|
set_wheel_speed_synced(90u, car_struct);
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(10000));
|
|
|
|
revert_wheel_direction();
|
|
set_wheel_speed_synced(90u, car_struct);
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(10000));
|
|
}
|
|
}
|
|
|
|
void
|
|
launch(car_struct_t *car_strut)
|
|
{
|
|
// isr to detect right motor slot
|
|
gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
|
|
gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, h_wheel_sensor_isr_handler);
|
|
|
|
// isr to detect left motor slot
|
|
gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
|
|
gpio_add_raw_irq_handler(SPEED_PIN_LEFT, h_wheel_sensor_isr_handler);
|
|
|
|
irq_set_enabled(IO_IRQ_BANK0, true);
|
|
|
|
// PID timer
|
|
struct repeating_timer pid_timer;
|
|
add_repeating_timer_ms(-50, repeating_pid_handler, car_strut, &pid_timer);
|
|
// add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer);
|
|
|
|
// Left wheel
|
|
//
|
|
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
|
|
xTaskCreate(monitor_wheel_speed_task,
|
|
"monitor_left_wheel_speed_task",
|
|
configMINIMAL_STACK_SIZE,
|
|
(void *)car_strut->p_left_motor,
|
|
WHEEL_SPEED_PRIO,
|
|
&h_monitor_left_wheel_speed_task_handle);
|
|
|
|
// Right wheel
|
|
//
|
|
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
|
|
xTaskCreate(monitor_wheel_speed_task,
|
|
"monitor_wheel_speed_task",
|
|
configMINIMAL_STACK_SIZE,
|
|
(void *)car_strut->p_right_motor,
|
|
WHEEL_SPEED_PRIO,
|
|
&h_monitor_right_wheel_speed_task_handle);
|
|
|
|
// control task
|
|
TaskHandle_t h_motor_turning_task_handle = NULL;
|
|
xTaskCreate(motor_control_task,
|
|
"motor_turning_task",
|
|
configMINIMAL_STACK_SIZE,
|
|
(void *)car_strut,
|
|
WHEEL_CONTROL_PRIO,
|
|
&h_motor_turning_task_handle);
|
|
|
|
vTaskStartScheduler();
|
|
}
|
|
|
|
int
|
|
main(void)
|
|
{
|
|
stdio_usb_init();
|
|
|
|
sleep_ms(4000);
|
|
printf("Test started!\n");
|
|
|
|
motor_pid_t g_pid = {
|
|
.kp_value = 600.f,
|
|
.ki_value = 66.67f,
|
|
.kd_value = 1350.f,
|
|
.use_pid = true,
|
|
};
|
|
|
|
motor_t g_motor_left = {
|
|
.pwm.level = 0u,
|
|
.pwm.channel = PWM_CHAN_A,
|
|
.speed.distance_cm = 0.0f,
|
|
.p_sem = &g_left_sem,
|
|
.p_pid = &g_pid,
|
|
};
|
|
|
|
motor_t g_motor_right = {
|
|
.pwm.level = 0u,
|
|
.pwm.channel = PWM_CHAN_B,
|
|
.speed.distance_cm = 0.0f,
|
|
.p_sem = &g_right_sem,
|
|
.p_pid = &g_pid,
|
|
};
|
|
|
|
car_struct_t car_struct = {
|
|
.p_left_motor = &g_motor_left,
|
|
.p_right_motor = &g_motor_right,
|
|
};
|
|
|
|
motor_init(&car_struct);
|
|
|
|
launch(&car_struct);
|
|
|
|
// for(;;);
|
|
|
|
return (0);
|
|
} |