// 2022/08/14
//MiniRadar Ultrasonic wave radar

#include <Servo.h> 
#include <SPI.h>
#include "Ucglib.h"   //Display library file If the library does not exist, press Ctrl+Shift+I to search for ucglib from the library manager and install it

#define  trigPin   6          //Trig port of ultrasonic module 6#
#define  echoPin   5          //echo port of ultrasonic module 5#
#define  ServoPin  3          //Base steering gear port 3#
int Ymax = 128;               //The number of portrait pixels on the screen
int Xmax = 160;               //The number of landscape pixels on the screen
int Xcent = Xmax / 2;         //x median
int base = 118;               //Height of base line
int scanline = 105;           //Radar scan line length

Servo baseServo; 
Ucglib_ST7735_18x128x160_HWSPI ucg(/*cd=*/ 9, /*cs=*/ 10, /*reset=*/ 8);                                                                                                                                                                                                                                                                                                                

void setup(void)
{
 
      ucg.begin(UCG_FONT_MODE_SOLID); //Initialize the screen
      ucg.setRotate90();              //Set to landscape if the screen is displayed in the opposite direction, you can modify the function setRotate90 or setRotate270
      
      pinMode(trigPin, OUTPUT);       //Set the trigPin port to output
      pinMode(echoPin, INPUT);        //Set the echoPin port to input
      Serial.begin(115200);           //Set the serial port transmission rate
      baseServo.attach(ServoPin);     //Initialize the steering gear
    
      //Welcome screen
      ucg.setFontMode(UCG_FONT_MODE_TRANSPARENT);
      ucg.setColor(0, 0, 100, 0);
      ucg.setColor(1, 0, 100, 0);
      ucg.setColor(2, 20, 20,20);
      ucg.setColor(3, 20, 20, 20);
      ucg.drawGradientBox(0, 0, 160, 128);
      ucg.setPrintDir(0);
      ucg.setColor(0, 5, 0);
      ucg.setPrintPos(27,42);
      ucg.setFont(ucg_font_logisoso18_tf);  
      ucg.print("Mini Radar");
      ucg.setColor(0, 255, 0);
      ucg.setPrintPos(25,40);
      ucg.print("Mini Radar");
      ucg.setFont(ucg_font_helvB08_tf);
      ucg.setColor(20, 255, 20);
      ucg.setPrintPos(40,100);
      ucg.print("Testing...");
      baseServo.write(90);
    
      //Test the running condition of the base, paying attention to the position and rotation attitude of the base, whether there is stuck (or wire entanglement).
      for(int x=0;x<180;x+=5)
          { baseServo.write(x);
            delay(50);
          }
      ucg.print("OK!");
      delay(500);
      ucg.setColor(0,0, 0, 0);
      ucg.setColor(1,0, 0, 0);
      ucg.setColor(2,0, 0, 0);
      ucg.setColor(3,0, 0, 0);
      ucg.drawGradientBox(0, 0, 160, 128);
      delay(400);
    
      //清屏 clearScreen
      //ucg.clearScreen();
      cls();
      ucg.setFontMode(UCG_FONT_MODE_SOLID);
      ucg.setFont(ucg_font_orgv01_hr);
  
}


void cls()
{
  //清屏 clearScreen
  ucg.setColor(0, 0, 0, 0);
  for(int s=0;s<128;s++)
  {
    ucg.drawHLine(0,s,160);
    delay(4);
  }
    
  //ucg.drawBox(0, 0, 160, 60);

}


int calculateDistance()
{ 
      long duration;
      digitalWrite(trigPin, LOW); 
      delayMicroseconds(2);
      digitalWrite(trigPin, HIGH); 
      delayMicroseconds(10);
      digitalWrite(trigPin, LOW);
      //Read the propagation time of echoPin returned sound wave (subtle)
      duration = pulseIn(echoPin, HIGH);
      //Convert the echo time to a distance value
      return duration*0.034/2;
}

void fix_font() 
{
      ucg.setColor(0, 180, 0);
      ucg.setPrintPos(70,14);
      ucg.print("1.00");
      ucg.setPrintPos(70,52);
      ucg.print("0.50");
      ucg.setPrintPos(70,90);
      ucg.print("0.25");
}

