DIY Arduino ultrasonic Sonar - Radar on TFT display

1#include <Servo.h> 
2#include <SPI.h>
3#include "Ucglib.h"           
4
5
6#define  trigPin   6       
7#define  echoPin   5        
8#define  ServoPin  3         
9int Ymax = 240;              
10int Xmax = 320;              
11
12int Xcent = Xmax / 2;       
13int base = 210;              
14int scanline = 185;         
15
16Servo baseServo; 
17//Ucglib_ILI9341_18x240x320_SWSPI ucg(/*sclk=*/ 13, /*data=*/ 11, /*cd=*/ 9, /*cs=*/ 10, /*reset=*/ 8);
18Ucglib_ILI9341_18x240x320_HWSPI ucg(/*cd=*/ 9, /*cs=*/ 10, /*reset=*/ 8);
19
20void setup(void)
21{
22 
23      ucg.begin(UCG_FONT_MODE_SOLID); 
24      ucg.setRotate90();             
25      
26      pinMode(trigPin, OUTPUT);      
27      pinMode(echoPin, INPUT);       
28      Serial.begin(115200);            
29      baseServo.attach(ServoPin);   
30    
31      
32      ucg.setFontMode(UCG_FONT_MODE_TRANSPARENT);
33      ucg.setColor(0, 0, 100, 0);
34      ucg.setColor(1, 0, 100, 0);
35      ucg.setColor(2, 20, 20,20);
36      ucg.setColor(3, 20, 20, 20);
37      ucg.drawGradientBox(0, 0, 320, 240);
38      ucg.setPrintDir(0);
39      ucg.setColor(0, 5, 0);
40      ucg.setPrintPos(70,120);
41      ucg.setFont(ucg_font_logisoso32_tf);  
42      ucg.print("Mini Radar");
43      ucg.setColor(0, 255, 0);
44      ucg.setPrintPos(70,120);
45      ucg.print("Mini Radar");
46      ucg.setFont(ucg_font_courB14_tf);
47      ucg.setColor(20, 255, 20);
48      ucg.setPrintPos(90,200);
49      ucg.print("Testing...");
50      baseServo.write(90);
51    
52    
53      for(int x=0;x<180;x+=5)
54          { baseServo.write(x);
55            delay(50);
56           }
57      ucg.print("OK!");
58      delay(500);
59      ucg.setColor(0,0, 0, 0);
60      ucg.setColor(1,0, 0, 0);
61      ucg.setColor(2,0, 0, 0);
62      ucg.setColor(3,0, 0, 0);
63      ucg.drawGradientBox(0, 0, 320, 240);
64      delay(10);
65    
66    
67      //ucg.clearScreen();
68      cls();
69      ucg.setFontMode(UCG_FONT_MODE_SOLID);
70      ucg.setFont(ucg_font_helvR08_hr);   // or freedoomr10_tr
71  
72}
73
74
75void cls()
76{
77 
78  ucg.setColor(0, 0, 0, 0);
79  for(int s=0;s<240;s++)
80  {
81    ucg.drawHLine(0,s,320);
82    delay(1);
83  }
84    
85  //ucg.drawBox(0, 0, 160, 60);
86
87}
88
89
90int calculateDistance()
91{ 
92      long duration;
93    
94      digitalWrite(trigPin, LOW); 
95      delayMicroseconds(2);
96      
97      digitalWrite(trigPin, HIGH); 
98      delayMicroseconds(10);
99      digitalWrite(trigPin, LOW);
100      
101      duration = pulseIn(echoPin, HIGH);
102     
103      return duration*0.034/2;
104}
105
106void fix_font() 
107{
108      ucg.setColor(0, 180, 0);
109      ucg.setPrintPos(144,44);
110      ucg.print("1.00");
111      ucg.setPrintPos(144,100);
112      ucg.print("0.60");
113      ucg.setPrintPos(144,165);
114      ucg.print("0.30");
115}
116
117void fix()
118{
119
120      ucg.setColor(0, 180, 0);
121    
122      ucg.drawDisc(Xcent, base+1, 3, UCG_DRAW_ALL); 
123      ucg.drawCircle(Xcent, base+1, 210, UCG_DRAW_UPPER_LEFT);
124      ucg.drawCircle(Xcent, base+1, 210, UCG_DRAW_UPPER_RIGHT);
125      ucg.drawCircle(Xcent, base+1, 135, UCG_DRAW_UPPER_LEFT);
126      ucg.drawCircle(Xcent, base+1, 135, UCG_DRAW_UPPER_RIGHT);
127      ucg.drawCircle(Xcent, base+1, 70, UCG_DRAW_UPPER_LEFT);
128      ucg.drawCircle(Xcent, base+1, 70, UCG_DRAW_UPPER_RIGHT);
129      ucg.drawLine(0, base+1, Xmax,base+1);
130     
131      ucg.setColor(0, 180, 0);
132     
133       for(int i= 40;i < 300; i+=2)
134       {
135
136        if (i % 10 == 0) 
137          ucg.drawLine(185*cos(radians(i))+Xcent,base - 185*sin(radians(i)) , 205*cos(radians(i))+Xcent,base - 205*sin(radians(i)));
138        
139        else
140        
141         ucg.drawLine(195*cos(radians(i))+Xcent,base - 195*sin(radians(i)) , 205*cos(radians(i))+Xcent,base - 205*sin(radians(i)));
142         
143       }
144          
145     
146       ucg.setColor(0,200,0);
147       ucg.drawLine(0,0,0,36);
148       for(int i= 0;i < 5; i++)
149       {
150          ucg.setColor(0,random(200)+50,0);
151          ucg.drawBox(2,i*8,random(28)+3,6);
152       }
153
154       ucg.setColor(0,180,0);
155       ucg.drawFrame(292,0,28,28);
156       ucg.setColor(0,60,0);
157       ucg.drawHLine(296,0,20);
158       ucg.drawVLine(292,4,20);
159       ucg.drawHLine(296,52,20);
160       ucg.drawVLine(318,4,20);
161        
162       ucg.setColor(0,220,0);
163       ucg.drawBox(296,4,8,8);
164       ucg.drawBox(296,16,8,8);
165       ucg.drawBox(308,16,8,8);
166       ucg.setColor(0,100,0);
167       ucg.drawBox(308,4,8,8);
168
169       ucg.setColor(0,90,0);
170       ucg.drawTetragon(124,220,116,230,196,230,204,220);
171       ucg.setColor(0,160,0);
172       ucg.drawTetragon(134,220,126,230,186,230,194,220);
173       ucg.setColor(0,210,0);
174       ucg.drawTetragon(144,220,136,230,176,230,184,220);
175}
176
177
178
179void loop(void)
180{
181  
182  int distance;
183  
184  fix(); 
185  fix_font(); 
186
187  for (int x=180; x > 4; x-=2){      
188     
189      baseServo.write(x);             
190      
191     
192      int f = x - 4; 
193      ucg.setColor(0, 255, 0);
194      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
195      f+=2;
196      ucg.setColor(0, 128, 0);
197      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
198      f+=2;
199      ucg.setColor(0, 0, 0);
200      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
201      ucg.setColor(0,200, 0);
202     
203      distance = calculateDistance();
204     
205     
206      if (distance < 100)
207      {
208        ucg.setColor(255,0,0);
209        ucg.drawDisc(2.2*distance*cos(radians(x))+ Xcent,-2.2*distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
210      }
211      else
212      { 
213        ucg.setColor(255,255,0);
214        ucg.drawDisc(208*cos(radians(x))+Xcent,-208*sin(radians(x))+base, 1, UCG_DRAW_ALL);
215      }
216    
217           
218     
219      Serial.print(x); 
220      Serial.print("    ,   ");
221      Serial.println(distance); 
222     
223
224      if (x > 70 and x < 110)  fix_font(); 
225
226
227      ucg.setColor(255,255,  0);
228      ucg.setPrintPos(20,230);
229      ucg.print("DEG: "); 
230      ucg.setPrintPos(54,230);
231      ucg.print(x);
232      ucg.print("  ");
233      ucg.setPrintPos(240,230);
234      ucg.print("     ");
235      ucg.print(distance);
236      ucg.print(" cm    "); 
237      
238  }
239  //ucg.clearScreen();  
240  delay(50);
241  cls();   
242 
243  fix(); 
244  fix_font();         
245  
246  for (int  x=1; x < 176; x+=2){     
247      baseServo.write(x);             
248      
249     
250      int f = x + 4;
251      ucg.setColor(0, 255, 0);
252      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
253      f-=2;
254      ucg.setColor(0, 128, 0);
255      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
256      f-=2;
257      ucg.setColor(0, 0, 0);
258      ucg.drawLine(Xcent, base, scanline*cos(radians(f))+Xcent,base - scanline*sin(radians(f)));
259      ucg.setColor(0, 200, 0);
260      
261      distance = calculateDistance();
262
263      
264      if (distance < 100)
265      {
266        ucg.setColor(255,0,0);
267        ucg.drawDisc(2.2*distance*cos(radians(x))+Xcent,-2.2*distance*sin(radians(x))+base, 1, UCG_DRAW_ALL);
268      }
269      else
270      { 
271        ucg.setColor(255,255,0);
272        ucg.drawDisc(208*cos(radians(x))+Xcent,-208*sin(radians(x))+base, 1, UCG_DRAW_ALL);
273      }
274           
275      
276      Serial.print(x); 
277      Serial.print("    ,   ");
278      Serial.println(distance); 
279     
280      if (x > 70 and x < 110)  fix_font(); 
281      
282      ucg.setColor(255,255,  0);
283      ucg.setPrintPos(20,230);
284      ucg.print("DEG: "); 
285      ucg.setPrintPos(54,230);
286      ucg.print(x);
287      ucg.print("  ");
288      ucg.setPrintPos(240,230);
289      ucg.print("     ");
290      ucg.print(distance);
291      ucg.print(" cm    "); 
292  
293  }
294 //ucg.clearScreen(); //
295 delay(50);
296 cls();
297
298
299}