[PROJEKT] Prosty robot z czujnikiem odległości Sharp GP2Y0D810Z0F
Nasz robot będzie jeździł do przodu, a gdy pojawi się przed nim przeszkoda w odległości mniejszej niż 10 cm zatrzyma się, następnie obróci się, aby uniknąć zderzenia i będzie kontynuował jazdę (pełen kod programu znajduje się w sekcji do pobrania na końcu artykułu).
Pierwszą czynnością jest deklaracja zmiennej przeszkoda. Przechowywana jest w niej informacja, czy przed robotem znajduje się przeszkoda (wartość 1), czy nie (wartość 0).
/* USER CODE BEGIN PV */ /* Private variables ---------------------------------------------------------*/ uint16_t przeszkoda = 0; /* USER CODE END PV */
Następnie w funkcji obsługi przerwania sprawdzana jest wartość zmiennej przeszkoda. Jeżeli jej wartość jest równa 0, zostanie ustawiona na 1:
/* USER CODE BEGIN PFP */ /* Private function prototypes -----------------------------------------------*/ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { if (flaga_przeszkoda==0) { flaga_przeszkoda = 1; } } /* USER CODE END PFP */
Teraz należy uruchomić oba timery, które są użyte do sterowania prędkością silników.
/* USER CODE BEGIN 2 */ HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); /* USER CODE END 2 */
W pętli głównej programu znajduje się kod, dzięki któremu robot porusza się do przodu z 1/4 pełnej prędkości (25% wypełnienia PWM). Następnie w instrukcja warunkowa if sprawdza wartość zmiennej przeszkoda. Jeżeli jej wartość wynosi 1, robot zatrzymuje się na 1 sekundę, obraca się w prawo przez 0,6 sekund, znowu się zatrzymuje i następnie robot jedzie do przodu, aż do momentu, gdy nie pojawi się przed nim jakaś przeszkoda. Zmienna przeszkoda przyjmuje wtedy wartość 0.
while (1) { /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ // Robot porusza się do przodu z 25 % wypełnieniem PWM TIM3->CCR1 = 25; HAL_GPIO_WritePin(GPIOB,M1_Pin,GPIO_PIN_SET); TIM2->CCR3 = 25; HAL_GPIO_WritePin(GPIOA,M2_Pin,GPIO_PIN_SET); if (przeszkoda==1) // Sprawdzanie zmiennej flaga_przeszkoda { // Zatrzymanie robota na 1 sekunde TIM3->CCR1 = 0; HAL_GPIO_WritePin(GPIOB,M1_Pin,GPIO_PIN_SET); TIM2->CCR3 = 0; HAL_GPIO_WritePin(GPIOA,M2_Pin,GPIO_PIN_SET); HAL_Delay(1000); // Obrót robot w lewo przez 0,6 sekund TIM3->CCR1 =20; HAL_GPIO_WritePin(GPIOB,M1_Pin,GPIO_PIN_SET); TIM2->CCR3 = 20; HAL_GPIO_WritePin(GPIOA,M2_Pin,GPIO_PIN_RESET); HAL_Delay(600); // Zatrzymanie robota na 1 sekunde TIM3->CCR1 = 0; HAL_GPIO_WritePin(GPIOB,M1_Pin,GPIO_PIN_SET); TIM2->CCR3 = 0; HAL_GPIO_WritePin(GPIOA,M2_Pin,GPIO_PIN_SET); HAL_Delay(1000); przeszkoda = 0; // Ustawienie wartości 0 dla zmiennej przeszkoda } }
Działanie robota zostało pokazane na poniższym materiale wideo: