| 116,64 → 116,14 |
|---|
| uint16_t pwm_left = PWM_NOMINAL; |
| uint16_t pwm_right = PWM_NOMINAL; |
| |
| #ifdef CONTROL_FROM_UART |
| uint8_t median; |
| int16_t L_X, L_Y, R_X, R_Y; |
| |
| uint8_t buffer[32]; |
| uint8_t length; |
| uint8_t uart_state = UART_STATE_READ_CMD; |
| uint8_t uart_cmd; |
| while(1) { |
| length = UART_Read(buffer); |
| for (uint8_t i = 0; i < length; i++) { |
| if (uart_state == UART_STATE_READ_CMD) { |
| if (buffer[i] == UART_CMD_RESET) { |
| RESET(); |
| } else { |
| // Read and save first byte (command) |
| uart_cmd = buffer[i]; |
| uart_state = UART_STATE_READ_DATA; |
| } |
| } else if (uart_state == UART_STATE_READ_DATA) { |
| uart_state = UART_STATE_READ_CMD; |
| |
| // Process received data |
| if (uart_cmd == UART_CMD_LEFT_FORWARD) { |
| pwm_left = PWM_NOMINAL + (uint16_t)(buffer[i] * 2); |
| if (buffer[i] != 0) LED_2_On(); |
| else LED_2_Off(); |
| } else if (uart_cmd == UART_CMD_LEFT_BACKWARD) { |
| pwm_left = PWM_NOMINAL - (uint16_t)(buffer[i] * 2); |
| if (buffer[i] != 0) LED_2_On(); |
| else LED_2_Off(); |
| } else if (uart_cmd == UART_CMD_RIGHT_FORWARD) { |
| pwm_right = PWM_NOMINAL + (uint16_t)(buffer[i] * 2); |
| if (buffer[i] != 0) LED_1_On(); |
| else LED_1_Off(); |
| } else if (uart_cmd == UART_CMD_RIGHT_BACKWARD) { |
| pwm_right = PWM_NOMINAL - (uint16_t)(buffer[i] * 2); |
| if (buffer[i] != 0) LED_1_On(); |
| else LED_1_Off(); |
| } |
| |
| if (pwm_left > PWM_MAX) |
| pwm_left = PWM_MAX; |
| if (pwm_left < PWM_MIN) |
| pwm_left = PWM_MIN; |
| |
| if (pwm_right > PWM_MAX) |
| pwm_right = PWM_MAX; |
| if (pwm_right < PWM_MIN) |
| pwm_right = PWM_MIN; |
| |
| PWM_1_Set(pwm_right); |
| PWM_2_Set(pwm_left); |
| } |
| } |
| } |
| #endif |
| |
| #ifdef CONTROL_FROM_CONTROLLER |
| uint8_t median; |
| int16_t L_X, L_Y, R_X, R_Y; |
| while (1) { |
| if (!Controller_Read(&ctrl)) { |
| median = ctrl.REF / 2; |
| 230,7 → 180,56 |
|---|
| |
| LED_1_Off(); |
| LED_2_Off(); |
| |
| // If no controller is attached, switch control to UART |
| while(1) { |
| length = UART_Read(buffer); |
| for (uint8_t i = 0; i < length; i++) { |
| if (uart_state == UART_STATE_READ_CMD) { |
| if (buffer[i] == UART_CMD_RESET) { |
| RESET(); |
| } else { |
| // Read and save first byte (command) |
| uart_cmd = buffer[i]; |
| uart_state = UART_STATE_READ_DATA; |
| } |
| } else if (uart_state == UART_STATE_READ_DATA) { |
| uart_state = UART_STATE_READ_CMD; |
| |
| // Process received data |
| if (uart_cmd == UART_CMD_LEFT_FORWARD) { |
| pwm_left = PWM_NOMINAL + (uint16_t) (buffer[i] * 2); |
| if (buffer[i] != 0) LED_2_On(); |
| else LED_2_Off(); |
| } else if (uart_cmd == UART_CMD_LEFT_BACKWARD) { |
| pwm_left = PWM_NOMINAL - (uint16_t) (buffer[i] * 2); |
| if (buffer[i] != 0) LED_2_On(); |
| else LED_2_Off(); |
| } else if (uart_cmd == UART_CMD_RIGHT_FORWARD) { |
| pwm_right = PWM_NOMINAL + (uint16_t) (buffer[i] * 2); |
| if (buffer[i] != 0) LED_1_On(); |
| else LED_1_Off(); |
| } else if (uart_cmd == UART_CMD_RIGHT_BACKWARD) { |
| pwm_right = PWM_NOMINAL - (uint16_t) (buffer[i] * 2); |
| if (buffer[i] != 0) LED_1_On(); |
| else LED_1_Off(); |
| } |
| |
| if (pwm_left > PWM_MAX) |
| pwm_left = PWM_MAX; |
| if (pwm_left < PWM_MIN) |
| pwm_left = PWM_MIN; |
| |
| if (pwm_right > PWM_MAX) |
| pwm_right = PWM_MAX; |
| if (pwm_right < PWM_MIN) |
| pwm_right = PWM_MIN; |
| |
| PWM_1_Set(pwm_right); |
| PWM_2_Set(pwm_left); |
| } |
| } |
| } |
| } |
| } |
| #endif |
| } |