EX11-使用 HC-SR04 做 Mini Radar

// PicoGame Mini Radar by Mason 2024/4/29

// Reference : https://arduinopoint.com/arduino-radar-project/

// Processing program convert to ILI9341 display 

// HC-SR04 Trig - P12, ECHO - P13 

// SG90 - P5

// 超音波接 PicoGame 左上角的擴充腳位，SG90 接右上角的擴充腳位

#include <TFT_eSPI.h> 

#include <SPI.h>

TFT_eSPI tft; 

// Includes the Servo library

#include <Servo.h>. 

// Defines Tirg and Echo pins of the Ultrasonic Sensor

const int trigPin = 12;

const int echoPin = 13;

// Variables for the duration and the distance

long duration;

int distance;

String angle = "";

String noObject;

int object_distance; // 只能是整數

int iAngle, iDistance;

String old_detect =""; 

int old_angle =0;

int old_distance =0; 

int old_object_distance;

int scan_length = floor(tft.height()* 0.5); 

float obj_x1,obj_y1,obj_x2,obj_y2,obj_x3,obj_y3; 

Servo myServo; // Creates a servo object for controlling the servo motor

void drawRadar() {

  tft.drawEllipse(tft.width() / 2, tft.height(), tft.width() * 0.95 *0.5, tft.width() * 0.95*0.5, TFT_GREEN);

  tft.drawEllipse(tft.width() / 2, tft.height(), tft.width() * 0.73*0.5, tft.width() * 0.73*0.5, TFT_GREEN);

  tft.drawEllipse(tft.width() / 2, tft.height(), tft.width() * 0.51*0.5, tft.width() * 0.51*0.5, TFT_GREEN);

  tft.drawEllipse(tft.width() / 2, tft.height(), tft.width() * 0.29*0.5, tft.width() * 0.29*0.5, TFT_GREEN);

  tft.drawLine(0, tft.height(), tft.width(), tft.height(), TFT_GREEN);

  tft.drawLine(tft.width() / 2, tft.height(), (tft.width() / 2 - (tft.width() / 2) * cos(radians(30))), (tft.height() - (tft.width() / 2) * sin(radians(30))), TFT_GREEN);

  tft.drawLine(tft.width() / 2, tft.height(), (tft.width() / 2 - (tft.width() / 2) * cos(radians(60))), (tft.height() - (tft.width() / 2) * sin(radians(60))), TFT_GREEN);

  tft.drawLine(tft.width() / 2, tft.height(), (tft.width() / 2 - (tft.width() / 2) * cos(radians(90))), (tft.height() - (tft.width() / 2) * sin(radians(90))), TFT_GREEN);

  tft.drawLine(tft.width() / 2, tft.height(), (tft.width() / 2 - (tft.width() / 2) * cos(radians(120))), (tft.height() - (tft.width() / 2) * sin(radians(120))), TFT_GREEN);

  tft.drawLine(tft.width() / 2, tft.height(), (tft.width() / 2 - (tft.width() / 2) * cos(radians(150))), (tft.height() - (tft.width() / 2) * sin(radians(150))), TFT_GREEN);

}

void drawObject() {  // 畫紅線

  object_distance = floor(iDistance * scan_length *0.025);

  if (iDistance < 40) {

  obj_x1 = tft.width() / 2+ object_distance *cos(radians(iAngle)); 

  obj_y1 = tft.height()-1- object_distance *sin(radians(iAngle));

  obj_x2 = tft.width() / 2+ scan_length *cos(radians(iAngle-2));

  obj_y2 = tft.height()-1- scan_length *sin(radians(iAngle-2));

  obj_x3 = tft.width() / 2+ scan_length *cos(radians(iAngle+1));

  obj_y3 = tft.height()-1- scan_length *sin(radians(iAngle+1));

  tft.fillTriangle(obj_x1,obj_y1,obj_x2,obj_y2,obj_x3,obj_y3, TFT_RED);

  }

}

void drawLine_up() {

  tft.drawLine(0, tft.height()-1, tft.width(),tft.height()-1, TFT_GREEN);

  tft.fillTriangle(tft.width() / 2, tft.height()+1,tft.width() / 2+(scan_length+2) * cos(radians(iAngle-13)), tft.height()-1-(scan_length+2)* sin(radians(iAngle-13)), tft.width() / 2+(scan_length+2) * cos(radians(iAngle-15)), tft.height()-1-(scan_length+2)* sin(radians(iAngle-15)), TFT_BLACK);

  tft.drawLine(tft.width() / 2, tft.height()-1, tft.width() / 2+scan_length * cos(radians(iAngle)), tft.height()-1-scan_length* sin(radians(iAngle)), TFT_GREEN);

}

void drawLine_dn() {

  tft.drawLine(0, tft.height()-1, tft.width(),tft.height()-1, TFT_GREEN);

  tft.fillTriangle(tft.width() / 2, tft.height()+1,tft.width() / 2+(scan_length+2) * cos(radians(iAngle+13)), tft.height()-1-(scan_length+2)* sin(radians(iAngle+13)), tft.width() / 2+(scan_length+2) * cos(radians(iAngle+15)), tft.height()-1-(scan_length+2)* sin(radians(iAngle+15)), TFT_BLACK);

  tft.drawLine(tft.width() / 2, tft.height()-1, tft.width() / 2+scan_length * cos(radians(iAngle)), tft.height()-1-scan_length* sin(radians(iAngle)), TFT_GREEN);

}

