Skip to content

Instantly share code, notes, and snippets.

@sprae sprae/radar.ino Secret
Created Nov 16, 2012

Embed
What would you like to do?
Program sterujący radarem ultradźwiękowym
// Program Radar dla Arduino
#include <Servo.h>
#include <SoftSerial.h>
// Cyfrowe PINy do których podłączyłeś czujnik
#define SENSOR_PIN_TX 2
#define SENSOR_PIN_RX 3
// Cyfrowy PIN do którego podłączyłęś sygnał sterujący PWM serwomechanizmu
#define SERVO_PIN 4
// Zmienna przechowująca aktualny kierune w jakim ma być skierowany czujnik
int direction = 90;
// Zmienna przechowująca informacje o tym czy zarządano podania odległości
byte read_distance = 0;
// Deklaracja obiektu komunikacji szeregowej z czujnikiem
SoftwareSerial sensor_serial(SENSOR_PIN_TX, SENSOR_PIN_RX, true);
// Deklaracja obiektu sterowania serwomechanizmem
Servo sensor_servo;
// Funkcja zwracająca odległość w calach
int get_distance()
{
char data[4] = "000";
char header = ' ';
while (header != 'R')
header = sensor_serial.read();
while (sensor_serial.available() < 4);
for (byte i = 0; i < 3; i++)
data[i] = sensor_serial.read();
header = sensor_serial.read();
if (header != 13)
return -1;
return atoi(data);
}
// Funkcja wysyłająca liczbę w formacie "nnn" (3-cyfrowym)
void send_number(int number)
{
if (number < 100)
Serial.print("0");
if (number < 10)
Serial.print("0");
Serial.print(number, DEC);
}
// Funkcja wysyłająca do komputera dane o odległości
void send_data(int distance)
{
Serial.print("D");
send_number(distance);
Serial.println("");
}
// Funkcja odbierająca rozkazy z komputera
// Argument: timeout - czas w [ms] po którym przerywane są próby odczytu
int get_direction(unsigned long timeout=1000)
{
char data[4] = "000";
char header = ' ';
// Zapamiętanie aktualnego czasu
unsigned long time = millis();
// Pobranie nagłówka
header = Serial.read();
// Sprawdzenie czy jest to żadanie odległości
if (header == 'D')
{
read_distance = 1;
return -4;
}
// Sprawdzenie czy jest to nagłówek rozkazu kierunku
if (header != 'R')
return -1; // Jeśli inny to zwrócenie błędu o kodzie -1
// Oczekiwanie na pojawienie się 3 bajtów danych
while (Serial.available() < 4)
{
// Sprawdzenie czy upłynął dopuszczalny czas oczekiwania
if ((millis() - time) > timeout)
return -2; // Jeśli tak to zwrócenie kodu błędu -2
}
// Odczyt cyfr odległości
for (byte i = 0; i < 3; i++)
data[i] = Serial.read();
// Sprawdzenie poprawności zakończenia nadawania danych
header = Serial.read();
if (header != '\n')
return -3; // Zwrócenie -3 oznacza niepoprawne zakończenie
return atoi(data);
}
// Funkcja sprawdza czy wysłano rozkaz o obrocie i wykonanie go
void rotate()
{
// Sprawdzenie czy w buforze czekają jakieś dane
while (Serial.available())
{
direction = get_direction();
if (direction >= 0)
// Jeśli dane były poprawne to ustawienie kąta serwomechanizmu
sensor_servo.write(direction);
}
}
// Czyszczenie bufora komunikacji programowej z czujnikiem
void clear_buffer()
{
while (sensor_serial.available())
sensor_serial.flush();
}
// Funkcja zwraca odległość jeśli tego zażądano
void distance()
{
// Sprawdzenie czy żądano zwrócenia odległości
if (read_distance == 1)
{
// Aktywacja czujnika
digitalWrite(SENSOR_PIN_RX, HIGH);
// 20 us dla czujnika na określenie odległości
delayMicroseconds(20);
// Oczekiwanie na dane z czujnika
while (sensor_serial.available())
{
int distance = get_distance();
if (distance > 0)
{
// Jeśli czujnik zwrócił poprawne dane to wysłanie ich do komputera
send_data(distance);
// Skasowanie żądania o podanie odległości
read_distance = 0;
// Zatrzymanie pracy czujnika
digitalWrite(SENSOR_PIN_RX, LOW);
// Wyczyszczenie bufora
clear_buffer();
}
}
}
}
void setup()
{
Serial.begin(9600);
pinMode(SENSOR_PIN_TX, INPUT);
pinMode(SENSOR_PIN_RX, OUTPUT);
digitalWrite(SENSOR_PIN_RX, LOW);
sensor_serial.begin(9600);
// Ustalenie pinu sterującego serwomechanizm
sensor_servo.attach(SERVO_PIN);
// Ustawienie serwomechanizmu na środek
sensor_servo.write(90);
// Odczekanie na ustawienie serwomechanizmu i uruchomienie czujnika
delay(1000);
// Czyszczenie bufora
clear_buffer();
}
void loop()
{
rotate();
distance();
delay(10);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.