Skip to content

Commit dc47562

Browse files
WesleyJ-128WesleyJ-128
committed
Finished configbuilder
Fixed documentation on DriveLibraries.
1 parent 878d867 commit dc47562

File tree

4 files changed

+217
-19
lines changed

4 files changed

+217
-19
lines changed

‎FLLMaster/AutoConfigBuilder.py‎

Lines changed: 201 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,15 @@
11
#!/usr/bin/env python3
22

33
from configparser import ConfigParser
4+
from time import sleep
45
from ev3dev2.display import *
56
from ev3dev2.button import *
6-
from ev3dev2.sensor import lego
7+
from ev3dev2.sensor import lego, INPUT_1, INPUT_2, INPUT_3, INPUT_4
78
from ev3dev2.motor import *
89
import re
9-
from time import sleep
10+
from ev3dev2.sensor.lego import GyroSensor, UltrasonicSensor
11+
from ev3dev2.sound import Sound
12+
from sys import stderr
1013

1114

1215
def configBuilder(configFile='robot.cfg'):
@@ -16,21 +19,44 @@ def configBuilder(configFile='robot.cfg'):
1619
global lego
1720
btn = Button()
1821
disp = Display()
22+
spkr = Sound()
1923
config = ConfigParser()
2024
config['Drivebase'] = {}
2125
config['Sensors'] = {}
2226
config['AuxMotors'] = {}
2327
config['Other'] = {}
2428
outPorts = ['OUTPUT_A', 'OUTPUT_B', 'OUTPUT_C', 'OUTPUT_D']
2529
senTypes = ['Color', 'Gyro', 'Touch', 'Ultrasonic', 'Infrared']
30+
31+
disp.text_pixels("Is this robot for FLL?")
32+
disp.text_pixels("Y", False, 10, 110)
33+
disp.text_pixels("N", False, 150, 110)
34+
disp.update()
35+
spkr.beep()
36+
while True:
37+
while not btn.any():
38+
pass
39+
selection = btn.buttons_pressed
40+
if selection[0] == 'left':
41+
btn.wait_for_released('left')
42+
config['Other']['ForFLL'] = 'TRUE'
43+
break
44+
elif selection[0] == 'right':
45+
btn.wait_for_released('right')
46+
config['Other']['ForFLL'] = 'FALSE'
47+
break
48+
else:
49+
spkr.beep()
50+
spkr.beep('-f 380')
51+
disp.clear()
52+
2653

