From 5c9dbd304260e224cbe95eace74907f6e9867f52 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Thu, 24 Sep 2020 06:49:57 -0400 Subject: [PATCH 01/18] Added dynamic support for auxillary motors; will only attempt to initalize if in config file. --- FLLMaster/DriveLibraries.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/FLLMaster/DriveLibraries.py b/FLLMaster/DriveLibraries.py index 39d96e1..30106e8 100644 --- a/FLLMaster/DriveLibraries.py +++ b/FLLMaster/DriveLibraries.py @@ -1,4 +1,5 @@ # The program doesn't work without this. Not sure why. +from time import sleep from ev3dev2.motor import * from ev3dev2.sensor.lego import * from ev3dev2.sensor import * @@ -79,9 +80,21 @@ def __init__(self, filename): self.AuxMotor1 = eval(conf.get('AuxMotors', 'AuxMotor1')) self.AuxMotor2 = eval(conf.get('AuxMotors', 'AuxMotor2')) - # Instantiate the auxillary motor objects (only if motors are connected) - #self.m1 = MediumMotor(self.AuxMotor1) - #self.m2 = MediumMotor(self.AuxMotor2) + # Instantiate the auxillary motor objects, checking if they are connected first. + if self.AuxMotor1 is not None: + try: + self.m1 = MediumMotor(self.AuxMotor1) + except: + print("Aux Motor 1 Not Connected!") + self.spkr.beep('-f 220') + if self.AuxMotor2 is not None: + try: + self.m2 = MediumMotor(self.AuxMotor2) + except: + print("Aux Motor 2 Not Connected!") + self.spkr.beep('-f 220') + sleep(0.3) + self.spkr.beep('-f 220') # Instantiate the sensor objects From 6525d503dd586bd5448d3146d5fbf102527f4885 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Thu, 24 Sep 2020 18:47:27 -0400 Subject: [PATCH 02/18] Commented Main.py --- FLLMaster/Main.py | 33 +++++++++++++++++++ FLLMaster/Missions.py | 73 ++++++++++++++++++++++--------------------- 2 files changed, 70 insertions(+), 36 deletions(-) diff --git a/FLLMaster/Main.py b/FLLMaster/Main.py index 324ed4f..ac3e3b3 100644 --- a/FLLMaster/Main.py +++ b/FLLMaster/Main.py @@ -1,40 +1,63 @@ #!/usr/bin/env python3 +# Initialize loopIndex variable, used in displaySensor() loopIndex = 0 +# Import only the functions required for setup; +# MenuLib needs them run before importing other things from MenuLib import init, initthread +# Used for printing to the VS Code output window from sys import stderr +# Run the init function from MenuLib, config file name, +# used to instatiate the robot object init('robot.cfg') +# Create the mission process used for running the current mission initthread() +# Now that setup has finished, import everything else from MenuLib from MenuLib import * + +# Calibrate the gyro calibrate() +# Calibrate the color sensor robot.reflectCal() +# Initialization has finished, print status message to VS Code output window print("Finished Init", file=stderr) +# Define the functions for button presses def left(): + # Make sure there is no mission process running; if it is not, decrease count and update the display + # Otherwise, abort the current mission if not mission.is_alive(): minusCount() display() else: abort() def right(): + # Make sure there is no mission process running; if it is not, increase count and update the display + # Otherwise, abort the current mission if not mission.is_alive(): addCount() display() else: abort() def down(): + # Make sure there is no mission process running; if it is not, recalibrate the gyro + # Otherwise, abort the current mission if not mission.is_alive(): calibrate() else: abort() def up(): + # Make sure there is no mission process running; if it is not, recalibrate the color sensor + # Otherwise, abort the current mission if not mission.is_alive(): robot.reflectCal() else: abort() def enter(): + # Make sure there is no mission process running; if it is not, run the current mission, increase count, and update the display + # Otherwise, abort the current mission if not mission.is_alive(): run() addCount() @@ -42,8 +65,10 @@ def enter(): else: abort() +# Print to the VS Code window that the button functions have been defined print("Functions Defined", file=stderr) +# Define a dictionary for calling the functions defined above based on the names for the buttons buttonMap = { 'left': left, 'right': right, @@ -52,14 +77,22 @@ def enter(): 'down': down } +# Print to the VS Code window that the button dictionary has been defined print("Map Defined", file=stderr) +# Start the infinite menu loop while True: + # This updates the status and process id of the mission process every loop cycle from MenuLib import mission + # Store the list of buttons currently pressed as buttonlist buttonlist = robot.btn.buttons_pressed + # Check if there are any buttons pressed; if there is, run the corresponding function if buttonlist: buttonMap[buttonlist[0]]() + # Increment loopIndex and reset to zero every hundreth loop loopIndex = (loopIndex + 1) % 100 + # Make sure there is no mission running (to prevent resource conflicts), + # and if there is none, display the sensor values and update the light color based on the gyro sensor if not mission.is_alive(): checkDrift() displaySensor() \ No newline at end of file diff --git a/FLLMaster/Missions.py b/FLLMaster/Missions.py index 01c0393..36eb2b4 100644 --- a/FLLMaster/Missions.py +++ b/FLLMaster/Missions.py @@ -1,37 +1,38 @@ -if __name__ == "__main__": - from DriveLibraries import * - robot = Robot('robot.cfg') -else: - from MenuLib import robot -# Write Mission Programs Here ---------------------- - -# Mission Names should be prefaced with a##, where ## is two numbers. These -# numbers will not show up on the display, and tell the program what order the -# missions should be in; a00 is first, followed by a01, and so on. It is -# reccomended that spaces are left between the numbers (a00, a05, a10...), to -# allow for future missions. - -def a00Star(): - # Drives a simple 4-pointed star, with arced sides - robot.ArcTurn(-90, 30, 30) - robot.ArcTurn(-90, 30, -30) - robot.ArcTurn(-90, 30, 30) - robot.ArcTurn(-90, 30, -30) - -def a05LineFollow(): - # Follows a line for 50 cm, at 50% speed, stopping at end - robot.LineFollow(50, 50, True) - -def a10Command_Key(): - # Drives in a pattern similar to the Apple command key - # o_o - # | | - # o‾o - robot.DriveAtHeading(0, 50, 30, False) - robot.ArcTurn(270, 10, 30) - robot.DriveAtHeading(270, 50, 30, False) - robot.ArcTurn(270, 10, 30) - robot.DriveAtHeading(270 * 2, 50, 30, False) - robot.ArcTurn(270, 10, 30) - robot.DriveAtHeading(270 * 3, 50, 30, True) +if __name__ == "__main__": + from DriveLibraries import * + robot = Robot('robot.cfg') +else: + from MenuLib import robot + +# Mission Names should be prefaced with a##, where ## is two numbers. These +# numbers will not show up on the display, and tell the program what order the +# missions should be in; a00 is first, followed by a01, and so on. It is +# reccomended that spaces are left between the numbers (a00, a05, a10...), to +# allow for future missions. + +# Write Mission Programs Here ---------------------- + +def a00Star(): + # Drives a simple 4-pointed star, with arced sides + robot.ArcTurn(-90, 30, 30) + robot.ArcTurn(-90, 30, -30) + robot.ArcTurn(-90, 30, 30) + robot.ArcTurn(-90, 30, -30) + +def a05LineFollow(): + # Follows a line for 50 cm, at 50% speed, stopping at end + robot.LineFollow(50, 50, True) + +def a10Command_Key(): + # Drives in a pattern similar to the Apple command key + # o_o + # | | + # o‾o + robot.DriveAtHeading(0, 50, 30, False) + robot.ArcTurn(270, 10, 30) + robot.DriveAtHeading(270, 50, 30, False) + robot.ArcTurn(270, 10, 30) + robot.DriveAtHeading(270 * 2, 50, 30, False) + robot.ArcTurn(270, 10, 30) + robot.DriveAtHeading(270 * 3, 50, 30, True) robot.GyroTurn(0) \ No newline at end of file From 98e89c385cc7f6f93efd2db13805531e94e8dd23 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Thu, 24 Sep 2020 21:10:42 -0400 Subject: [PATCH 03/18] Added support for GearRatio. Made auxillary sensor instantiation dynamic. --- FLLMaster/DriveLibraries.py | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/FLLMaster/DriveLibraries.py b/FLLMaster/DriveLibraries.py index 30106e8..c322bf0 100644 --- a/FLLMaster/DriveLibraries.py +++ b/FLLMaster/DriveLibraries.py @@ -55,8 +55,7 @@ def __init__(self, filename): # values are incorrect, and they are the first things to check if the drive functions do not work correctly. self.MotorInverted = bool(conf.get('Drivebase', 'MotorInverted') == "TRUE") self.GyroInverted = bool(conf.get('Drivebase', 'GyroInverted') == "TRUE") - # Reads and stores the gear ratio value. Currently not used, except for motor direction. - # (1 and -1 work, with -1 meaning a 1:1 ratio that reverses the motor direction) + # Reads and stores the gear ratio value. self.GearRatio = float(conf.get('Drivebase', 'GearRatio')) # Reads and stores the PID gains for driving in a straight line. self.kp = float(conf.get('Drivebase', 'kp')) @@ -96,14 +95,18 @@ def __init__(self, filename): sleep(0.3) self.spkr.beep('-f 220') - # Instantiate the sensor objects self.cs = ColorSensor(self.ColorPort) self.gs = GyroSensor(self.GyroPort) - self.ir = InfraredSensor(self.InfraredPort) - self.us = UltrasonicSensor(self.UltrasonicPort) - - # Instantiate the drive motor objecta, as well as the motorset objects for controlling both simultainiously + # Only instantiate auxillary sensors if the config file shows they exist + if self.InfraredPort is not None: + self.ir = InfraredSensor(self.InfraredPort) + if self.UltrasonicPort is not None: + self.us = UltrasonicSensor(self.UltrasonicPort) + if self.TouchPort is not None: + self.ts = TouchSensor(self.TouchPort) + + # Instantiate the drive motor objects, as well as the motorset objects for controlling both simultainiously self.tank = MoveTank(self.LeftMotor, self.RightMotor) self.steer = MoveSteering(self.LeftMotor, self.RightMotor) self.lm = LargeMotor(self.LeftMotor) @@ -237,7 +240,7 @@ def DriveAtHeading(self, Heading, Distance, Speed, Stop): sign = Speed / abs(Speed) # Find number of degrees that motors need to rotate to reach the desired number of cm. - target = (Distance * 360) / self.WheelCircumference + target = (Distance * 360) / self.WheelCircumference * abs(self.GearRatio) # Find the average of the left and right encoders, as they could be different from PID correction avg = abs((left_motor_change + right_motor_change) / 2) @@ -604,7 +607,7 @@ def LineFollow(self, Distance, Speed, Stop): sign = Speed / abs(Speed) # Find number of degrees that motors need to rotate to reach the desired number of cm. - target = (Distance * 360) / self.WheelCircumference + target = (Distance * 360) / self.WheelCircumference * abs(self.GearRatio) # Find the average of the left and right encoders, as they could be different from PID correction avg = abs((left_motor_change + right_motor_change) / 2) From f15ee08d75173a0af97d5e57edc721190a57ecf4 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Fri, 25 Sep 2020 06:59:56 -0400 Subject: [PATCH 04/18] Fixed comments in Main.py to use docstrings Added a "how to use" section to README.md --- FLLMaster/Main.py | 33 +++++++++++++++++++++++---------- README.md | 19 +++++++++++++++++-- 2 files changed, 40 insertions(+), 12 deletions(-) diff --git a/FLLMaster/Main.py b/FLLMaster/Main.py index ac3e3b3..9859ac2 100644 --- a/FLLMaster/Main.py +++ b/FLLMaster/Main.py @@ -1,5 +1,8 @@ #!/usr/bin/env python3 +# This is the program that should be run during use; it creates the interactive menu. +# It is also the only executable file other than test.py, which is only for testing things. + # Initialize loopIndex variable, used in displaySensor() loopIndex = 0 @@ -26,38 +29,48 @@ # Define the functions for button presses def left(): - # Make sure there is no mission process running; if it is not, decrease count and update the display - # Otherwise, abort the current mission + """ + Called when left button pressed; if there is no mission running, + decrease count and update the display. Otherwise, abort the current mission. + """ if not mission.is_alive(): minusCount() display() else: abort() def right(): - # Make sure there is no mission process running; if it is not, increase count and update the display - # Otherwise, abort the current mission + """ + Called when right button pressed; if there is no mission running, + increase count and update the display. Otherwise, abort the current mission. + """ if not mission.is_alive(): addCount() display() else: abort() def down(): - # Make sure there is no mission process running; if it is not, recalibrate the gyro - # Otherwise, abort the current mission + """ + Called when bottom button pressed; if there is no mission running, + recalibrate the gyro. Otherwise, abort the current mission. + """ if not mission.is_alive(): calibrate() else: abort() def up(): - # Make sure there is no mission process running; if it is not, recalibrate the color sensor - # Otherwise, abort the current mission + """ + Called when top button pressed; if there is no mission running, + recalibrate the color sensor. Otherwise, abort the current mission. + """ if not mission.is_alive(): robot.reflectCal() else: abort() def enter(): - # Make sure there is no mission process running; if it is not, run the current mission, increase count, and update the display - # Otherwise, abort the current mission + """ + Called when center button pressed; if there is no mission running, launch the selected mission, + increase count, and update the display. Otherwise, abort the current mission. + """ if not mission.is_alive(): run() addCount() diff --git a/README.md b/README.md index 9de9705..14ea4e8 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,17 @@ -# FLL_Python_Codebase -Driving libraries and a master program designed for FLL-legal robots. Uses a robot.cfg file for robot parameters, and uses the ev3dev2 python library and brick firmware. +# FLL_Python_Codebase +Driving libraries and a master program designed for FLL-legal robots. Uses a robot.cfg file for robot parameters, and uses the ev3dev2 python library and brick firmware. + +# Use: +The file ``Main.py`` is the only executable file other than ``test.py``, and is the program that creates the menu. This program takes a while to start, so it should be started well before any timers. On startup, the robot will eventually emit the normal beep (A4) signalling that the config file has been read and some menu systems have been initialized. It will then emit a low-pitched beep (different from any other beep) and turn the lights yellow. This is standard procedure for recalibrating the gyro (inside this program, anyway), and the robot should be absolutely still from the first low-pitched beep to the second normal beep. After the second normal beep, the robot will start the color sensor calibration routine, emitting a high-pitched beep. This signals that the color sensor should be placed on the white surface. After pressing the center button, the robot will emit a normal beep signalling that the white surface has been stored. The robot will then emit a low-pitched beep. Again, place the robot on the black surface and press the center button. The robot will emit a normal beep, and the complete setup is done. + +The screen will then show the name of the current mission across the top; and the current gyro and color sensor values (angle and RLI towards the bottom in the center. The lights will also likely begin flickering between red and green. The color of the light is determined by the rate reported by the gyro sensor; if it is within 1 degree/second of zero, the light will turn green. If the gyro reports that it is moving any faster than that, the light will turn red. Therefore, if the robot is not being manipulated and is not running a mission, the light should be green most of the time. If it is not, the gyro is likely drifting and needs to be recalibrated. + +Button functions: +*The center button runs the nission whose name is currently being displayed on the screen. It also advances to the next +mission afterward. +*The left button changes the selection to the previous mission, looping around to the last when pressing left on the first +mission. +*The right button works the same way as the left, but moving foward instead of back +*The bottom button recalibrates the gyro +*The top button recalibrates the color sensor +When a mission is running, all buttons lose their normal functioon, and will abort the current mission. From 3fc1f284530b071915b80f88150f37fb0145c58e Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Fri, 25 Sep 2020 07:02:40 -0400 Subject: [PATCH 05/18] Fixed formatting in README.md --- README.md | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 14ea4e8..74d6233 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,15 @@ # FLL_Python_Codebase Driving libraries and a master program designed for FLL-legal robots. Uses a robot.cfg file for robot parameters, and uses the ev3dev2 python library and brick firmware. -# Use: +## Use: The file ``Main.py`` is the only executable file other than ``test.py``, and is the program that creates the menu. This program takes a while to start, so it should be started well before any timers. On startup, the robot will eventually emit the normal beep (A4) signalling that the config file has been read and some menu systems have been initialized. It will then emit a low-pitched beep (different from any other beep) and turn the lights yellow. This is standard procedure for recalibrating the gyro (inside this program, anyway), and the robot should be absolutely still from the first low-pitched beep to the second normal beep. After the second normal beep, the robot will start the color sensor calibration routine, emitting a high-pitched beep. This signals that the color sensor should be placed on the white surface. After pressing the center button, the robot will emit a normal beep signalling that the white surface has been stored. The robot will then emit a low-pitched beep. Again, place the robot on the black surface and press the center button. The robot will emit a normal beep, and the complete setup is done. The screen will then show the name of the current mission across the top; and the current gyro and color sensor values (angle and RLI towards the bottom in the center. The lights will also likely begin flickering between red and green. The color of the light is determined by the rate reported by the gyro sensor; if it is within 1 degree/second of zero, the light will turn green. If the gyro reports that it is moving any faster than that, the light will turn red. Therefore, if the robot is not being manipulated and is not running a mission, the light should be green most of the time. If it is not, the gyro is likely drifting and needs to be recalibrated. -Button functions: -*The center button runs the nission whose name is currently being displayed on the screen. It also advances to the next -mission afterward. -*The left button changes the selection to the previous mission, looping around to the last when pressing left on the first -mission. -*The right button works the same way as the left, but moving foward instead of back -*The bottom button recalibrates the gyro -*The top button recalibrates the color sensor -When a mission is running, all buttons lose their normal functioon, and will abort the current mission. +### Button functions: +* The center button runs the nission whose name is currently being displayed on the screen. It also advances to the next mission afterward. +* The left button changes the selection to the previous mission, looping around to the last when pressing left on the first mission. +* The right button works the same way as the left, but moving foward instead of back +* The bottom button recalibrates the gyro +* The top button recalibrates the color sensor +**When a mission is running, all buttons lose their normal functioon, and will abort the current mission.** From 3d2b72d1762dd6d3f602b4e1d9baf7ee553f0021 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Fri, 25 Sep 2020 07:03:36 -0400 Subject: [PATCH 06/18] Fixed formatting again... --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 74d6233..1f289c7 100644 --- a/README.md +++ b/README.md @@ -12,4 +12,5 @@ The screen will then show the name of the current mission across the top; and th * The right button works the same way as the left, but moving foward instead of back * The bottom button recalibrates the gyro * The top button recalibrates the color sensor + **When a mission is running, all buttons lose their normal functioon, and will abort the current mission.** From 1320bbaff46130b4c6bbc6cf2a45df1ba9bde51f Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Fri, 25 Sep 2020 07:04:42 -0400 Subject: [PATCH 07/18] Fixed spelling --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 1f289c7..d1e8fcb 100644 --- a/README.md +++ b/README.md @@ -13,4 +13,4 @@ The screen will then show the name of the current mission across the top; and th * The bottom button recalibrates the gyro * The top button recalibrates the color sensor -**When a mission is running, all buttons lose their normal functioon, and will abort the current mission.** +**When a mission is running, all buttons lose their normal function, and will abort the current mission.** From f2f6bba50113b0be41ad597f14b6a79313a30cb2 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Fri, 25 Sep 2020 12:29:18 -0400 Subject: [PATCH 08/18] Commented MenuLib Finished commenting Missions Documentation done! --- FLLMaster/MenuLib.py | 316 +++++++++++++++++++++++++++++------------- FLLMaster/Missions.py | 78 ++++++----- 2 files changed, 259 insertions(+), 135 deletions(-) diff --git a/FLLMaster/MenuLib.py b/FLLMaster/MenuLib.py index 231a745..d585b0e 100644 --- a/FLLMaster/MenuLib.py +++ b/FLLMaster/MenuLib.py @@ -1,99 +1,219 @@ -from DriveLibraries import * -import multiprocessing -from time import sleep -Missions = None -mission = None -robot = None -programs = [] -numPrograms = 0 -count = 1 -loopIndex = 0 - -def init(confFile): - global programs - global numPrograms - global robot - global Missions - robot = Robot(confFile) - import Missions - programs = dir(Missions) - i = 0 - index = -1 - - length = len(programs) - while i < length: - if not ((programs[index][0] == "a") and (programs[index][1].isnumeric()) and (programs[index][2].isnumeric())): - programs.pop(index) - else: - index -= 1 - i += 1 - numPrograms = len(programs) - - robot.disp.clear() - robot.disp.text_grid(programs[0][3:], font=robot.fonts.load('timR24')) - -def runCurrentMission(): - method_to_call = getattr(Missions, programs[count - 1]) - method_to_call() - robot.rm.off(brake=False) - robot.lm.off(brake=False) - -def initthread(): - global mission - mission = multiprocessing.Process(target=runCurrentMission) - -def run(): - global mission - mission = multiprocessing.Process(target=runCurrentMission) - robot.zeroGyro() - mission.start() - -def display(): - name = programs[count - 1][3:] - robot.disp.text_grid(name, font=robot.fonts.load('timR24')) - -def abort(): - mission.terminate() - mission.join() - sleep(1) - robot.lm.off(brake=False) - robot.rm.off(brake=False) - minusCount() - display() - -def addCount(): - global count - count = (count % numPrograms) + 1 - -def minusCount(): - global count - count = ((count + (numPrograms - 2)) % numPrograms) + 1 - -def checkDrift(): - ar = robot.gs.angle_and_rate - rate = ar[1] - if rate < -0.5 or rate > 0.5: - robot.led.all_off() - robot.led.set_color('LEFT', 'RED') - robot.led.set_color('RIGHT', 'RED') - else: - robot.led.reset() - -def displaySensor(): - if loopIndex == 0: - robot.disp.rectangle(False, 0, 89, 177, 120, 'white', 'white') - robot.disp.update - robot.cs._ensure_mode(robot.cs.MODE_COL_REFLECT) - robot.disp.text_pixels("P1, Color:" + str(robot.correctedRLI), False, 40, 90, font=robot.fonts.load('courR10')) - robot.disp.text_pixels("P2, Gyro:" + str(robot.correctedAngle), False, 40, 100, font=robot.fonts.load('courR10')) - robot.disp.update() - -def calibrate(): - robot.led.all_off() - robot.led.set_color('LEFT', 'YELLOW') - robot.led.set_color('RIGHT', 'YELLOW') - robot.spkr.beep('-f 369.99') - robot.gs.calibrate() - sleep(1) - robot.spkr.beep() +# Import everything from DriveLibraries to allow interaction with the robot, and to create a Robot object +from DriveLibraries import * +# Used for running a mission in a killable way +import multiprocessing +# Some things need delays +from time import sleep +# Define variables with null values so they can be used as global variables later +Missions = None +mission = None +robot = None +programs = [] +numPrograms = 0 +loopIndex = 0 + +# count is used to determine which mission should be run, starts at 1 not 0 +count = 1 + +def init(confFile): + """ + Creates a ``Robot`` oject named ``robot`` using the filename given in ``confFile``, loads all mission functions + from ``Missions.py`` into ``programs`` alphabetically, using the three-character prefix on all mission function names, + and shows the first mission name on the display. + """ + # Declare global variables + global programs + global numPrograms + global robot + global Missions + + # Instantiate a Robot object named "robot" + robot = Robot(confFile) + # Now that a robot object exists, the mission programs, which need a robot object, can be imported + import Missions + # This is how the menu auto-adds missions. dir() returns a list of all objects contained in whatever it was given. + # However, there are a lot more objects than just the mission functions in Missions.py, so the list needs to be pruned. + programs = dir(Missions) + # Setting up some varibles for pruning + i = 0 + index = -1 + length = len(programs) + + # This will repeat the mission check on every element of the list of all objects in Missions.py + while i < length: + # This checks if the name of the object currently being checked starts with a##, like all missions should. + if not ((programs[index][0] == "a") and (programs[index][1].isnumeric()) and (programs[index][2].isnumeric())): + # If it does not, the object will be removed from the programs list, as it is not a mission + programs.pop(index) + else: + # If it is, the index varible wil be decreased by one to skip over the mission program the next time around + # (the scan starts at the back of the list). + index -= 1 + # Increment i by 1 every loop cycle + i += 1 + + # Now that the only things left in programs are missions, the length of programs can be used as the number of missions + # (used in AddCount and MinusCount, for rollover). + numPrograms = len(programs) + + # Clear the display + robot.disp.clear() + # Dislpay the first mission name (the [3:] is a string split; it prevents the a## from being displayed) + robot.disp.text_grid(programs[0][3:], font=robot.fonts.load('timR24')) + +def runCurrentMission(): + """ + Gets the currently selected mission from ``Missions.py`` and runs it. Once the mission has completed, + it stops the motors and allows them to glide to allow for manual robot positioning. + """ + # Finds the object in Missions.py that has the same name as the currently selected mission + # (count - 1 is used because count starts at 1 not 0), and creates the function pointer + # method_to_call, which is the selected mission + method_to_call = getattr(Missions, programs[count - 1]) + # Runs the selected mission + method_to_call() + # Stops the drive motors and allows them to glide + robot.rm.off(brake=False) + robot.lm.off(brake=False) + # Attempts to stop auxillary motors. If there are none, the try blocks will fail and the program will continue + try: + robot.m1.off(brake=False) + try: + robot.m2.off(brake=False) + +def initthread(): + """ + Creates the ``mission`` process that is used to run missions while being able to abort said mission. + This is also done in ``run()``, but needs to be done before other things will work properly + """ + # Declare mission as a global variable because it is being edited + global mission + # Create a process that will call runCurrentMission + mission = multiprocessing.Process(target=runCurrentMission) + +def run(): + """ + Called when the center button is pressed, this creates a process + to run the current mission, reset the gyro to zero, then start the mission. + """ + # Declare mission as a global variable because it is being edited + global mission + # Create a process that will call runCurrentMission + mission = multiprocessing.Process(target=runCurrentMission) + + # Reset the gyro angle to zero + robot.zeroGyro() + # Start the mission process + mission.start() + +def display(): + """ + Display the currently selected mission on the screen + """ + # Read the currently selected mission from the programs list, using count + # The [3:] is a string split so the a## prefix is not displayed, and 1 is subtracted + # from count because count starts at one, while list indexes start at zero. + name = programs[count - 1][3:] + # Display the mission name on the screen, in 24pt Times New Roman + robot.disp.text_grid(name, font=robot.fonts.load('timR24')) + +def abort(): + """ + Kills the current mission process, stops the motors and allows them to glide, and undoes auto-advance + """ + # Kill the mission process + mission.terminate() + # Wait for it to completely finish + mission.join() + # Stop the drive motors + robot.lm.off(brake=False) + robot.rm.off(brake=False) + # Attempts to stop auxillary motors. If there are none, the try blocks will fail and the program will continue + try: + robot.m1.off(brake=False) + try: + robot.m2.off(brake=False) + # Undo the auto-advance so the mission could easily be redone + minusCount() + display() + +def addCount(): + """ + Increases the ``count`` variable by one, resetting to one when numPrograms would have been ecxeeded + """ + # Declare count as global because it is being edited + global count + # Take the modulo of count and numPrograms (to rollover), then add one + count = (count % numPrograms) + 1 + +def minusCount(): + """ + Decreases the ``count`` variable by one, resetting to numPrograms when count would have gone below 1 + """ + # Declare count as global because it is being edited + global count + # (count + (numPrograms - 2)) % numPrograms is equal to two less than count, with the desired rollover behavior. + # However, that is decreasing by two, and its range is zero to numPrograms - 1. Thus, 1 is added to the result to + # correct the range and decrease by only one. + count = ((count + (numPrograms - 2)) % numPrograms) + 1 + +def checkDrift(): + """ + Turns the brick light green if the gyro reports a rate within one degree/second of zero, and red otherwise. + This is used as a signal to the user that the gyro may be drifting. + """ + # Read the gyro angle and rate. Angle and rate mode is consistantly used because switching modes resets the angle. + ar = robot.gs.angle_and_rate + # angle_and_rate returns a list; this function only needs rate + rate = ar[1] + # Check if the reported rate is outside of 1 degree/second from zero + if rate < -0.5 or rate > 0.5: + # If it is, turn both lights red (lights are turned off to clear) + robot.led.all_off() + robot.led.set_color('LEFT', 'RED') + robot.led.set_color('RIGHT', 'RED') + else: + # Otherwise, reset the lights to green + robot.led.reset() + +def displaySensor(): + """ + Periodically displays the color and gyro sensor values at the bottom of the screen. + """ + # loopIndex is equal to zero every hundeth loop cycle, so this function only runs that frequently. + # This is to prevent flickering of the printout. + if loopIndex == 0: + # Draws a white rectangle where the sensor values will be. This effectively clears only + # that section of the screen. + robot.disp.rectangle(False, 0, 89, 177, 120, 'white', 'white') + # Display commands do not take effect until update() is called + robot.disp.update() + # Displays the current calibrated color sensor RLI reading in the correct place in 10pt Courier New + robot.disp.text_pixels("P1, Color:" + str(robot.correctedRLI), False, 40, 90, font=robot.fonts.load('courR10')) + # Displays the current gyro sensor angle reading + robot.disp.text_pixels("P2, Gyro:" + str(robot.correctedAngle), False, 40, 100, font=robot.fonts.load('courR10')) + # Apply pending changes + robot.disp.update() + +def calibrate(): + """ + Provides a UI wrapper for the built-in ``robot.gs.calibrate`` function to calibrate the gyro + + Use: When called, the brick lights will turn yellow, the robot will emit a low pitched beep that is not used anywhere else, + and when calibration is complete, the robot will emit a normal beep and the lights will turn green again. The robot should + be completely still between the two beeps while the lights are yellow. + """ + # Turn the lights yellow (clearing first) + robot.led.all_off() + robot.led.set_color('LEFT', 'YELLOW') + robot.led.set_color('RIGHT', 'YELLOW') + # Unique low pitched beep (Don't touch the robot!) + robot.spkr.beep('-f 369.99') + # Calibrate the gyro + robot.gs.calibrate() + # Ensure calibration has completely finished + sleep(1) + # Normal beep (Calibration finished; safe to touch robot) + robot.spkr.beep() + # Reset the lights to green robot.led.reset() \ No newline at end of file diff --git a/FLLMaster/Missions.py b/FLLMaster/Missions.py index 36eb2b4..303246e 100644 --- a/FLLMaster/Missions.py +++ b/FLLMaster/Missions.py @@ -1,38 +1,42 @@ -if __name__ == "__main__": - from DriveLibraries import * - robot = Robot('robot.cfg') -else: - from MenuLib import robot - -# Mission Names should be prefaced with a##, where ## is two numbers. These -# numbers will not show up on the display, and tell the program what order the -# missions should be in; a00 is first, followed by a01, and so on. It is -# reccomended that spaces are left between the numbers (a00, a05, a10...), to -# allow for future missions. - -# Write Mission Programs Here ---------------------- - -def a00Star(): - # Drives a simple 4-pointed star, with arced sides - robot.ArcTurn(-90, 30, 30) - robot.ArcTurn(-90, 30, -30) - robot.ArcTurn(-90, 30, 30) - robot.ArcTurn(-90, 30, -30) - -def a05LineFollow(): - # Follows a line for 50 cm, at 50% speed, stopping at end - robot.LineFollow(50, 50, True) - -def a10Command_Key(): - # Drives in a pattern similar to the Apple command key - # o_o - # | | - # o‾o - robot.DriveAtHeading(0, 50, 30, False) - robot.ArcTurn(270, 10, 30) - robot.DriveAtHeading(270, 50, 30, False) - robot.ArcTurn(270, 10, 30) - robot.DriveAtHeading(270 * 2, 50, 30, False) - robot.ArcTurn(270, 10, 30) - robot.DriveAtHeading(270 * 3, 50, 30, True) +# Check if the program is running (rare) or if it is being used as a library +if __name__ == "__main__": + # If it is running, import everything from Drivelibraries and create a robot object. + from DriveLibraries import * + robot = Robot('robot.cfg') +else: + # If it is a library, import the robot object from MenuLib + # (which would have called Missions in the first place). + from MenuLib import robot + +# Mission Names should be prefaced with a##, where ## is two numbers. These +# numbers will not show up on the display, and tell the program what order the +# missions should be in; a00 is first, followed by a01, and so on. It is +# reccomended that spaces are left between the numbers (a00, a05, a10...), to +# allow for future missions. + +# Write Mission Programs Here ---------------------- + +def a05Star(): + # Drives a simple 4-pointed star, with arced sides + robot.ArcTurn(-90, 30, 30) + robot.ArcTurn(-90, 30, -30) + robot.ArcTurn(-90, 30, 30) + robot.ArcTurn(-90, 30, -30) + +def a00LineFollow(): + # Follows a line for 50 cm, at 50% speed, stopping at end + robot.LineFollow(50, 50, True) + +def a10Command_Key(): + # Drives in a pattern similar to the Apple command key + # o_o + # | | + # o‾o + robot.DriveAtHeading(0, 50, 30, False) + robot.ArcTurn(270, 10, 30) + robot.DriveAtHeading(270, 50, 30, False) + robot.ArcTurn(270, 10, 30) + robot.DriveAtHeading(270 * 2, 50, 30, False) + robot.ArcTurn(270, 10, 30) + robot.DriveAtHeading(270 * 3, 50, 30, True) robot.GyroTurn(0) \ No newline at end of file From 5ad6b1e74f41d4360558861f6692d8d66f46bcf6 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Fri, 25 Sep 2020 13:32:33 -0400 Subject: [PATCH 09/18] Added mission creation details to README.md. Also fixed double-click to abort a while ago. --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index d1e8fcb..8a8a425 100644 --- a/README.md +++ b/README.md @@ -14,3 +14,6 @@ The screen will then show the name of the current mission across the top; and th * The top button recalibrates the color sensor **When a mission is running, all buttons lose their normal function, and will abort the current mission.** + +### Adding Missions +To add a mission, simply create a new function in ``Missions.py``. Mission Names should be prefaced with a##, where ## is two numbers. These numbers will not show up on the display, they are used to alphabetically order the missions; a00 is first, followed by a01, and so on. It is reccomended that spaces are left between the numbers (a00, a05, a10...), to allow for future missions. From 58933ccb9c0bf4a18adb555e67a1a57a66b5003c Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Sat, 26 Sep 2020 11:46:05 -0400 Subject: [PATCH 10/18] Started a config file builder; currently gets sensor ports robot2.cfg is the auto-generated file. --- FLLMaster/AutoConfigBuilder.py | 43 ++++++++++++++++++++++++++++++++++ FLLMaster/robot.cfg | 10 ++++---- FLLMaster/robot2.cfg | 13 ++++++++++ 3 files changed, 61 insertions(+), 5 deletions(-) create mode 100644 FLLMaster/AutoConfigBuilder.py create mode 100644 FLLMaster/robot2.cfg diff --git a/FLLMaster/AutoConfigBuilder.py b/FLLMaster/AutoConfigBuilder.py new file mode 100644 index 0000000..e14a642 --- /dev/null +++ b/FLLMaster/AutoConfigBuilder.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 + +from configparser import ConfigParser +from ev3dev2 import sensor +from ev3dev2.display import * +from ev3dev2.button import * +from ev3dev2.sensor import lego +import ev3dev2.sensor.lego +from ev3dev2.motor import * +import re + + +def configBuilder(configFile='robot.cfg'): + """ + Creates a robot config file based on user input and sensor/motor values + """ + global lego + config = ConfigParser() + config['Drivebase'] = {} + config['Sensors'] = {} + config['AuxMotors'] = {} + config['Other'] = {} + senTypes = ['Color', 'Gyro', 'Touch', 'Ultrasonic', 'Infrared'] + + for i in senTypes: + try: + sensorClass = getattr(lego, i + 'Sensor') + mySensor = sensorClass() + port = str(mySensor.address) + port = re.sub('.*in([0-9]).*', 'INPUT_\g<1>', port) + config['Sensors'][i + 'Port'] = port + except: + config['Sensors'][i + 'Port'] = 'None' + finally: + mySensor = None + sensorClass = None + + with open(configFile, 'w') as configfile: + config.write(configfile) + +if __name__ == "__main__": + configBuilder('robot2.cfg') + diff --git a/FLLMaster/robot.cfg b/FLLMaster/robot.cfg index cd9a001..3eaa3e8 100644 --- a/FLLMaster/robot.cfg +++ b/FLLMaster/robot.cfg @@ -11,11 +11,11 @@ ki = 0 kd = 0.5 [Sensors] -ColorPort=INPUT_1 -GyroPort=INPUT_2 -InfraredPort=INPUT_4 -TouchPort= None -UltrasonicPort=INPUT_3 +ColorPort = INPUT_1 +GyroPort = INPUT_2 +InfraredPort = INPUT_4 +TouchPort = None +UltrasonicPort = INPUT_3 kpLine = 0.45 kiLine = 0.01163 kdLine = 2.528 diff --git a/FLLMaster/robot2.cfg b/FLLMaster/robot2.cfg new file mode 100644 index 0000000..ea35eb5 --- /dev/null +++ b/FLLMaster/robot2.cfg @@ -0,0 +1,13 @@ +[Drivebase] + +[Sensors] +colorport = INPUT_1 +gyroport = INPUT_2 +touchport = None +ultrasonicport = INPUT_3 +infraredport = INPUT_4 + +[AuxMotors] + +[Other] + From 878d867e09b4fa6790ef9aeb2cf03bc984b354e1 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Sat, 26 Sep 2020 18:41:25 -0400 Subject: [PATCH 11/18] Config builder now detects drive motors. --- FLLMaster/AutoConfigBuilder.py | 32 ++++++++++++++++++++++++++++++-- FLLMaster/robot2.cfg | 2 ++ 2 files changed, 32 insertions(+), 2 deletions(-) diff --git a/FLLMaster/AutoConfigBuilder.py b/FLLMaster/AutoConfigBuilder.py index e14a642..41ff524 100644 --- a/FLLMaster/AutoConfigBuilder.py +++ b/FLLMaster/AutoConfigBuilder.py @@ -1,13 +1,12 @@ #!/usr/bin/env python3 from configparser import ConfigParser -from ev3dev2 import sensor from ev3dev2.display import * from ev3dev2.button import * from ev3dev2.sensor import lego -import ev3dev2.sensor.lego from ev3dev2.motor import * import re +from time import sleep def configBuilder(configFile='robot.cfg'): @@ -15,11 +14,14 @@ def configBuilder(configFile='robot.cfg'): Creates a robot config file based on user input and sensor/motor values """ global lego + btn = Button() + disp = Display() config = ConfigParser() config['Drivebase'] = {} config['Sensors'] = {} config['AuxMotors'] = {} config['Other'] = {} + outPorts = ['OUTPUT_A', 'OUTPUT_B', 'OUTPUT_C', 'OUTPUT_D'] senTypes = ['Color', 'Gyro', 'Touch', 'Ultrasonic', 'Infrared'] for i in senTypes: @@ -28,6 +30,7 @@ def configBuilder(configFile='robot.cfg'): mySensor = sensorClass() port = str(mySensor.address) port = re.sub('.*in([0-9]).*', 'INPUT_\g<1>', port) + globals()[i] = port config['Sensors'][i + 'Port'] = port except: config['Sensors'][i + 'Port'] = 'None' @@ -35,9 +38,34 @@ def configBuilder(configFile='robot.cfg'): mySensor = None sensorClass = None + for i in outPorts: + try: + motor = LargeMotor(eval(i)) + disp.text_pixels("Found a Large Motor at " + "port " + i[-1]) + disp.text_pixels("What kind of motor is this?", False, 0, 10) + disp.text_pixels("Press left for left motor,", False, 0, 20) + disp.text_pixels("right for right motor,", False, 0, 30) + disp.text_pixels("up for Aux1, or down for Aux2", False, 0, 40) + disp.update() + while not btn.any(): + pass + selection = btn.buttons_pressed + if selection[0] == 'left': + btn.wait_for_released('left') + lm = motor + config['Drivebase']['LeftMotorPort'] = i + elif selection[0] == 'right': + btn.wait_for_released('right') + rm = motor + config['Drivebase']['RightMotorPort'] = i + + except: + pass + with open(configFile, 'w') as configfile: config.write(configfile) + if __name__ == "__main__": configBuilder('robot2.cfg') diff --git a/FLLMaster/robot2.cfg b/FLLMaster/robot2.cfg index ea35eb5..1d8defa 100644 --- a/FLLMaster/robot2.cfg +++ b/FLLMaster/robot2.cfg @@ -1,4 +1,6 @@ [Drivebase] +leftmotorport = OUTPUT_B +rightmotorport = OUTPUT_C [Sensors] colorport = INPUT_1 From dc475626a12bc6f38cb1b929450550ba7534d3be Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Tue, 29 Sep 2020 19:27:22 -0400 Subject: [PATCH 12/18] Finished configbuilder Fixed documentation on DriveLibraries. --- FLLMaster/AutoConfigBuilder.py | 217 ++++++++++++++++++++++++++++++--- FLLMaster/DriveLibraries.py | 3 +- FLLMaster/robot2.cfg | 14 +++ FLLMaster/test.py | 2 +- 4 files changed, 217 insertions(+), 19 deletions(-) diff --git a/FLLMaster/AutoConfigBuilder.py b/FLLMaster/AutoConfigBuilder.py index 41ff524..d85a330 100644 --- a/FLLMaster/AutoConfigBuilder.py +++ b/FLLMaster/AutoConfigBuilder.py @@ -1,12 +1,15 @@ #!/usr/bin/env python3 from configparser import ConfigParser +from time import sleep from ev3dev2.display import * from ev3dev2.button import * -from ev3dev2.sensor import lego +from ev3dev2.sensor import lego, INPUT_1, INPUT_2, INPUT_3, INPUT_4 from ev3dev2.motor import * import re -from time import sleep +from ev3dev2.sensor.lego import GyroSensor, UltrasonicSensor +from ev3dev2.sound import Sound +from sys import stderr def configBuilder(configFile='robot.cfg'): @@ -16,6 +19,7 @@ def configBuilder(configFile='robot.cfg'): global lego btn = Button() disp = Display() + spkr = Sound() config = ConfigParser() config['Drivebase'] = {} config['Sensors'] = {} @@ -23,6 +27,29 @@ def configBuilder(configFile='robot.cfg'): config['Other'] = {} outPorts = ['OUTPUT_A', 'OUTPUT_B', 'OUTPUT_C', 'OUTPUT_D'] senTypes = ['Color', 'Gyro', 'Touch', 'Ultrasonic', 'Infrared'] + + disp.text_pixels("Is this robot for FLL?") + disp.text_pixels("Y", False, 10, 110) + disp.text_pixels("N", False, 150, 110) + disp.update() + spkr.beep() + while True: + while not btn.any(): + pass + selection = btn.buttons_pressed + if selection[0] == 'left': + btn.wait_for_released('left') + config['Other']['ForFLL'] = 'TRUE' + break + elif selection[0] == 'right': + btn.wait_for_released('right') + config['Other']['ForFLL'] = 'FALSE' + break + else: + spkr.beep() + spkr.beep('-f 380') + disp.clear() + for i in senTypes: try: @@ -30,7 +57,6 @@ def configBuilder(configFile='robot.cfg'): mySensor = sensorClass() port = str(mySensor.address) port = re.sub('.*in([0-9]).*', 'INPUT_\g<1>', port) - globals()[i] = port config['Sensors'][i + 'Port'] = port except: config['Sensors'][i + 'Port'] = 'None' @@ -47,25 +73,184 @@ def configBuilder(configFile='robot.cfg'): disp.text_pixels("right for right motor,", False, 0, 30) disp.text_pixels("up for Aux1, or down for Aux2", False, 0, 40) disp.update() - while not btn.any(): + spkr.beep() + while True: + while not btn.any(): + pass + selection = btn.buttons_pressed + if selection[0] == 'left': + btn.wait_for_released('left') + config['Drivebase']['LeftMotorPort'] = i + break + elif selection[0] == 'right': + btn.wait_for_released('right') + config['Drivebase']['RightMotorPort'] = i + break + elif selection[0] == 'up': + btn.wait_for_released('up') + config['AuxMotors']['AuxMotor1'] = i + break + elif selection[0] == 'down': + btn.wait_for_released('down') + config['AuxMotors']['AuxMotor2'] = i + break + else: + spkr.beep() + spkr.beep('-f 380') + except: + pass + + for i in outPorts: + try: + motor = MediumMotor(eval(i)) + disp.text_pixels("Found a Med Motor at port " + i[-1]) + disp.text_pixels("Which AuxMotor is this?", False, 0, 10) + disp.text_pixels("1", False, 10, 110) + disp.text_pixels("2", False, 150, 110) + disp.update() + spkr.beep() + while True: + while not btn.any(): + pass + selection = btn.buttons_pressed + if selection[0] == 'left': + btn.wait_for_released('left') + config['AuxMotors']['AuxMotor1'] = i + break + elif selection[0] == 'right': + btn.wait_for_released('right') + config['AuxMotors']['AuxMotor2'] = i + break + else: + spkr.beep() + spkr.beep('-f 380') + except: + pass + + mtrs = MoveTank(eval(config['Drivebase']['LeftMotorPort']), eval(config['Drivebase']['RightMotorPort'])) + + if config['Sensors']['UltrasonicPort'] is not 'None': + us = UltrasonicSensor(eval(config['Sensors']['UltrasonicPort'])) + disp.text_pixels("Does the Ultrasonic sensor") + disp.text_pixels("face foward?", False, 0, 10) + disp.text_pixels("Y", False, 10, 110) + disp.text_pixels("N", False, 150, 110) + disp.update() + while not btn.any(): pass - selection = btn.buttons_pressed - if selection[0] == 'left': - btn.wait_for_released('left') - lm = motor - config['Drivebase']['LeftMotorPort'] = i - elif selection[0] == 'right': - btn.wait_for_released('right') - rm = motor - config['Drivebase']['RightMotorPort'] = i + selection = btn.buttons_pressed[0] + if selection == 'left': + usNav = True + elif selection == 'right': + usNav = False + else: + usNav = False + spkr.beep() + else: + usNav = False + us = None + + if usNav: + disp.text_pixels("Place the robot facing a wall, ") + disp.text_pixels("and press any button.", False, 0, 10) + disp.update() + while not btn.any(): + pass + circsum = 0 + sign = 1 + tests = 3 + for j in range(tests): + if j / 2 == round(j / 2): + parity = 1 + else: + parity = -1 + usSum = 0 + readings = 5 + for i in range(readings): + usSum += us.distance_centimeters + sleep(0.1) + stavg = round(usSum / readings, 2) + usSum = 0 + print(stavg, file=stderr) + mtrs.on_for_rotations(parity * 25, parity * 25, 1) + for i in range(readings): + usSum += us.distance_centimeters + sleep(0.1) + enavg = round(usSum / readings, 2) + print(enavg, file=stderr) + circsum += abs(enavg - stavg) + sign = (enavg - stavg) / abs(enavg - stavg) + avg = round(circsum / tests, 2) + config['Drivebase']['WheelCircumfrence'] = str(avg) - except: + polarity = 'normal' + if sign < 0: + config['Drivebase']['MotorsInverted'] = 'FALSE' + polarity = 'normal' + elif sign > 0: + config['Drivebase']['MotorsInverted'] = 'TRUE' + polarity = 'inversed' + + mtrs.set_polarity(polarity) + disp.text_pixels("Place robot in clear area,") + disp.text_pixels("and press any button.", False, 0, 10) + disp.update() + while not btn.any(): pass + gs = GyroSensor(eval(config['Sensors']['GyroPort'])) + widthbetweenwheelssum = 0 + ang = 0 + for i in range(tests): + gs._ensure_mode(gs.MODE_GYRO_RATE) + gs._ensure_mode(gs.MODE_GYRO_ANG) + mtrs.on_for_degrees(25, -25, 360) + sleep(0.5) + ang = gs.angle + wbw = abs((360 * float(config['Drivebase']['WheelCircumfrence'])) / (ang * math.pi)) + widthbetweenwheelssum += wbw + config['Drivebase']['WidthBetweenWheels'] = str(round(widthbetweenwheelssum / tests, 2)) + + if ang > 0: + config['Drivebase']['GyroInverted'] = 'FALSE' + elif ang < 0: + config['Drivebase']['GyroInverted'] = 'TRUE' + + else: + spkr.beep() + spkr.beep() + config['Drivebase']['WheelCircumfrence'] = '' + config['Drivebase']['WidthBetweenWheels'] = '' + config['Drivebase']['MotorsInverted'] = '' + config['Drivebase']['GyroInverted'] = '' + disp.text_pixels("The config file is missing some") + disp.text_pixels("values that will need to be", False, 0, 10) + disp.text_pixels("filled in before use.", False, 0, 20) + disp.text_pixels("Press any button to exit.", False, 0, 40) + disp.update() + while not btn.any(): + pass + + config['Drivebase']['GearRatio'] = '1' + config['Drivebase']['kp'] = '1' + config['Drivebase']['ki'] = '0' + config['Drivebase']['kd'] = '0.5' + config['Sensors']['kpLine'] = '0.5' + config['Sensors']['kiLine'] = '0' + config['Sensors']['kdLine'] = '2' + + spkr.beep() + spkr.beep() + disp.text_pixels("The PID gains in the config") + disp.text_pixels("file are placeholder values", False, 0, 10) + disp.text_pixels("and should be manually tuned.", False, 0, 20) + disp.text_pixels("Press any button to end.", False, 0, 40) + disp.update() + while not btn.any(): + pass with open(configFile, 'w') as configfile: config.write(configfile) if __name__ == "__main__": - configBuilder('robot2.cfg') - + configBuilder('robot2.cfg') \ No newline at end of file diff --git a/FLLMaster/DriveLibraries.py b/FLLMaster/DriveLibraries.py index c322bf0..edc6abe 100644 --- a/FLLMaster/DriveLibraries.py +++ b/FLLMaster/DriveLibraries.py @@ -50,8 +50,7 @@ def __init__(self, filename): # Read and store the wheel circumference and width between the wheels self.WheelCircumference = float(conf.get('Drivebase', 'WheelCircumference')) self.WidthBetweenWheels = float(conf.get('Drivebase', 'WidthBetweenWheels')) - # Read and store MotorInverted and GyroInverted. GyroInverted shoould be true if the gyro is inverted relative to the motors, - # and MotorInverted should be true if the drive motors are upside-down. Drive functions will not work correctly if these + # Read and store MotorInverted and GyroInverted. Both are relative to the robot, and drive functions will not work correctly if these # values are incorrect, and they are the first things to check if the drive functions do not work correctly. self.MotorInverted = bool(conf.get('Drivebase', 'MotorInverted') == "TRUE") self.GyroInverted = bool(conf.get('Drivebase', 'GyroInverted') == "TRUE") diff --git a/FLLMaster/robot2.cfg b/FLLMaster/robot2.cfg index 1d8defa..d7d55b0 100644 --- a/FLLMaster/robot2.cfg +++ b/FLLMaster/robot2.cfg @@ -1,6 +1,14 @@ [Drivebase] leftmotorport = OUTPUT_B rightmotorport = OUTPUT_C +wheelcircumfrence = 29.91 +motorsinverted = TRUE +widthbetweenwheels = 11.57 +gyroinverted = TRUE +gearratio = 1 +kp = 1 +ki = 0 +kd = 0.5 [Sensors] colorport = INPUT_1 @@ -8,8 +16,14 @@ gyroport = INPUT_2 touchport = None ultrasonicport = INPUT_3 infraredport = INPUT_4 +kpline = 0.5 +kiline = 0 +kdline = 2 [AuxMotors] +auxmotor1 = OUTPUT_A +auxmotor2 = OUTPUT_D [Other] +forfll = TRUE diff --git a/FLLMaster/test.py b/FLLMaster/test.py index 5687c4a..51f632e 100644 --- a/FLLMaster/test.py +++ b/FLLMaster/test.py @@ -1,3 +1,3 @@ #!/usr/bin/env python3 -# Program for testing stuff without all of the framework; less things to go wrong \ No newline at end of file +# Program for testing stuff without all of the framework; less things to go wrong From 20f2856b0a922be964d983d4b12735a6c2cc647b Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Sat, 17 Oct 2020 20:21:11 -0400 Subject: [PATCH 13/18] Documented AutoConfigBuilder.py --- FLLMaster/AutoConfigBuilder.py | 118 ++++++++++++++++++++++++++++++--- 1 file changed, 107 insertions(+), 11 deletions(-) diff --git a/FLLMaster/AutoConfigBuilder.py b/FLLMaster/AutoConfigBuilder.py index d85a330..a4f5591 100644 --- a/FLLMaster/AutoConfigBuilder.py +++ b/FLLMaster/AutoConfigBuilder.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 +# Import stuff from configparser import ConfigParser from time import sleep from ev3dev2.display import * @@ -16,90 +17,143 @@ def configBuilder(configFile='robot.cfg'): """ Creates a robot config file based on user input and sensor/motor values """ + # Make sure this is the global lego module, as it is used dynamicly later. Unsure if needed. global lego + # Instantiate brick interface objects btn = Button() disp = Display() spkr = Sound() + # Instantiate a ConfigParser object to writ and read the INI format config file config = ConfigParser() + + # Create the config file sections config['Drivebase'] = {} config['Sensors'] = {} config['AuxMotors'] = {} config['Other'] = {} + + # Used when detecting motors and sensors, as iterables outPorts = ['OUTPUT_A', 'OUTPUT_B', 'OUTPUT_C', 'OUTPUT_D'] senTypes = ['Color', 'Gyro', 'Touch', 'Ultrasonic', 'Infrared'] + # Ask the user if the robot is to be used for FLL disp.text_pixels("Is this robot for FLL?") disp.text_pixels("Y", False, 10, 110) disp.text_pixels("N", False, 150, 110) disp.update() spkr.beep() + # Wait for a button press (the outer loop is to allow for selection retry if + # an invalid button is pressed) while True: while not btn.any(): pass + # Get the list of currently pressed buttons selection = btn.buttons_pressed + # Check if the first buton in the list is the left button if selection[0] == 'left': + # If it is, set the ForFLL value to TRUE and exit the outer loop btn.wait_for_released('left') config['Other']['ForFLL'] = 'TRUE' break elif selection[0] == 'right': + # Otherwise, check if the selection is right. If it is, set ForFLL to FLASE + # and exit the outer loop btn.wait_for_released('right') config['Other']['ForFLL'] = 'FALSE' break else: + # If both checks fail, play an error tone and go back to waiting for a button spkr.beep() spkr.beep('-f 380') + # Clear the display for the next prompt disp.clear() - + # Auto-detect sensors for i in senTypes: try: + # When a sensor object is instantiated without a port, it will find the first port with + # that type of sensor connected. If that sensor is not connected, it will throw an error. + # The getattr function is used to get the correct sensor type dynamically sensorClass = getattr(lego, i + 'Sensor') + # Instantiate the current sensor type mySensor = sensorClass() + # Get the port that the sensor is connected to port = str(mySensor.address) + # sensor.adress will return sometheing like ev3:in1, so this replaces anything containing in# with INPUT_# port = re.sub('.*in([0-9]).*', 'INPUT_\g<1>', port) + # Add port value to the config file config['Sensors'][i + 'Port'] = port except: + # If the sensor instantiation failed, none of that kind of sensor are connected, so write the port value as None config['Sensors'][i + 'Port'] = 'None' finally: + # Clear the object and class variables as they are reused every loop cycle mySensor = None sensorClass = None + # Detect motors + # Repeat test for each port for i in outPorts: try: + # Instanitiate a Large Motor object at the current port. If there is a Lrge Motor at said port, this will suceed. + # Otherwise, it will throw an error and exit the try block. motor = LargeMotor(eval(i)) + # Print where the motor was found (port A, B, C, or D), and ask the user what the motor is being used for. disp.text_pixels("Found a Large Motor at " + "port " + i[-1]) disp.text_pixels("What kind of motor is this?", False, 0, 10) disp.text_pixels("Press left for left motor,", False, 0, 20) disp.text_pixels("right for right motor,", False, 0, 30) disp.text_pixels("up for Aux1, or down for Aux2", False, 0, 40) disp.update() + # Beep to signal that user input is required spkr.beep() + + # Loop is used to allow for invalid button presses to repeat selection while True: + # Wait for any button to be pressed while not btn.any(): pass - selection = btn.buttons_pressed - if selection[0] == 'left': + # Store what it is (first button pressed, if multiple) + selection = btn.buttons_pressed[0] + if selection == 'left': + # Wait for the button to be released, so the operation only occurs once btn.wait_for_released('left') + # If left, store the current port as the left motor port config['Drivebase']['LeftMotorPort'] = i + # Exit the loop, as this is a valid selection break - elif selection[0] == 'right': + elif selection == 'right': + # Wait for the button to be released, so the operation only occurs once btn.wait_for_released('right') + # If right, store the current port as the right motor port config['Drivebase']['RightMotorPort'] = i + # Exit the loop, as this is a valid selection break - elif selection[0] == 'up': + elif selection == 'up': + # Wait for the button to be released, so the operation only occurs once btn.wait_for_released('up') + # If up, store the current port as the first auxillary motor port config['AuxMotors']['AuxMotor1'] = i + # Exit the loop, as this is a valid selection break - elif selection[0] == 'down': + elif selection == 'down': + # Wait for the button to be released, so the operation only occurs once btn.wait_for_released('down') + # If down, store the current port as the second auxillary motor port config['AuxMotors']['AuxMotor2'] = i + # Exit the loop, as this is a valid selection break else: + # If the pressed button is not valid, play an error tone and repeat the selection menu spkr.beep() spkr.beep('-f 380') + + # If a motor is not found, no action is required. Apparently 'try' doesn't work without an 'except' except: pass + # This works exactly the same as the Large Motor detection, except that it is detecting Medium Motors instead, + # and there are no drivemotor selection options. for i in outPorts: try: motor = MediumMotor(eval(i)) @@ -112,12 +166,12 @@ def configBuilder(configFile='robot.cfg'): while True: while not btn.any(): pass - selection = btn.buttons_pressed - if selection[0] == 'left': + selection = btn.buttons_pressed[0] + if selection == 'left': btn.wait_for_released('left') config['AuxMotors']['AuxMotor1'] = i break - elif selection[0] == 'right': + elif selection == 'right': btn.wait_for_released('right') config['AuxMotors']['AuxMotor2'] = i break @@ -127,15 +181,19 @@ def configBuilder(configFile='robot.cfg'): except: pass + # Create a MoveTank object with the detected drive motors, as it is needed for the upcoming tests. mtrs = MoveTank(eval(config['Drivebase']['LeftMotorPort']), eval(config['Drivebase']['RightMotorPort'])) + # A Foward-Facing Ultrasonic sensor is the only way to allow for calculating the wheel circumfrence. if config['Sensors']['UltrasonicPort'] is not 'None': + # Ask the user if the detected Ultrasonic sensor is facing fowards us = UltrasonicSensor(eval(config['Sensors']['UltrasonicPort'])) disp.text_pixels("Does the Ultrasonic sensor") disp.text_pixels("face foward?", False, 0, 10) disp.text_pixels("Y", False, 10, 110) disp.text_pixels("N", False, 150, 110) disp.update() + # Same selection system as before while not btn.any(): pass selection = btn.buttons_pressed[0] @@ -150,39 +208,54 @@ def configBuilder(configFile='robot.cfg'): usNav = False us = None + # Using the variable usNav set previously, check if the Ultrasonic sensor faces fowards. if usNav: + # Ask the user to place the robot facing a wall, then wait for a button press. disp.text_pixels("Place the robot facing a wall, ") disp.text_pixels("and press any button.", False, 0, 10) disp.update() while not btn.any(): pass + # Set up some variables for averaging circsum = 0 sign = 1 tests = 3 + # Repeat the wheelcircumfrence test multiple times to get a more accurate average for j in range(tests): + # Determine if this is an even or odd loop cycle. This is used for calculating MotorsInverted, + # as the motors are run backwards on odd cycles if j / 2 == round(j / 2): parity = 1 else: parity = -1 + # More variables for averaging usSum = 0 readings = 5 + # Take multiple sensor readings of how far away from the wall the robot is, and average them for i in range(readings): usSum += us.distance_centimeters sleep(0.1) stavg = round(usSum / readings, 2) + # Reset averaging variable usSum = 0 - print(stavg, file=stderr) + # Move the robot one wheel rotation, foward on even loop cycles, or backward on odd mtrs.on_for_rotations(parity * 25, parity * 25, 1) + # Take multiple sensor readings of how far away from the wall the robot is, and average them for i in range(readings): usSum += us.distance_centimeters sleep(0.1) enavg = round(usSum / readings, 2) - print(enavg, file=stderr) + # The absolute difference between the starting average and the ending average is the wheel circumfrence; + # however, this is a cumulative sum of all wheel circumfrence measurements for averaging. circsum += abs(enavg - stavg) + # Isolate the sign (-x or x to -1 or 1) of the wheel circumfrence (Used for MotorsInverted) sign = (enavg - stavg) / abs(enavg - stavg) + # Calculate the average wheel circumfrence avg = round(circsum / tests, 2) + # Write to config file variable config['Drivebase']['WheelCircumfrence'] = str(avg) + # Set MotorsInverted in the config file, and set the 'polarity' variable (for inverting motor commands if necessary) polarity = 'normal' if sign < 0: config['Drivebase']['MotorsInverted'] = 'FALSE' @@ -191,31 +264,49 @@ def configBuilder(configFile='robot.cfg'): config['Drivebase']['MotorsInverted'] = 'TRUE' polarity = 'inversed' + # Invert motor commands if necessary mtrs.set_polarity(polarity) + # Ask the user to place the robot in a clear area, and wait for a button press disp.text_pixels("Place robot in clear area,") disp.text_pixels("and press any button.", False, 0, 10) disp.update() while not btn.any(): pass + # Instantiate a Gyro Sensor object as the degrees turned is necessary for calculating the Width Between Wheels gs = GyroSensor(eval(config['Sensors']['GyroPort'])) + # Create variables for averaging widthbetweenwheelssum = 0 ang = 0 + # Repeat the trial multiple times and average the results for i in range(tests): + # Reset the reported gyro angle to zero gs._ensure_mode(gs.MODE_GYRO_RATE) gs._ensure_mode(gs.MODE_GYRO_ANG) + # Turn in place for one wheel rotation mtrs.on_for_degrees(25, -25, 360) + # Wait for the robot to settle sleep(0.5) + # Read the current angle of the robot ang = gs.angle + # Calculate the width between the robot's wheels using the formula for arc length (ArcLength = (π * d * Θ) / 360) solved for d, + # the diameter of the circle, which is the width between wheels. Absolute value is used so the direction of turn does not matter. wbw = abs((360 * float(config['Drivebase']['WheelCircumfrence'])) / (ang * math.pi)) + # Add the calculated value to the running total (used for averaging) widthbetweenwheelssum += wbw + # Calculate the average WBW value, round it to remove floating point errors, and store in the ConfigParser object config['Drivebase']['WidthBetweenWheels'] = str(round(widthbetweenwheelssum / tests, 2)) + # The motor move command previously made the robot turn right. If the gyro reported this as a turn towards positive, it is not inverted. + # Otherwise, the gyro is inverted. if ang > 0: config['Drivebase']['GyroInverted'] = 'FALSE' elif ang < 0: config['Drivebase']['GyroInverted'] = 'TRUE' else: + # If the robot does not have an ultrasonic sensor that faces fowards, the robot cannot calculate the wheel circumfrence, which is required + # to calculate widthbetweenwheels. The robot also cannot calculate MotorsInverted or GyroInverted. Therefore, empty valuse are written to + # those parts of the config file, and the user is notified that they need to manually fill in that information. spkr.beep() spkr.beep() config['Drivebase']['WheelCircumfrence'] = '' @@ -230,6 +321,9 @@ def configBuilder(configFile='robot.cfg'): while not btn.any(): pass + # Write placeholder values for the PID gains, so those functions may be usable, if poorly. + # Because the WheelCircumfrence is calculated, the wheelcircumfrence value will include any gearing, + # so the effective gear ratio is one. config['Drivebase']['GearRatio'] = '1' config['Drivebase']['kp'] = '1' config['Drivebase']['ki'] = '0' @@ -238,6 +332,7 @@ def configBuilder(configFile='robot.cfg'): config['Sensors']['kiLine'] = '0' config['Sensors']['kdLine'] = '2' + # Tell the user that the PID values may (will) need to be tuned. spkr.beep() spkr.beep() disp.text_pixels("The PID gains in the config") @@ -248,6 +343,7 @@ def configBuilder(configFile='robot.cfg'): while not btn.any(): pass + # Write all the values stored in the ConfigParser object named config to a file in INI format, with the name passed by configFile. with open(configFile, 'w') as configfile: config.write(configfile) From 40edf31a78cd22906b5f5fbdf10f6d0d605ffb33 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 <59632924+WesleyJ-128@users.noreply.github.com> Date: Wed, 11 Nov 2020 10:43:21 -0500 Subject: [PATCH 14/18] Fixed typographical errors in README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 8a8a425..a028c64 100644 --- a/README.md +++ b/README.md @@ -4,10 +4,10 @@ Driving libraries and a master program designed for FLL-legal robots. Uses a ro ## Use: The file ``Main.py`` is the only executable file other than ``test.py``, and is the program that creates the menu. This program takes a while to start, so it should be started well before any timers. On startup, the robot will eventually emit the normal beep (A4) signalling that the config file has been read and some menu systems have been initialized. It will then emit a low-pitched beep (different from any other beep) and turn the lights yellow. This is standard procedure for recalibrating the gyro (inside this program, anyway), and the robot should be absolutely still from the first low-pitched beep to the second normal beep. After the second normal beep, the robot will start the color sensor calibration routine, emitting a high-pitched beep. This signals that the color sensor should be placed on the white surface. After pressing the center button, the robot will emit a normal beep signalling that the white surface has been stored. The robot will then emit a low-pitched beep. Again, place the robot on the black surface and press the center button. The robot will emit a normal beep, and the complete setup is done. -The screen will then show the name of the current mission across the top; and the current gyro and color sensor values (angle and RLI towards the bottom in the center. The lights will also likely begin flickering between red and green. The color of the light is determined by the rate reported by the gyro sensor; if it is within 1 degree/second of zero, the light will turn green. If the gyro reports that it is moving any faster than that, the light will turn red. Therefore, if the robot is not being manipulated and is not running a mission, the light should be green most of the time. If it is not, the gyro is likely drifting and needs to be recalibrated. +The screen will then show the name of the current mission across the top; and the current gyro and color sensor values (angle and RLI) towards the bottom in the center. The lights will also likely begin flickering between red and green. The color of the light is determined by the rate reported by the gyro sensor; if it is within 1 degree/second of zero, the light will turn green. If the gyro reports that it is moving any faster than that, the light will turn red. Therefore, if the robot is not being manipulated and is not running a mission, the light should be green most of the time. If it is not, the gyro is likely drifting and needs to be recalibrated. ### Button functions: -* The center button runs the nission whose name is currently being displayed on the screen. It also advances to the next mission afterward. +* The center button runs the mission whose name is currently being displayed on the screen. It also advances to the next mission afterward. * The left button changes the selection to the previous mission, looping around to the last when pressing left on the first mission. * The right button works the same way as the left, but moving foward instead of back * The bottom button recalibrates the gyro From 7ce262c5b374fda976cbdc622b5a6b77e04a6845 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Sat, 12 Dec 2020 16:47:28 -0500 Subject: [PATCH 15/18] Started a TriangleAvoid function Made aux sensor setup fully dynamic --- FLLMaster/DriveLibraries.py | 139 ++++++++++++++++++++++++++++++++++-- FLLMaster/MenuLib.py | 8 +++ FLLMaster/Missions.py | 3 + 3 files changed, 146 insertions(+), 4 deletions(-) diff --git a/FLLMaster/DriveLibraries.py b/FLLMaster/DriveLibraries.py index edc6abe..89378fe 100644 --- a/FLLMaster/DriveLibraries.py +++ b/FLLMaster/DriveLibraries.py @@ -1,5 +1,6 @@ # The program doesn't work without this. Not sure why. from time import sleep +from types import prepare_class from ev3dev2.motor import * from ev3dev2.sensor.lego import * from ev3dev2.sensor import * @@ -99,11 +100,20 @@ def __init__(self, filename): self.gs = GyroSensor(self.GyroPort) # Only instantiate auxillary sensors if the config file shows they exist if self.InfraredPort is not None: - self.ir = InfraredSensor(self.InfraredPort) + try: + self.ir = InfraredSensor(self.InfraredPort) + except: + pass if self.UltrasonicPort is not None: - self.us = UltrasonicSensor(self.UltrasonicPort) + try: + self.us = UltrasonicSensor(self.UltrasonicPort) + except: + pass if self.TouchPort is not None: - self.ts = TouchSensor(self.TouchPort) + try: + self.ts = TouchSensor(self.TouchPort) + except: + pass # Instantiate the drive motor objects, as well as the motorset objects for controlling both simultainiously self.tank = MoveTank(self.LeftMotor, self.RightMotor) @@ -656,4 +666,125 @@ def LineFollow(self, Distance, Speed, Stop): # If the robot is to stop, stop the motors. Otherwise, leave the motors on and return. if not Stop == False: - self.steer.stop() \ No newline at end of file + self.steer.stop() + + def TriangleAvoid(self, Heading, Distance, Speed): + """ + Moves the robot in a specified direction at a specified speed for a certian number of centimeters, while using the gyro sensor to keep the robot moving in a straight line. + + ``Heading``: The angle at which to drive, with the direction the gyro was last calibrated in being zero. + ``Distance``: The distance to drive, in centimeters (positive only). + ``Speed``: The speed at which to drive, in motor percentage (same speed units as EV3-G). A negative value will make the robot drive backwards. + ``Stop``: Stop motors after completion. If ``FALSE``, motors will continue running after ``Distance`` has been traveled. Otherwise, motors will stop after ``Distance`` cm. + """ + # Ensure values are within reasonable limits, and change them if necessary (Idiotproofing). + if Distance <= 0: + print("Distance must be greater than zero. Use negative speed to drive backwards.") + return + elif Distance > 265: + # The longest distance on an FLL table (diagonal) is about 265cm. + if self.ForFLL: + print("Please don't use silly distances (max = 265cm)") + return + 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 not self.us: + print("Avoidance Functions need the US sensor to work.") + return + + # "Reset" motor encoders by subtracting start values + left_motor_start = self.lm.degrees + right_motor_start = self.rm.degrees + left_motor_now = self.lm.degrees + right_motor_now = self.rm.degrees + left_motor_change = left_motor_now - left_motor_start + right_motor_change = right_motor_now - right_motor_start + + # Determine the sign of the speed, for PID correction + sign = Speed / abs(Speed) + + # Find number of degrees that motors need to rotate to reach the desired number of cm. + target = (Distance * 360) / self.WheelCircumference * abs(self.GearRatio) + + # Find the average of the left and right encoders, as they could be different from PID correction + avg = abs((left_motor_change + right_motor_change) / 2) + + # Initialize variables for PID control + integral = 0.0 + last_error = 0.0 + derivative = 0.0 + + # Check if the motors have gone far enough + while avg < target: + # Read the gyro and ultrasonic sensors + current_angle = self.correctedAngle + dist_to_obstacle = self.us.distance_centimeters + + # Calculate the PID components + error = current_angle - Heading + integral = integral + error + derivative = error - last_error + last_error = error + + # Calculate Steering value based on PID components and kp, ki, and kd + turn_native_units = sign * max([min([(self.kp * error) + (self.ki * integral) + (self.kd * derivative), 100]), -100]) + + # Check if the motors will stop at the end. If not, the speed will be adjusted to come to a smooth stop. + # Check if the robot has gone 70% or more of the distance. If so, start slowing down + if (target - avg) <= (0.3 * target): + # Calculate the pecrentage of the distance left to travel + targDist = 1 - (avg / target) + # Calculate speedMult based on pecentage; linear function was designed to bring the robot to a + # smooth stop while still reaching the target, resulting in 20% of the intial speed at end. + speedMult = ((8 / 3) * targDist) + 0.2 + else: + speedMult = 1 + + if dist_to_obstacle <= 30: + left_motor_now = self.lm.degrees + right_motor_now = self.rm.degrees + left_motor_change = left_motor_now - left_motor_start + right_motor_change = right_motor_now - right_motor_start + avg = abs((left_motor_change + right_motor_change) / 2) + driven_so_far = (avg * self.WheelCircumference) / 360 + self.steer.off(brake=False) + dist_to_obstacle = self.us.distance_centimeters + start_angle = self.correctedAngle + while self.us.distance_centimeters < 60: + self.tank.on(-10, 10) + self.tank.off() + self.GyroTurn(self.correctedAngle - 5) + end_angle = self.correctedAngle + degrees_turned = abs(end_angle - start_angle) + first_hypotenuse = dist_to_obstacle / math.cos(math.radians(degrees_turned)) + self.TriangleAvoid(end_angle, first_hypotenuse, (Speed / 2)) + first_triangle_second_angle = math.degrees(math.asin(dist_to_obstacle / first_hypotenuse)) + second_triangle_short_leg = math.sqrt((first_hypotenuse ** 2) - (dist_to_obstacle ** 2)) + second_triangle_long_leg = Distance - (driven_so_far + dist_to_obstacle + 2.21) + second_triangle_second_angle = math.degrees(math.atan(second_triangle_long_leg / second_triangle_short_leg)) + degrees_to_turn = 180 - (first_triangle_second_angle + second_triangle_second_angle) + self.GyroTurn(Heading) + sleep(5) + self.GyroTurn(degrees_to_turn - Heading) + second_hypotenuse = math.sqrt((second_triangle_long_leg ** 2) + (second_triangle_short_leg ** 2)) + self.TriangleAvoid(self.correctedAngle, second_hypotenuse, (Speed / 2)) + self.GyroTurn(Heading) + return + + + # Start the motors, using the steering value calculated by the PID control, and the input speed multiplied by speedMult (0 - 1, see above). + self.steer.on(-turn_native_units, (Speed * speedMult)) + + # Update corrected encoder values + left_motor_now = self.lm.degrees + right_motor_now = self.rm.degrees + left_motor_change = left_motor_now - left_motor_start + right_motor_change = right_motor_now - right_motor_start + avg = abs((left_motor_change + right_motor_change) / 2) + + # Stop the motors. + self.steer.stop() \ No newline at end of file diff --git a/FLLMaster/MenuLib.py b/FLLMaster/MenuLib.py index d585b0e..6d5b85c 100644 --- a/FLLMaster/MenuLib.py +++ b/FLLMaster/MenuLib.py @@ -78,8 +78,12 @@ def runCurrentMission(): # Attempts to stop auxillary motors. If there are none, the try blocks will fail and the program will continue try: robot.m1.off(brake=False) + except: + pass try: robot.m2.off(brake=False) + except: + pass def initthread(): """ @@ -131,8 +135,12 @@ def abort(): # Attempts to stop auxillary motors. If there are none, the try blocks will fail and the program will continue try: robot.m1.off(brake=False) + except: + pass try: robot.m2.off(brake=False) + except: + pass # Undo the auto-advance so the mission could easily be redone minusCount() display() diff --git a/FLLMaster/Missions.py b/FLLMaster/Missions.py index 303246e..a53884b 100644 --- a/FLLMaster/Missions.py +++ b/FLLMaster/Missions.py @@ -27,6 +27,9 @@ def a00LineFollow(): # Follows a line for 50 cm, at 50% speed, stopping at end robot.LineFollow(50, 50, True) +def a06AvoidTest(): + robot.TriangleAvoid(0, 100, 50) + def a10Command_Key(): # Drives in a pattern similar to the Apple command key # o_o From 240a5fbfc71c83f8163438b66c7ef7cb3158ca39 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Sun, 13 Dec 2020 09:39:59 -0500 Subject: [PATCH 16/18] Finished TriangleAvoid --- FLLMaster/DriveLibraries.py | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/FLLMaster/DriveLibraries.py b/FLLMaster/DriveLibraries.py index 89378fe..9a4fc2c 100644 --- a/FLLMaster/DriveLibraries.py +++ b/FLLMaster/DriveLibraries.py @@ -1,6 +1,6 @@ # The program doesn't work without this. Not sure why. +from sys import stderr from time import sleep -from types import prepare_class from ev3dev2.motor import * from ev3dev2.sensor.lego import * from ev3dev2.sensor import * @@ -713,6 +713,10 @@ def TriangleAvoid(self, Heading, Distance, Speed): # Find the average of the left and right encoders, as they could be different from PID correction avg = abs((left_motor_change + right_motor_change) / 2) + # Find the number of centimeters driven and left to go + driven_so_far = (avg * self.WheelCircumference) / 360 + left_to_go = Distance - driven_so_far + # Initialize variables for PID control integral = 0.0 last_error = 0.0 @@ -744,14 +748,14 @@ def TriangleAvoid(self, Heading, Distance, Speed): else: speedMult = 1 - if dist_to_obstacle <= 30: + if (dist_to_obstacle <= 30) and (left_to_go > 30): + self.steer.off(brake=False) left_motor_now = self.lm.degrees right_motor_now = self.rm.degrees left_motor_change = left_motor_now - left_motor_start right_motor_change = right_motor_now - right_motor_start avg = abs((left_motor_change + right_motor_change) / 2) driven_so_far = (avg * self.WheelCircumference) / 360 - self.steer.off(brake=False) dist_to_obstacle = self.us.distance_centimeters start_angle = self.correctedAngle while self.us.distance_centimeters < 60: @@ -761,17 +765,15 @@ def TriangleAvoid(self, Heading, Distance, Speed): end_angle = self.correctedAngle degrees_turned = abs(end_angle - start_angle) first_hypotenuse = dist_to_obstacle / math.cos(math.radians(degrees_turned)) - self.TriangleAvoid(end_angle, first_hypotenuse, (Speed / 2)) - first_triangle_second_angle = math.degrees(math.asin(dist_to_obstacle / first_hypotenuse)) + self.DriveAtHeading(end_angle, first_hypotenuse, 20, True) second_triangle_short_leg = math.sqrt((first_hypotenuse ** 2) - (dist_to_obstacle ** 2)) second_triangle_long_leg = Distance - (driven_so_far + dist_to_obstacle + 2.21) second_triangle_second_angle = math.degrees(math.atan(second_triangle_long_leg / second_triangle_short_leg)) - degrees_to_turn = 180 - (first_triangle_second_angle + second_triangle_second_angle) + degrees_to_turn = 90 - second_triangle_second_angle self.GyroTurn(Heading) - sleep(5) - self.GyroTurn(degrees_to_turn - Heading) + self.GyroTurn(Heading + degrees_to_turn) second_hypotenuse = math.sqrt((second_triangle_long_leg ** 2) + (second_triangle_short_leg ** 2)) - self.TriangleAvoid(self.correctedAngle, second_hypotenuse, (Speed / 2)) + self.TriangleAvoid(self.correctedAngle, second_hypotenuse, 20) self.GyroTurn(Heading) return @@ -785,6 +787,7 @@ def TriangleAvoid(self, Heading, Distance, Speed): left_motor_change = left_motor_now - left_motor_start right_motor_change = right_motor_now - right_motor_start avg = abs((left_motor_change + right_motor_change) / 2) + driven_so_far = (avg * self.WheelCircumference) / 360 # Stop the motors. self.steer.stop() \ No newline at end of file From 54003e433639e6ab96ead2a1f5f0accfb73afad0 Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Sun, 13 Dec 2020 11:25:27 -0500 Subject: [PATCH 17/18] Added CircleAvoid --- FLLMaster/DriveLibraries.py | 126 ++++++++++++++++++++++++++++++++++++ FLLMaster/Missions.py | 2 +- 2 files changed, 127 insertions(+), 1 deletion(-) diff --git a/FLLMaster/DriveLibraries.py b/FLLMaster/DriveLibraries.py index 9a4fc2c..f756a2a 100644 --- a/FLLMaster/DriveLibraries.py +++ b/FLLMaster/DriveLibraries.py @@ -789,5 +789,131 @@ def TriangleAvoid(self, Heading, Distance, Speed): avg = abs((left_motor_change + right_motor_change) / 2) driven_so_far = (avg * self.WheelCircumference) / 360 + # Stop the motors. + self.steer.stop() + + def CircleAvoid(self, Heading, Distance, Speed): + """ + Moves the robot in a specified direction at a specified speed for a certian number of centimeters, while using the gyro sensor to keep the robot moving in a straight line. + + ``Heading``: The angle at which to drive, with the direction the gyro was last calibrated in being zero. + ``Distance``: The distance to drive, in centimeters (positive only). + ``Speed``: The speed at which to drive, in motor percentage (same speed units as EV3-G). A negative value will make the robot drive backwards. + ``Stop``: Stop motors after completion. If ``FALSE``, motors will continue running after ``Distance`` has been traveled. Otherwise, motors will stop after ``Distance`` cm. + """ + # Ensure values are within reasonable limits, and change them if necessary (Idiotproofing). + if Distance <= 0: + print("Distance must be greater than zero. Use negative speed to drive backwards.") + return + elif Distance > 265: + # The longest distance on an FLL table (diagonal) is about 265cm. + if self.ForFLL: + print("Please don't use silly distances (max = 265cm)") + return + 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 not self.us: + print("Avoidance Functions need the US sensor to work.") + return + + # "Reset" motor encoders by subtracting start values + left_motor_start = self.lm.degrees + right_motor_start = self.rm.degrees + left_motor_now = self.lm.degrees + right_motor_now = self.rm.degrees + left_motor_change = left_motor_now - left_motor_start + right_motor_change = right_motor_now - right_motor_start + + # Determine the sign of the speed, for PID correction + sign = Speed / abs(Speed) + + # Find number of degrees that motors need to rotate to reach the desired number of cm. + target = (Distance * 360) / self.WheelCircumference * abs(self.GearRatio) + + # Find the average of the left and right encoders, as they could be different from PID correction + avg = abs((left_motor_change + right_motor_change) / 2) + + # Find the number of centimeters driven and left to go + driven_so_far = (avg * self.WheelCircumference) / 360 + left_to_go = Distance - driven_so_far + + # Initialize variables for PID control + integral = 0.0 + last_error = 0.0 + derivative = 0.0 + + # Check if the motors have gone far enough + while avg < target: + # Read the gyro and ultrasonic sensors + current_angle = self.correctedAngle + dist_to_obstacle = self.us.distance_centimeters + + # Calculate the PID components + error = current_angle - Heading + integral = integral + error + derivative = error - last_error + last_error = error + + # Calculate Steering value based on PID components and kp, ki, and kd + turn_native_units = sign * max([min([(self.kp * error) + (self.ki * integral) + (self.kd * derivative), 100]), -100]) + + # Check if the motors will stop at the end. If not, the speed will be adjusted to come to a smooth stop. + # Check if the robot has gone 70% or more of the distance. If so, start slowing down + if (target - avg) <= (0.3 * target): + # Calculate the pecrentage of the distance left to travel + targDist = 1 - (avg / target) + # Calculate speedMult based on pecentage; linear function was designed to bring the robot to a + # smooth stop while still reaching the target, resulting in 20% of the intial speed at end. + speedMult = ((8 / 3) * targDist) + 0.2 + else: + speedMult = 1 + + if (dist_to_obstacle <= 30) and (left_to_go > 30): + self.steer.off(brake=False) + left_motor_now = self.lm.degrees + right_motor_now = self.rm.degrees + left_motor_change = left_motor_now - left_motor_start + right_motor_change = right_motor_now - right_motor_start + avg = abs((left_motor_change + right_motor_change) / 2) + driven_so_far = (avg * self.WheelCircumference) / 360 + dist_to_obstacle = self.us.distance_centimeters + start_angle = self.correctedAngle + while self.us.distance_centimeters < 60: + self.tank.on(-10, 10) + self.tank.off() + self.GyroTurn(self.correctedAngle - 5) + end_angle = self.correctedAngle + degrees_turned = abs(end_angle - start_angle) + second_point_y = dist_to_obstacle * math.tan(math.radians(degrees_turned)) + third_point_x = Distance - driven_so_far + x_numerator = -(third_point_x ** 2) * second_point_y + y_numerator = -((dist_to_obstacle ** 2) * third_point_x) - ((second_point_y ** 2) * third_point_x) + ((third_point_x ** 2) * dist_to_obstacle) + denominator = -2 * third_point_x * second_point_y + h = x_numerator / denominator + k = y_numerator / denominator + radius = math.sqrt((h ** 2) + (k ** 2)) + begin_arc_angle = math.degrees(math.atan(h / -k)) + arc_angle = -2 * begin_arc_angle + self.GyroTurn(Heading + begin_arc_angle) + self.ArcTurn(arc_angle, radius, 10) + self.GyroTurn(Heading) + return + + + # Start the motors, using the steering value calculated by the PID control, and the input speed multiplied by speedMult (0 - 1, see above). + self.steer.on(-turn_native_units, (Speed * speedMult)) + + # Update corrected encoder values + left_motor_now = self.lm.degrees + right_motor_now = self.rm.degrees + left_motor_change = left_motor_now - left_motor_start + right_motor_change = right_motor_now - right_motor_start + avg = abs((left_motor_change + right_motor_change) / 2) + driven_so_far = (avg * self.WheelCircumference) / 360 + # Stop the motors. self.steer.stop() \ No newline at end of file diff --git a/FLLMaster/Missions.py b/FLLMaster/Missions.py index a53884b..8faab70 100644 --- a/FLLMaster/Missions.py +++ b/FLLMaster/Missions.py @@ -28,7 +28,7 @@ def a00LineFollow(): robot.LineFollow(50, 50, True) def a06AvoidTest(): - robot.TriangleAvoid(0, 100, 50) + robot.CircleAvoid(0, 100, 20) def a10Command_Key(): # Drives in a pattern similar to the Apple command key From d8d2e914c631893442fb1d8b705097bee3710d0f Mon Sep 17 00:00:00 2001 From: WesleyJ-128 Date: Fri, 26 Nov 2021 17:25:59 -0500 Subject: [PATCH 18/18] Removed unessecary import within main loop --- FLLMaster/Main.py | 54 +++++++++++++++++++++++------------------------ 1 file changed, 26 insertions(+), 28 deletions(-) diff --git a/FLLMaster/Main.py b/FLLMaster/Main.py index 9859ac2..91e7399 100644 --- a/FLLMaster/Main.py +++ b/FLLMaster/Main.py @@ -18,12 +18,12 @@ # Create the mission process used for running the current mission initthread() # Now that setup has finished, import everything else from MenuLib -from MenuLib import * +import MenuLib # Calibrate the gyro -calibrate() +MenuLib.calibrate() # Calibrate the color sensor -robot.reflectCal() +MenuLib.robot.reflectCal() # Initialization has finished, print status message to VS Code output window print("Finished Init", file=stderr) @@ -33,50 +33,50 @@ def left(): Called when left button pressed; if there is no mission running, decrease count and update the display. Otherwise, abort the current mission. """ - if not mission.is_alive(): - minusCount() - display() + if not MenuLib.mission.is_alive(): + MenuLib.minusCount() + MenuLib.display() else: - abort() + MenuLib.abort() def right(): """ Called when right button pressed; if there is no mission running, increase count and update the display. Otherwise, abort the current mission. """ - if not mission.is_alive(): - addCount() - display() + if not MenuLib.mission.is_alive(): + MenuLib.addCount() + MenuLib.display() else: - abort() + MenuLib.abort() def down(): """ Called when bottom button pressed; if there is no mission running, recalibrate the gyro. Otherwise, abort the current mission. """ - if not mission.is_alive(): - calibrate() + if not MenuLib.mission.is_alive(): + MenuLib.calibrate() else: - abort() + MenuLib.abort() def up(): """ Called when top button pressed; if there is no mission running, recalibrate the color sensor. Otherwise, abort the current mission. """ - if not mission.is_alive(): - robot.reflectCal() + if not MenuLib.mission.is_alive(): + MenuLib.robot.reflectCal() else: - abort() + MenuLib.abort() def enter(): """ Called when center button pressed; if there is no mission running, launch the selected mission, increase count, and update the display. Otherwise, abort the current mission. """ - if not mission.is_alive(): - run() - addCount() - display() + if not MenuLib.mission.is_alive(): + MenuLib.run() + MenuLib.addCount() + MenuLib.display() else: - abort() + MenuLib.abort() # Print to the VS Code window that the button functions have been defined print("Functions Defined", file=stderr) @@ -95,10 +95,8 @@ def enter(): # Start the infinite menu loop while True: - # This updates the status and process id of the mission process every loop cycle - from MenuLib import mission # Store the list of buttons currently pressed as buttonlist - buttonlist = robot.btn.buttons_pressed + buttonlist = MenuLib.robot.btn.buttons_pressed # Check if there are any buttons pressed; if there is, run the corresponding function if buttonlist: buttonMap[buttonlist[0]]() @@ -106,6 +104,6 @@ def enter(): loopIndex = (loopIndex + 1) % 100 # Make sure there is no mission running (to prevent resource conflicts), # and if there is none, display the sensor values and update the light color based on the gyro sensor - if not mission.is_alive(): - checkDrift() - displaySensor() \ No newline at end of file + if not MenuLib.mission.is_alive(): + MenuLib.checkDrift() + MenuLib.displaySensor() \ No newline at end of file