-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.py
104 lines (79 loc) · 3.31 KB
/
main.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
import serial
import time
from networktables import NetworkTables
# i2c = board.I2C() # uses board.SCL and board.SDA
NetworkTables.initialize("10.76.72.13")
table = NetworkTables.getTable("joystick")
xAxisVal = yAxisVal = x2AxisVal = y2AxisVal = 0
condition1 = condition2 = condition3 = condition4 = 0
frri_val = frle_val = reri_val = rele_val = frriup_val = frleup_val = reriup_val = releup_val = 1500
joystickMessage = "" # Incoming message from Joystick
serialMessage = "" # Data that we send to Arduino
mode = 0 # There are three mods: Teleop (0), Autonomus (1) and Security (2)
arduino = serial.Serial(port="/dev/ttyUSB0", baudrate=115200, timeout=0.01)
maxValue = 1940
minValue = 1060
baseValue = "15001500150015000000"
time.sleep(3)
def write_read(com, message):
com.write(bytes(message, 'utf-8'))
time.sleep(0.01)
data = arduino.readline()
return data
def fix_motor_direction(value):
return 3000 - value
def limit_value(value, min_val, max_val):
return min(max(value, min_val), max_val)
for x in range(20):
write_read(arduino, baseValue)
time.sleep(0.04)
print("Calibration")
while True:
joystickMessage = table.getString("value", baseValue)
# print(joystickMessage)
xAxisVal = int(joystickMessage[:4])
yAxisVal = int(joystickMessage[4:8])
x2AxisVal = int(joystickMessage[8:12])
y2AxisVal = int(joystickMessage[12:16])
# print(yAxisVal, xAxisVal, x2AxisVal, y2AxisVal)
condition1 = int(joystickMessage[16])
condition2 = int(joystickMessage[17])
condition3 = int(joystickMessage[18])
condition4 = int(joystickMessage[19])
# front-right-up // rear-right-up
frriup_val = frleup_val = reriup_val = releup_val = 3000 - xAxisVal
frri_val = 1500 + (x2AxisVal - 1500) - (y2AxisVal - 1500) - (yAxisVal - 1500)
frle_val = 1500 + (x2AxisVal - 1500) + (y2AxisVal - 1500) + (yAxisVal - 1500)
reri_val = 1500 + (x2AxisVal - 1500) + (y2AxisVal - 1500) - (yAxisVal - 1500)
rele_val = 1500 + (x2AxisVal - 1500) - (y2AxisVal - 1500) + (yAxisVal - 1500)
### Fixing reversed motors:
frri_val = fix_motor_direction(frri_val)
frle_val = fix_motor_direction(frle_val)
reri_val = fix_motor_direction(reri_val)
rele_val = fix_motor_direction(rele_val)
frriup_val = fix_motor_direction(frriup_val)
frleup_val = 3000 - fix_motor_direction(frleup_val)
#frleup_val = fix_motor_direction(frleup_val)
reriup_val = fix_motor_direction(reriup_val)
releup_val = 3000 - fix_motor_direction(releup_val)
#releup_val = fix_motor_direction(releup_val)
frri_val = limit_value(frri_val, minValue, maxValue)
frle_val = limit_value(frle_val, minValue, maxValue)
reri_val = limit_value(reri_val, minValue, maxValue)
rele_val = limit_value(rele_val, minValue, maxValue)
frriup_val = limit_value(frriup_val, minValue, maxValue)
frleup_val = limit_value(frleup_val, minValue, maxValue)
reriup_val = limit_value(reriup_val, minValue, maxValue)
releup_val = limit_value(releup_val, minValue, maxValue)
serialMessage = (
str(int(frri_val))
+ str(int(frle_val))
+ str(int(reri_val))
+ str(int(rele_val))
+ str(frriup_val)
+ str(frleup_val)
+ str(reriup_val)
+ str(releup_val)
)
print(write_read(arduino, serialMessage))
print(serialMessage)