void fix()
{

      ucg.setColor(0, 40, 0);
      //Draw the baseline disk
      ucg.drawDisc(Xcent, base+1, 3, UCG_DRAW_ALL); 
      ucg.drawCircle(Xcent, base+1, 115, UCG_DRAW_UPPER_LEFT);
      ucg.drawCircle(Xcent, base+1, 115, UCG_DRAW_UPPER_RIGHT);
      ucg.drawCircle(Xcent, base+1, 78, UCG_DRAW_UPPER_LEFT);
      ucg.drawCircle(Xcent, base+1, 78, UCG_DRAW_UPPER_RIGHT);
      ucg.drawCircle(Xcent, base+1, 40, UCG_DRAW_UPPER_LEFT);
      ucg.drawCircle(Xcent, base+1, 40, UCG_DRAW_UPPER_RIGHT);
      ucg.drawLine(0, base+1, Xmax,base+1);
     
      ucg.setColor(0, 120, 0);
      //Draw scale meter
       for(int i= 40;i < 140; i+=2)
       {

        if (i % 10 == 0) 
          ucg.drawLine(105*cos(radians(i))+Xcent,base - 105*sin(radians(i)) , 113*cos(radians(i))+Xcent,base - 113*sin(radians(i)));
        else
        
         ucg.drawLine(110*cos(radians(i))+Xcent,base - 110*sin(radians(i)) , 113*cos(radians(i))+Xcent,base - 113*sin(radians(i)));
       }
          
       //Draw some decorative designs 
       ucg.setColor(0,200,0);
       ucg.drawLine(0,0,0,18);
       for(int i= 0;i < 5; i++)
       {
          ucg.setColor(0,random(200)+50,0);
          ucg.drawBox(2,i*4,random(14)+2,3);
       }

       ucg.setColor(0,180,0);
       ucg.drawFrame(146,0,14,14);
       ucg.setColor(0,60,0);
       ucg.drawHLine(148,0,10);
       ucg.drawVLine(146,2,10);
       ucg.drawHLine(148,13,10);
       ucg.drawVLine(159,2,10);
        
       ucg.setColor(0,220,0);
       ucg.drawBox(148,2,4,4);
       ucg.drawBox(148,8,4,4);
       ucg.drawBox(154,8,4,4);
       ucg.setColor(0,100,0);
       ucg.drawBox(154,2,4,4);

       ucg.setColor(0,90,0);
       ucg.drawTetragon(62,123,58,127,98,127,102,123);
       ucg.setColor(0,160,0);
       ucg.drawTetragon(67,123,63,127,93,127,97,123);
       ucg.setColor(0,210,0);
       ucg.drawTetragon(72,123,68,127,88,127,92,123);
}



void loop(void)
{
  
  int distance;
  
  fix(); 
  fix_font();  //Repaint the screen background elements

  for (int x=180; x > 4; x-=2){       //Base steering gear circulates from 0 to 180 degrees
     
      baseServo.write(x);             //Adjust the steering gear Angle
      
      //Draw the radar scan line
      int f = x - 4; 
      ucg.setColor(0, 255, 0);
      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
      f+=2;
      ucg.setColor(0, 128, 0);
      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
      f+=2;
      ucg.setColor(0, 0, 0);
      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
      ucg.setColor(0,200, 0);
      //ranging
      distance = calculateDistance();
     
      //Draw points according to the measured distance
      if (distance < 100)
      {
        ucg.setColor(255,0,0);
        ucg.drawDisc(distance*cos(radians(x))+Xcent,-distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
      }
      else
      { //If it is over 1 meter, draw yellow on the edge area
        ucg.setColor(255,255,0);
        ucg.drawDisc(116*cos(radians(x))+Xcent,-116*sin(radians(x))+base, 1, UCG_DRAW_ALL);
      }
    
           
      //Debug code, output Angle and ranging values  
      Serial.print(x); 
      Serial.print("    ,   ");
      Serial.println(distance); 
     

      if (x > 70 and x < 110)  fix_font();  //Redraw the number when the scan line overlaps with it
 
      ucg.setColor(0,155,  0);
      ucg.setPrintPos(0,126);
      ucg.print("DEG: "); 
      ucg.setPrintPos(24,126);
      ucg.print(x);
      ucg.print("  ");
      ucg.setPrintPos(125,126);
      ucg.print("  ");
      ucg.print(distance);
      ucg.print("cm  "); 
      
  }
  //ucg.clearScreen();  //If the arduino power supply is insufficient, it may cause a white screen (display signal is interrupted). You can use cls(); Function replaces ucg.clearScreen(); 
  delay(50);
  cls();   //If the white screen is frequently displayed, use this function. Or add external power supply
 
  fix(); 
  fix_font();          //Repaint the screen background elements
  
  for (int  x=1; x < 176; x+=2){     
      baseServo.write(x);             //Adjust the steering gear Angle
      
      //Draw the radar scan line
      int f = x + 4;
      ucg.setColor(0, 255, 0);
      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
      f-=2;
      ucg.setColor(0, 128, 0);
      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
      f-=2;
      ucg.setColor(0, 0, 0);
      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
      ucg.setColor(0, 200, 0);
      //测距
      distance = calculateDistance();

      //Draw points according to the measured distance
      if (distance < 100)
      {
        ucg.setColor(255,0,0);
        ucg.drawDisc(distance*cos(radians(x))+Xcent,-distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
      }
      else
      { //Draw points according to the measured distance
        ucg.setColor(255,255,0);
        ucg.drawDisc(116*cos(radians(x))+Xcent,-116*sin(radians(x))+base, 1, UCG_DRAW_ALL);
      }
           
      //Debug code, output Angle and ranging values  
      Serial.print(x); 
      Serial.print("    ,   ");
      Serial.println(distance); 
     
      if (x > 70 and x < 110)  fix_font();  //Redraw the number when the scan line overlaps with it
      
      ucg.setColor(0,155,  0);
      ucg.setPrintPos(0,126);
      ucg.print("DEG: "); 
      ucg.setPrintPos(24,126);
      ucg.print(x);
      ucg.print("   ");
      ucg.setPrintPos(125,126);
      ucg.print("   ");
      ucg.print(distance);
      ucg.print("cm   "); 
  
  }
 //ucg.clearScreen(); //
 delay(50);
 cls();
}
