API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 18th, 2021
42
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 29.63 KB | None | 0 0
raw download clone embed print report
  1. from PyQt5.QtCore import QDateTime, Qt, QTimer
  2. from PyQt5.QtWidgets import (QApplication, QCheckBox, QComboBox, QDateTimeEdit,
  3. QDial, QDialog, QGridLayout, QGroupBox, QHBoxLayout, QLabel, QLineEdit,
  4. QProgressBar, QPushButton, QRadioButton, QScrollBar, QSizePolicy,
  5. QSlider, QSpinBox, QStyleFactory, QTableWidget, QTabWidget, QTextEdit,
  6. QTableWidgetItem, QVBoxLayout, QWidget, QHeaderView)
  7. from PyQt5.QtGui import *
  8. import time
  9. import serial
  10. from QLed import QLed
  11.  
  12. from PyQt5 import uic, QtCore, QtWidgets
  13. from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg as FigureCanvas
  14. from matplotlib.backends.backend_qt4agg import NavigationToolbar2QT as NavigationToolbar
  15. from matplotlib.figure import Figure
  16.  
  17. import math
  18. #import vrep
  19.  
  20.  
  21. import matplotlib
  22. matplotlib.use('Qt5Agg')
  23. from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg, NavigationToolbar2QT as NavigationToolbar
  24.  
  25. """
  26. arduinoData=serial.Serial('COM6',9600)
  27.  
  28. def led_on():
  29. arduinoData.write(b'1')
  30. def led_off():
  31. arduinoData.write(b'0')
  32. def led2_on():
  33. arduinoData.write(b'3')
  34. def led2_off():
  35. arduinoData.write(b'2')
  36. def led3_on():
  37. arduinoData.write(b'5')
  38. def led3_off():
  39. arduinoData.write(b'4')
  40. t=0
  41. while(t<3000000):
  42. t+=1
  43. led3_off()
  44. """
  45. class MplCanvas(FigureCanvasQTAgg):
  46. def __init__(self, parent=None, width=5, height=4, dpi=120):
  47. fig = Figure(figsize=(width, height), dpi=dpi)
  48. self.axes = fig.add_subplot(111)
  49. super(MplCanvas, self).__init__(fig)
  50.  
  51. class WidgetGallery(QDialog):
  52. def __init__(self, parent=None):
  53. super(WidgetGallery, self).__init__(parent)
  54. self.setFixedHeight(800)
  55. self.setFixedWidth(1300)
  56. styleComboBox = QComboBox()
  57.  
  58. styleLabel = QLabel("Style:")
  59.  
  60. self.useStylePaletteCheckBox = QCheckBox("&Use style's standard palette")
  61. self.useStylePaletteCheckBox.setChecked(True)
  62. disableWidgetsCheckBox = QCheckBox("&Disable widgets")
  63. self.createTopLeftGroupBox()
  64. self.createTopRightGroupBox()
  65. self.createBottomLeftTabWidget()
  66. self.bottomLeftTabWidget.setFixedHeight(220)
  67. self.createBottomRightGroupBox()
  68. self.createProgressBar()
  69.  
  70. topLayout = QHBoxLayout()
  71.  
  72. mainLayout = QGridLayout()
  73. mainLayout.addWidget(self.topLeftGroupBox, 1, 0)
  74. mainLayout.addWidget(self.topRightGroupBox, 1, 1)
  75.  
  76. mainLayout.addWidget(self.bottomLeftTabWidget, 2, 0)
  77. mainLayout.addWidget(self.bottomRightGroupBox, 2, 1)
  78. self.setLayout(mainLayout)
  79. self.setWindowTitle("Styles")
  80. self.changeStyle('Windows')
  81. # self.label.setFont(QVBoxLayout.QFont("Sanserif", 15))
  82.  
  83. def changeStyle(self, styleName):
  84. QApplication.setStyle(QStyleFactory.create(styleName))
  85. self.changePalette()
  86.  
  87. def changePalette(self):
  88. if (self.useStylePaletteCheckBox.isChecked()):
  89. QApplication.setPalette(QApplication.style().standardPalette())
  90. else:
  91. QApplication.setPalette(self.originalPalette)
  92. def advanceProgressBar(self):
  93. curVal = self.progressBar.value()
  94. maxVal = self.progressBar.maximum()
  95. self.progressBar.setValue(curVal + (maxVal - curVal) // 100)
  96.  
  97. def createTopLeftGroupBox(self):
  98. self.topLeftGroupBox = QGroupBox("Dynamic Graph")
  99. #self.sc = MplCanvas(self, width=5, height=4, dpi=120)
  100.  
  101.  
  102. # self.sc.resize(120,80)
  103. # self.sc.setMaximumHeight(500)
  104. self.figure = Figure(figsize=(8,6), dpi=80)
  105. gs = self.figure.add_gridspec(2, 1)
  106. self.ax1 = self.figure.add_subplot(gs[0, 0])
  107. self.ax2 = self.figure.add_subplot(gs[1, 0])
  108. self.ax1.grid(True)
  109. self.ax2.grid(True)
  110. self.canvas = FigureCanvas(self.figure)
  111. self.toolbar = NavigationToolbar(self.canvas, self)
  112. self.ax1.set_ylabel("Ticks", fontsize = 11)
  113. self.ax1.set_xlabel("Time, 100*milliseconds", fontsize = 11)
  114. self.ax1.set_title("Encoder measurements", fontsize = 11)
  115. self.ax1.set_ybound(0, 1080)
  116. self.ax1.set_yticks([0, 180,360, 540, 720, 900, 1080])
  117. self.ax1.set_yticklabels(('0', '180', '360', '540', '720', '900', '1080' ))
  118.  
  119. self.ax2.set_ylim(auto=True)
  120. self.ax2.set_ylabel('Force, 0.01*Newtons', fontsize = 11)
  121. self.ax2.set_xlabel("Time, 100*milliseconds", fontsize = 11)
  122. self.ax2.set_title("Force sensor measurements", fontsize = 11)
  123. #self.sc.axes.set_borders(bottom=0.1)
  124. #self.sc.axes.autoscale_view(True,True,True)
  125. #self.sc.axes.yticks(np.arange(0, 15, 1))
  126. #self.sc.axes.set(aspect=1,ylim=(0, 35))
  127.  
  128. self.ax1.set_position([0.1, 0.6, 0.85, 0.35])
  129. self.ax2.set_position([0.1, 0.1, 0.85, 0.35])
  130. #self.sc.axes.legend(handletextpad=0.1)
  131. #self.sc.axes.ymargin=0.1
  132. layout = QVBoxLayout()
  133. layout.addWidget(self.toolbar)
  134. layout.addWidget(self.canvas)
  135.  
  136.  
  137. self.show()
  138.  
  139. self.topLeftGroupBox.setLayout(layout)
  140.  
  141. def createBottomLeftTabWidget(self):
  142. self.bottomLeftTabWidget = QTabWidget()
  143. self.tab1 = QWidget()
  144. self.tab1.resize(1600,400)
  145. self.tableWidget = QTableWidget(9, 100)
  146. header = self.tableWidget.horizontalHeader()
  147. header.setSectionResizeMode(QHeaderView.Stretch)
  148. self.tableWidget.resize(120,80)
  149. self.tab1hbox = QHBoxLayout()
  150. self.tab1hbox.addWidget(self.tableWidget)
  151. self.tab1.setLayout(self.tab1hbox)
  152. self.bottomLeftTabWidget.addTab(self.tab1, "&Table")
  153.  
  154. def dialMoved(self):
  155. self.label.setText(" Dialer Value : " + str(self.dial3.value()))
  156.  
  157. def createBottomRightGroupBox(self):
  158. self.bottomRightGroupBox = QGroupBox("Actuators")
  159. self.Button_Right = QPushButton("Act: 1")
  160. self.Button_Right.setCheckable(True)
  161. self.Button_Right.clicked.connect(self.Button_Right_Handler)
  162. self.Button_Left = QPushButton("Act: 2")
  163. self.Button_Left.setCheckable(True)
  164. self.Button_Left.clicked.connect(self.Button_Left_Handler)
  165. self.Button_Top = QPushButton("Act: 3")
  166. self.Button_Top.setCheckable(True)
  167. self.Button_Top.clicked.connect(self.Button_Top_Handler)
  168. self.Button_Roll = QPushButton("Act: Roll")
  169. self.Button_Roll.setCheckable(True)
  170. self.Button_Roll.clicked.connect(self.Button_Roll_Handler)
  171. layout2 = QVBoxLayout()
  172. layout2.addWidget(self.Button_Right)
  173. layout2.addWidget(self.Button_Left)
  174. layout2.addWidget(self.Button_Top)
  175. layout2.addWidget(self.Button_Roll)
  176. self.bottomRightGroupBox.setLayout(layout2)
  177.  
  178. def createTopRightGroupBox(self):
  179.  
  180.  
  181. self.topRightGroupBox = QGroupBox("Control Panel")
  182.  
  183. stopAllButton = QPushButton("Turn off the actuators")
  184. stopAllButton.setDefault(True)
  185. stopAllButton.clicked.connect(self.StopAll_Handler)
  186.  
  187. readForceButton=QPushButton("Read force values")
  188. readForceButton.setDefault(True)
  189. readForceButton.clicked.connect(self.readForce)
  190.  
  191.  
  192. readEncButton=QPushButton("Read encoder")
  193. readEncButton.setDefault(True)
  194. readEncButton.clicked.connect(self.readEncoder)
  195.  
  196. self.dial3 = QDial(self.topRightGroupBox)
  197. self.dial3.setMinimum(-120)
  198. self.dial3.setMaximum(120)
  199. self.dial3.setNotchesVisible(True)
  200. self.dial3.valueChanged.connect(self.dialMoved)
  201.  
  202. self.dialInfo = QLabel()
  203. self.dialInfo.setText(" "+ str(self.dial3.minimum()+5) + " " + str(self.dial3.maximum()-5))
  204.  
  205. self.label = QLabel()
  206.  
  207. layout = QVBoxLayout()
  208. layout.addWidget(stopAllButton)
  209. layout.addWidget(readForceButton)
  210. layout.addWidget(readEncButton)
  211. layout.addWidget(self.dial3)
  212. layout.addWidget(self.dialInfo)
  213. layout.addWidget(self.label)
  214.  
  215.  
  216. self.led1=QLed(self, onColour=QLed.Red, shape=QLed.Circle)
  217. self.led1.value=False
  218.  
  219. self.led2=QLed(self, onColour=QLed.Red, shape=QLed.Circle)
  220. self.led2.value=False
  221.  
  222. self.led3=QLed(self, onColour=QLed.Red, shape=QLed.Circle)
  223. self.led3.value=False
  224.  
  225. self.led4=QLed(self, onColour=QLed.Red, shape=QLed.Circle)
  226. self.led4.value=False
  227.  
  228. self.light = 0
  229.  
  230. layout.addWidget(self.led1)
  231. layout.addWidget(self.led2)
  232. layout.addWidget(self.led3)
  233. layout.addWidget(self.led4)
  234. layout.addStretch(1)
  235. self.topRightGroupBox.setLayout(layout)
  236.  
  237. def sendSerial(self):
  238. if ((self.dial3.value()>=5) or (self.dial3.value()<-5)):
  239. ser=serial.Serial('COM3',9600,timeout=7)
  240. val = self.dial3.value()
  241. real_val = val+5 if val<0 else val-5
  242. print(real_val)
  243.  
  244. buf = str(self.dial3.value()).encode("utf-8")
  245. ser.write(b'' + buf + b' ')
  246. ser.close()
  247. time.sleep(2)
  248. self.readEncFrs()
  249. else:
  250. print("low value")
  251.  
  252. def readEncoder(self):
  253. ser=serial.Serial('COM3',9600)
  254. ser.write(b'254')
  255. ser.close()
  256.  
  257. print("Starting reading...")
  258.  
  259. ser=serial.Serial('COM3',9600, timeout=7)
  260. n_cnt = 0
  261. n_list = []
  262. enc_list = []
  263. enc_val1 = []
  264. enc_val2 = []
  265. enc_val3 = []
  266. enc_val4 = []
  267. enc_last = []
  268. zero_cnt = [0, 0, 0, 0, 0]
  269. while(n_cnt<=400):
  270. enc = ser.readline()
  271. enc_str = str(enc)
  272. if ("enc" in enc_str):
  273. enc_list = enc_str.split("|", 5)
  274. n_list.append(n_cnt)
  275. if int(enc_list[1])==0:
  276. zero_cnt[1] += 1
  277. enc_val1.append(int(enc_list[1]))
  278. if int(enc_list[2])==0:
  279. zero_cnt[2] += 1
  280. enc_val2.append(int(enc_list[2]))
  281. if int(enc_list[3])==0:
  282. zero_cnt[3] += 1
  283. enc_val3.append(int(enc_list[3]))
  284. if int(enc_list[4])==0:
  285. zero_cnt[4] += 1
  286. enc_val4.append(int(enc_list[4]))
  287. zero_cnt[0] += 1
  288. print(enc)
  289. n_cnt = n_cnt + 1
  290. '''if enc_list[9]=="off":
  291. enc_last = enc_list
  292. break;
  293. '''
  294. ser.close()
  295. print("End")
  296. if len(self.ax1.lines)>0:
  297. self.ax1.lines.clear()
  298.  
  299.  
  300. if zero_cnt[1]<=n_cnt-1:
  301. self.ax1.plot(n_list, enc_val1)
  302. if zero_cnt[2]<=n_cnt-1:
  303. self.ax1.plot(n_list, enc_val2)
  304. if zero_cnt[3]<=n_cnt-1:
  305. self.ax1.plot(n_list, enc_val3)
  306. if zero_cnt[4]<=n_cnt-1:
  307.  
  308. self.ax1.plot(n_list,enc_val4)
  309. print(n_list, enc_val4)
  310. if zero_cnt[0]!=0:
  311. for i in range(zero_cnt[0]-1):
  312. if i==0:
  313. self.tableWidget.setItem(0,0, QTableWidgetItem("data no:"))
  314. self.tableWidget.setItem(1,0, QTableWidgetItem("enc1:"))
  315. self.tableWidget.setItem(2,0, QTableWidgetItem("enc2:"))
  316. self.tableWidget.setItem(3,0, QTableWidgetItem("enc3:"))
  317. self.tableWidget.setItem(4,0, QTableWidgetItem("enc4:"))
  318. elif i>=1:
  319. self.tableWidget.setItem(0,i, QTableWidgetItem(str(i)))
  320. self.tableWidget.setItem(1,i, QTableWidgetItem(enc_val1[i-1]))
  321. self.tableWidget.setItem(2,i, QTableWidgetItem(enc_val2[i-1]))
  322. self.tableWidget.setItem(3,i, QTableWidgetItem(enc_val3[i-1]))
  323. self.tableWidget.setItem(4,i, QTableWidgetItem(enc_val4[i-1]))
  324.  
  325.  
  326. self.canvas.draw()
  327. self.StopAll_Handler()
  328. print("test")
  329. radius = float(9.2/10**3)
  330. one_degree_len = float((2*math.pi*radius)/1080)
  331. #degree passed = (1080-init_degree) + (number of rotations * 1080 degrees) + current degree: _(*)_
  332. '''
  333.  
  334. if len(self.ax1.lines)>0:
  335. self.ax1.lines.clear()
  336. if enc_zero_cnt[1]<=n_cnt-1:
  337. line1, = self.ax1.plot(enc_val1, 'b', label='Motor1')
  338. deg_passed[1] = (1080-int(init_deg[1])) + (int(enc_last[5])*1080) + int(enc_last[1])
  339. dist_passed[1] = one_degree_len * float(deg_passed[1])
  340. if enc_zero_cnt[2]<=n_cnt-1:
  341. deg_passed[2] = (1080-int(init_deg[2])) + (int(enc_last[6])*1080) + int(enc_last[2])
  342. dist_passed[2] = one_degree_len * float(deg_passed[2])
  343. line2, = self.ax1.plot(enc_val2, 'g', label='Motor2')
  344. if enc_zero_cnt[3]<=n_cnt-1:
  345. deg_passed[3] = (1080-int(init_deg[3])) + (int(enc_last[7])*1080) + int(enc_last[3])
  346. dist_passed[3] = one_degree_len * float(deg_passed[3])
  347. line3, = self.ax1.plot(enc_val3, 'y', label='Motor3')
  348. if enc_zero_cnt[4]<=n_cnt-1:
  349. deg_passed[4] = (1080.0-int(init_deg[4])) + (int(enc_last[8])*1080) + int(enc_last[4])
  350. dist_passed[4] = one_degree_len * float(deg_passed[4])
  351.  
  352. line4, = self.ax1.plot(enc_val4, 'm', label='Motor4')
  353. self.ax1.legend()
  354. '''
  355. #with open("ctrl.cfg","w") as ctrl_cfg:
  356. #ctrl_cfg.write('enc:|' + str(enc_last[1]) + '|' + str(enc_last[2]) + '|' + str(enc_last[3]) + '|' + str(enc_last[4]) + '|\n')
  357. #ctrl_cfg.write('angle passed(degrees):|' + str(deg_passed[1]) + '|' + str(deg_passed[2]) + '|' + str(deg_passed[3]) + '|' + str(deg_passed[4]) + '|\n')
  358. #dist_passed_float = [float(i) for i in dist_passed]
  359. #ctrl_cfg.write('distance passed(m):|' + "{:.4f}".format(dist_passed_float[1]) + '|' + "{:.4f}".format(dist_passed_float[2]) + '|' + "{:.4f}".format(dist_passed_float[3]) + '|' + "{:.4f}".format(dist_passed_float[4]) + '|\n')
  360. print("test")
  361.  
  362. def readForce(self):
  363. ser=serial.Serial('COM3',9600)
  364. ser.write(b'255')
  365. ser.close()
  366.  
  367. print("Starting reading...")
  368.  
  369. ser=serial.Serial('COM3',9600, timeout=7)
  370. n_cnt = 0
  371. self.n_list = []
  372. enc_list = []
  373. self.enc_val1 = []
  374. self.enc_val2 = []
  375. self.enc_val3 = []
  376. zero_cnt = [0, 0, 0, 0]
  377.  
  378. self._plot_ref1 = None
  379. self._plot_ref2 = None
  380. self._plot_ref3 = None
  381.  
  382. self.show()
  383.  
  384. while(n_cnt<=10):
  385. enc = ser.readline()
  386. enc_str = str(enc)
  387. print(n_cnt, enc_str)
  388.  
  389. if ("fs" in enc_str):
  390. enc_list = enc_str.split("|", 5)
  391. self.n_list.append(n_cnt)
  392. print("Test")
  393. if float(enc_list[1])==0.0:
  394. zero_cnt[1] += 1
  395. self.enc_val1.append(int(enc_list[1]))
  396. if float(enc_list[2])==0.0:
  397. zero_cnt[2] += 1
  398. self.enc_val2.append(int(enc_list[2]))
  399. if float(enc_list[3])==0.0:
  400. zero_cnt[3] += 1
  401. self.enc_val3.append(int(enc_list[3]))
  402.  
  403. #if n_cnt>=10:
  404. #self.update_plot()
  405. #self.show()
  406.  
  407. #self.sc.refresh()
  408. self.sc.draw()
  409. print(enc)
  410. print("Test")
  411. if int(enc_list[1])==0:
  412. enc_zero_cnt[1] += 1
  413. enc_val1.append(int(enc_list[1]))
  414.  
  415. if int(enc_list[2])==0:
  416. enc_zero_cnt[2] += 1
  417. enc_val2.append(int(enc_list[2]))
  418.  
  419. if int(enc_list[3])==0:
  420. enc_zero_cnt[3] += 1
  421. enc_val3.append(int(enc_list[3]))
  422.  
  423. if int(enc_list[4])==0:
  424. enc_zero_cnt[4] += 1
  425. enc_val4.append(int(enc_list[4]))
  426.  
  427. enc_zero_cnt[0] += 1
  428. n_cnt = n_cnt + 1
  429. ser.close()
  430. print("End")
  431. if len(self.ax2.lines)>0:
  432. self.ax2.lines.clear()
  433. while(n_cnt<=10):
  434. frs = ser.readline()
  435. print(frs)
  436. frs_str = str(frs)
  437. if ("fs" in frs_str):
  438. frs_list = frs_str.split("|", 5)
  439. if float(frs_list[1])==0.0:
  440. frs_zero_cnt[1] += 1
  441. frs_val1.append(float(frs_list[1]))
  442. if float(frs_list[2])==0.0:
  443. frs_zero_cnt[2] += 1
  444. frs_val2.append(float(frs_list[2]))
  445. if float(frs_list[3])==0.0:
  446. frs_zero_cnt[3] += 1
  447. frs_val3.append(float(frs_list[3]))
  448.  
  449. enc = ser.readline()
  450. print(enc)
  451. enc_str = str(enc)
  452. if ("enc" in enc_str):
  453. enc_list = enc_str.split("|", 10)
  454. if (n_cnt == 0):
  455. init_deg[1:5] = enc_list[1:5]
  456.  
  457. if int(enc_list[1])==0:
  458. enc_zero_cnt[1] += 1
  459. enc_val1.append(int(enc_list[1]))
  460.  
  461. if int(enc_list[2])==0:
  462. enc_zero_cnt[2] += 1
  463. enc_val2.append(int(enc_list[2]))
  464.  
  465. if int(enc_list[3])==0:
  466. enc_zero_cnt[3] += 1
  467. enc_val3.append(int(enc_list[3]))
  468.  
  469. if int(enc_list[4])==0:
  470. enc_zero_cnt[4] += 1
  471. enc_val4.append(int(enc_list[4]))
  472.  
  473. enc_zero_cnt[0] += 1
  474.  
  475. n_cnt = n_cnt + 1
  476.  
  477.  
  478. #if enc_list[9]=="off":
  479. # enc_last = enc_list
  480. #break;
  481.  
  482. if zero_cnt[1]<=n_cnt-1:
  483. self.ax2.plot(self.n_list, self.enc_val1)
  484. if zero_cnt[2]<=n_cnt-1:
  485. self.ax2.plot(self.n_list, self.enc_val2)
  486. if zero_cnt[3]<=n_cnt-1:
  487. self.ax2.plot(self.n_list, self.enc_val3)
  488. for i in range(n_cnt-1):
  489. if i==0:
  490. self.tableWidget.setItem(0,0, QTableWidgetItem("data no:"))
  491. self.tableWidget.setItem(1,0, QTableWidgetItem("FS1: *0.01 N"))
  492. self.tableWidget.setItem(2,0, QTableWidgetItem("FS2: *0.01 N"))
  493. self.tableWidget.setItem(3,0, QTableWidgetItem("FS3: *0.01 N"))
  494. else:
  495. self.tableWidget.setItem(0,i, QTableWidgetItem(str(i)))
  496. self.tableWidget.setItem(1,i, QTableWidgetItem(self.enc_val1[i-1]))
  497. self.tableWidget.setItem(2,i, QTableWidgetItem(self.enc_val2[i-1]))
  498. self.tableWidget.setItem(3,i, QTableWidgetItem(self.enc_val3[i-1]))
  499. self.canvas.draw()
  500.  
  501. def readEncFrs(self):
  502. ser=serial.Serial('COM3',9600)
  503. ser.write(b'253' + b' ')
  504. ser.close()
  505.  
  506. print("Starting reading...")
  507.  
  508. ser=serial.Serial('COM3',9600, timeout=7)
  509. n_cnt = 0
  510. init_deg = [0,0,0,0,0]
  511. deg_passed = [0,0,0,0,0]
  512. dist_passed = [0,0,0,0,0]
  513. enc_list = []
  514. enc_last = []
  515. enc_val1 = []
  516. enc_val2 = []
  517. enc_val3 = []
  518. enc_val4 = []
  519.  
  520. frs_list = []
  521. frs_val1 = []
  522. frs_val2 = []
  523. frs_val3 = []
  524. enc_zero_cnt = [0, 0, 0, 0, 0]
  525. frs_zero_cnt = [0, 0, 0, 0, 0]
  526.  
  527. while(n_cnt<=700):
  528. frs = ser.readline()
  529. print(frs)
  530. frs_str = str(frs)
  531. if ("fs" in frs_str):
  532. frs_list = frs_str.split("|", 5)
  533. if float(frs_list[1])==0.0:
  534. frs_zero_cnt[1] += 1
  535. frs_val1.append(float(frs_list[1]))
  536. if float(frs_list[2])==0.0:
  537. frs_zero_cnt[2] += 1
  538. frs_val2.append(float(frs_list[2]))
  539. if float(frs_list[3])==0.0:
  540. frs_zero_cnt[3] += 1
  541. frs_val3.append(float(frs_list[3]))
  542.  
  543. enc = ser.readline()
  544. print(enc)
  545. enc_str = str(enc)
  546. if ("enc" in enc_str):
  547. enc_list = enc_str.split("|", 10)
  548. if (n_cnt == 0):
  549. init_deg[1:5] = enc_list[1:5]
  550.  
  551. if int(enc_list[1])==0:
  552. enc_zero_cnt[1] += 1
  553. enc_val1.append(int(enc_list[1]))
  554.  
  555. if int(enc_list[2])==0:
  556. enc_zero_cnt[2] += 1
  557. enc_val2.append(int(enc_list[2]))
  558.  
  559. if int(enc_list[3])==0:
  560. enc_zero_cnt[3] += 1
  561. enc_val3.append(int(enc_list[3]))
  562.  
  563. if int(enc_list[4])==0:
  564. enc_zero_cnt[4] += 1
  565. enc_val4.append(int(enc_list[4]))
  566.  
  567. enc_zero_cnt[0] += 1
  568.  
  569. n_cnt = n_cnt + 1
  570.  
  571.  
  572. if enc_list[9]=="off":
  573. enc_last = enc_list
  574. break;
  575.  
  576.  
  577. ser.close()
  578. print("End")
  579.  
  580. #radius is 9.2mm
  581. radius = float(9.2/10**3)
  582. one_degree_len = float((2*math.pi*radius)/1080)
  583. #degree passed = (1080-init_degree) + (number of rotations * 1080 degrees) + current degree: _(*)_
  584. if len(self.ax1.lines)>0:
  585. self.ax1.lines.clear()
  586. if enc_zero_cnt[1]<=n_cnt-1:
  587. line1, = self.ax1.plot(enc_val1, 'b', label='Motor1')
  588. deg_passed[1] = (360-int(init_deg[1])) + (int(enc_last[5])*360) + int(enc_last[1])
  589. dist_passed[1] = one_degree_len * float(deg_passed[1])
  590. if enc_zero_cnt[2]<=n_cnt-1:
  591. deg_passed[2] = (360-int(init_deg[2])) + (int(enc_last[6])*360) + int(enc_last[2])
  592. dist_passed[2] = one_degree_len * float(deg_passed[2])
  593. line2, = self.ax1.plot(enc_val2, 'g', label='Motor2')
  594. if enc_zero_cnt[3]<=n_cnt-1:
  595. deg_passed[3] = (360-int(init_deg[3])) + (int(enc_last[7])*360) + int(enc_last[3])
  596. dist_passed[3] = one_degree_len * float(deg_passed[3])
  597. line3, = self.ax1.plot(enc_val3, 'y', label='Motor3')
  598. if enc_zero_cnt[4]<=n_cnt-1:
  599. deg_passed[4] = (360-int(init_deg[4])) + (int(enc_last[8])*360) + int(enc_last[4])
  600. dist_passed[4] = one_degree_len * float(deg_passed[4])
  601. line4, = self.ax1.plot(enc_val4, 'm', label='Motor4')
  602. self.ax1.legend()
  603.  
  604. with open("ctrl.cfg","w") as ctrl_cfg:
  605. ctrl_cfg.write('pos:|' + str(pos_last[1]) + '|' + str(pos_last[2]) + '|' + str(pos_last[3]) + '|' + str(pos_last[4]) + '|\n')
  606. ctrl_cfg.write('enc:|' + str(enc_last[1]) + '|' + str(enc_last[2]) + '|' + str(enc_last[3]) + '|' + str(enc_last[4]) + '|\n')
  607. ctrl_cfg.write('angle passed(degrees):|' + str(deg_passed[1]) + '|' + str(deg_passed[2]) + '|' + str(deg_passed[3]) + '|' + str(deg_passed[4]) + '|\n')
  608. dist_passed_float = [float(i) for i in dist_passed]
  609. ctrl_cfg.write('distance passed(m):|' + "{:.4f}".format(dist_passed_float[1]) + '|' + "{:.4f}".format(dist_passed_float[2]) + '|' + "{:.4f}".format(dist_passed_float[3]) + '|' + "{:.4f}".format(dist_passed_float[4]) + '|\n')
  610.  
  611. if len(self.ax2.lines)>0:
  612. self.ax2.lines.clear()
  613. if frs_zero_cnt[1]<=n_cnt-1:
  614. self.ax2.plot(frs_val1, 'b', label='Motor1')
  615. if frs_zero_cnt[2]<=n_cnt-1:
  616. self.ax2.plot(frs_val2, 'g', label='Motor2')
  617. if frs_zero_cnt[3]<=n_cnt-1:
  618. self.ax2.plot(frs_val3, 'y', label='Motor3')
  619. self.ax2.legend()
  620.  
  621. for i in range(n_cnt-1):
  622. if i==0:
  623. self.tableWidget.setItem(0,0, QTableWidgetItem("data no:"))
  624. self.tableWidget.setItem(1,0, QTableWidgetItem("enc1:"))
  625. self.tableWidget.setItem(2,0, QTableWidgetItem("enc2:"))
  626. self.tableWidget.setItem(3,0, QTableWidgetItem("enc3:"))
  627. self.tableWidget.setItem(4,0, QTableWidgetItem("enc4:"))
  628.  
  629. self.tableWidget.setItem(6,0, QTableWidgetItem("FS1: *0.01 N"))
  630. self.tableWidget.setItem(7,0, QTableWidgetItem("FS2: *0.01 N"))
  631. self.tableWidget.setItem(8,0, QTableWidgetItem("FS3: *0.01 N"))
  632. elif i>=1:
  633. self.tableWidget.setItem(0,i, QTableWidgetItem(str(i)))
  634. self.tableWidget.setItem(1,i, QTableWidgetItem(str(enc_val1[i-1])))
  635. self.tableWidget.setItem(2,i, QTableWidgetItem(str(enc_val2[i-1])))
  636. self.tableWidget.setItem(3,i, QTableWidgetItem(str(enc_val3[i-1])))
  637. self.tableWidget.setItem(4,i, QTableWidgetItem(str(enc_val4[i-1])))
  638.  
  639. self.tableWidget.setItem(6,i, QTableWidgetItem(str(frs_val1[i-1])))
  640. self.tableWidget.setItem(7,i, QTableWidgetItem(str(frs_val2[i-1])))
  641. self.tableWidget.setItem(8,i, QTableWidgetItem(str(frs_val3[i-1])))
  642.  
  643. self.canvas.draw()
  644. self.StopAll_Handler()
  645.  
  646. def Button_Right_Handler(self):
  647. if self.Button_Right.isChecked():
  648. self.led1.setValue(True)
  649. self.led1.repaint()
  650. self.sendType_1()
  651.  
  652. else:
  653. self.led1.value = False
  654. self.led1.repaint()
  655. self.sendStop_1()
  656. def Button_Left_Handler(self):
  657. if self.Button_Left.isChecked():
  658. self.led2.setValue(True)
  659. self.led2.repaint()
  660. self.sendType_2()
  661.  
  662. else:
  663. self.led2.value = False
  664. self.led2.repaint()
  665. self.sendStop_2()
  666. def Button_Top_Handler(self):
  667. if self.Button_Top.isChecked():
  668. self.led3.setValue(True)
  669. self.led3.repaint()
  670. self.sendType_3()
  671. else:
  672. self.led3.value = False
  673. self.led3.repaint()
  674. self.sendStop_3()
  675. def Button_Roll_Handler(self):
  676. if self.Button_Roll.isChecked():
  677. self.led4.setValue(True)
  678. self.led4.repaint()
  679. self.sendType_4()
  680. else:
  681. self.led4.value = False
  682. self.led4.repaint()
  683. self.sendStop_4()
  684. def StopAll_Handler(self):
  685. if self.Button_Right.isChecked():
  686. self.Button_Right.toggle()
  687. if self.Button_Left.isChecked():
  688. self.Button_Left.toggle()
  689. if self.Button_Top.isChecked():
  690. self.Button_Top.toggle()
  691. if self.Button_Roll.isChecked():
  692. self.Button_Roll.toggle()
  693. self.led1.value = False
  694. self.led1.repaint()
  695. self.led2.value = False
  696. self.led2.repaint()
  697. self.led3.value = False
  698. self.led3.repaint()
  699. self.led4.value = False
  700. self.led4.repaint()
  701. self.sendStopAll()
  702. def sendType_1(self):
  703. ser=serial.Serial('COM3',9600)
  704. ser.write(b'1')
  705. ser.close()
  706. time.sleep(1)
  707. print("act: Right")
  708. self.sendSerial()
  709. """
  710. time.sleep(12)
  711. self.led1.value = False
  712. self.led1.repaint()
  713. if self.Button_Right.isChecked():
  714. self.Button_Right.toggle()
  715. self.sendStop_1()
  716. """
  717. def sendType_2(self):
  718. ser=serial.Serial('COM3',9600)
  719. ser.write(b'2')
  720. ser.close()
  721. time.sleep(1)
  722. print("act: Left")
  723. self.sendSerial()
  724. """
  725. time.sleep(12)
  726. self.led2.value = False
  727. self.led2.repaint()
  728. if self.Button_Left.isChecked():
  729. self.Button_Left.toggle()
  730. self.sendStop_2()
  731. """
  732. def sendType_3(self):
  733. ser=serial.Serial('COM3',9600)
  734. ser.write(b'3')
  735. ser.close()
  736. time.sleep(1)
  737. print("act: Top")
  738. self.sendSerial()
  739. """
  740. time.sleep(12)
  741. self.led3.value = False
  742. self.led3.repaint()
  743. if self.Button_Top.isChecked():
  744. self.Button_Top.toggle()
  745. self.sendStop_3()
  746. """
  747. def sendType_4(self):
  748. ser=serial.Serial('COM3',9600)
  749. ser.write(b'4' + b' ')
  750. ser.close()
  751. print("act: Roll")
  752. self.sendSerial()
  753. def sendStop_1(self):
  754. self.light = -1
  755. ser=serial.Serial('COM3',9600)
  756. ser.write(b'-1')
  757. ser.close()
  758. def sendStop_2(self):
  759. self.light = -2
  760. ser=serial.Serial('COM3',9600)
  761. ser.write(b'-2')
  762. ser.close()
  763. def sendStop_3(self):
  764. self.light = -3
  765. ser=serial.Serial('COM3',9600)
  766. ser.write(b'-3')
  767. ser.close()
  768. def sendStop_4(self):
  769. self.light = -4
  770. ser=serial.Serial('COM3',9600)
  771. ser.write(b'-4')
  772. ser.close()
  773. def sendStopAll(self):
  774. self.light = -5
  775. ser=serial.Serial('COM3',9600)
  776. ser.write(b'-5')
  777. ser.close()
  778.  
  779. def createProgressBar(self):
  780. self.progressBar = QProgressBar()
  781. self.progressBar.setRange(0, 10000)
  782. self.progressBar.setValue(0)
  783. timer = QTimer(self)
  784. timer.timeout.connect(self.advanceProgressBar)
  785. timer.start(1000)
  786.  
  787. if __name__ == '__main__':
  788. import sys
  789. app = QApplication(sys.argv)
  790. gallery = WidgetGallery()
  791. gallery.show()
  792. sys.exit(app.exec_())
  793.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!