void drawText() {

  if(iDistance>40) {

    noObject = "Nothing";

  }

  else {

    noObject = "Detect";

  }

  tft.setTextColor(TFT_WHITE); 

  tft.setTextSize(1); 

  tft.setCursor(tft.width() - (tft.width() * 0.3854), tft.height() - (tft.height() * 0.0833));

  tft.print("10cm");

  tft.setCursor(tft.width() - (tft.width() * 0.281), tft.height() - (tft.height() * 0.0833));

  tft.print("20cm");

  tft.setCursor(tft.width() - (tft.width() * 0.177), tft.height() - (tft.height() * 0.0833));

  tft.print("30cm");

  tft.setCursor(tft.width() - (tft.width() * 0.0729), tft.height() - (tft.height() * 0.0833));

  tft.print("40cm");

  tft.setTextSize(2); 

  tft.setTextColor(TFT_WHITE); 

  tft.setCursor(0, 0);

  tft.print("Object:");

  tft.setCursor(100,0);

  tft.setTextColor(TFT_BLACK); 

  tft.print(old_detect);

  tft.setCursor(100,0);

  tft.setTextColor(TFT_WHITE); 

  tft.print(noObject);

  old_detect = noObject; 

  tft.setCursor(0, 18);

  tft.print("Angle: ");

  tft.setCursor(80,18);

  tft.setTextColor(TFT_BLACK);

  tft.print(old_angle);

  tft.setCursor(80,18);

  tft.setTextColor(TFT_WHITE);

  tft.print(iAngle);

  old_angle = iAngle; 

  tft.setCursor(130,18);

  tft.print("Degree");

  tft.setCursor(0,36);

  tft.print("Distance: ");

    tft.setCursor(120,36);

    tft.setTextColor(TFT_BLACK); 

    tft.print(old_distance);

    tft.setCursor(120,36);

    tft.setTextColor(TFT_WHITE);  

    tft.print(iDistance);

    old_distance = iDistance; 

  tft.setCursor(150,36);

  tft.print(" cm");

  tft.setTextSize(1); 

  tft.setTextColor(TFT_RED);

  tft.setCursor(tft.width() / 2 + (tft.width() / 2) * cos(radians(30)), tft.height() - (tft.height() * 0.0907) - (tft.width() / 2) * sin(radians(30)));

  tft.print("30d");

  tft.setCursor(tft.width() / 2 + (tft.width() / 2) * cos(radians(60))-5, tft.height() - (tft.height() * 0.0888) - (tft.width() / 2) * sin(radians(60)));

  tft.print("60d");

  tft.setCursor(tft.width() / 2 + (tft.width() / 2) * cos(radians(90))-10, tft.height() - (tft.height() * 0.0833) - (tft.width() / 2) * sin(radians(90)));

  tft.print("90d");

  tft.setCursor(tft.width() / 2 + (tft.width() / 2) * cos(radians(120))-15, tft.height() - (tft.height() * 0.07129) - (tft.width() / 2) * sin(radians(120)));

  tft.print("120d");

  tft.setCursor(tft.width() / 2 + (tft.width() / 2) * cos(radians(150))-20, tft.height() - (tft.height() * 0.0574) - (tft.width() / 2) * sin(radians(150)));

  tft.print("150d");

}

void setup() {

  tft.begin(); // 初始化 TFT_eSPI

  tft.setRotation(1); 

  tft.invertDisplay(false);

  tft.fillScreen(TFT_BLACK); 

  tft.setTextColor(TFT_GREEN); 

  tft.setTextSize(2); 

  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output

  pinMode(echoPin, INPUT); // Sets the echoPin as an Input

  Serial.begin(9600);

  myServo.attach( 5, 400, 2600); // Defines on which pin is the servo motor attached

}

void loop() {

  // rotates the servo motor from 15 to 165 degrees

  for(int i=15;i<=165;i++){  

   myServo.write(i);

   delay(30);

   distance = calculateDistance();// Calls a function for calculating the distance measured by the Ultrasonic sensor for each degree

   iAngle = i;

   iDistance = distance;

   drawRadar();

   drawLine_up();

   drawObject();

   drawText();

   Serial.print(i); // Sends the current degree into the Serial Port

   Serial.print(","); // Sends addition character right next to the previous value needed later in the Processing IDE for indexing

   Serial.print(distance); // Sends the distance value into the Serial Port

   Serial.println("."); // Sends addition character right next to the previous value needed later in the Processing IDE for indexing

 }

  // Repeats the previous lines from 165 to 15 degrees

  for(int i=165;i>15;i--){  

   myServo.write(i);

   delay(30);

   distance = calculateDistance();

   iAngle = i;

   iDistance = distance;

   drawRadar(); 

   drawLine_dn();

   drawObject();

   drawText();

   Serial.print(i);

   Serial.print(",");

   Serial.print(distance);

   Serial.println(".");

  }

}

// Function for calculating the distance measured by the Ultrasonic sensor

int calculateDistance(){ 

  digitalWrite(trigPin, LOW); 

  delayMicroseconds(2);

  // Sets the trigPin on HIGH state for 10 micro seconds

  digitalWrite(trigPin, HIGH); 

  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH); // Reads the echoPin, returns the sound wave travel time in microseconds

  distance= duration*0.034/2;

  if (distance == 0) {

    distance = 200; // connect fail 

  }

  return distance;

}