ME 405 Romi
Loading...
Searching...
No Matches
pc_micro_reader.py
Go to the documentation of this file.
1from serial import Serial
2from matplotlib import pyplot
3from time import sleep
4from datetime import datetime
5import csv
6
10NUMBERS_OF_TESTS = 10
11VALUES_SENT = 50
12period=13/1000
13motor_gains_l = []
14time_constnats_l = []
15motor_gains_r = []
16time_constnats_r = []
17test_end=1
18arc_lim = 100 #arc radius in mm
19velo_lim = 300 #velocity in mm/s
20angular_lim = 10 #angular velocity in rad/s
21test_done=True
22
23
24def saveHeaders():
25 thead, dhead = ser.readline().decode().strip().split(",")
26 #print(thead, dhead)
27 return thead, dhead
28
29
30def saveData():
31 t,d = map(float, ser.readline().decode().strip().split(","))
32 return t, d
33
34
40def plotData(times, data, thead, dhead, legend):
41 pyplot.plot(times,data, label = legend)
42 pyplot.xlabel(thead)
43 pyplot.ylabel(dhead)
44
49def average(data_list):
50 sum = 0
51 for i in data_list:
52 sum += i
53 return sum/len(data_list)
54
59def timeConstant(data):
60 steady_state = data[-1]
61 tau_val= 0.63*steady_state
62 for i, value in enumerate(data):
63 if value > tau_val:
64 tau= i*period
65 return tau
66 return
67
73def motorGain(test_num, data):
74 steady_state = data[-1]
75 if test_num <= NUMBERS_OF_TESTS:
76 motor_gain = steady_state/(test_num/10)
77 elif test_num <= 2* NUMBERS_OF_TESTS:
78 motor_gain = steady_state/((test_num-NUMBERS_OF_TESTS)/10)
79 return motor_gain
80
84def get_test_plots():
85
86 values_read = 0
87 times = []
88 data = []
89 times2 = []
90 data2 = []
91 while not ser.in_waiting: continue
92 thead, dhead = saveHeaders()
93 while True:
94 if values_read < VALUES_SENT:
95 if ser.in_waiting:
96 t, d = saveData()
97 times.append(t)
98 data.append(d)
99 values_read+=1
100 elif values_read>=VALUES_SENT:
101 if ser.in_waiting:
102 t2, d2 = saveData()
103 times2.append(t2)
104 data2.append(d2)
105 values_read+=1
106 if values_read==2*VALUES_SENT:
107 #print('plotting values')
108 pyplot.figure()
109 # print(times, data)
110 # print(times2, data2)
111 plotData(times, data, thead, dhead, "Left")
112 plotData(times2, data2, thead, dhead, "Right")
113 timestamp=datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
114 pyplot.title(f"Velocity_{timestamp}")
115 pyplot.legend()
116 pyplot.xlim(0,VALUES_SENT)
117 pyplot.ylim(0,400)
118 pyplot.savefig(f'Velocity_{timestamp}.svg')
119 pyplot.clf()
120 break
121
122
123
124 # while ser.in_waiting:
125
126 # print(ser.readline().decode().strip())
127
128 # ser.write(str(0).encode())
129 # while not ser.in_waiting: continue
130 # while ser.in_waiting:
131
132 # print(ser.readline().decode().strip())
133
134 # print(f'{values_read} values_read')
135 # if values_read == 0:
136 # thead, dhead = saveHeaders()
137
138
139 # if values_read < VALUES_SENT:
140 # times = []
141 # data = []
142 # times, data = saveData(times, data)
143
144 # elif values_read < 2* VALUES_SENT:
145 # times2 = []
146 # data2 = []
147 # times2, data2 = saveData(times, data)
148
149 # elif values_read == 2* VALUES_SENT:
150 # plotData(times, data, thead, dhead, "Right")
151 # plotData(times2, data2, thead, dhead, "Left")
152 # timestamp=datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
153 # pyplot.title("Velocity")
154 # pyplot.legend()
155 # pyplot.savefig(f'Velocity_{timestamp}.svg')
156 # pyplot.clf()
157 # values_read += 1
158 return
159
163def run_tests():
164
165
166 while True:
167
168 user_input = input("Send a command: s=full motor step response, t=test, 0=stop romi, 1=straight, 2=Arc turn, 3= pivot, q=quit: ")
169
170 if user_input=='q':
171 print('exiting test loop')
172 ser.write(str(0).encode())
173 break
174
175 if user_input =='s':
176 ser.write(str(user_input).encode())
177 continue
178 if user_input =='t':
179 ser.write(str(user_input).encode())
180 while True:
181 test_input=input("Send a command: 0=stop romi, 1=straight, 2=Arc turn, 3= pivot: ")
182 try:
183 test_input=int(test_input)
184 except ValueError:
185 #print("please select: 0=stop romi, 1=straight, 2=Arc turn, 3= pivot")
186 continue
187 if test_input==1:
188 ser.write(str(test_input).encode())
189 while True:
190 j=input(f"please select a velocity in mm/s from {-velo_lim} to {velo_lim}: ")
191 try:
192 j=int(j)
193 if j>=-velo_lim and j<=velo_lim and j!=0:
194
195 ser.write(str(j).encode())
196
197 get_test_plots()
198 break
199 #else:
200 #print(f"please select a non zeroint from {-velo_lim} to {velo_lim}")
201 except ValueError:
202 #print(f"please select a non zeroint from {-velo_lim} to {velo_lim}")
203 continue
204 break
205 elif test_input==2:
206 ser.write(str(test_input).encode())
207 while True:
208 j = input(f"Please enter a non zero integer arc radius in mm from {-arc_lim} to {arc_lim}: ")
209 try:
210 j = int(j)
211 if j>=-arc_lim and j<= arc_lim and j!=0:
212 ser.write(str(j).encode())
213 get_test_plots()
214 break
215 # else:
216 # print(f"Please enter a non zero integer from {-arc_lim} to {arc_lim}")
217 except ValueError:
218 #print(f"Please select a non zero integer from {-arc_lim} to {arc_lim}")
219 continue
220 break
221 elif test_input==3:
222 ser.write(str(test_input).encode())
223 while True:
224 j = input(f"Please enter an integer arc radius in mm from {-angular_lim} to {angular_lim}: ")
225 try:
226 j = int(j)
227 if j>=-angular_lim and j<= angular_lim and j!=0:
228 ser.write(str(j).encode())
229 get_test_plots()
230 break
231 # else:
232 # print(f"Please enter a non zero integer from {-angular_lim} to {angular_lim}")
233 except ValueError:
234 #print(f"Please select a non zero integer from {-angular_lim} to {angular_lim}")
235 continue
236 break
237 elif test_input==0:
238 ser.write(str(test_input).encode())
239 continue
240 continue
241 try:
242 user_input= int(user_input)
243 except ValueError:
244 #print("please select: s=full motor step response, t=test, 0=stop romi, 1=straight, 2=Arc turn, 3= pivot, q=quit\n1")
245 continue
246 if user_input==1:
247 ser.write(str(user_input).encode())
248 while True:
249 n=input(f"please select a velocity in mm/s from {-velo_lim} to {velo_lim}: ")
250 try:
251 n=int(n)
252 if n>=-velo_lim and n<=velo_lim and n!=0:
253 ser.write(str(n).encode())
254 break
255 # else:
256 # print(f"Please enter a non zero integer from {-velo_lim} to {velo_lim}")
257 except ValueError:
258 #print(f"please select a non zeroint from {-velo_lim} to {velo_lim}")
259 continue
260 continue
261 elif user_input==2:
262 ser.write(str(user_input).encode())
263 while True:
264 n = input(f"Please enter a non zero integer arc radius in mm from {-arc_lim} to {arc_lim}: ")
265 try:
266 n = int(n)
267 if n>=-arc_lim and n<= arc_lim and n!=0:
268 ser.write(str(n).encode())
269 break
270 # else:
271 # print(f"Please enter a non zero integer from {-arc_lim} to {arc_lim}")
272 except ValueError:
273 #print(f"Please select a non zero integer from {-arc_lim} to {arc_lim}")
274 continue
275 continue
276 elif user_input==3:
277 ser.write(str(user_input).encode())
278 while True:
279 n = input(f"Please enter an integer arc radius in mm from {-angular_lim} to {angular_lim}: ")
280 try:
281 n = int(n)
282 if n>=-angular_lim and n<= angular_lim and n!=0:
283 ser.write(str(n).encode())
284 break
285 # else:
286 # print(f"Please enter a non zero integer from {-angular_lim} to {angular_lim}")
287 except ValueError:
288 #print(f"Please select a non zero integer from {-angular_lim} to {angular_lim}")
289 continue
290 continue
291 elif user_input==0:
292 ser.write(str(user_input).encode())
293 continue
294 return
295
296def line_test():
297
298 print ("Opening serial port")
299 sleep(0.5)
300
301 print ("Flushing serial port")
302 ser.reset_input_buffer()
303
304 while True:
305
306 black_test=input("select b when on black: ")
307
308 if black_test=="b":
309 ser.write(str(1).encode())
310 print("Black test done")
311
312 white_test=input("select w when on white: ")
313 if white_test=="w":
314 ser.write(str(1).encode())
315 print("White test done: ")
316
317 break
318 else:
319 continue
320 else:
321
322 continue
323 return "Black and white test complete"
324
325while True:
326 with Serial(port ="COM4", baudrate = 115200, timeout=1) as ser:
327 user_input=input("press c for calibrate, t for step test, q to quit, g to go, i for imu data")
328 if user_input=="c":
329 line_test()
330 continue
331 elif user_input=='t':
332
333 run_tests()
334 elif user_input=='g':
335 ser.write(str('g').encode())
336 continue
337 elif user_input=='q':
338 ser.write(str(0).encode())
339 elif user_input=="i":
340 ser.write(str('i').encode())
341
342
343
344 # test_num = 0
345 # values_read = 0
346 # open("output_LP.csv", "w").close()
347 # open("output_RP.csv", "w").close()
348 # open("output_LV.csv", "w").close()
349 # open("output_RV.csv", "w").close()
350
351 # # Forever loop
352
353
354 # while 1:
355 # # Block until something comes from bluetooth
356 # while not ser.in_waiting: continue
357 # test_num += 1
358 # # sleep(.5)
359
360 # # First ten tests
361 # if test_num == 1:
362 # thead, dhead = saveHeaders()
363 # if test_num <= NUMBERS_OF_TESTS:
364 # if values_read < 20:
365 # times = []
366 # data = []
367
368 # times, data = saveData(times, data)
369 # time_constnats_l.append(timeConstant(data))
370 # motor_gains_l.append(motorGain(test_num, data))
371
372
373 # with open("output_LV.csv", "a", newline="") as f:
374 # writer = csv.writer(f)
375 # writer.writerow(["Times", "Data"])
376 # for t, d in zip(times, data):
377 # writer.writerow([t, d])
378
379 # plotData(times, data, thead, dhead)
380 # if test_num == NUMBERS_OF_TESTS:
381 # pyplot.title("Left Velocity")
382 # pyplot.savefig("Velocity_Left.svg")
383 # pyplot.clf()
384 # print(f" average motor gain left: {average(motor_gains_l)}")
385 # print(f" average time const left: {average(time_constnats_l)}")
386 # values_read += 1
387 # else:
388 # values_read = 0
389 # # Second ten tests
390 # elif test_num <= 2*NUMBERS_OF_TESTS:
391 # if values_read < VALUES_SENT:
392 # times = []
393 # data = []
394 # times, data = saveData(times, data)
395
396 # time_constnats_r.append(timeConstant(data))
397 # motor_gains_r.append(motorGain(test_num, data))
398
399 # with open("output_RV.csv", "a", newline="") as f:
400 # writer = csv.writer(f)
401 # writer.writerow(["Times", "Data"])
402 # for t, d in zip(times, data):
403 # writer.writerow([t, d])
404 # plotData(times, data, thead, dhead)
405
406 # if test_num == 2 * NUMBERS_OF_TESTS:
407 # pyplot.title("Right Velocity")
408 # pyplot.savefig("Velocity_Right.svg")
409 # pyplot.clf()
410
411 # print(f"average motor gain right:{average(motor_gains_r)}")
412 # print(motor_gains_r)
413 # print(f"average time const right:{average(time_constnats_r)}")
414 # values_read += 1
415 # else:
416 # values_read = 0
417 # # Third ten tests
418 # elif test_num == (2 * NUMBERS_OF_TESTS) + 1:
419 # print("About to save them headers")
420 # thead, dhead = saveHeaders()
421 # print("headers saved")
422
423 # elif test_num <= 3 * NUMBERS_OF_TESTS:
424 # if values_read < VALUES_SENT:
425 # times = []
426 # data = []
427 # times, data = saveData(times, data)
428 # with open("output_LP.csv", "a", newline="") as f:
429 # writer = csv.writer(f)
430 # writer.writerow(["Times", "Data"])
431 # for t, d in zip(times, data):
432 # writer.writerow([t, d])
433 # plotData(times, data, thead, dhead)
434 # if test_num == 3 * NUMBERS_OF_TESTS:
435 # pyplot.title("Left Position")
436 # pyplot.savefig("Position_Left.svg")
437 # pyplot.clf()
438
439 # values_read += 1
440 # else:
441 # values_read = 0
442 # # Fourth ten tests
443 # elif test_num <= 4 * NUMBERS_OF_TESTS:
444 # if values_read < VALUES_SENT:
445 # times = []
446 # data = []
447 # times, data = saveData(times, data)
448 # with open("output_RP.csv", "a", newline="") as f:
449 # writer = csv.writer(f)
450 # # writer.writerow(["Times", "Data"])
451 # for t, d in zip(times, data):
452 # writer.writerow([t, d])
453 # plotData(times, data, thead, dhead)
454 # if test_num == 4 * NUMBERS_OF_TESTS:
455 # pyplot.title("Right Position")
456 # pyplot.savefig("Position_Right.svg")
457 # pyplot.clf()
458
459 # values_read += 1
460 # else:
461 # values_read = 0
pc_micro_reader.get_test_plots
get_test_plots()
brief Saves and creates test plots Effects
Definition pc_micro_reader.py:84
pc_micro_reader.saveData
saveData()
brief Saves the data sent from a single test
Definition pc_micro_reader.py:30
pc_micro_reader.saveHeaders
saveHeaders()
brief Reads the header line from the data
Definition pc_micro_reader.py:24
pc_micro_reader.line_test
line_test()
Definition pc_micro_reader.py:296
pc_micro_reader.run_tests
run_tests()
brief Runs the User tests
Definition pc_micro_reader.py:163
pc_micro_reader.plotData
plotData(times, data, thead, dhead, legend)
brief Plots passed data
Definition pc_micro_reader.py:40
pc_micro_reader.average
average(data_list)
brief Averages data
Definition pc_micro_reader.py:49
pc_micro_reader.motorGain
motorGain(test_num, data)
brief Returns motor gain of motor
Definition pc_micro_reader.py:73
pc_micro_reader.timeConstant
timeConstant(data)
brief Returns time cosntant of motor
Definition pc_micro_reader.py:59