8000 Added an AuxMotors section to robot.cfg and added to __init__ in Driv… · WesleyJ-128/FLL_Python_Codebase@bdb44cb · GitHub
[go: up one dir, main page]
Skip to content

Commit bdb44cb

Browse files
WesleyJ-128WesleyJ-128
committed
Added an AuxMotors section to robot.cfg and added to __init__ in DriveLibraries.py to allow for medium motor usage. Added AuxMotorBumpStop to DriveLibraries.py
1 parent 2693552 commit bdb44cb

File tree

3 files changed

+49
-2
lines changed

3 files changed

+49
-2
lines changed

‎FLLMaster/DriveLibraries.py

Lines changed: 44 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,13 @@ def __init__(self, filename):
3333
self.TouchPort = eval(conf.get('Sensors', 'TouchPort'))
3434
self.UltrasonicPort = eval(conf.get('Sensors', 'UltrasonicPort'))
3535

36+
self.AuxMotor1 = eval(conf.get('AuxMotors', 'AuxMotor1'))
37+
self.AuxMotor2 = eval(conf.get('AuxMotors', 'AuxMotor2'))
38+
39+
#self.m1 = MediumMotor(self.AuxMotor1)
40+
#self.m2 = MediumMotor(self.AuxMotor2)
41+
42+
3643
self.cs = ColorSensor(self.ColorPort)
3744
self.gs = GyroSensor(self.GyroPort)
3845
self.ir = InfraredSensor(self.InfraredPort)
@@ -275,4 +282,40 @@ def DriveBump(self, Heading, Speed):
275282

276283
# Stop the motors
277284
self.lm.stop()
278-
self.rm.stop()
285+
self.rm.stop()
286+
287+
def AuxMotorBumpStop(self, Speed, Threshold, Port):
288+
if Speed > 75:
289+
Speed = 75
290+
print("Speed must be between -75 and 75 (inclusive).")
291+
elif Speed < -75:
292+
Speed = -75
293+
print("Speed must be between -75 and 75 (inclusive).")
294+
if Threshold <= 0:
295+
print("Threshold must be greater than zero and less than one")
296+
return
297+
elif Threshold > 100:
298+
print("Threshold must be greater than zero and less than or equal to 100")
299+
return
300+
301+
target = abs((self.lm.max_speed * Speed) / 100)
302+
303+
if Port == self.AuxMotor1:
304+
msNative = (Speed * self.m1.max_speed) / 100
305+
self.m1.on(SpeedNativeUnits(msNative))
306+
time.sleep(0.5)
307+
motrspeed = abs(self.m1.speed)
308+
else:
309+
msNative = (Speed * self.m2.max_speed) / 100
310+
self.m2.on(SpeedNativeUnits(msNative))
311+
time.sleep(0.5)
312+
motrspeed = abs(self.m2.speed)
313+
while motrspeed > (target * Threshold) / 100:
314+
if Port == self.AuxMotor1:
315+
motrspeed = abs(self.m1.speed)
316+
else:
317+
motrspeed = abs(self.m2.speed)
318+
if Port == self.AuxMotor1:
319+
self.m1.off()
320+
else:
321+
self.m2.off()

‎FLLMaster/Main.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,4 +7,4 @@
77
R1.zeroGyro()
88
#R1.spkr.speak("Starting")
99
R1.spkr.beep()
10-
R1.DriveBump(0, 30)
10+
R1.AuxMotorBumpStop(-50, 100, OUTPUT_A)

‎FLLMaster/robot.cfg

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,5 +17,9 @@ InfraredPort=INPUT_4
1717
TouchPort= 0
1818
UltrasonicPort=INPUT_3
1919

20+
[AuxMotors]
21+
AuxMotor1 = OUTPUT_D
22+
AuxMotor2 = OUTPUT_A
23+
2024
[Other]
2125
ForFLL = TRUE

0 commit comments

Comments
 (0)
0