2754
for i in senTypes:
2855
try:
2956
sensorClass = getattr(lego, i + 'Sensor')
3057
mySensor = sensorClass()
3158
port = str(mySensor.address)
3259
port = re.sub('.*in([0-9]).*', 'INPUT_\g<1>', port)
33-
globals()[i] = port
3460
config['Sensors'][i + 'Port'] = port
3561
except:
3662
config['Sensors'][i + 'Port'] = 'None'
@@ -47,25 +73,184 @@ def configBuilder(configFile='robot.cfg'):
4773
disp.text_pixels("right for right motor,", False, 0, 30)
4874
disp.text_pixels("up for Aux1, or down for Aux2", False, 0, 40)
4975
disp.update()
50-
while not btn.any():
76+
spkr.beep()
77+
while True:
78+
while not btn.any():
79+
pass
80+
selection = btn.buttons_pressed
81+
if selection[0] == 'left':
82+
btn.wait_for_released('left')
83+
config['Drivebase']['LeftMotorPort'] = i
84+
break
85+
elif selection[0] == 'right':
86+
btn.wait_for_released('right')
87+
config['Drivebase']['RightMotorPort'] = i
88+
break
89+
elif selection[0] == 'up':
90+
btn.wait_for_released('up')
91+
config['AuxMotors']['AuxMotor1'] = i
92+
break
93+
elif selection[0] == 'down':
94+
btn.wait_for_released('down')
95+
config['AuxMotors']['AuxMotor2'] = i
96+
break
97+
else:
98+
spkr.beep()
99+
spkr.beep('-f 380')
100+
except:
101+
pass
102+
103+
for i in outPorts:
104+
try:
105+
motor = MediumMotor(eval(i))
106+
disp.text_pixels("Found a Med Motor at port " + i[-1])
107+
disp.text_pixels("Which AuxMotor is this?", False, 0, 10)
108+
disp.text_pixels("1", False, 10, 110)
109+
disp.text_pixels("2", False, 150, 110)
110+
disp.update()
111+
spkr.beep()
112+
while True:
113+
while not btn.any():
114+
pass
115+
selection = btn.buttons_pressed
116+
if selection[0] == 'left':
117+
btn.wait_for_released('left')
118+
config['AuxMotors']['AuxMotor1'] = i
119+
break
120+
elif selection[0] == 'right':
121+
btn.wait_for_released('right')
122+
config['AuxMotors']['AuxMotor2'] = i
123+
break
124+
else:
125+
spkr.beep()
126+
spkr.beep('-f 380')
127+
except:
128+
pass
129+
130+
mtrs = MoveTank(eval(config['Drivebase']['LeftMotorPort']), eval(config['Drivebase']['RightMotorPort']))
131+
132+
if config['Sensors']['UltrasonicPort'] is not 'None':
133+
us = UltrasonicSensor(eval(config['Sensors']['UltrasonicPort']))
134+
disp.text_pixels("Does the Ultrasonic sensor")
135+
disp.text_pixels("face foward?", False, 0, 10)
136+
disp.text_pixels("Y", False, 10, 110)
137+
disp.text_pixels("N", False, 150, 110)
138+
disp.update()
139+
while not btn.any():
51140
pass
52-
selection = btn.buttons_pressed
53-
if selection[0] == 'left':
54-
btn.wait_for_released('left')
55-
lm = motor
56-
config['Drivebase']['LeftMotorPort'] = i
57-
elif selection[0] == 'right':
58-
btn.wait_for_released('right')
59-
rm = motor
60-
config['Drivebase']['RightMotorPort'] = i
141+
selection = btn.buttons_pressed[0]
142+
if selection == 'left':
143+
usNav = True
144+
elif selection == 'right':
145+
usNav = False
146+
else:
147+
usNav = False
148+
spkr.beep()
149+
else:
150+
usNav = False
151+
us = None
152+
153+
if usNav:
154+
disp.text_pixels("Place the robot facing a wall, ")
155+
disp.text_pixels("and press any button.", False, 0, 10)
156+
disp.update()
157+
while not btn.any():
158+
pass
159+
circsum = 0
160+
sign = 1
161+
tests = 3
162+
for j in range(tests):
163+
if j / 2 == round(j / 2):
164+
parity = 1
165+
else:
166+
parity = -1
167+
usSum = 0
168+
readings = 5
169+
for i in range(readings):
170+
usSum += us.distance_centimeters
171+
sleep(0.1)
172+
stavg = round(usSum / readings, 2)
173+
usSum = 0
174+
print(stavg, file=stderr)
175+
mtrs.on_for_rotations(parity * 25, parity * 25, 1)
176+
for i in range(readings):
177+
usSum += us.distance_centimeters
178+
sleep(0.1)
179+
enavg = round(usSum / readings, 2)
180+
print(enavg, file=stderr)
181+
circsum += abs(enavg - stavg)
182+
sign = (enavg - stavg) / abs(enavg - stavg)
183+
avg = round(circsum / tests, 2)
184+
config['Drivebase']['WheelCircumfrence'] = str(avg)
61185

