8000 Started a TriangleAvoid function · WesleyJ-128/FLL_Python_Codebase@7ce262c · GitHub
[go: up one dir, main page]

Skip to content

Commit 7ce262c

Browse files
committed
Started a TriangleAvoid function
Made aux sensor setup fully dynamic
1 parent 20f2856 commit 7ce262c

File tree

3 files changed

+146
-4
lines changed

3 files changed

+146
-4
lines changed

FLLMaster/DriveLibraries.py

Lines changed: 135 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
# The program doesn't work without this. Not sure why.
22
from time import sleep
3+
from types import prepare_class
34
from ev3dev2.motor import *
45
from ev3dev2.sensor.lego import *
56
from ev3dev2.sensor import *
@@ -99,11 +100,20 @@ def __init__(self, filename):
99100
self.gs = GyroSensor(self.GyroPort)
100101
# Only instantiate auxillary sensors if the config file shows they exist
101102
if self.InfraredPort is not None:
102-
self.ir = InfraredSensor(self.InfraredPort)
103+
try:
104+
self.ir = InfraredSensor(self.InfraredPort)
105+
except:
106+
pass
103107
if self.UltrasonicPort is not None:
104-
self.us = UltrasonicSensor(self.UltrasonicPort)
108+
try:
109+
self.us = UltrasonicSensor(self.UltrasonicPort)
110+
except:
111+
pass
105112
if self.TouchPort is not None:
106-
self.ts = TouchSensor(self.TouchPort)
113+
try:
114+
self.ts = TouchSensor(self.TouchPort)
115+
except:
116+
pass
107117

108118
# Instantiate the drive motor objects, as well as the motorset objects for controlling both simultainiously
109119
self.tank = MoveTank(self.LeftMotor, self.RightMotor)
@@ -656,4 +666,125 @@ def LineFollow(self, Distance, Speed, Stop):
656666

