class myRobot:
# Initialize the Class
def __init__(self, motorL=None, motorR=None, motorM=None, gyroPort=None, colorL=None, colorR=None, colorM=None):
if motorL: self.motorL = LargeMotor(motorL)
if motorR: self.motorR = LargeMotor(motorR)
if motorM: self.motorM = Motor(motorM)
if gyroPort:
self.gyro = GyroSensor(gyroPort)
self.gyro.mode = 'GYRO-CAL'
time.sleep(0.2)
self.gyro.mode = 'GYRO-ANG'
if colorL: self.colorL = ColorSensor(colorL)
if colorR: self.colorR = ColorSensor(colorR)
if colorM: self.colorM = ColorSensor(colorM)
if motorL and motorR:
self.drive = MoveSteering(motorL, motorR)
self.move = MoveTank(motorL, motorR)
print("Successful initialization!")
# Function to go forward, by default it uses the gyro for correcting, but it can be disabled.
# Can use either rotations to measure distance, or time
def forward(self, speed, distance, useRots=True, correction=2, useGyro=True, way=None):
initialGyro = way if way else self.gyro.angle
if useRots:
initialMotor = abs((self.motorL.position/360 + self.motorR.position/360)/2) # Get the average of the 2 motor values
while abs((self.motorL.position/360 + self.motorR.position/360)/2) < initialMotor+distance:
self.drive.on(initialGyro - self.gyro.angle*correction, speed)
print("%f : %i" % (abs((self.motorL.position/360 + self.motorR.position/360)/2), initialMotor+distance))
print("gyro: %i" % self.gyro.angle)
elif not useRots:
initialTime = time.time()
while time.time()-initialTime < distance:
self.drive.on(initialGyro - self.gyro.angle*correction, speed)
print("%f : %i" % (time.time()-initialTime, distance))
print("gyro: %i" % self.gyro.angle)
def stop(self, brake=False):
self.drive.stop(brake=brake)
# Function that can turn either absolute or relative to current rotation
# if way is float, then destination will be gyro.angle+way, if int, then just way
def turn(self, way, speed, linear=False, leeway=1, debug=False, minSpeed=5):
if type(way)==type(1.1):
way = int(self.gyro.angle+way)
leewayList = range(way-leeway, way+leeway+1)
if linear:
while self.gyro.angle not in leewayList:
if debug: print(self.gyro.angle)
lin = abs(way/self.gyro.angle)
print(lin)
self.move.on(speed*lin+minSpeed, speed*-1*lin+minSpeed)
elif not linear:
while self.gyro.angle not in leewayList:
if debug: print(self.gyro.angle)
self.move.on(speed, speed*-1)
# Will fix gyro drift when called
def nodrift(self, t=0.2):
self.gyro.mode = 'GYRO-CAL'
time.sleep(t)
self.gyro.mode = 'GYRO-ANG'
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C from time import sleep # Demonstrate how to use MoveSteering class. # Have the robot rotate 360 degrees on one wheel then the other # Using the same four methods we are now familiar with: # on(), on_for_degrees(), on_for_rotations(), on_for_seconds() steering_drive = MoveSteering(OUTPUT_B, OUTPUT_C) steering_drive.on(-100, 35) sleep(2) steering_drive.on(100, 35) sleep(2) steering_drive.stop() # Store the value of 2 360 rotations in a variable degrees = 360 * 2 # -100 = turn all the way to the left; steering_drive.on_for_degrees(-100, 30, degrees) # 100 = turn all the way to the right steering_drive.on_for_degrees(100, 30, degrees) steering_drive.on_for_rotations(-100, 30, 2) steering_drive.on_for_rotations(100, 30, 2)
from ev3dev2.motor import MoveSteering
a = MoveSteering("outB", "outC")
a.stop(brake=False)
class CarControl:
def __init__(self):
self.tank_pair = MoveTank(OUTPUT_A, OUTPUT_B)
self.steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B)
self.line_sensor = ColorSensor(INPUT_2)
self.line_counter = ColorSensor(INPUT_3)
self.sound = Sound()
self.gyro = GyroSensor(INPUT_1)
self.gyro.reset()
self.dir_state = "S"
def drive(self, l_speed, r_speed):
self.tank_pair.on(left_speed=l_speed, right_speed=r_speed)
def reverse(self):
self.tank_pair.on_for_degrees(left_speed=-90,
right_speed=-90,
degrees=360,
brake=True)
def brake(self):
self.tank_pair.off(brake=True)
def turn_left(self):
self.steer_pair.on_for_degrees(steering=-100,
speed=90,
degrees=180,
brake=True)
def turn_right(self):
self.steer_pair.on_for_degrees(steering=100,
speed=90,
degrees=170,
brake=True)
def turn_arround(self):
self.steer_pair.on_for_degrees(steering=-100,
speed=90,
degrees=370,
brake=True)
def read_line_sensor(self):
return (self.line_sensor.reflected_light_intensity)
def read_line_counter(self):
return (self.line_counter.reflected_light_intensity)
def play_sound(self):
self.sound.beep()
def read_gyro(self):
return self.gyro.angle
def turn_left_new(self):
self.tank_pair.on(left_speed=-90, right_speed=90)
if self.dir_state == "S":
while True:
if self.read_gyro(
) <= -90: #Ikke sikker på at den altid kan læse ved præcist -90
self.tank_pair.off(brake=True)
self.dir_state = "L"
return 0
elif self.dir_state == "L":
while True:
if self.read_gyro() == 180 or self.read_gyro() == -180:
self.steer_pair.stop()
self.dir_state = "B"
return 0
elif self.dir_state == "B":
while True:
if self.read_gyro() == 90 or self.read_gyro() == -270:
self.steer_pair.stop()
self.dir_state = "R"
return 0
elif self.dir_state == "R":
while True:
if self.read_gyro(
) == 0 or self.read_gyro == -360 or self.read_gyro == 360:
self.gyro.reset()
self.steer_pair.stop()
self.dir_state = "S"
return 0
def turn_right_new(self):
self.steer_pair.on(90, 90)
print("JEG ER HER")
if self.dir_state == "S":
while True:
if self.read_gyro == 90:
self.steer_pair.stop()
self.dir_state = "R"
return 0
elif self.dir_state == "L":
while True:
if self.read_gyro() == 360 or self.read_gyro() == 0:
self.gyro.reset()
self.steer_pair.stop()
self.dir_state = "S"
return 0
elif self.dir_state == "B":
while True:
if self.read_gyro() == 270 or self.read_gyro == -90:
self.steer_pair.stop()
self.dir_state == "R"
return 0
elif self.dir_state == "R":
while True:
if self.read_gyro() == 180 or self.read_gyro() == -180:
self.steer_pair.stop()
self.dir_state == "B"
return 0
class myRobot():
def __init__(self,
motorPort1,
motorPort2,
modulePort,
colorPort1,
colorPort2,
gyro1=None,
gyro2=None,
motorDiff=1,
colMode="COL-COLOR",
moduleSensor=None,
printErrors=True,
enableConsole=False,
modulePort2=None):
if motorPort1 and motorPort2:
self.ms = MoveSteering(
motorPort1,
motorPort2) # If defined in parameters, define MoveSteering
if motorPort1 and motorPort2:
self.mt = MoveTank(
motorPort1,
motorPort2) # If defined in parameters, define MoveTank
if motorPort1:
self.m1 = LargeMotor(
motorPort1) # If defined in parameters, define Left Motor
if motorPort2:
self.m2 = LargeMotor(
motorPort2) # If defined in parameters, define Right Motor
if modulePort:
self.mmt = Motor(modulePort)
# If defined in parameters, define module motor
if modulePort2:
self.mmt2 = Motor(modulePort2)
# If defined in parameters, define module motor
if enableConsole: self.resetMotors()
if colorPort1 != None: # If defined in parameters, define Left Color sensor
self.csLeft = ColorSensor(colorPort1)
self.csLeft.mode = colMode # Set color mode to the one in the parameters
if colorPort2 != None: # If defined in parameters, define Right Color sensor
self.csRight = ColorSensor(colorPort2)
self.csRight.mode = colMode # Set color mode to the one in the parameters
if moduleSensor != None: # If defined in parameters, define module sensor
self.moduleSensor = ColorSensor(moduleSensor)
self.moduleSensor.mode = "COL-COLOR"
try:
self.gs = GyroSensor(gyro1)
self.gs.mode = "GYRO-RATE"
self.gs.mode = "GYRO-ANG"
except:
print("Gyro 1 can't be defined!"
) # Define gyro if possible, otherwise show error
try:
self.gs2 = GyroSensor(gyro2)
self.gs2.mode = "GYRO-RATE"
self.gs2.mode = "TILT-ANG"
except:
print("Gyro 2 can't be defined!"
) # Define gyro if possible, otherwise show error
self.using2ndGyro = False # Set default gyro to the 1st one
self.motorDiff = motorDiff # Set motor difference to the one defined
self.leds = Leds() # Setup brick leds
self.bt = Button() # Setup brick buttons
if enableConsole:
self.console = Console(
font="Lat15-Terminus32x16") # Enable console access if needed
try:
with open("/home/robot/sappers2/pid.txt", "w") as pid:
pid.write(str(os.getpid()))
except:
pass # Write PID into file for other applications.
if printErrors: print("Successfully Initialized. ")
self.timerStart = time.time()
def prepareForRun(self): # Reset gyro and motors
self.ms.reset()
self.gyroReset()
def changeGyro(self, newGyroPort): # Change gyro to 2nd one
try:
self.gs = GyroSensor(newGyroPort)
self.using2ndGyro = True
except:
print("Can't change Gyro!")
def attachNew(self,
isMotor=True,
port="outA",
largeMotor=False): # Attach new motor
if isMotor:
self.m3 = Motor(port)
else:
self.m3 = LargeMotor(port)
def followLine(self,
timeOfGoing,
speed,
multiplier=5,
debug=False): # Simple line follower
currentTime = time.time()
while time.time() - currentTime < timeOfGoing:
colorLeft = int(self.csLeft.value() / 1)
colorRight = int(self.csRight.value() / 1)
print(colorLeft, colorRight)
if colorLeft != 6 and colorRight != 6:
tempSteering = 0
elif colorLeft != 6:
tempSteering = -multiplier
elif colorRight != 6:
tempSteering = multiplier
else:
tempSteering = 0
self.ms.on(tempSteering, speed * self.motorDiff)
def goByWay(self,
waySteering,
timeToGo,
speed=80,
debug=False): # MoveSteering but for time
currentTime = time.time()
while time.time() - currentTime < timeToGo:
self.ms.on(waySteering, speed * self.motorDiff)
def goUntilLine(self,
speed,
lineColor,
multiplier=1,
debug=False
): # Goes forward with gyro, but stops when detecting line
pastGyro = self.gs.value()
while (self.csLeft.value() not in lineColor) and (self.csRight.value()
not in lineColor):
self.ms.on((self.gs.value() - pastGyro) * multiplier,
speed * self.motorDiff)
if debug:
print(self.gs.value(), self.csLeft.value(),
self.csRight.value())
print(self.gs.value(), self.csLeft.value(), self.csRight.value())
def goWithGyro(self, timeToGo, speed=70, multiplier=2, debug=False):
# Forward with gyro for time
pastGyro = self.gs.value()
currentTime = time.time()
while time.time() - currentTime < timeToGo:
self.ms.on(self.max100((self.gs.value() - pastGyro) * multiplier),
speed * self.motorDiff)
if debug: print(self.gs.value())
def moveModule(self,
length,
speed=30,
stopMoving=True,
useLarge=False,
waitAfter=False): # Module motor access
if useLarge:
try:
self.m3.on(speed)
time.sleep(length)
if stopMoving: self.m3.reset()
except:
print("not working bruh you should fix it like rn")
else:
while True:
try:
self.mmt.on(speed * -1)
time.sleep(length)
if stopMoving: self.mmt.reset()
if waitAfter: time.sleep(waitAfter)
break
except Exception as e:
print(e)
break
def moveModuleByRot(self,
rotations,
speed,
block=True): # Deprecated module moving
self.mmt.on_for_rotations(-speed, rotations, block=block)
def moveModuleByRot2(self,
rotations,
speed,
block=True): # Deprecated module moving
self.mmt.on_for_rotations(-speed, rotations, block=block)
def moveBothModules(self,
rotations,
speed,
block=True,
speed2=False): # Deprecated module moving
self.mmt.on_for_rotations(-speed, rotations, block=False)
self.mmt2.on_for_rotations((speed2 if speed2 else -speed),
rotations,
block=block)
def resetMotors(self): # Motor reset
self.ms.reset()
self.mmt.on(100)
time.sleep(1)
self.mmt.reset()
def gyroReset(self): # Gyro reset
self.gs.mode = "GYRO-RATE"
self.gs.mode = "GYRO-ANG"
try:
self.gs2.mode = "GYRO-RATE"
self.gs2.mode = "TILT-ANG"
except:
print("Gyro2 not found!")
def resetGyro(self, gyroObject, desiredMode="TILT-ANG"):
gyroObject.mode = "GYRO-RATE"
gyroObject.mode = desiredMode
def fullStop(self, brakeOrNo=True): # Stops the robot, breaks if defined.
self.ms.stop(brake=brakeOrNo)
self.mmt.reset()
try:
self.mmt2.reset()
except:
pass
def turnWithGyro(
self,
degreesToTurn=90,
speedToTurnAt=20): # Turns with gyro, and saves past value
gyroPast = self.gs.value()
self.ms.on(90, speedToTurnAt)
while gyroPast - self.gs.value() <= degreesToTurn:
print(gyroPast - self.gs.value())
self.ms.stop(brake=True)
print(self.gs.value())
def justGo(self, speed,
timeOfGoing): # Goes forward without gyro, for time
self.ms.on(0, speed * self.motorDiff)
time.sleep(timeOfGoing)
def justTurn(self, speed,
timeOfGoing): # Turns withot gyro, useful for quick turns.
self.ms.on(90, speed * self.motorDiff)
time.sleep(timeOfGoing)
def absoluteTurn(self,
speed,
absoluteLoc,
rotWay=-1,
giveOrTake=1,
debug=False,
ver2=False): # Turns using absolute value from gyro
self.ms.on(90 * rotWay, speed * self.motorDiff)
# while self.gs.value() > absoluteLoc+giveOrTake or self.gs.value() < absoluteLoc+giveOrTake :
# if debug: print(self.gs.value())
# pass
while self.gs.value() > absoluteLoc + giveOrTake or self.gs.value(
) < absoluteLoc - giveOrTake:
if debug: print(self.gs.value())
pass
def isPositive(self, number):
if number > 0:
return 1
elif number < 0:
return -1
else:
return 0
def absolute(self, speed, location, giveOrTake=2, debug=False):
"""
Absolute turning
"""
while self.gs.value() > location + giveOrTake or self.gs.value(
) < location - giveOrTake:
self.ms.on(90 * self.isPositive(self.gs.value() - location),
speed * self.motorDiff)
if debug: print(self.gs.value())
pass
def turnForMore(
self,
speed,
minDeg,
rotWay=-1,
debug=False,
ver2=False
): # Turns with gyro, but stops quicker than absolute turn (useful for big turns)
if debug: print(self.gs.value())
self.ms.on(90 * rotWay, speed * self.motorDiff)
if ver2:
currentDeg = int(self.gs.value())
while minDeg - 1 <= currentDeg <= minDeg + 1:
if debug: print(self.gs.value())
else:
if rotWay < 0:
while int(self.gs.value()) < int(minDeg):
if debug: print(self.gs.value())
pass
else:
while int(self.gs.value()) > int(minDeg):
if debug: print(self.gs.value())
pass
def goWithShift(self,
timeToGo,
speed=70,
multiplier=2,
shift=1): # Goes with gyro using shift
pastGyro = self.gs.value()
currentTime = time.time()
while time.time() - currentTime < timeToGo:
if self.gs.value() == 0:
gsValue = shift
else:
gsValue = self.gs.value()
self.ms.on((gsValue - pastGyro) * multiplier,
speed * self.motorDiff)
def goWithGyroRot(self,
rotations,
speed,
multiplier,
debug=False,
startValue=None,
stopAtEnd=False
): # Goes using gyro using one motor's rotation value
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = self.m1.position
steerDiff = -1 if speed < 0 else 1
while abs((currentRot - self.m1.position) / 360) < rotations:
if debug:
print(
int(abs(
(currentRot - self.m1.position) / 360) * 100) / 100,
self.gs.value())
self.ms.on((self.gs.value() - pastGyro) * multiplier * steerDiff,
speed * self.motorDiff)
if stopAtEnd: self.ms.stop(brake=True)
def waitForColor(self,
color=[1, 2, 3, 4, 5, 6],
amountOfTime=0.5,
debug=False,
waitingTime=0.5): # Waits until color detected
colorArray = []
while True:
if self.moduleSensor.value() in color:
colorArray.append(self.moduleSensor.value())
time.sleep(amountOfTime / 10)
if debug: print(colorArray, end="\r")
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
elif self.bt.down == True:
print("awaiting input")
return False
else:
colorArray = []
if debug: print("waiting", self.moduleSensor.value(), end="\r")
self.leds.set_color("LEFT", "RED")
self.leds.set_color("RIGHT", "RED")
if len(colorArray) == 9:
break
self.leds.set_color("LEFT", "ORANGE")
self.leds.set_color("RIGHT", "AMBER")
time.sleep(waitingTime)
if debug: print(max(set(colorArray), key=colorArray.count), end="\n")
return max(set(colorArray), key=colorArray.count)
def turnWithGyro2(self,
degreesToTurn=90,
speedToTurnAt=20,
debug=False): # Deprecated turn with gyro
gyroPast = self.gs.value()
while gyroPast - self.gs.value() <= degreesToTurn:
self.ms.on(90, ((degreesToTurn - speedToTurnAt) + abs(
(degreesToTurn - speedToTurnAt))) / 2)
if debug: print(gyroPast - self.gs.value())
def startRun(self,
toExec,
resetGyro=True,
colorArray=[3]): # Starts run using magic wand
self.waitForColor([3], 1, True)
self.gyroReset()
exec(toExec)
def gyroSlowLinearly(
self,
rotations,
startSpeed,
multiplier,
minSpeed=5,
debug=False,
startValue=None,
stopAtEnd=False
): # Goes with gyro and slows down as it nears to destination
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = self.m1.position
steerDiff = -1 if startSpeed < 0 else 1
while abs((currentRot - self.m1.position) / 360) < rotations:
currSpeed = 1 - (
(int(abs((currentRot - self.m1.position) / 360) * 100) / 100) /
rotations)
if debug:
print(
currSpeed,
round(
(startSpeed * currSpeed + minSpeed) * self.motorDiff))
self.ms.on(
(self.gs.value() - pastGyro) * multiplier * steerDiff,
round((startSpeed * currSpeed + minSpeed) * self.motorDiff))
if stopAtEnd: self.ms.stop(brake=True)
def goWithGyroRot2(
self,
rotations,
speed,
multiplier,
debug=False,
startValue=None,
stopAtEnd=False,
timeout=100000): # Goes forward using both motor's rotation value
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = (self.m1.position + self.m1.position) / 2
steerDiff = -1 if speed < 0 else 1
startTime = time.time()
while abs((currentRot - (self.m1.position + self.m1.position) / 2) /
360) < rotations or time.time() - startTime >= timeout:
if debug:
print(
int(abs(
(currentRot - self.m1.position) / 360) * 100) / 100,
self.gs.value())
self.ms.on((self.gs.value() - pastGyro) * multiplier * steerDiff,
speed * self.motorDiff)
if stopAtEnd: self.ms.stop(brake=True)
def returnButtons(self, pack2=False): # Returns pressed buttons for menu
if pack2:
packable = list(sys.stdin.readline().replace("\x1b[", "").rstrip())
return [''.join(x) for x in zip(packable[0::2], packable[1::2])]
else:
return sys.stdin.readline().replace("\x1b[", "").rstrip()
def goWithGyroForLevel(
self,
startRots,
speed,
multiplier,
debug=False,
startValue=None,
levelNeeded=5,
overrideDegrees=0
): # Goes forward with gyro until water-level is detected
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = (self.m1.position + self.m1.position) / 2
steerDiff = -1 if speed < 0 else 1
self.goWithGyroRot2(startRots, speed, multiplier, False, startValue,
False)
print(abs(self.gs2.value()))
while abs(self.gs2.value()) > levelNeeded:
if debug:
print(
(abs(self.gs2.value()), self.gs.value(),
((self.gs.value() - pastGyro) * multiplier * steerDiff) +
overrideDegrees))
self.ms.on(
self.max100((
(self.gs.value() - pastGyro) * multiplier * steerDiff) +
overrideDegrees), speed * self.motorDiff)
def playSound(self, mscCommand): # Plays a sound
try:
os.system(mscCommand)
except:
pass
def max100(self, value): # Sets value to a max of 100
if value > 100:
return 100
elif value < -100:
return -100
else:
return value
def sleep(self, seconds): # sleep.
time.sleep(seconds)
def stopAtLine(
self,
speed,
color,
correctionSpeed=5): # Goes forward and stops when detecting lines
pastGyro = self.gs.value()
while True:
if self.csLeft.value() == color and self.csRight.value() == color:
break
elif self.csRight.value() != color and self.csLeft.value(
) == color:
self.mt.on(0, correctionSpeed * self.motorDiff)
elif self.csLeft.value() != color and self.csRight.value(
) == color:
self.mt.on(correctionSpeed * self.motorDiff, 0)
elif self.csLeft.value() != color and self.csRight.value(
) != color:
self.ms.on((self.gs.value() - pastGyro),
speed * self.motorDiff)
print({
"right": self.csRight.value(),
"left": self.csLeft.value(),
"gyro": self.gs.value()
})
print(
"final", {
"right": self.csRight.value(),
"left": self.csLeft.value(),
"gyro": self.gs.value()
})
def stayInPlace(self, multiplier
): # Using gyro and motor values, keeps the robot in place
motor1 = self.m1.position
motor2 = self.m2.position
while True:
corrig1 = -(self.m1.position - motor1) * multiplier
corrig2 = -(self.m2.position - motor2) * multiplier
if round(corrig1, 2) == 0 and round(corrig2, 2) == 0:
self.mt.stop(brake=False)
else:
self.mt.on(corrig1, corrig2)
print(corrig1, corrig2)
# print(corrig1, corrig2)
def moveModuleRepeating(self, raiseTime, downTime, speed1, speed2,
iterations): # Module motor access
for i in range(iterations):
self.moveModule(raiseTime, speed1, False)
self.moveModule(downTime, speed2, True)
def section(self, sectionName, resetTimer=False):
"""
Provides timing functionality, and makes it easier to see sections of runs in logs.
`resetTimer` parameter optional, it will reset the main runTimer.
"""
if resetTimer:
self.timerStart = time.time()
print(sectionName)
else:
print("%s, current run time: %i" %
(sectionName, self.timerStart - time.time()))
def goWithGyroRot2Maxed(
self,
rotations,
speed,
multiplier,
debug=False,
startValue=None,
stopAtEnd=False,
timeout=100000): # Goes forward using both motor's rotation value
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = (self.m1.position + self.m1.position) / 2
steerDiff = -1 if speed < 0 else 1
startTime = time.time()
while abs((currentRot - (self.m1.position + self.m1.position) / 2) /
360) < rotations or time.time() - startTime >= timeout:
if debug:
print(
int(abs(
(currentRot - self.m1.position) / 360) * 100) / 100,
self.gs.value())
self.ms.on(
self.max100(
(self.gs.value() - pastGyro) * multiplier * steerDiff),
speed * self.motorDiff)
if stopAtEnd: self.ms.stop(brake=True)