62-
except:
186+
polarity = 'normal'
187+
if sign < 0:
188+
config['Drivebase']['MotorsInverted'] = 'FALSE'
189+
polarity = 'normal'
190+
elif sign > 0:
191+
config['Drivebase']['MotorsInverted'] = 'TRUE'
192+
polarity = 'inversed'
193+
194+
mtrs.set_polarity(polarity)
195+
disp.text_pixels("Place robot in clear area,")
196+
disp.text_pixels("and press any button.", False, 0, 10)
197+
disp.update()
198+
while not btn.any():
63199
pass
200+
gs = GyroSensor(eval(config['Sensors']['GyroPort']))
201+
widthbetweenwheelssum = 0
202+
ang = 0
203+
for i in range(tests):
204+
gs._ensure_mode(gs.MODE_GYRO_RATE)
205+
gs._ensure_mode(gs.MODE_GYRO_ANG)
206+
mtrs.on_for_degrees(25, -25, 360)
207+
sleep(0.5)
208+
ang = gs.angle
209+
wbw = abs((360 * float(config['Drivebase']['WheelCircumfrence'])) / (ang * math.pi))
210+
widthbetweenwheelssum += wbw
211+
config['Drivebase']['WidthBetweenWheels'] = str(round(widthbetweenwheelssum / tests, 2))
212+
213+
if ang > 0:
214+
config['Drivebase']['GyroInverted'] = 'FALSE'
215+
elif ang < 0:
216+
config['Drivebase']['GyroInverted'] = 'TRUE'
217+
218+
else:
219+
spkr.beep()
220+
spkr.beep()
221+
config['Drivebase']['WheelCircumfrence'] = ''
222+
config['Drivebase']['WidthBetweenWheels'] = ''
223+
config['Drivebase']['MotorsInverted'] = ''
224+
config['Drivebase']['GyroInverted'] = ''
225+
disp.text_pixels("The config file is missing some")
226+
disp.text_pixels("values that will need to be", False, 0, 10)
227+
disp.text_pixels("filled in before use.", False, 0, 20)
228+
disp.text_pixels("Press any button to exit.", False, 0, 40)
229+
disp.update()
230+
while not btn.any():
231+
pass
232+
233+
config['Drivebase']['GearRatio'] = '1'
234+
config['Drivebase']['kp'] = '1'
235+
config['Drivebase']['ki'] = '0'
236+
config['Drivebase']['kd'] = '0.5'
237+
config['Sensors']['kpLine'] = '0.5'
238+
config['Sensors']['kiLine'] = '0'
239+
config['Sensors']['kdLine'] = '2'
240+
241+
spkr.beep()
242+
spkr.beep()
243+
disp.text_pixels("The PID gains in the config")
244+
disp.text_pixels("file are placeholder values", False, 0, 10)
245+
disp.text_pixels("and should be manually tuned.", False, 0, 20)
246+
disp.text_pixels("Press any button to end.", False, 0, 40)
247+
disp.update()
248+
while not btn.any():
249+
pass
64250

65251
with open(configFile, 'w') as configfile:
66252
config.write(configfile)
67253

68254

69255
if __name__ == "__main__":
70-
configBuilder('robot2.cfg')
71-
256+
configBuilder('robot2.cfg')

‎FLLMaster/DriveLibraries.py‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -50,8 +50,7 @@ def __init__(self, filename):
5050
# Read and store the wheel circumference and width between the wheels
5151
self.WheelCircumference = float(conf.get('Drivebase', 'WheelCircumference'))
5252
self.WidthBetweenWheels = float(conf.get('Drivebase', 'WidthBetweenWheels'))
53-
# Read and store MotorInverted and GyroInverted. GyroInverted shoould be true if the gyro is inverted relative to the motors,
54-
# and MotorInverted should be true if the drive motors are upside-down. Drive functions will not work correctly if these
53+
# Read and store MotorInverted and GyroInverted. Both are relative to the robot, and drive functions will not work correctly if these
5554
# values are incorrect, and they are the first things to check if the drive functions do not work correctly.
5655
self.MotorInverted = bool(conf.get('Drivebase', 'MotorInverted') == "TRUE")
5756
self.GyroInverted = bool(conf.get('Drivebase', 'GyroInverted') == "TRUE")

‎FLLMaster/robot2.cfg‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,29 @@
11
[Drivebase]
22
leftmotorport = OUTPUT_B
33
rightmotorport = OUTPUT_C
4+
wheelcircumfrence = 29.91
5+
motorsinverted = TRUE
6+
widthbetweenwheels = 11.57
7+
gyroinverted = TRUE
8+
gearratio = 1
9+
kp = 1
10+
ki = 0
11+
kd = 0.5
412

513
[Sensors]
614
colorport = INPUT_1
715
gyroport = INPUT_2
816
touchport = None
917
ultrasonicport = INPUT_3
1018
infraredport = INPUT_4
19+
kpline = 0.5
20+
kiline = 0
21+
kdline = 2
1122

1223
[AuxMotors]
24+
auxmotor1 = OUTPUT_A
25+
auxmotor2 = OUTPUT_D
1326

1427
[Other]
28+
forfll = TRUE
1529

‎FLLMaster/test.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,3 @@
11
#!/usr/bin/env python3
22

3-
# Program for testing stuff without all of the framework; less things to go wrong
3+
# Program for testing stuff without all of the framework; less things to go wrong

0 commit comments

Comments
 (0)