#include <Servo.h> 
#include <SPI.h>
#include "Ucglib.h"           

#define  trigPin   6          //超声波模块的Trig口 6#
#define  echoPin   5          //超声波模块的echo口 5#
#define  ServoPin  3          //底座舵机端口 3#
int Ymax = 128;               //屏幕的竖向像素数
int Xmax = 160;               //屏幕的横向像素数
int Xcent = Xmax / 2;         //x中位
int base = 118;               //基线高度
int scanline = 105;           //雷达扫描线长度
int outcircle=80;
int midcircle=60;
int midcircle2=40;
int incircle=20;
unsigned long time=0;
Servo baseServo; 
Ucglib_ST7735_18x128x160_HWSPI ucg(/*cd=*/ 9, /*cs=*/ 10, /*reset=*/ 8);

void setup(void)
{
  ucg.begin(UCG_FONT_MODE_SOLID); //初始化屏幕
  ucg.setRotate90();              //设置成横屏  如果屏幕显示方向是反的，可以修改函数 setRotate90 或 setRotate270     
  pinMode(trigPin, OUTPUT);       //设置trigPin端口模式
  pinMode(echoPin, INPUT);        //设置echoPin端口模式
  Serial.begin(9600);             //设置串口传输率
  baseServo.attach(ServoPin);     //初始化舵机
  baseServo.write(180);
  ucg.clearScreen();
  //cls();
  ucg.setFontMode(UCG_FONT_MODE_SOLID);
  ucg.setFont(ucg_font_orgv01_hr);  
}

void cls()
{
  //清屏
  ucg.setColor(0, 0, 0, 0);  
  for(int s=0;s<128;s+=8)
  for(int t=0;t<160;t+=16)
  {
    ucg.drawBox(t,s,16,8);
  }
}

int calculateDistance()
{ 
  long duration;
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); 
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  return duration*0.034/2;
}
void fix_font() 
{
  ucg.setColor(0, 180, 0);
  ucg.setPrintPos(70,46);
  ucg.print("1.00");
  ucg.setPrintPos(70,66);
  ucg.print("0.75");
  ucg.setPrintPos(70,86);
  ucg.print("0.50");
  ucg.setPrintPos(70,106);
  ucg.print("0.25");
}
void fix()
{
  ucg.setColor(0, 40, 0);
  //画基线圆盘
  ucg.drawDisc(Xcent, base+1, 3, UCG_DRAW_ALL); 
  ucg.drawCircle(Xcent, base+1, outcircle, UCG_DRAW_UPPER_LEFT);
  ucg.drawCircle(Xcent, base+1, outcircle, UCG_DRAW_UPPER_RIGHT);
  ucg.drawCircle(Xcent, base+1, midcircle, UCG_DRAW_UPPER_LEFT);
  ucg.drawCircle(Xcent, base+1, midcircle, UCG_DRAW_UPPER_RIGHT);
  ucg.drawCircle(Xcent, base+1, midcircle2, UCG_DRAW_UPPER_LEFT);
  ucg.drawCircle(Xcent, base+1, midcircle2, UCG_DRAW_UPPER_RIGHT);
  ucg.drawCircle(Xcent, base+1, incircle, UCG_DRAW_UPPER_LEFT);
  ucg.drawCircle(Xcent, base+1, incircle, UCG_DRAW_UPPER_RIGHT);
  ucg.drawLine(0, base+1, Xmax,base+1);     
  ucg.setColor(0, 120, 0);
}
void loop(void)
{  
  int distance;  
  fix(); 
  fix_font();  //重绘屏幕背景元素
  Serial.print("clear:");
  Serial.println(millis()-time);
  time=millis();
  for (int x=180; x >= 0; x-=2){       //底座舵机从180~0度循环     
      baseServo.write(x);             //调整舵机角度
      //测距
      distance = calculateDistance();     
      //根据测得距离在对应位置画点
      if (distance < 100)
      {
        ucg.setColor(255,0,0);
        ucg.drawDisc(0.8*distance*cos(radians(x))+Xcent,-0.8*distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
      }
      else
      { //超过1米以上的，用黄色画在边缘区域示意
        ucg.setColor(255,255,0);
        ucg.drawDisc((outcircle-1)*cos(radians(x))+Xcent,-(outcircle-1)*sin(radians(x))+base, 1, UCG_DRAW_ALL);
      }
  }
  Serial.print("left:");
  Serial.println(millis()-time);
  time=millis();
  ucg.clearScreen();
  delay(50);
  //cls(); 
  fix(); 
  fix_font();          //重绘屏幕背景元素
  Serial.print("clear:");
  Serial.println(millis()-time);
  time=millis();  
  for (int  x=0; x <= 180; x+=2){     
      baseServo.write(x);             //调整舵机角度
      //测距
      distance = calculateDistance();
      //根据测得距离在对应位置画点
      if (distance < 100)
      {
        ucg.setColor(255,0,0);
        ucg.drawDisc(0.8*distance*cos(radians(x))+Xcent,-0.8*distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
      }
      else
      { //超过1米以上的，用黄色画在边缘区域示意
        ucg.setColor(255,255,0);
        ucg.drawDisc((outcircle-1)*cos(radians(x))+Xcent,-(outcircle-1)*sin(radians(x))+base, 1, UCG_DRAW_ALL);
      }
  }
  Serial.print("right:");
  Serial.println(millis()-time);
  time=millis();
  ucg.clearScreen();
  delay(50);
  //cls();
}