- switch (ad->dir_state) {
- case DIR_STATE_F:
- if (ad->f_value) {
- if (ad->r_value) {
- ad->dir_state = DIR_STATE_S;
- resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 0);
- resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 0);
- } else {
- ad->dir_state = DIR_STATE_B;
- __servo_motor_test();
- resource_set_motor_driver_L298N_speed(MOTOR_ID_1, -2000);
- resource_set_motor_driver_L298N_speed(MOTOR_ID_2, -2000);
- }
- }
- break;
- case DIR_STATE_B:
- if (ad->r_value) {
- if (ad->f_value) {
- ad->dir_state = DIR_STATE_S;
- resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 0);
- resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 0);
- } else {
- ad->dir_state = DIR_STATE_F;
- __servo_motor_test();
- resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 2000);
- resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 2000);
- }
- }
- break;
- case DIR_STATE_S:
- if (!ad->f_value) {
- ad->dir_state = DIR_STATE_F;
- __servo_motor_test();
- resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 2000);
- resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 2000);
- } else if (!ad->r_value) {
- ad->dir_state = DIR_STATE_B;
- __servo_motor_test();
- resource_set_motor_driver_L298N_speed(MOTOR_ID_1, -2000);
- resource_set_motor_driver_L298N_speed(MOTOR_ID_2, -2000);
- }
- break;
- }