657667
# If the robot is to stop, stop the motors. Otherwise, leave the motors on and return.
658668
if not Stop == False:
659-
self.steer.stop()
669+
self.steer.stop()
670+
671+
def TriangleAvoid(self, Heading, Distance, Speed):
672+
"""
673+
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.
674+
675+
``Heading``: The angle at which to drive, with the direction the gyro was last calibrated in being zero.
676+
``Distance``: The distance to drive, in centimeters (positive only).
677+
``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.
678+
``Stop``: Stop motors after completion. If ``FALSE``, motors will continue running after ``Distance`` has been traveled. Otherwise, motors will stop after ``Distance`` cm.
679+
"""
680+
# Ensure values are within reasonable limits, and change them if necessary (Idiotproofing).
681+
if Distance <= 0:
682+
print("Distance must be greater than zero. Use negative speed to drive backwards.")
683+
return
684+
elif Distance > 265:
685+
# The longest distance on an FLL table (diagonal) is about 265cm.
686+
if self.ForFLL:
687+
print("Please don't use silly distances (max = 265cm)")
688+
return
689+
if Speed > 75:
690+
Speed = 75
691+
print("Speed must be between -75 and 75 (inclusive).")
692+
elif Speed < -75:
693+
Speed = -75
694+
print("Speed must be between -75 and 75 (inclusive).")
695+
if not self.us:
696+
print("Avoidance Functions need the US sensor to work.")
697+
return
698+
699+
# "Reset" motor encoders by subtracting start values
700+
left_motor_start = self.lm.degrees
701+
right_motor_start = self.rm.degrees
702+
left_motor_now = self.lm.degrees
703+
right_motor_now = self.rm.degrees
704+
left_motor_change = left_motor_now - left_motor_start
705+
right_motor_change = right_motor_now - right_motor_start
706+
707+
# Determine the sign of the speed, for PID correction
708+
sign = Speed / abs(Speed)
709+
710+
# Find number of degrees that motors need to rotate to reach the desired number of cm.
711+
target = (Distance * 360) / self.WheelCircumference * abs(self.GearRatio)
712+
713+
# Find the average of the left and right encoders, as they could be different from PID correction
714+
avg = abs((left_motor_change + right_motor_change) / 2)
715+
716+
# Initialize variables for PID control
717+
integral = 0.0
718+
last_error = 0.0
719+
derivative = 0.0
720+
721+
# Check if the motors have gone far enough
722+
while avg < target:
723+
# Read the gyro and ultrasonic sensors
724+
current_angle = self.correctedAngle
725+
dist_to_obstacle = self.us.distance_centimeters
726+
727+
# Calculate the PID components
728+
error = current_angle - Heading
729+
integral = integral + error
730+
derivative = error - last_error
731+
last_error = error
732+
733+
# Calculate Steering value based on PID components and kp, ki, and kd
734+
turn_native_units = sign * max([min([(self.kp * error) + (self.ki * integral) + (self.kd * derivative), 100]), -100])
735+
736+
# Check if the motors will stop at the end. If not, the speed will be adjusted to come to a smooth stop.
737+
# Check if the robot has gone 70% or more of the distance. If so, start slowing down
738+
if (target - avg) <= (0.3 * target):
739+
# Calculate the pecrentage of the distance left to travel
740+
targDist = 1 - (avg / target)
741+
# Calculate speedMult based on pecentage; linear function was designed to bring the robot to a
742+
# smooth stop while still reaching the target, resulting in 20% of the intial speed at end.
743+
speedMult = ((8 / 3) * targDist) + 0.2
744+
else:
745+
speedMult = 1
746+
747+
if dist_to_obstacle <= 30:
748+
left_motor_now = self.lm.degrees
749+
right_motor_now = self.rm.degrees
750+
left_motor_change = left_motor_now - left_motor_start
751+
right_motor_change = right_motor_now - right_motor_start
752+
avg = abs((left_motor_change + right_motor_change) / 2)
753+
driven_so_far = (avg * self.WheelCircumference) / 360
754+
self.steer.off(brake=False)
755+
dist_to_obstacle = self.us.distance_centimeters
756+
start_angle = self.correctedAngle
757+
while self.us.distance_centimeters < 60:
758+
self.tank.on(-10, 10)
759+
self.tank.off()
760+
self.GyroTurn(self.correctedAngle - 5)
761+
end_angle = self.correctedAngle
762+
degrees_turned = abs(end_angle - start_angle)
763+
first_hypotenuse = dist_to_obstacle / math.cos(math.radians(degrees_turned))
764+
self.TriangleAvoid(end_angle, first_hypotenuse, (Speed / 2))
765+
first_triangle_second_angle = math.degrees(math.asin(dist_to_obstacle / first_hypotenuse))
766+
second_triangle_short_leg = math.sqrt((first_hypotenuse ** 2) - (dist_to_obstacle ** 2))
767+
second_triangle_long_leg = Distance - (driven_so_far + dist_to_obstacle + 2.21)
768+
second_triangle_second_angle = math.degrees(math.atan(second_triangle_long_leg / second_triangle_short_leg))
769+
degrees_to_turn = 180 - (first_triangle_second_angle + second_triangle_second_angle)
770+
self.GyroTurn(Heading)
771+
sleep(5)
772+
self.GyroTurn(degrees_to_turn - Heading)
773+
second_hypotenuse = math.sqrt((second_triangle_long_leg ** 2) + (second_triangle_short_leg ** 2))
774+
self.TriangleAvoid(self.correctedAngle, second_hypotenuse, (Speed / 2))
775+
self.GyroTurn(Heading)
776+
return
777+
778+
779+
# Start the motors, using the steering value calculated by the PID control, and the input speed multiplied by speedMult (0 - 1, see above).
780+
self.steer.on(-turn_native_units, (Speed * speedMult))
781+
782+
# Update corrected encoder values
783+
left_motor_now = self.lm.degrees
784+
right_motor_now = self.rm.degrees
785+
left_motor_change = left_motor_now - left_motor_start
786+
right_motor_change = right_motor_now - right_motor_start
787+
avg = abs((left_motor_change + right_motor_change) / 2)
788+
789+
# Stop the motors.
790+
self.steer.stop()

FLLMaster/MenuLib.py

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -78,8 +78,12 @@ def runCurrentMission():
7878
# Attempts to stop auxillary motors. If there are none, the try blocks will fail and the program will continue
7979
try:
8080
robot.m1.off(brake=False)
81+
except:
82+
pass
8183
try:
8284
robot.m2.off(brake=False)
85+
except:
86+
pass
8387

8488
def initthread():
8589
"""
@@ -131,8 +135,12 @@ def abort():
131135
# Attempts to stop auxillary motors. If there are none, the try blocks will fail and the program will continue
132136
try:
133137
robot.m1.off(brake=False)
138+
except:
139+
pass
134140
try:
135141
robot.m2.off(brake=False)
142+
except:
143+
pass
136144
# Undo the auto-advance so the mission could easily be redone
137145
minusCount()
138146
display()

FLLMaster/Missions.py

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,9 @@ def a00LineFollow():
2727
# Follows a line for 50 cm, at 50% speed, stopping at end
2828
robot.LineFollow(50, 50, True)
2929

30+
def a06AvoidTest():
31+
robot.TriangleAvoid(0, 100, 50)
32+
3033
def a10Command_Key():
3134
# Drives in a pattern similar to the Apple command key
3235
# o_o

0 commit comments

Comments
 (0)
0