-
Notifications
You must be signed in to change notification settings - Fork 1
/
robot2.py
140 lines (118 loc) · 4.49 KB
/
robot2.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
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
# import curses and GPIO
import curses
import RPi.GPIO as GPIO
from mpu6050 import mpu6050
import time
import os
# tk
from tkinter import *
from tkmacosx import Button
from PIL import ImageTk, Image # python image library
root = Tk()
root.title("ROBOT ZSŁ")
root.iconbitmap("images/favicon.ico")
def wyswietlGyro(ox,oy,oz):
OX = Label(root, text=f"Wartość OX: {ox} ")
OY = Label(root, text=f"Wartość OX: {oz} ")
OZ = Label(root, text=f"Wartość OX: {oz} ")
OX.grid(row=0, column=0 )
OY.grid(row=0, column=1 )
OZ.grid(row=0, column=2 )
def wyswietlAccel(ox,oy,oz):
OXA = Label(root, text=f"Wartość OX: {ox} ")
OYA = Label(root, text=f"Wartość OX: {oy} ")
OZA = Label(root, text=f"Wartość OX: {oz} ")
OXA.grid(row=1, column=0 )
OYA.grid(row=1, column=1 )
OZA.grid(row=1, column=2 )
GPIO.setwarnings(False)
#set GPIO numbering mode and define output pins
GPIO.setmode(GPIO.BOARD)
motor1a = 7
motor1b = 11
mpu = mpu6050(0x68)
motor2a = 13
motor2b = 16
GPIO.cleanup()
GPIO.setup(motor1a,GPIO.OUT)
GPIO.setup(motor1b,GPIO.OUT)
GPIO.setup(motor2a,GPIO.OUT)
GPIO.setup(motor2b,GPIO.OUT)
GPIO.setup(21, GPIO.OUT)
pwm=GPIO.PWM(21, 50)
pwm.start(0)
def SetAngle(angle):
duty = angle / 18 + 2
GPIO.output(21, True)
pwm.ChangeDutyCycle(duty)
# Get the curses window, turn off echoing of keyboard to screen, turn on
# instant (no waiting) key response, and use special values for cursor keys
screen = curses.initscr()
curses.noecho()
curses.cbreak()
curses.halfdelay(3)
screen.keypad(True)
GPIO.output(motor1a,GPIO.LOW)
GPIO.output(motor1b,GPIO.LOW)
GPIO.output(motor2a,GPIO.LOW)
GPIO.output(motor2b,GPIO.LOW)
SetAngle(90)
try:
while True:
os.system('clear')
print("Temp : "+str(round(mpu.get_temp(),2)))
accel_data = mpu.get_accel_data()
print("Acc X : "+str(round(accel_data['x'],2)))
print("Acc Y : "+str(round(accel_data['y'],2)))
print("Acc Z : "+str(round(accel_data['z'],2)))
wyswietlAccel(str(round(accel_data['x'],2)), str(round(accel_data['y'],2)), str(round(accel_data['z'],2)) )
gyro_data = mpu.get_gyro_data()
print("Gyro X : "+str(round(gyro_data['x'],2)))
print("Gyro Y : "+str(round(gyro_data['y'],2)))
print("Gyro Z : "+str(round(gyro_data['z'],2)))
wyswietlGyro(str(round(gyro_data['x'],2)), str(round(gyro_data['y'],2)), str(round(gyro_data['z'],2)))
time.sleep(0.1)
char = screen.getch()
oldChar = char
if char == ord('q'):
break
elif char == curses.KEY_RIGHT:
GPIO.output(motor1a,GPIO.HIGH)
GPIO.output(motor1b,GPIO.LOW)
GPIO.output(motor2a,GPIO.HIGH)
GPIO.output(motor2b,GPIO.LOW)
elif char == ord('a'):
SetAngle(180)
elif char == ord('d'):
SetAngle(0)
elif char == ord('s'):
SetAngle(90)
elif char == curses.KEY_LEFT:
GPIO.output(motor1a,GPIO.LOW)
GPIO.output(motor1b,GPIO.HIGH)
GPIO.output(motor2a,GPIO.LOW)
GPIO.output(motor2b,GPIO.HIGH)
elif char == curses.KEY_UP:
GPIO.output(motor1a,GPIO.HIGH)
GPIO.output(motor1b,GPIO.LOW)
GPIO.output(motor2a,GPIO.LOW)
GPIO.output(motor2b,GPIO.HIGH)
elif char == curses.KEY_DOWN:
GPIO.output(motor1a,GPIO.LOW)
GPIO.output(motor1b,GPIO.HIGH)
GPIO.output(motor2a,GPIO.HIGH)
GPIO.output(motor2b,GPIO.LOW)
else:
GPIO.output(21, True)
pwm.ChangeDutyCycle(0)
GPIO.output(motor1a,GPIO.LOW)
GPIO.output(motor1b,GPIO.LOW)
GPIO.output(motor2a,GPIO.LOW)
GPIO.output(motor2b,GPIO.LOW)
finally:
#Close down curses properly, inc turn echo back on!
SetAngle(90)
curses.nocbreak(); screen.keypad(0); curses.echo()
curses.endwin()
GPIO.cleanup()
root.mainloop()