984 lines
No EOL
41 KiB
Python
984 lines
No EOL
41 KiB
Python
# LEGO type:standard slot:3 autostart
|
|
|
|
import math
|
|
from spike import PrimeHub, Motor, MotorPair, ColorSensor
|
|
from spike.control import wait_for_seconds, Timer
|
|
from hub import battery
|
|
hub = PrimeHub()
|
|
|
|
|
|
import hub as hub2
|
|
|
|
import sys
|
|
|
|
|
|
"""
|
|
Initialize motor and color Sensors
|
|
"""
|
|
# adjust the sensor ports until they match your configuration,
|
|
# we recommend assigning your ports to the ones in the program for ease of use
|
|
colorE = ColorSensor('E')
|
|
colorF = ColorSensor('E')
|
|
smallMotorA = Motor('A')
|
|
smallMotorD = Motor('B')
|
|
|
|
#Preperation for parallel code execution
|
|
accelerate = True
|
|
run_generator = True
|
|
runSmall = True
|
|
|
|
lastAngle = 0
|
|
oldAngle = 0
|
|
|
|
gyroValue = 0
|
|
|
|
# Create your objects here.
|
|
hub = PrimeHub()
|
|
|
|
#PID value Definition
|
|
pRegler = 0.0
|
|
iRegler = 0.0
|
|
dRegler = 0.0
|
|
|
|
pReglerLight = 0.0
|
|
iReglerLight = 0.0
|
|
dReglerLight = 0.0
|
|
|
|
#Set variables based on robot
|
|
circumference = 17.6 #circumference of the wheel powered by the robot in cm
|
|
sensordistance = 7 #distance between the two light sensors in cm. Used in Tangent alignment 6.4 in studs
|
|
|
|
|
|
|
|
cancel = False
|
|
inMain = True
|
|
|
|
class DriveBase:
|
|
|
|
def __init__(self, hub, leftMotor, rightMotor):
|
|
self.hub = hub
|
|
self.leftMotor = Motor(leftMotor)
|
|
self.rightMotor = Motor(rightMotor)
|
|
self.movement_motors = MotorPair(leftMotor, rightMotor)
|
|
|
|
def lineFollower(self, distance, startspeed, maxspeed, endspeed, sensorPort, side, addspeed = 0.2, brakeStart = 0.7 , stopMethod=None, generator = None, stop = True):
|
|
"""
|
|
This is the function used to let the robot follow a line until either the entered distance has been achieved or the other sensor of the robot senses a line.
|
|
Like all functions that drive the robot this function has linear acceleration and breaking. It also uses PID values that are automatically set depending on the
|
|
current speed of the robot (See function PIDCalculationLight)
|
|
Parameters
|
|
-------------
|
|
distance: The value tells the program the distance the robot has to drive. Type: Integer. Default: No default value
|
|
speed: The speed which the robot is supposed to start at. Type: Integer. Default: No default value
|
|
maxspeed: The highest speed at which the robot drives. Type: Integer. Default: No default value
|
|
endspeed: The speed which the robot achieves at the end of the function. Type: Integer. Default: No default value
|
|
addspeed: The percentage after which the robot reaches its maxspeed. Type: Float. Default: No default value
|
|
brakeStart: The value which we use to tell the robot after what percentage of the distance we need to slow down. Type: Float. Default: No default value
|
|
stopMethod: the Stopmethod the robot uses to stop. If no stopMethod is passed stopDistance is used instead. Default: stopDistance
|
|
generator: the generator that runs something parallel while driving. Default: No default value
|
|
stop: the boolean that determines whether the robot should stop the motors after driving or not. Default: True
|
|
"""
|
|
|
|
if cancel:
|
|
return
|
|
|
|
global run_generator, runSmall
|
|
|
|
if generator == None:
|
|
run_generator = False
|
|
|
|
#set the speed the robot starts at
|
|
speed = startspeed
|
|
#reset PID values to eliminate bugs
|
|
change = 0
|
|
old_change = 0
|
|
integral = 0
|
|
#reset the driven distance of the robot to eliminate bugs
|
|
|
|
#specifies the color sensor
|
|
colorsensor = ColorSensor(sensorPort)
|
|
#Get degrees of motors turned before robot has moved, allows for distance calculation without resetting motors
|
|
loop = True
|
|
#Going backwards is not supported on our robot due to the motors then being in front of the colour sensors and the program not working
|
|
if distance < 0:
|
|
print('ERR: distance < 0')
|
|
distance = abs(distance)
|
|
#Calculate target values for the motors to turn to
|
|
finalDistance = (distance / 17.6) * 360
|
|
#Calculate after what distance the robot has to reach max speed
|
|
accelerateDistance = finalDistance * addspeed
|
|
deccelerateDistance = finalDistance * (1 - brakeStart)
|
|
|
|
invert = 1
|
|
|
|
#Calculation of steering factor, depending on which side of the line we are on
|
|
if side == "left":
|
|
invert = 1
|
|
elif side == "right":
|
|
invert = -1
|
|
|
|
#Calculation of the start of the robot slowing down
|
|
|
|
self.left_Startvalue = self.leftMotor.get_degrees_counted()
|
|
self.right_Startvalue = self.rightMotor.get_degrees_counted()
|
|
drivenDistance = getDrivenDistance(self)
|
|
|
|
brakeStartValue = brakeStart * finalDistance
|
|
while loop:
|
|
if cancel:
|
|
print("cancel")
|
|
break
|
|
|
|
if run_generator: #run parallel code execution
|
|
next(generator)
|
|
|
|
#Checks the driven distance as an average of both motors for increased accuracy
|
|
oldDrivenDistance = drivenDistance
|
|
drivenDistance = getDrivenDistance(self)
|
|
#Calculates target value for Robot as the edge of black and white lines
|
|
old_change = change
|
|
|
|
change = colorsensor.get_reflected_light() - 50
|
|
|
|
|
|
#Steering factor calculation using PID, sets new I value
|
|
|
|
|
|
steering = (((change * pReglerLight) + (integral * iReglerLight) + (dReglerLight * (change - old_change)))) * invert
|
|
integral = change + integral
|
|
#Calculation of current speed for robot, used for acceleratiion, braking etc.
|
|
speed = speedCalculation(speed, startspeed, maxspeed, endspeed, accelerateDistance, deccelerateDistance, brakeStartValue, drivenDistance, oldDrivenDistance)
|
|
|
|
pidCalculationLight(speed)
|
|
#PID value updates
|
|
steering = max(-100, min(steering, 100))
|
|
|
|
#Driving using speed values calculated with PID and acceleration for steering, use of distance check
|
|
self.movement_motors.start_at_power(int(speed), int(steering))
|
|
|
|
if stopMethod != None:
|
|
if stopMethod.loop():
|
|
loop = False
|
|
else:
|
|
if finalDistance < drivenDistance:
|
|
break
|
|
|
|
if stop:
|
|
self.movement_motors.stop()
|
|
|
|
run_generator = True
|
|
runSmall = True
|
|
generator = 0
|
|
return
|
|
|
|
def gyroRotation(self, angle, startspeed, maxspeed, endspeed, addspeed = 0.3, brakeStart = 0.7, rotate_mode = 0, stopMethod = None, generator = None, stop = True):
|
|
"""
|
|
This is the function that we use to make the robot turn the length of a specific angle or for the robot to turn until it senses a line. Even in this function the robot
|
|
can accelerate and slow down. It also has Gyrosensor calibrations based on our experimental experience.
|
|
Parameters
|
|
-------------
|
|
angle: The angle which the robot is supposed to turn. Use negative numbers to turn counterclockwise. Type: Integer. Default value: No default value
|
|
startspeed: The speed which the robot is supposed to start at. Type: Integer. Default: No default value
|
|
maxspeed: The highest speed at which the robot drives. Type: Integer. Default: No default value
|
|
endspeed: The speed which the robot achieves at the end of the function. Type: Integer. Default: No default value
|
|
addspeed: The percentage after which the robot reaches the maxspeed. Type: Float. Default: No default value
|
|
brakeStart: The percentage after which the robot starts slowing down until it reaches endspeed. Type: Float. Default: No default value
|
|
rotate_mode: Different turning types. 0: Both motors turn, robot turns on the spot. 1: Only the outer motor turns, resulting in a corner. Type: Integer. Default: 0
|
|
stopMethod: the Stopmethod the robot uses to stop. If no stopMethod is passed stopDistance is used instead. Default: stopDistance
|
|
generator: the generator that runs something parallel while driving. Default: No default value
|
|
stop: the boolean that determines whether the robot should stop the motors after driving or not. Default: True
|
|
"""
|
|
|
|
if cancel:
|
|
return
|
|
|
|
global run_generator, runSmall
|
|
|
|
if generator == None:
|
|
run_generator = False
|
|
|
|
if rotate_mode == 0:
|
|
startspeed = abs(startspeed)
|
|
maxspeed = abs(maxspeed)
|
|
endspeed = abs(endspeed)
|
|
|
|
speed = startspeed
|
|
|
|
#set standard variables
|
|
rotatedDistance = 0
|
|
steering = 1
|
|
|
|
accelerateDistance = abs(angle * addspeed)
|
|
deccelerateDistance = abs(angle * (1 - brakeStart))
|
|
|
|
#gyro sensor calibration
|
|
angle = angle * (2400/2443) #experimental value based on 20 rotations of the robot
|
|
|
|
#Setting variables based on inputs
|
|
loop = True
|
|
gyroStartValue = getGyroValue() #Yaw angle used due to orientation of the self.hub. This might need to be changed
|
|
brakeStartValue = (angle + gyroStartValue) * brakeStart
|
|
|
|
#Inversion of steering value for turning counter clockwise
|
|
if angle < 0:
|
|
steering = -1
|
|
|
|
#Testing to see if turining is necessary, turns until loop = False
|
|
|
|
while loop:
|
|
if cancel:
|
|
break
|
|
|
|
if run_generator: #run parallel code execution
|
|
next(generator)
|
|
|
|
oldRotatedDistance = rotatedDistance
|
|
rotatedDistance = getGyroValue() #Yaw angle used due to orientation of the self.hub. This might need to be changed
|
|
|
|
#Checking for variants
|
|
#Both Motors turn, robot moves on the spot
|
|
if rotate_mode == 0:
|
|
self.movement_motors.start_tank_at_power(int(speed) * steering, -int(speed) * steering)
|
|
#Only outer motor turns, robot has a wide turning radius
|
|
|
|
elif rotate_mode == 1:
|
|
|
|
if angle * speed > 0:
|
|
self.leftMotor.start_at_power(- int(speed))
|
|
else:
|
|
self.rightMotor.start_at_power(+ int(speed))
|
|
|
|
if stopMethod != None:
|
|
if stopMethod.loop():
|
|
loop = False
|
|
break
|
|
elif abs(angle) <= abs(rotatedDistance - gyroStartValue):
|
|
loop = False
|
|
break
|
|
|
|
|
|
|
|
#Stops movement motors for increased accuracy while stopping
|
|
if stop:
|
|
self.movement_motors.stop()
|
|
|
|
run_generator = True
|
|
runSmall = True
|
|
|
|
return # End of gyroStraightDrive
|
|
|
|
def gyroStraightDrive(self, distance, startspeed, maxspeed, endspeed, addspeed = 0.3, brakeStart = 0.7, stopMethod=None, offset = 0, generator = None, stop = True):
|
|
"""
|
|
This is the function that we use to make the robot go forwards or backwards without drifting. It can accelerate, it can slow down and there's also PID. You can set the values
|
|
in a way where you can either drive until the entered distance has been achieved or until the robot senses a line.
|
|
Parameters
|
|
-------------
|
|
distance: the distance that the robot is supposed to drive. Type: Integer. Default: No default value
|
|
speed: The speed which the robot is supposed to start at. Type: Integer. Default: No default value
|
|
maxspeed: The highest speed at which the robot drives. Type: Integer. Default: No default value
|
|
endspeed: The speed which the robot achieves at the end of the function. Type: Integer. Default: No default value
|
|
addspeed: The speed which the robot adds in order to accelerate. Type: Float. Default: 0.2
|
|
brakeStart: The value which we use to tell the robot after what percentage of the distance we need to slow down. Type: Float. Default: 0.8
|
|
port: This value tells the program whether the robot is supposed to check for a black line with the specified light snsor. Type: String. Default: 0
|
|
lightValue: This value tells the program the value the robot should stop at if port sees it. Type: Integer. Default: 0
|
|
align_variant: Tells the robot to align itself to a line if it sees one. 0: No alignment. 1: standard alignment. 2: tangent based alignment Type: Integer. Default: 0
|
|
detectLineStart: The value which we use to tell the robot after what percentage of the distance we need to look for the line to drive to. Type: Float. Default: 0
|
|
offset: The value sends the robot in a direction which is indicated by the value entered. Type: Integer. Default: 0
|
|
generator: Function executed while robot is executing gyroStraightDrive. Write the wanted function and its parameters here. Type: . Default: 0
|
|
stopMethod: the Stopmethod the robot uses to stop. If no stopMethod is passed stopDistance is used instead. Default: stopDistance
|
|
generator: the generator that runs something parallel while driving. Default: No default value
|
|
stop: the boolean that determines whether the robot should stop the motors after driving or not. Default: True
|
|
"""
|
|
|
|
if cancel:
|
|
return
|
|
|
|
global run_generator, runSmall
|
|
global pRegler, iRegler, dRegler
|
|
|
|
if generator == None:
|
|
run_generator = False
|
|
|
|
#Set starting speed of robot
|
|
speed = startspeed
|
|
#Sets PID values
|
|
|
|
change = 0
|
|
old_change = 0
|
|
integral = 0
|
|
steeringSum = 0
|
|
|
|
invert = -1
|
|
|
|
#Sets values based on user inputs
|
|
loop = True
|
|
|
|
|
|
gyroStartValue = getGyroValue()
|
|
|
|
#Error check for distance
|
|
if distance < 0:
|
|
print('ERR: distance < 0')
|
|
distance = abs(distance)
|
|
|
|
#Calulation of degrees the motors should turn to
|
|
#17.6 is wheel circumference in cm. You might need to adapt it
|
|
rotateDistance = (distance / 17.6) * 360
|
|
accelerateDistance = rotateDistance * addspeed
|
|
deccelerateDistance = rotateDistance * (1 - brakeStart)
|
|
|
|
#Inversion of target rotation value for negative values
|
|
if speed < 0:
|
|
invert = 1
|
|
|
|
#Calculation of braking point
|
|
self.left_Startvalue = self.leftMotor.get_degrees_counted()
|
|
self.right_Startvalue = self.rightMotor.get_degrees_counted()
|
|
brakeStartValue = brakeStart * rotateDistance
|
|
drivenDistance = getDrivenDistance(self)
|
|
|
|
while loop:
|
|
if cancel:
|
|
break
|
|
if run_generator: #run parallel code execution
|
|
next(generator)
|
|
|
|
#Calculation of driven distance and PID values
|
|
oldDrivenDistance = drivenDistance
|
|
drivenDistance = getDrivenDistance(self)
|
|
|
|
pidCalculation(speed)
|
|
change = getGyroValue() - gyroStartValue #yaw angle used due to orientation of the self.hub
|
|
|
|
|
|
currenSteering = (change * pRegler + integral * iRegler + dRegler * (change - old_change)) + offset + steeringSum*0.02
|
|
|
|
currenSteering = max(-100, min(currenSteering, 100))
|
|
#print("steering: " + str(currenSteering) + " gyro: " + str(change) + " integral: " + str(integral))
|
|
|
|
steeringSum += change
|
|
integral += change - old_change
|
|
old_change = change
|
|
|
|
#Calculation of speed based on acceleration and braking, calculation of steering value for robot to drive perfectly straight
|
|
speed = speedCalculation(speed, startspeed,maxspeed, endspeed, accelerateDistance, deccelerateDistance, brakeStartValue, drivenDistance, oldDrivenDistance)
|
|
self.movement_motors.start_at_power(int(speed), invert * int(currenSteering))
|
|
|
|
|
|
if stopMethod != None:
|
|
if stopMethod.loop():
|
|
loop = False
|
|
elif rotateDistance < drivenDistance:
|
|
loop = False
|
|
|
|
|
|
if stop:
|
|
self.movement_motors.stop()
|
|
|
|
run_generator = True
|
|
runSmall = True
|
|
|
|
return #End of gyroStraightDrive
|
|
|
|
def arcRotation(self, radius, angle, startspeed, maxspeed, endspeed, addspeed = 0.3, brakeStart = 0.7, stopMethod=None, generator = None, stop = True):
|
|
"""
|
|
This is the function that we use to make the robot drive a curve with a specified radius and to a given angle
|
|
Parameters
|
|
-------------
|
|
radius: the radius of the curve the robot is supposed to drive; measured from the outside edge of the casing. Type: Integer. Default: 0
|
|
angle: the angle that the robot is supposed to rotate on the curve. Type: Integer. Default: 0
|
|
speed: The speed which the robot is supposed to start at. Type: Integer. Default: No default value
|
|
maxspeed: The highest speed at which the robot drives. Type: Integer. Default: No default value
|
|
endspeed: The speed which the robot achieves at the end of the function. Type: Integer. Default: No default value
|
|
addspeed: The speed which the robot adds in order to accelerate. Type: Float. Default: 0.2
|
|
brakeStart: The value which we use to tell the robot after what percentage of the distance we need to slow down. Type: Float. Default: 0.8
|
|
stopMethod: the Stopmethod the robot uses to stop. If no stopMethod is passed stopDistance is used instead. Default: stopDistance
|
|
generator: the generator that runs something parallel while driving. Default: No default value
|
|
stop: the boolean that determines whether the robot should stop the motors after driving or not. Default: True
|
|
"""
|
|
|
|
if cancel:
|
|
print("cancel")
|
|
return
|
|
|
|
global run_generator, runSmall
|
|
|
|
if generator == None:
|
|
run_generator = False
|
|
|
|
angle = angle * (336/360) #gyro calibration
|
|
|
|
gyroStartValue = getGyroValue()
|
|
finalGyroValue = gyroStartValue + angle
|
|
currentAngle = gyroStartValue
|
|
|
|
accelerateDistance = abs(angle * addspeed)
|
|
deccelerateDistance = abs(angle * (1 - brakeStart))
|
|
brakeStartValue = abs(angle * brakeStart)
|
|
|
|
loop = True
|
|
|
|
#Calculating the speed ratios based on the given radius
|
|
if angle * startspeed > 0:
|
|
speed_ratio_left = (radius+14) / (radius+2) #calculate speed ratios for motors. These will need to be adapted based on your robot design
|
|
speed_ratio_right = 1
|
|
else:
|
|
speed_ratio_left = 1
|
|
speed_ratio_right = (radius+14) / (radius+2)
|
|
|
|
#Calculating the first speed to drive with
|
|
left_speed = speedCalculation(startspeed, startspeed, maxspeed, endspeed, accelerateDistance, deccelerateDistance, brakeStartValue, 1, 0)
|
|
right_speed = speedCalculation(startspeed, startspeed , maxspeed , endspeed , accelerateDistance, deccelerateDistance, brakeStartValue, 1, 0)
|
|
while loop:
|
|
#when the cancel button is pressed stop the gyrostraight drive directly
|
|
if cancel:
|
|
break
|
|
|
|
if run_generator: #run parallel code execution
|
|
next(generator)
|
|
|
|
currentAngle = getGyroValue() #Yaw angle used due to orientation of the self.hub. This might need to be changed
|
|
|
|
#Calculating the current speed the robot should drive
|
|
left_speed = speedCalculation(left_speed, startspeed, maxspeed, endspeed, accelerateDistance, deccelerateDistance, brakeStartValue, 1, 0)
|
|
right_speed = speedCalculation(right_speed, startspeed , maxspeed , endspeed , accelerateDistance, deccelerateDistance, brakeStartValue, 1, 0)
|
|
|
|
|
|
self.movement_motors.start_tank_at_power(int(left_speed* speed_ratio_left), int(right_speed* speed_ratio_right))
|
|
|
|
#if there is a stopMethod passed use it and stop the loop if it returns true otherwise check if the robot has rotated to the given angle
|
|
if stopMethod != None:
|
|
#print("stoMeth")
|
|
if stopMethod.loop():
|
|
loop = False
|
|
break
|
|
|
|
(angle / abs(angle))
|
|
if finalGyroValue * (angle / abs(angle)) < currentAngle * (angle / abs(angle)):
|
|
#print("finalGyroValue: " + str(finalGyroValue) + " rotatedDistance: " + str(currentAngle))
|
|
loop = False
|
|
break
|
|
|
|
|
|
|
|
|
|
#if stop is true then stop the motors otherwise don't stop the motor
|
|
if stop:
|
|
self.movement_motors.stop()
|
|
|
|
run_generator = True
|
|
runSmall = True
|
|
return #End of arcRotation
|
|
|
|
def resetGyroValue():
|
|
global gyroValue
|
|
hub2.motion.yaw_pitch_roll(0)
|
|
|
|
gyroValue = 0
|
|
|
|
def getGyroValue():
|
|
|
|
#this method is used to return the absolute gyro Angle and the angle returned by this method doesn't reset at 180 degree
|
|
global lastAngle
|
|
global oldAngle
|
|
global gyroValue
|
|
|
|
#gets the angle returned by the spike prime program. The problem is the default get_yaw_angle resets at 180 and -179 back to 0
|
|
angle = hub.motion_sensor.get_yaw_angle()
|
|
|
|
if angle != lastAngle:
|
|
oldAngle = lastAngle
|
|
|
|
lastAngle = angle
|
|
|
|
if angle == 179 and oldAngle == 178:
|
|
hub2.motion.yaw_pitch_roll(0)#reset
|
|
gyroValue += 179
|
|
angle = 0
|
|
|
|
if angle == -180 and oldAngle == -179:
|
|
hub2.motion.yaw_pitch_roll(0) #reset
|
|
gyroValue -= 180
|
|
angle = 0
|
|
|
|
return gyroValue + angle
|
|
|
|
def getDrivenDistance(data):
|
|
|
|
|
|
#print(str(abs(data.leftMotor.get_degrees_counted() - data.left_Startvalue)) + " .:. " + str(abs(data.rightMotor.get_degrees_counted() - data.right_Startvalue)))
|
|
|
|
drivenDistance = (
|
|
abs(data.leftMotor.get_degrees_counted() - data.left_Startvalue) +
|
|
abs(data.rightMotor.get_degrees_counted() - data.right_Startvalue)) / 2
|
|
|
|
return drivenDistance
|
|
|
|
def defaultClass(object, db):
|
|
object.db = db
|
|
object.leftMotor = db.leftMotor
|
|
object.rightMotor = db.rightMotor
|
|
|
|
object.left_Startvalue = abs(db.leftMotor.get_degrees_counted())
|
|
object.right_Startvalue = abs(db.rightMotor.get_degrees_counted())
|
|
return object
|
|
|
|
class stopMethods(): #This class has all our stopmethods for easier coding and less redundancy
|
|
|
|
class stopLine():
|
|
"""
|
|
Drive until a Line is detected
|
|
Parameters
|
|
-------------
|
|
db: the drivebase of the robot
|
|
port: Port to detect line on
|
|
lightvalue: Value of the light to detect
|
|
detectLineDistance: Distance until start detecting a line
|
|
"""
|
|
def __init__(self, db, port, lightvalue, detectLineDistance):
|
|
self = defaultClass(self, db)
|
|
|
|
self.port = port
|
|
self.detectLineDistance = (detectLineDistance / 17.6) * 360
|
|
|
|
#if lightvalue bigger 50 stop when lightvalue is higher
|
|
self.lightvalue = lightvalue
|
|
|
|
|
|
def loop(self):
|
|
|
|
|
|
drivenDistance = getDrivenDistance(self)
|
|
|
|
if abs(self.detectLineDistance) < abs(drivenDistance):
|
|
if self.lightvalue > 50:
|
|
if ColorSensor(self.port).get_reflected_light() > self.lightvalue:
|
|
return True
|
|
else:
|
|
if ColorSensor(self.port).get_reflected_light() < self.lightvalue:
|
|
return True
|
|
|
|
return False
|
|
|
|
class stopAlign():
|
|
"""
|
|
Drive until a Line is detected
|
|
Parameters
|
|
-------------
|
|
db: the drivebase of the robot
|
|
port: Port to detect line on
|
|
lightvalue: Value of the light to detect
|
|
speed: speed at which the robot searches for other line
|
|
"""
|
|
def __init__(self, db, lightvalue, speed):
|
|
self = defaultClass(self, db)
|
|
self.speed = speed
|
|
|
|
|
|
#if lightvalue bigger 50 stop when lightvalue is higher
|
|
self.lightValue = lightvalue
|
|
|
|
|
|
def loop(self):
|
|
|
|
if colorE.get_reflected_light() < self.lightValue:
|
|
self.rightMotor.stop()
|
|
#Turning robot so that other colour sensor is over line
|
|
while True:
|
|
|
|
self.leftMotor.start_at_power(-int(self.speed))
|
|
|
|
#Line detection and stopping
|
|
if colorF.get_reflected_light() < self.lightValue or cancel:
|
|
self.leftMotor.stop()
|
|
return True
|
|
|
|
|
|
#Colour sensor F sees line first
|
|
elif colorF.get_reflected_light() < self.lightValue:
|
|
|
|
self.leftMotor.stop()
|
|
|
|
#Turning robot so that other colour sensor is over line
|
|
while True:
|
|
self.rightMotor.start_at_power(int(self.speed))
|
|
|
|
#Line detection and stopping
|
|
if colorE.get_reflected_light() < self.lightValue or cancel:
|
|
self.rightMotor.stop()
|
|
return True
|
|
|
|
|
|
return False
|
|
|
|
class stopTangens():
|
|
"""
|
|
Drive until a Line is detected
|
|
Parameters
|
|
-------------
|
|
db: the drivebase of the robot
|
|
port: Port to detect line on
|
|
lightvalue: Value of the light to detect
|
|
speed: Distance until start detecting a line
|
|
"""
|
|
def __init__(self, db, lightvalue, speed):
|
|
self.count = 0
|
|
self = defaultClass(self, db)
|
|
self.speed = speed
|
|
#if lightvalue bigger 50 stop when lightvalue is higher
|
|
self.lightValue = lightvalue
|
|
self.detectedLineDistance = 0
|
|
|
|
self.invert = 1
|
|
if speed < 0:
|
|
self.invert = -1
|
|
|
|
def loop(self):
|
|
drivenDistance = getDrivenDistance(self)
|
|
if colorE.get_reflected_light() < self.lightValue:
|
|
#measuring the distance the robot has driven since it has seen the line
|
|
if(self.detectedLineDistance == 0):
|
|
self.detectedLineDistance = getDrivenDistance(self)
|
|
self.detectedPort = 'E'
|
|
|
|
elif self.detectedPort == 'F':
|
|
db.movement_motors.stop() #Stops robot with sensor F on the line
|
|
angle = math.degrees(math.atan(((drivenDistance - self.detectedLineDistance) / 360 * circumference) / sensordistance)) #Calculating angle that needs to be turned using tangent
|
|
#print("angle: " + str(angle))
|
|
db.gyroRotation(-angle, self.invert * self.speed, self.invert * self.speed, self.invert * self.speed, rotate_mode=1) #Standard gyrorotation for alignment, but inverting speed values if necessary
|
|
|
|
db.movement_motors.stop() #Stopping robot for increased reliability
|
|
return True
|
|
|
|
#Colour sensor F sees line first
|
|
elif colorF.get_reflected_light() < self.lightValue:
|
|
#measuring the distnace the robot has driven since it has seen the line
|
|
if(self.detectedLineDistance == 0):
|
|
self.detectedLineDistance = drivenDistance
|
|
self.detectedPort = 'F'
|
|
|
|
elif self.detectedPort == 'E':
|
|
db.movement_motors.stop() #Stops robot with sensor E on the line
|
|
angle = math.degrees(math.atan(((drivenDistance - self.detectedLineDistance) / 360 * circumference) / sensordistance)) #Calculation angle that needs to be turned using tangent
|
|
db.gyroRotation(angle, self.invert * self.speed, self.invert * self.speed, self.invert * self.speed, rotate_mode=1) #Standard gyrorotation for alignment, but inverting speed values if necessary
|
|
db.movement_motors.stop() #Stopping robot for increased reliablity
|
|
return True
|
|
|
|
return False
|
|
class stopDegree():
|
|
"""
|
|
Roates until a certain degree is reached
|
|
Parameters
|
|
-------------
|
|
db: the drivebase of the robot
|
|
angle: the angle to rotate
|
|
"""
|
|
def __init__(self, db, angle):
|
|
self.angle = angle * (336/360)
|
|
|
|
self.gyroStartValue = getGyroValue() #Yaw angle used due to orientation of the self.hub.
|
|
|
|
|
|
def loop(self):
|
|
rotatedDistance = getGyroValue() #Yaw angle used due to orientation of the self.hub.
|
|
|
|
if abs(self.angle) <= abs(rotatedDistance - self.gyroStartValue):
|
|
return True
|
|
else:
|
|
return False
|
|
|
|
class stopTime():
|
|
|
|
"""
|
|
Drive until a certain time is reached
|
|
Parameters
|
|
-------------
|
|
db: the drivebase of the robot
|
|
time: the time to drive
|
|
"""
|
|
|
|
def __init__(self, db, time) -> None:
|
|
self = defaultClass(self, db)
|
|
self.time = time
|
|
self.timer = Timer()
|
|
self.startTime = self.timer.now()
|
|
|
|
def loop(self):
|
|
if self.timer.now() > self.startTime + self.time:
|
|
return True
|
|
else:
|
|
return False
|
|
|
|
class stopResistance():
|
|
|
|
"""
|
|
Drive until the Robot doesn't move anymore
|
|
Parameters
|
|
-------------
|
|
db: the drivebase of the robot
|
|
restistance: the value the resistance has to be below to stop
|
|
"""
|
|
def __init__(self, db, resistance):
|
|
self = defaultClass(self, db)
|
|
self.resistance = resistance
|
|
self.timer = Timer()
|
|
|
|
self.startTime = 0
|
|
self.lower = False
|
|
self.runs = 0
|
|
|
|
def loop(self):
|
|
|
|
self.runs += 1
|
|
motion = abs(hub2.motion.accelerometer(True)[2])
|
|
|
|
if motion < self.resistance:
|
|
self.lower = True
|
|
|
|
if self.runs > 15:
|
|
if self.lower:
|
|
if self.startTime == 0:
|
|
self.startTime = self.timer.now()
|
|
|
|
if self.timer.now() > self.startTime:
|
|
return True
|
|
|
|
else:
|
|
self.lower = False
|
|
return False
|
|
|
|
def motorResistance(speed, port, resistancevalue):
|
|
"""
|
|
lets the motor stop when it hits an obstacle
|
|
"""
|
|
if abs(resistancevalue) > abs(speed):
|
|
return
|
|
|
|
|
|
if cancel:
|
|
return
|
|
|
|
if port == "A":
|
|
smallMotorA.start_at_power(speed)
|
|
while True:
|
|
old_position = smallMotorA.get_position()
|
|
wait_for_seconds(0.4)
|
|
if abs(old_position - smallMotorA.get_position())<resistancevalue or cancel:
|
|
smallMotorA.stop()
|
|
print("detected stalling")
|
|
return
|
|
|
|
elif port == "D":
|
|
smallMotorD.start_at_power(speed)
|
|
while True:
|
|
old_position = smallMotorD.get_position()
|
|
wait_for_seconds(0.4)
|
|
if abs(old_position - smallMotorD.get_position())<resistancevalue or cancel:
|
|
smallMotorD.stop()
|
|
print("detected stalling")
|
|
return
|
|
else:
|
|
print("wrong port selected. Select A or D")
|
|
return
|
|
|
|
def speedCalculation(speed, startspeed, maxspeed, endspeed, accelerateDistance, deccelerateDistance, brakeStartValue, drivenDistance, oldDrivenDistance):
|
|
"""
|
|
Used to calculate all the speeds in out programs. Done seperatly to reduce redundancy. Brakes and accelerates
|
|
Parameters
|
|
-------------
|
|
speed: The current speed the robot has
|
|
startspeed: Speed the robot starts at. Type: Integer. Default: No default value.
|
|
maxspeed: The maximum speed the robot reaches. Type: Integer. Default: No default value.
|
|
endspeed: Speed the robot aims for while braking, minimum speed at the end of the program. Type: Integer. Default: No default value.
|
|
addspeed: Percentage of the distance after which the robot reaches the maximum speed. Type: Integer. Default: No default value.
|
|
brakeStartValue: Percentage of the driven distance after which the robot starts braking. Type: Integer. Default: No default value.
|
|
drivenDistance: Calculation of the driven distance in degrees. Type: Integer. Default: No default value.
|
|
"""
|
|
|
|
addSpeedPerDegree = (maxspeed - startspeed) / accelerateDistance
|
|
subSpeedPerDegree = (maxspeed - endspeed) / deccelerateDistance
|
|
|
|
|
|
subtraction = (abs(drivenDistance) - abs(oldDrivenDistance) if abs(drivenDistance) - abs(oldDrivenDistance) >= 1 else 1) * subSpeedPerDegree
|
|
addition = (abs(drivenDistance) - abs(oldDrivenDistance) if abs(drivenDistance) - abs(oldDrivenDistance) >= 1 else 1) * addSpeedPerDegree
|
|
|
|
if abs(drivenDistance) > abs(brakeStartValue):
|
|
|
|
if abs(speed) > abs(endspeed):
|
|
speed = speed - subtraction
|
|
|
|
elif abs(speed) < abs(maxspeed):
|
|
|
|
speed = speed + addition
|
|
|
|
return speed
|
|
|
|
def breakFunction(args):
|
|
"""
|
|
Allows you to manually stop currently executing round but still stays in main.
|
|
This is much quicker and more reliable than pressing the center button.
|
|
"""
|
|
global cancel, inMain
|
|
if not inMain:
|
|
cancel = True
|
|
|
|
def pidCalculation(speed):
|
|
#golbally sets PID values based on current speed of the robot, allows for fast and accurate driving
|
|
global pRegler
|
|
global iRegler
|
|
global dRegler
|
|
#Important note: These PID values are experimental and based on our design for the robot. You will need to adjust them manually. You can also set them statically as you can see below
|
|
if speed > 0:
|
|
pRegler = -0.17 * speed + 12.83
|
|
iRegler = 12
|
|
dRegler = 1.94 * speed - 51.9
|
|
if pRegler < 3.2:
|
|
pRegler = 3.2
|
|
else:
|
|
pRegler = (11.1 * abs(speed))/(0.5 * abs(speed) -7) - 20
|
|
iRegler = 10
|
|
#iRegler = 0.02
|
|
dRegler = 1.15**(- abs(speed)+49) + 88
|
|
|
|
def pidCalculationLight(speed):
|
|
#Sets the PID values for the lineFollower based on current speed. Allows for accurate and fast driving
|
|
#Important note: these PID values are experimental and based on our design for the robot. You will need to adjust them. See above on how to do so
|
|
global pReglerLight
|
|
global dReglerLight
|
|
|
|
pReglerLight = -0.04 * speed + 4.11
|
|
dReglerLight = 0.98 * speed - 34.2
|
|
#set hard bottom for d value, as otherwise the values don't work
|
|
if dReglerLight < 5:
|
|
dReglerLight = 5
|
|
|
|
def driveMotor(rotations, speed, port):
|
|
"""
|
|
Allows you to drive a small motor in parallel to driving with gyroStraightDrive
|
|
Parameters
|
|
-------------
|
|
rotations: the rotations the motor turns
|
|
speed: the speed at which the motor turns
|
|
port: the motor used. Note: this cannot be the same motors as configured in the motor Drivebase
|
|
"""
|
|
|
|
global runSmall
|
|
global run_generator
|
|
|
|
if cancel:
|
|
runSmall = False
|
|
run_generator = False
|
|
|
|
while runSmall:
|
|
smallMotor = Motor(port)
|
|
smallMotor.set_degrees_counted(0)
|
|
|
|
loop_small = True
|
|
while loop_small:
|
|
drivenDistance = smallMotor.get_degrees_counted()
|
|
smallMotor.start_at_power(speed)
|
|
if (abs(drivenDistance) > abs(rotations) * 360):
|
|
loop_small = False
|
|
if cancel:
|
|
loop_small = False
|
|
yield
|
|
|
|
smallMotor.stop()
|
|
runSmall = False
|
|
run_generator = False
|
|
yield
|
|
|
|
hub2.motion.yaw_pitch_roll(0)
|
|
|
|
db = DriveBase(hub, 'A', 'B') #this lets us conveniently hand over our motors (B: left driver; C: right driver). This is necessary for the cancel function
|
|
|
|
def exampleOne():
|
|
#This example aims to show all the options for following a line. See the specific documentation of the function for further information.
|
|
db.lineFollower(15, 25, 35, 25, 'E', 'left') #follows the left side of a line on the E sensor for 15cm. Accelerates from speed 25 to 35 and ends on 25 again
|
|
hub.left_button.wait_until_pressed()
|
|
db.lineFollower(15, 25, 35, 25, 'E', 'left', 0.4, 0.6) #same line follower as before but with a longer acceleration and breaking period
|
|
hub.left_button.wait_until_pressed()
|
|
db.lineFollower(15, 25, 35, 25, 'E', 'left', stopMethod=stopMethods.stopLine(db, 'F', 0.7)) #same linefollower as the first, but this time stopping, when the other sensor sees a black line after at least 70% of the driven distance
|
|
hub.left_button.wait_until_pressed()
|
|
db.lineFollower(15, 25, 35, 25, 'E', 'left', stopMethod=stopMethods.stopResistance(db, 20)) #same as first linefollower, but stops when desired resistance is reached. Test the resistance value based on your robot
|
|
hub.left_button.wait_until_pressed()
|
|
generator = driveMotor(5, 100, 'A')
|
|
db.lineFollower(15, 25, 35, 25, 'E', 'left', generator=generator) #same as first linefollower, but drives while turning the A-Motor for 5 rotations
|
|
hub.left_button.wait_until_pressed()
|
|
db.lineFollower(15, 25, 35, 25, 'E', 'left', stop=False) #same as first linefollower, but does not actively brake the motors. The transistion form this action to the next is then smoother
|
|
return
|
|
|
|
def exampleTwo():
|
|
#This example aims to show all the options for turning the robot. See the specific documentation of the function for further information.
|
|
db.gyroRotation(90, 25, 35, 25) #turns the robot 90° clockwise while accelerating from speed 25 to 35 and back down to 25
|
|
hub.left_button.wait_until_pressed()
|
|
db.gyroRotation(90, 25, 35, 25, 0.4, 0.5) #same turning as in first rotation but with longer acceleration/braking phase
|
|
hub.left_button.wait_until_pressed()
|
|
db.gyroRotation(90, 25, 35, 25, rotate_mode=1) #same turn as in first rotation but this time turning using only one wheel rather than turning on the spot. Your speeds may need to be higher for this
|
|
hub.left_button.wait_until_pressed()
|
|
db.gyroRotation(90, 25, 35, 25, stopMethod=stopMethods.stopAlign(db, 25, 25)) #aligns the robot with a line in turning path
|
|
hub.left_button.wait_until_pressed()
|
|
db.gyroRotation(90, 25, 35, 25, stopMethod=stopMethods.stopLine(db, 'E', 25, 0.7)) #turns until the robot sees a line on sensor E after at least 70% of turning
|
|
hub.left_button.wait_until_pressed()
|
|
db.gyroRotation(90, 25, 35, 25, stopMethod=stopMethods.stopTangens(db, 25, 25)) #aligns the robot like stopAlign but is a bit more precise
|
|
hub.left_button.wait_until_pressed()
|
|
#remaining parameters are the same as in linefollower. Please refer to exampleOne or the documentation of the individual functions
|
|
return
|
|
|
|
def exampleThree():
|
|
#This example aims to show all the options for driving in a straight line. See the specific documentation of the function for further information.
|
|
db.gyroStraightDrive(30, 25, 35, 25) #drives in a straight line for 30cm
|
|
hub.left_button.wait_until_pressed()
|
|
db.gyroStraightDrive(30, 25, 55, 25, 0.1, 0.9) #same as first drive, but faster and with harder acceleration/braking
|
|
hub.left_button.wait_until_pressed()
|
|
db.gyroStraightDrive(30, 25, 35, 25, offset=15) #same as first drive, but aims 15° in clockwise direction as target orientation
|
|
#remaining features of code are explained in previous examples. Please refer to exampleOne, exampleTwo and additional documentation within individual functions
|
|
return
|
|
|
|
def exampleFour():
|
|
#This example aims to show all the options for turning in a large curve. See the specific documentation of the function for further information.
|
|
db.arcRotation(5, 35, 25, 30, 25) #robot drives 35° on a circle with a radius of 5cm measured from the inside edge of the robot
|
|
#remaining features of code are explained in previous examples. Please refer to exampleOne, exampleTwo and additional documentation within individual functions
|
|
return
|
|
|
|
def exampleFive():
|
|
#add your own code here
|
|
|
|
|
|
return
|
|
|
|
def exampleSix():
|
|
#add your own code here
|
|
|
|
|
|
return
|
|
|
|
class bcolors:
|
|
BATTERY = '\033[32m'
|
|
BATTERY_LOW = '\033[31m'
|
|
|
|
ENDC = '\033[0m'
|
|
|
|
pReglerLight = 1.6
|
|
iReglerLight = 0.009
|
|
dReglerLight = 16
|
|
|
|
accelerate = True
|
|
|
|
# see comment for breakFunction
|
|
#hub2.button.right.callback(breakFunction)
|
|
|
|
gyroValue = 0
|
|
|
|
|
|
#Battery voltage printout in console for monitoring charge
|
|
if battery.voltage() < 8000: #set threshold for battery level
|
|
print(bcolors.BATTERY_LOW + "battery voltage is too low: " + str(battery.voltage()) + " \n ----------------------------- \n >>>> please charge robot <<<< \n ----------------------------- \n"+ bcolors.ENDC)
|
|
else:
|
|
print(bcolors.BATTERY + "battery voltage: " + str(battery.voltage()) + bcolors.ENDC)
|
|
|
|
|
|
db.movement_motors.set_stop_action("hold") #hold motors on wait for increased reliability
|
|
|
|
|
|
print("successfully loaded the Go-Robot code :)") |