Motor und Lidar Steuerung

2021-07-26 22:40:34 +02:00
parent 546fbbf6ff
commit ee4d7aa5bb
4 changed files with 483 additions and 0 deletions
--- a/PoinCloudWeb.Scanner/StepperSteuerung/StepperSteuerung.ino
+++ b/PoinCloudWeb.Scanner/StepperSteuerung/StepperSteuerung.ino
@@ -0,0 +1,59 @@
+#include <AFMotor.h>
+
+double degreeY = 0;
+String rxData = ""; //Empfangen
+
+// Connect a stepper motor with 200 steps per revolution (1.8 degree)
+// to motor port #1 (M1 and M2)
+AF_Stepper motor(200, 1);
+
+void setup() {
+  Serial.begin(9600); // opens serial port, sets data rate to 9600 bps
+  motor.setSpeed(60);  // 60 rpm 
+}
+
+void loop() {
+  // send data only when you receive data:
+  if (Serial.available() > 0) {
+    // read the incoming byte:
+    rxData = Serial.readString();
+    String command = rxData.substring(rxData.indexOf('<')+1,rxData.indexOf('>'));
+    if (command == "set"){
+      String temp = rxData.substring(rxData.indexOf("><")+2,rxData.indexOf('>',rxData.indexOf("><")+2));
+      Serial.println(moveMotor(temp.toDouble()));
+    }
+    else if(command == "get")
+      Serial.println("<get><" + (String)degreeY + ">");
+    else if(command == "reset"){
+      Serial.println(moveMotor(0));
+    }
+    else if(command == "zero"){
+      degreeY = 0;
+      Serial.println("<set><zero>");
+    }
+    else
+      Serial.println("<error><command>");
+  }
+}
+
+String moveMotor(double y){
+  if(y < degreeY){
+    motor.step((int)calculateStepps(degreeY - y), BACKWARD, INTERLEAVE); //"interleave" means that it alternates between single and double to get twice the resolution (but of course its half the speed)
+    motor.release();  // Strom sparen und Überhitzung des Controllers vorbeugen!
+  }
+  else{
+    motor.step((int)calculateStepps(y - degreeY), FORWARD, INTERLEAVE);
+    motor.release();
+  }
+  degreeY = y;
+  return "<move><" + (String)y + ">";
+}
+
+double calculateStepps(double y){
+  double temp = 0;
+  if(y < degreeY)
+    temp = degreeY - y;
+  else
+    temp = y - degreeY;
+  return (y / 1.8) * 2; // *2 wegen interleave stepps / Falls Untersetzung, multiplikator anpassen!
+}
--- a/PoinCloudWeb.Scanner/datafile.txt
+++ b/PoinCloudWeb.Scanner/datafile.txt
@@ -0,0 +1,360 @@
+a:0 d:230
+a:1 d:469
+a:2 d:466
+a:3 d:431
+a:4 d:319
+a:5 d:192
+a:6 d:362
+a:7 d:473
+a:8 d:474
+a:9 d:480
+a:10 d:475
+a:11 d:476
+a:12 d:482
+a:13 d:480
+a:14 d:482
+a:15 d:484
+a:16 d:446
+a:17 d:489
+a:18 d:491
+a:19 d:495
+a:20 d:182
+a:21 d:156
+a:22 d:0
+a:23 d:0
+a:24 d:0
+a:25 d:256
+a:26 d:514
+a:27 d:516
+a:28 d:522
+a:29 d:527
+a:30 d:535
+a:31 d:536
+a:32 d:546
+a:33 d:550
+a:34 d:561
+a:35 d:570
+a:36 d:580
+a:37 d:587
+a:38 d:591
+a:39 d:607
+a:40 d:631
+a:41 d:641
+a:42 d:642
+a:43 d:663
+a:44 d:677
+a:45 d:689
+a:46 d:691
+a:47 d:721
+a:48 d:735
+a:49 d:753
+a:50 d:739
+a:51 d:782
+a:52 d:800
+a:53 d:792
+a:54 d:840
+a:55 d:390
+a:56 d:0
+a:57 d:533
+a:58 d:64
+a:59 d:165
+a:60 d:314
+a:61 d:121
+a:62 d:0
+a:63 d:315
+a:64 d:0
+a:65 d:118
+a:66 d:0
+a:67 d:107
+a:68 d:43
+a:69 d:41
+a:70 d:127
+a:71 d:128
+a:72 d:55
+a:73 d:111
+a:74 d:94
+a:75 d:125
+a:76 d:139
+a:77 d:141
+a:78 d:144
+a:79 d:43
+a:80 d:0
+a:81 d:0
+a:82 d:0
+a:83 d:0
+a:84 d:0
+a:85 d:0
+a:86 d:60
+a:87 d:225
+a:88 d:103
+a:89 d:47
+a:90 d:237
+a:91 d:277
+a:92 d:597
+a:93 d:556
+a:94 d:686
+a:95 d:648
+a:96 d:674
+a:97 d:672
+a:98 d:597
+a:99 d:665
+a:100 d:135
+a:101 d:625
+a:102 d:1079
+a:103 d:1089
+a:104 d:1117
+a:105 d:1108
+a:106 d:1111
+a:107 d:1010
+a:108 d:1095
+a:109 d:1091
+a:110 d:1020
+a:111 d:506
+a:112 d:726
+a:113 d:1464
+a:114 d:1464
+a:115 d:1519
+a:116 d:1482
+a:117 d:1501
+a:118 d:1350
+a:119 d:1606
+a:120 d:1615
+a:121 d:1634
+a:122 d:1654
+a:123 d:1665
+a:124 d:1692
+a:125 d:1720
+a:126 d:1735
+a:127 d:1761
+a:128 d:1788
+a:129 d:1828
+a:130 d:1737
+a:131 d:1783
+a:132 d:1405
+a:133 d:1134
+a:134 d:1054
+a:135 d:666
+a:136 d:304
+a:137 d:451
+a:138 d:455
+a:139 d:461
+a:140 d:470
+a:141 d:475
+a:142 d:482
+a:143 d:358
+a:144 d:411
+a:145 d:418
+a:146 d:372
+a:147 d:222
+a:148 d:401
+a:149 d:401
+a:150 d:403
+a:151 d:406
+a:152 d:409
+a:153 d:411
+a:154 d:375
+a:155 d:335
+a:156 d:376
+a:157 d:201
+a:158 d:267
+a:159 d:419
+a:160 d:429
+a:161 d:567
+a:162 d:558
+a:163 d:552
+a:164 d:458
+a:165 d:494
+a:166 d:450
+a:167 d:465
+a:168 d:476
+a:169 d:383
+a:170 d:297
+a:171 d:396
+a:172 d:395
+a:173 d:393
+a:174 d:391
+a:175 d:388
+a:176 d:387
+a:177 d:387
+a:178 d:386
+a:179 d:386
+a:180 d:386
+a:181 d:387
+a:182 d:387
+a:183 d:388
+a:184 d:388
+a:185 d:389
+a:186 d:391
+a:187 d:393
+a:188 d:394
+a:189 d:397
+a:190 d:399
+a:191 d:403
+a:192 d:407
+a:193 d:410
+a:194 d:415
+a:195 d:419
+a:196 d:424
+a:197 d:429
+a:198 d:434
+a:199 d:442
+a:200 d:452
+a:201 d:233
+a:202 d:0
+a:203 d:0
+a:204 d:0
+a:205 d:0
+a:206 d:2064
+a:207 d:2177
+a:208 d:2581
+a:209 d:2490
+a:210 d:2401
+a:211 d:2327
+a:212 d:2265
+a:213 d:2203
+a:214 d:2142
+a:215 d:2082
+a:216 d:2033
+a:217 d:1991
+a:218 d:1951
+a:219 d:1933
+a:220 d:1949
+a:221 d:1978
+a:222 d:2011
+a:223 d:2054
+a:224 d:975
+a:225 d:185
+a:226 d:2188
+a:227 d:2226
+a:228 d:2267
+a:229 d:2244
+a:230 d:2214
+a:231 d:2186
+a:232 d:2161
+a:233 d:1965
+a:234 d:765
+a:235 d:2098
+a:236 d:732
+a:237 d:322
+a:238 d:355
+a:239 d:0
+a:240 d:0
+a:241 d:79
+a:242 d:330
+a:243 d:440
+a:244 d:437
+a:245 d:435
+a:246 d:436
+a:247 d:436
+a:248 d:435
+a:249 d:432
+a:250 d:195
+a:251 d:35
+a:252 d:346
+a:253 d:422
+a:254 d:420
+a:255 d:418
+a:256 d:352
+a:257 d:416
+a:258 d:414
+a:259 d:338
+a:260 d:301
+a:261 d:297
+a:262 d:408
+a:263 d:379
+a:264 d:372
+a:265 d:405
+a:266 d:405
+a:267 d:404
+a:268 d:404
+a:269 d:404
+a:270 d:404
+a:271 d:404
+a:272 d:404
+a:273 d:404
+a:274 d:405
+a:275 d:326
+a:276 d:212
+a:277 d:263
+a:278 d:262
+a:279 d:243
+a:280 d:241
+a:281 d:239
+a:282 d:222
+a:283 d:224
+a:284 d:127
+a:285 d:278
+a:286 d:242
+a:287 d:162
+a:288 d:232
+a:289 d:195
+a:290 d:224
+a:291 d:249
+a:292 d:256
+a:293 d:31
+a:294 d:56
+a:295 d:43
+a:296 d:0
+a:297 d:0
+a:298 d:0
+a:299 d:0
+a:300 d:0
+a:301 d:0
+a:302 d:0
+a:303 d:0
+a:304 d:0
+a:305 d:0
+a:306 d:0
+a:307 d:0
+a:308 d:0
+a:309 d:0
+a:310 d:0
+a:311 d:0
+a:312 d:0
+a:313 d:0
+a:314 d:0
+a:315 d:0
+a:316 d:0
+a:317 d:50
+a:318 d:137
+a:319 d:130
+a:320 d:176
+a:321 d:476
+a:322 d:476
+a:323 d:540
+a:324 d:487
+a:325 d:427
+a:326 d:422
+a:327 d:363
+a:328 d:432
+a:329 d:506
+a:330 d:524
+a:331 d:208
+a:332 d:371
+a:333 d:464
+a:334 d:456
+a:335 d:222
+a:336 d:390
+a:337 d:498
+a:338 d:384
+a:339 d:450
+a:340 d:491
+a:341 d:495
+a:342 d:488
+a:343 d:483
+a:344 d:207
+a:345 d:341
+a:346 d:478
+a:347 d:474
+a:348 d:471
+a:349 d:469
+a:350 d:472
+a:351 d:467
+a:352 d:466
+a:353 d:466
+a:354 d:471
+a:355 d:465
+a:356 d:465
+a:357 d:471
+a:358 d:288
+a:359 d:135
--- a/PoinCloudWeb.Scanner/lida.py
+++ b/PoinCloudWeb.Scanner/lida.py
@@ -0,0 +1,21 @@
+import PyLidar3
+import time # Time module
+#Serial port to which lidar connected, Get it from device manager windows
+#In linux type in terminal -- ls /dev/tty* 
+#port = input("Enter port name which lidar is connected:") #windows
+#port = "/dev/ttyUSB0" #linux
+f = open("PoinCloudWeb.Scanner\datafile.txt","wt")
+Obj = PyLidar3.YdLidarX4(port='COM6',chunk_size=10000) #PyLidar3.your_version_of_lidar(port,chunk_size) 
+if(Obj.Connect()):
+    print(Obj.GetDeviceInfo())
+    gen = Obj.StartScanning()
+    t = time.time() # start time 
+    data = next(gen)
+    #print(data)
+    for x,y  in data.items():
+        f.write("a:" + str(x) + " d:" + str(y) + "\n")
+    f.close()
+    Obj.StopScanning()
+    Obj.Disconnect()
+else:
+    print("Error connecting to device")
--- a/PoinCloudWeb.Scanner/server.py
+++ b/PoinCloudWeb.Scanner/server.py
@@ -0,0 +1,43 @@
+# Importing Libraries
+import serial
+import time
+
+arduino = serial.Serial(port='COM5', baudrate=9600)
+
+print("Start ...")
+time.sleep(2)
+print("Ready:")
+
+def write_read(x):
+    arduino.write(bytes(x, 'utf-8'))
+    data = arduino.readline()
+    return filterY(str(data))
+
+def setY(y):
+    print(write_read("<set><"+str(y)+">"))
+
+def getY():
+    print(write_read("<get>"))
+
+def resetY():
+    print(write_read("<reset>"))
+
+def zerotY():
+    print(write_read("<zero>"))
+
+def filterY(data):
+    temp = data[data.find("<"):data.find(">")]
+    return temp + data[data.find("><"):data.find(">", data.find("><")+2)+1]
+
+def startScan(mode):
+    print("Scan gestartet")
+
+def stopScan():
+    print("Scan gestoppt")
+
+def getScanStatus():
+    return "%"
+
+while True:
+    for x in range(11):
+        setY(x*36)