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)