This repository has been archived by the owner on Mar 6, 2022. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
manualControl.py
62 lines (53 loc) · 1.86 KB
/
manualControl.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
import serial
import time
# change COM6 to /dev/ttyAMA0 in raspberry pi
ser = serial.Serial('/dev/tty/ACM0', 9600, timeout = 0.5, write_timeout=0.5)
velocity = 0
compass = ""
power = 0
stage = 0
data = {}
# flushing data before use
def flush_data():
serial.Serial.flush(ser)
serial.Serial.flushOutput(ser)
serial.Serial.flushInput(ser)
def data_display():
tmp_velocity =0
tmp_power = 0
tmp_direction =""
global stage
global compass
global power
global velocity
while ser.isOpen():
try:
flush_data()
data = ser.readline()
decode_data = data.decode('utf-8', 'ignore') #ignore the error and move to the next
sensor = decode_data.split() # split each data by whitespace
power = float(sensor[0]) // 1000
velocity = float(sensor[1]) * 2.04 / 60 # cm/s
orientation = float(sensor[2])
stage = sensor[3]
if orientation < 45 or orientation >= 315:
compass = "North"
elif 45 <= orientation < 135:
compass = "West"
elif 135 <= orientation < 225:
compass = "South"
elif 225 <= orientation < 315:
compass = "East"
tmp_direction = compass
tmp_power = power
tmp_velocity = velocity
data = {'velocity': velocity, 'power': power, 'direction': compass}
return data
except IndexError:
data = {'velocity': tmp_velocity, 'power': tmp_power, 'direction': tmp_direction}
return data
except ValueError:
data = {'velocity': tmp_velocity, 'power': tmp_power, 'direction': tmp_direction}
return data
def get_stage():
return stage