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

Author: WesleyJ-128

FLLMaster/DriveLibraries.py

@@ -33,6 +33,13 @@ def __init__(self, filename):
         self.TouchPort = eval(conf.get('Sensors', 'TouchPort'))
         self.UltrasonicPort = eval(conf.get('Sensors', 'UltrasonicPort'))
 
+        self.AuxMotor1 = eval(conf.get('AuxMotors', 'AuxMotor1'))
+        self.AuxMotor2 = eval(conf.get('AuxMotors', 'AuxMotor2'))
+
+        #self.m1 = MediumMotor(self.AuxMotor1)
+        #self.m2 = MediumMotor(self.AuxMotor2)
+
+
         self.cs = ColorSensor(self.ColorPort)
         self.gs = GyroSensor(self.GyroPort)
         self.ir = InfraredSensor(self.InfraredPort)
@@ -275,4 +282,40 @@ def DriveBump(self, Heading, Speed):
 
         # Stop the motors
         self.lm.stop()
-        self.rm.stop()
+        self.rm.stop()
+
+    def AuxMotorBumpStop(self, Speed, Threshold, Port):
+        if Speed > 75:
+            Speed = 75
+            print("Speed must be between -75 and 75 (inclusive).")
+        elif Speed < -75:
+            Speed = -75
+            print("Speed must be between -75 and 75 (inclusive).")
+        if Threshold <= 0:
+            print("Threshold must be greater than zero and less than one")
+            return
+        elif Threshold > 100:
+            print("Threshold must be greater than zero and less than or equal to 100")
+            return
+        
+        target = abs((self.lm.max_speed * Speed) / 100)
+        
+        if Port == self.AuxMotor1:
+            msNative = (Speed * self.m1.max_speed) / 100
+            self.m1.on(SpeedNativeUnits(msNative))
+            time.sleep(0.5)
+            motrspeed = abs(self.m1.speed)
+        else:
+            msNative = (Speed * self.m2.max_speed) / 100
+            self.m2.on(SpeedNativeUnits(msNative))
+            time.sleep(0.5)
+            motrspeed = abs(self.m2.speed)
+        while motrspeed > (target * Threshold) / 100:
+            if Port == self.AuxMotor1:
+                motrspeed = abs(self.m1.speed)
+            else:
+                motrspeed = abs(self.m2.speed)
+        if Port == self.AuxMotor1:
+            self.m1.off()
+        else:
+            self.m2.off()

FLLMaster/Main.py

@@ -7,4 +7,4 @@
 R1.zeroGyro()
 #R1.spkr.speak("Starting")
 R1.spkr.beep()
-R1.DriveBump(0, 30)
+R1.AuxMotorBumpStop(-50, 100, OUTPUT_A)

FLLMaster/robot.cfg

@@ -17,5 +17,9 @@ InfraredPort=INPUT_4
 TouchPort= 0
 UltrasonicPort=INPUT_3
 
+[AuxMotors]
+AuxMotor1 = OUTPUT_D
+AuxMotor2 = OUTPUT_A
+
 [Other]
 ForFLL = TRUE