def reset(): fb_motor = LargeMotor(OUTPUT_C) lr_motor = LargeMotor(OUTPUT_B) ud_motor = Motor(OUTPUT_A) fb_motor.stop() lr_motor.stop() ud_motor.stop()
def main(): MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("outD") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") # change color sensor to color and define colors C4.mode = 'COL-COLOR' colors = ('', 'Black', 'Blue', 'Brown', 'Green', 'Yellow', 'Red', 'White') # turn on motors forever MB.run_forever(speed_sp=75) MC.run_forever(speed_sp=75) # while color sensor doesn't sense black. Wait until sensing black. while C4.value() != 1: print(colors[C4.value()]) sleep(0.005) # after loop ends, brake motor B and C MB.stop(stop_action="hold") MC.stop(stop_action="hold")
class Pen: def __init__(self, port_motor_move, port_motor_pen): # Motor to move pen sideways self.motor_move = LargeMotor(port_motor_move) self.motor_move.reset() self.motor_move.speed_sp = 400 self.motor_move.stop_action = 'brake' self.motor_move_positions = (0, -60, -210, -350, -500, -640) # Motor te move pen up or down self.motor_pen = LargeMotor(port_motor_pen) self.motor_pen.stop_action = 'hold' # TouchSensor to indicate pen is up self.ts_pen = TouchSensor('pistorms:BBS2') # Move pen up self.motor_pen.run_forever(speed_sp=-200) self.ts_pen.wait_for_pressed() self.motor_pen.stop() @try_except def pen_up(self): ''' Move pen up.''' self.motor_pen.run_to_abs_pos(speed_sp=400, position_sp=-20) wait_while_motors_running(self.motor_pen) @try_except def pen_down(self): ''' Move pen down.''' self.motor_pen.run_to_abs_pos(speed_sp=400, position_sp=20) wait_while_motors_running(self.motor_pen) @try_except def move_to_abs_pos(self, pos): ''' Move pen sidways to absolute position [0,..,5].''' self.motor_move.run_to_abs_pos( position_sp=self.motor_move_positions[pos])
def main(): Sound.speak("").wait() MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") GY.mode = 'GYRO-ANG' GY.mode = 'GYRO-RATE' GY.mode = 'GYRO-ANG' tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) loop_gyro = 0 gyro_adjust = 1 # change this to whatever speed what you want left_wheel_speed = -300 right_wheel_speed = -300 # change the loop_gyro verses the defined value argument to however far you want to go # if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left # change the value to how far you want the robot to go while MB.position > -160: if GY.value() == 0: left_wheel_speed = -300 right_wheel_speed = -300 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) gyro_adjust = 12 else: if GY.value() > 0: correct_rate = abs( GY.value() ) # This captures the gyro value at the beginning of the statement left_wheel_speed = left_wheel_speed + gyro_adjust right_wheel_speed = right_wheel_speed - gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 if abs( GY.value() ) > correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time gyro_adjust = gyro_adjust + 1 else: correct_rate = abs( GY.value()) # Same idea as the other if statement right_wheel_speed = right_wheel_speed + gyro_adjust left_wheel_speed = left_wheel_speed - gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 if abs(GY.value()) > correct_rate: gyro_adjust = gyro_adjust + 1 # stop all motors MB.stop(stop_action="hold") MC.stop(stop_action="hold")
def main(): Sound.speak("").wait() MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") GY.mode = 'GYRO-ANG' GY.mode = 'GYRO-RATE' GY.mode = 'GYRO-ANG' tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) loop_gyro = 0 starting_value = GY.value() # change 20 to whatever speed what you want left_wheel_speed = 100 right_wheel_speed = 100 # change 999999999999 to however you want to go # if Gyro value is the same as the starting value, go straigt. if more turn right. if less turn left while loop_gyro < 999: if GY.value() == starting_value: left_wheel_speed = -300 right_wheel_speed = -300 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) else: if GY.value() > starting_value: left_wheel_speed = left_wheel_speed - GY.value() / 2 right_wheel_speed = right_wheel_speed + GY.value() * -1 / 2 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 else: right_wheel_speed = right_wheel_speed + GY.value() / 2 left_wheel_speed = left_wheel_speed - GY.value() / 2 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 loop_gyro + 1 # stop all motors MB.stop(stop_action="hold") MC.stop(stop_action="hold")
class Robot: # Hardware class def __init__(self): # Motors self.m1 = LargeMotor(OUTPUT_B) self.m2 = LargeMotor(OUTPUT_C) # Sensors self.g = GyroSensor(INPUT_3) def drive(self, go, turn): # drive with the specified forward and steer values self.m1.on(SpeedPercent(clampa(go + turn, 100))) self.m2.on(SpeedPercent(clampa(go - turn, 100))) def stop(self): # self documenting self.m1.stop() self.m2.stop()
def main(): Sound.speak("").wait() MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") GY.mode = 'GYRO-ANG' GY.mode = 'GYRO-RATE' GY.mode = 'GYRO-ANG' tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) loop_gyro = 0 starting_value = GY.value() # change 20 to whatever speed what you want left_wheel_speed = 20 right_wheel_speed = 20 # change 999999999999 to however you want to go # if Gyro value is the same as the starting value, go straigt. if more turn right. if less turn left while loop_gyro < 999999999999: if GY.value() == starting_value: left_wheel_speed = 20 right_wheel_speed = 20 tank_drive.on_for_degrees(SpeedPercent(left_wheel_speed), SpeedPercent(right_wheel_speed), 180) else: if GY.value() > starting_value: left_wheel_speed -= 5 tank_drive.on_for_degrees(SpeedPercent(left_wheel_speed), SpeedPercent(right_wheel_speed), 180) left_wheel_speed = 20 right_wheel_speed = 20 else: right_wheel_speed -= 5 tank_drive.on_for_degrees(SpeedPercent(left_wheel_speed), SpeedPercent(right_wheel_speed), 180) left_wheel_speed = 20 right_wheel_speed = 20 # stop all motors MB.stop(stop_action="hold") MC.stop(stop_action="hold")
def make_drink(order, length): _id = order["_id"] name = order["name"] tea = order["options"]["tea"] sugar = order["options"]["sugar"] ice = order["options"]["ice"] console.text_at( "Making Order for " + name + "\n" + tea + " " + str(sugar) + "%" + " " + str(ice) + "%" + "\nQueue Size: " + str(length), column=mid_col, row=mid_row, alignment=alignment, reset_console=True, ) sound = Sound() sound.speak("Dispensing boba") # dispense boba m = LargeMotor(OUTPUT_B) m.on_for_rotations(SpeedPercent(75), 10) sound.speak("Dispensing " + tea) # dispense liquid m = LargeMotor(OUTPUT_A) m.run_forever(speed_sp=1000) sleep(10) m.stop() s = name + ", your boba drink is finished. Please come pick it up" console.text_at(s, column=mid_col, row=mid_row, alignment=alignment, reset_console=True) sound.speak(s) requests.patch(URL + "/queue/" + _id, json={})
def main(): MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("outD") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) # change color sensor to color and define colors C4.mode = 'COL-COLOR' colors = ('', 'Black', 'Blue', 'Brown', 'Green', 'Yellow', 'Red', 'White') # turn on motors forever MB.run_forever(speed_sp=75) MC.run_forever(speed_sp=75) # while color sensor doesn't sense black. Wait until sensing black. vice versa for white while C4.value() == 0 or 2 or 3 or 4 or 5 or 6: if C4.value() != 1: #this makes the robot move left when sensing black tank_drive.on_for_degrees(SpeedPercent(-5), SpeedPercent(-20), 20) sleep(0.5) elif C4.value() != 7: tank_drive.on_for_degrees(SpeedPercent(-20), SpeedPercent(-5), 20) sleep(0.5) #this makes the robot move right when sensing white else: tank_drive.on_for_degrees(SpeedPercent(-20), SpeedPercent(-20), 20) sleep(0.5) #this makes the robot move forward when anything else happens # after loop ends, brake motor B and C MB.stop(stop_action="hold") MC.stop(stop_action="hold")
cs.mode = "RGB-RAW" #cs.mode = "COL-AMBIENT" t = time() while True: dist = us.distance_centimeters if time() - t > 1: print("dist:", dist) print("colr:", tuple(map(cs.value, [0, 1, 2]))) # print("colr:", cs.value()) t = time() if ts.is_pressed: speed = min(max(1, 100 - dist), 100) my_motor.on(speed) else: my_motor.stop() ''' rots = 10 my_motor.on_for_rotations(100, rots, block=False) print("Rotating {} times...".format(rots)) print("Motor holding:", my_motor.is_holding) my_motor.wait(lambda state: "holding" in state) print("Done!") print("Stopped at position", my_motor.position) # MoveSteering below - convenience for controlling two tires at once sleep(3)
goStraight(0.7) turn90DegreeRight(0.3) goStraight(0.5) turn90DegreeRight(0.3) goUntil = False # vi kör tills vi hittar tillbaka till den svarta linjen. while goUntil == False: goUntil = searchForBlackLine(50,50,1, goUntil) turn90DegreeLeft(0.3) # när vi hittat svarta linjen igen gör vi en 90-graders-sväng igen. def turn90DegreeLeft(goforsec): motors.on_for_seconds(SpeedPercent(-50), SpeedPercent(50), goforsec) def turn90DegreeRight(goforsec): motors.on_for_seconds(SpeedPercent(50), SpeedPercent(-50), goforsec) def goStraight(goforsec): motors.on_for_seconds(SpeedPercent(50), SpeedPercent(50), goforsec) main() # vi stänger av motorerna i slutet på programmet. motorA.stop(stop_action='brake') motorB.stop(stop_action='brake') motorA.run_forever(speed_sp=0) motorB.run_forever(speed_sp=0) # Johan Lind, 2019 Örebro universitet # GNU GENERAL PUBLIC LICENSE
motorA.run_forever(speed_sp = 200 * direcao) else: motorA.stop(stop_action = 'hold') if sensor2.color == cor: motorB.run_forever(speed_sp = 200 * direcao) else: motorB.stop(stop_action = 'hold') if sensor1.color != cor and sensor2.color != cor: motorA.stop(stop_action = 'hold') motorB.stop(stop_action = 'hold') break '''while sensor3.distance_centimeters > 20: print('dist > 20') print() motorA.run_forever(speed_sp = 400) motorB.run_forever(speed_sp = 400) while sensor4.distance_centimeters > 8: print('dist > 8') print() motorA.run_forever(speed_sp = -400) motorB.run_forever(speed_sp = 400) motorA.stop() motorB.stop() while sensor4.distance_centimeters > 5: motorA.run_forever(speed_sp = 400) motorB.run_forever(speed_sp = 400)'''
i['0'] = 2 else: cs2_black['0'] = False t = Thread(target=notBlack, args=(cs, cs2)) t.start() sleep(1) while (True): while (cs_black['0'] or cs2_black['0']): sleep(0.4) lm.run_forever(speed_sp=150, stop_action="hold") lm2.run_forever(speed_sp=150, stop_action="hold") lm.stop(stop_action="hold") lm2.stop(stop_action="hold") if (i['0'] == 1): while (cs_black['0'] == False and cs2_black['0'] == False): lm.run_to_rel_pos(position_sp=0, stop_action="hold") lm2.run_to_rel_pos(position_sp=7, speed_sp=200, stop_action="hold") lm.run_to_rel_pos(position_sp=0, stop_action="hold") lm2.run_to_rel_pos(position_sp=30, speed_sp=500, stop_action="hold") elif (i['0'] == 2): while (cs_black['0'] == False and cs2_black['0'] == False): lm.run_to_rel_pos(position_sp=7, speed_sp=200, stop_action="hold") lm2.run_to_rel_pos(position_sp=0, stop_action="hold") lm.run_to_rel_pos(position_sp=30, speed_sp=500, stop_action="hold") lm2.run_to_rel_pos(position_sp=0, stop_action="hold")
def Crane(): Sound.speak("").wait() MA = MediumMotor("outA") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("outD") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") GY.mode='GYRO-ANG' GY.mode='GYRO-RATE' GY.mode='GYRO-ANG' tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) loop_gyro = 0 gyro_adjust = 12 # change this to whatever speed what you want left_wheel_speed = -300 right_wheel_speed = -300 tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01) #wheel alignment # change the loop_gyro verses the defined value argument to however far you want to go # if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left # change the value to how far you want the robot to go. V while MB.position > -900: if GY.value() == 0: left_wheel_speed = -300 right_wheel_speed = -300 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) gyro_adjust = 9 else: if GY.value() < 0: correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement left_wheel_speed = left_wheel_speed + gyro_adjust right_wheel_speed = right_wheel_speed - gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 if abs (GY.value()) > correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time gyro_adjust = gyro_adjust + 1 else: correct_rate = abs (GY.value()) # Same idea as the other if statement right_wheel_speed = right_wheel_speed + gyro_adjust left_wheel_speed = left_wheel_speed - gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 if abs (GY.value()) > correct_rate: gyro_adjust = gyro_adjust + 1 # stop all motors MB.stop(stop_action="hold") MC.stop(stop_action="hold") # wait for the block to drop. V sleep(1.5) # pulling away from the crane. V tank_drive.on_for_rotations(SpeedPercent(20), SpeedPercent(10), 0.50) # Unlocking attachment. V MD.on_for_degrees(SpeedPercent(50), 360) # pulling away from unlocked attachment. V tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 1) # gyro 90 degree spin turn while GY.value() < 91: MB.run_forever(speed_sp=300) MC.run_forever(speed_sp=-300) # drive into home tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 4) program_running = 0 Launchrun()
def Krab(): MA = MediumMotor("outA") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("outD") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") #Setting the Gyro. V GY.mode='GYRO-ANG' GY.mode='GYRO-RATE' GY.mode='GYRO-ANG' tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) loop_gyro = 0 gyro_adjust = 1 starting_value = GY.value() gyro_correct_loops = 0 straight_correct_loops = 0 gyro_correct_straight = 0 # change this to whatever speed what you want. V left_wheel_speed = 100 right_wheel_speed = 100 # change the loop_gyro verses the defined value argument to however far you want to go # if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left # Wheel alignment. VVVV tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01) #Pulling out of Launch area. V tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 1.625) #Gyro Turn toowards the red circle. V while GY.value() < 80: left_wheel_speed = 100 right_wheel_speed = -100 #MB is left wheel & MC is right wheel MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) MB.stop(stop_action="hold") MC.stop(stop_action="hold") #Driving forward towards the red circle. V while MB.position > -1700: #was -2550, Joshua is changing it to -2300 if GY.value() == 90: left_wheel_speed = -500 right_wheel_speed = -500 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) gyro_adjust = 8 gyro_correct_loops = 0 gyro_correct_straight = 0 straight_correct_loops = 0 else: if GY.value() < 90: correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -500 right_wheel_speed = -500 if abs (GY.value()) >= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time gyro_adjust = gyro_adjust + 1 gyro_correct_loops = gyro_correct_loops + 1 if GY.value() == 0 and gyro_correct_straight == 0: while straight_correct_loops < gyro_correct_loops + 1: right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 gyro_correct_loops = 0 straight_correct_loops = 0 else: correct_rate = abs (GY.value()) # Same idea as the other if statement left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -500 right_wheel_speed = -500 gyro_correct_loops = gyro_correct_loops + 1 if abs (GY.value()) >= correct_rate: gyro_adjust = gyro_adjust + 1 if GY.value() == 0 and gyro_correct_straight == 0: #this code corrects the gyro back to the right line while straight_correct_loops < gyro_correct_loops + 1: #runs this loop until it makes the gyro the opposite of what it was when it was wrong in the first place left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 #makes sure that when the gyro is corrected to both straight and the line it was on that gyro is not messed up again gyro_correct_loops = 0 straight_correct_loops = 0 # stop all motors MB.stop(stop_action="hold") MC.stop(stop_action="hold") #unlocking arm to get elevator MD.on_for_degrees(SpeedPercent(-50), 176.26) #pushing down the beams from safety factor tank_drive.on_for_rotations(SpeedPercent(-50), SpeedPercent(-50), 2.75) #going back to home tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 6) tank_drive.on_for_rotations(SpeedPercent(40), SpeedPercent(40), 3) Launchrun() def Launchrun(): Sound.speak("ready to go") btn = Button() #adjust the while statement for however long you want it to go. while True: # V if up button is pressed, wait 1 second. if button is still pressed run program 1 if else run program 2 (repeat for each button) if btn.up: sleep(1) if btn.up: Krab() else: Krab() if btn.down: sleep(1) if btn.down: Spider() else: Spider() if btn.left: sleep(1) if btn.left: Crane() else: Crane() if btn.right: sleep(1) if btn.right: Bulldozer() else: Bulldozer() if btn.enter: sleep(1) if btn.enter: Redcircle() else: Traffic() #running launchrun Launchrun()
class TrueTurn: def __init__(self, motor1Port, motor2Port, gyroPort=None, wheelDiameter=None): #init if GyroSensor != None: self.GS = GyroSensor(gyroPort) else: self.GS = GyroSensor() self.M1 = LargeMotor(motor1Port) self.M2 = LargeMotor(motor2Port) self.motor_stop = True self.wheelDiameter = wheelDiameter self.time = 0 self.MDistanceRunning = True self.distance = 0 self.pauseDistance = [] self.GS.mode = 'GYRO-ANG' def turn(self, degrees, speed = 150, tolerance = 0.05): self.stopMotors() self.tolerance = tolerance self.speed = speed multiplier = -1 if degrees > 0: multiplier = 1 self.resetValue() #~ self.GS.mode = 'GYRO-ANG' angle = self.GS.value() running = False self.breaker = False rightTurn = False # not actually right leftTurn = False # not actually left slowRightTurn = False # not actually right slowLeftTurn = False # not actually left if tolerance > 0: field = range(math.ceil(degrees - self.tolerance * degrees), math.ceil(degrees + self.tolerance * degrees), multiplier) advancedField = range(math.ceil(degrees - 0.1 * degrees), math.ceil(degrees + 0.1 * degrees), multiplier) print (advancedField) else: field = [self.tolerance] advancedField = range(math.ceil(degrees - 0.1 * degrees), math.ceil(degrees + 0.1 * degrees), multiplier) print (advancedField) while self.GS.value() - angle not in field: print (advancedField) print (self.GS.value() - angle) print(abs(self.GS.value() - angle)) if self.GS.value() - angle in advancedField: #~ print("minor") print(self.GS.value()) if abs(self.GS.value() - angle) < abs(field[0]): #we have to make them absolute because we want to turn on both sides if not slowRightTurn: #~ print("slow right") self.M1.run_forever(speed_sp=self.speed * multiplier / 2.5) self.M2.run_forever(speed_sp=self.speed * multiplier * -1 /2.5) slowRightTurn = True slowLeftTurn = False sleep(0.001) if abs(self.GS.value() - angle) > abs(field[len(field) - 1]): #we have to make them absolute because we want to turn on both sides if not leftTurn: #~ print("slow right") self.M1.run_forever(speed_sp=self.speed * multiplier * -1 / 2) self.M2.run_forever(speed_sp=self.speed * multiplier / 2) slowRightTurn = False slowLeftTurn = True sleep(0.001) else: if abs(self.GS.value() - angle) < abs(field[0]): #we have to make them absolute because we want to turn on both sides if not rightTurn: #~ print("normal") print(self.GS.value()) self.M1.run_forever(speed_sp=self.speed * multiplier) self.M2.run_forever(speed_sp=self.speed * multiplier * -1) rightTurn = True leftTurn = False else: sleep(0.0012) if abs(self.GS.value() - angle) > abs(field[len(field) - 1]): #we have to make them absolute because we want to turn on both sides if not leftTurn: print(self.GS.value()) #~ print("normal left") self.M1.run_forever(speed_sp=self.speed * multiplier * -1) self.M2.run_forever(speed_sp=self.speed * multiplier) rightTurn = False leftTurn = True else: sleep(0.0012) self.M1.stop() self.M2.stop() sleep(0.1) print("ok it works") leftTurn = False rightTurn = False slowLeftTurn = False slowRightTurn = False if self.GS.value() - angle not in field: while self.GS.value() - angle not in field: if abs(self.GS.value() - angle) < abs(field[0]): #we have to make them absolute because we won to turn on both sides if not rightTurn: print(self.GS.value() - angle) #~ print ("micro") self.M1.run_forever(speed_sp=self.speed * multiplier / 5) self.M2.run_forever(speed_sp=self.speed * multiplier * -1 /5) rightTurn = True leftTurn = False sleep(0.001) if abs(self.GS.value() - angle) > abs(field[len(field) - 1]): #we have to make them absolute because we won to turn on both sides if not leftTurn: print(self.GS.value() - angle) #~ print("working") self.M1.run_forever(speed_sp=self.speed * multiplier * -1 / 5) self.M2.run_forever(speed_sp=self.speed * multiplier / 5) rightTurn = False leftTurn = True sleep(0.001) self.M1.stop() self.M2.stop() self.resetValue() return True def straight(self, direction, speed, tolerance): self.stopMotors() self.resetValue() #~ self.GS.mode = 'GYRO-ANG' angle = self.GS.value() multiplier = 1 if angle < 0: multiplier = -1 self.motor_stop = False def inField(field, thing): succes = 0 j = 0 for i in field: if j == 0: if i < thing: succes = 2 break if j == len(field) - 1: if i > thing: succes = 3 break if thing == i: succes = 1 break j = j + 1 return succes field = range(angle-tolerance, angle+tolerance) while self.motor_stop == False: self.M1.run_forever(speed_sp=speed * direction) self.M2.run_forever(speed_sp=speed * direction) sleep(0.2) value = self.GS.value() if inField(field, value) == 2: #~ print("compesating 2") self.M1.run_forever(speed_sp=speed - 50 * direction) while self.GS.value() not in field: sleep(0.02) #~ print(self.GS.value()) self.M1.run_forever(speed_sp=speed * direction) self.M2.run_forever(speed_sp=speed * direction) elif inField(field, value) == 3: #~ print("compesating 3") self.M2.run_forever(speed_sp=speed - 50 * direction) #~ print("value") #~ print(self.GS.value()) while self.GS.value() not in field: #~ print(self.GS.value()) sleep(0.02) self.M2.run_forever(speed_sp=speed * direction) self.M1.run_forever(speed_sp=speed * direction) if self.motor_stop is True: self.stopMotors() def measureDistanceStart(self): self.distance = self.M1.position # ~ self.MDistanceRunning = True def measureDistance(self, wheelDiameter = 5.5): turns = (self.M1.position - self.distance) / 360 dist = turns * wheelDiameter * math.pi return dist + 4 def measureDistanceRunning(self): return self.MDistanceRunning def stopMotors(self): self.motor_stop = True self.M2.stop() self.M1.stop() self.resetValue() def resetValue(self): self.GS.mode = 'GYRO-FAS' sleep(0.002) self.GS.mode = 'GYRO-ANG' sleep(0.001) def isRunning(self): return not self.motor_stop
t1 = 0.0 dt = 0.0 tachospeed = 0.0 # degrs Per Sec ev3dev2_speed = 0 d_deg_avg = 0.0 tachospeed_avg = 0.0 start_pos = 0 end_pos = 0 start_time = 0.0 end_time = 0.0 m = LargeMotor(OUTPUT_A) time.sleep(0.1) #m.reset() m.stop_action = 'coast' m.stop() m.position = 0 time.sleep(0.1) start_time = time.time() #m.on(SpeedDPS(800)) while True: for i in range(6000): t1 = time.time() old_pos = encoder_pos time.sleep(0.05) encoder_pos = m.position dt = time.time() - t1 d_deg = encoder_pos - old_pos tachospeed = d_deg / dt d_deg_avg = ((d_deg_avg + d_deg) / (2))
class ALBERT(object): def __init__(self): # ev3dev initialization self.leds = Leds() self.sound = Sound() self.arm_base = LargeMotor(OUTPUT_D) self.arm_shoulder = LargeMotor(OUTPUT_C) self.arm_wrist = LargeMotor(OUTPUT_B) self.arm_gripper = MediumMotor(OUTPUT_A) self.station = Workstation() self.color_sensor = ColorSensor(INPUT_1) self.find_indicator() self.rotate_base(STATION_COLOR) self.reset_all_motors() def find_indicator(self): ''' Search for a valid color indicator. ''' if self.color_sensor.color in VALID_COLORS: return self.arm_base.on(10) while self.color_sensor.color not in VALID_COLORS: pass self.arm_base.stop() def reset_all_motors(self): ''' Reset all motor tach counts. ''' self.arm_base.reset() self.arm_shoulder.reset() self.arm_wrist.reset() self.arm_gripper.reset() def rotate_base(self, color): ''' Rotate from one color indicator to another. Color order is: YELLOW <--> BLUE <--> RED STORE <--> STATION <--> STERILE ''' current_color = self.color_sensor.color if current_color == color: return direction = 1 if (current_color == STATION_COLOR and color == STERILE_COLOR) or current_color == STORE_COLOR: direction = -1 self.arm_base.on(SPEEDS[0] * direction, block=False) while self.color_sensor.color != color: pass self.arm_base.stop() self.arm_base.on_for_rotations(SPEEDS[0], direction * STRIPE_BIAS) def make_plate(self): ''' Sequence to make a plate. ''' self.get_plate() self.lift_lid() self.swab_plate() self.lower_lid() self.store_plate() def check_plate(self): ''' Sequence to check plate. ''' self.get_plate(from_storage=True, upside_down=True) self.move_to_keypoint(KP_UP_HORZ) refl = self.station.check_status() self.move_to_keypoint(KP_DOWN_HORZ) self.store_plate(is_upside_down=True) return refl def get_plate(self, from_storage=False, upside_down=False): ''' Sequence to get a plate and place it in the workstation. Post-conditions Gripper: WIDE Arm: DOWN Base: STATION ''' src = STORE_COLOR if from_storage else STERILE_COLOR self.move_to_keypoint(KP_UP_HORZ) self.rotate_base(src) self.set_gripper(GRIP_NARROW) self.move_to_keypoint(KP_DOWN_HORZ) self.set_gripper(GRIP_CLOSED) self.move_to_keypoint(KP_UP_HORZ) self.rotate_base(STATION_COLOR) dest_up = KP_UP_VERT_INVERT if upside_down else KP_UP_VERT dest_down = KP_DOWN_VERT_INVERT if upside_down else KP_DOWN_VERT self.move_to_keypoint(dest_up) self.move_to_keypoint(dest_down) self.set_gripper(GRIP_WIDE) self.move_to_keypoint(KP_DOWN_HORZ) def lift_lid(self): ''' Lift the dish lid. Pre-condition Gripper: WIDE Arm: DOWN Base: STATION Post-condition Gripper: CLOSED Arm: UP ''' self.move_to_keypoint(KP_DOWN_HORZ_LID) self.set_gripper(GRIP_CLOSED) self.move_to_keypoint(KP_UP_HORZ) def lower_lid(self): ''' Lower the dish lid. Pre-condition Gripper: CLOSED Arm: UP Base: STATION Post-condition Gripper: WIDE Arm: DOWN ''' self.move_to_keypoint(KP_DOWN_HORZ_LID) self.set_gripper(GRIP_WIDE) def swab_plate(self): ''' Call the Workstation swab routine. ''' self.station.swab() def store_plate(self, is_upside_down=False): ''' Sequence to store a plate. ''' src_down = KP_DOWN_VERT_INVERT if is_upside_down else KP_DOWN_VERT self.move_to_keypoint(src_down) self.set_gripper(GRIP_CLOSED) self.move_to_keypoint(KP_UP_HORZ) self.rotate_base(STORE_COLOR) self.move_to_keypoint(KP_DOWN_HORZ) self.set_gripper(GRIP_NARROW) self.move_to_keypoint(KP_UP_VERT) self.rotate_base(STATION_COLOR) self.set_gripper(GRIP_CLOSED) def move_to_keypoint(self, keypoint): ''' Move the should/wrist to a keypoint.''' # keypoint is [shoulder, wrist] with unit of rotations self.arm_shoulder.on_to_position( SPEEDS[1], keypoint[0] * self.arm_shoulder.count_per_rot) self.arm_wrist.on_to_position( SPEEDS[2], keypoint[1] * self.arm_wrist.count_per_rot) # pause to let things settle time.sleep(MOVE_PAUSE) def set_gripper(self, position): ''' Set the gripper position. ''' self.arm_gripper.on_to_position( 25, self.arm_gripper.count_per_rot * position) time.sleep(MOVE_PAUSE)
class MindstormsGadget(AlexaGadget): """ A Mindstorms gadget that performs movement based on voice commands. """ def __init__(self): """ Performs Alexa Gadget initialization routines and ev3dev resource allocation. """ super().__init__() # Ev3dev initialization self.leds = Leds() self.sound = Sound() self.drive = LargeMotor(OUTPUT_B) self.dealmotor = MediumMotor(OUTPUT_A) self.bcolor = ColorSensor(INPUT_1) self.pushsensor = TouchSensor(INPUT_2) self.leftmargin = 0 self.rigtmargin = 0 self._init_reset() def _init_reset(self): self.numCards = 2 self.numPlayers = 0 self.game = 'blackjack' self.degreeStep = 180 self.players = ["red","yellow"] #Default player self._send_event(EventName.SPEECH, {'speechOut': "Restart game"}) def on_connected(self, device_addr): """ Gadget connected to the paired Echo device. :param device_addr: the address of the device we connected to """ self.leds.set_color("LEFT", "GREEN") self.leds.set_color("RIGHT", "GREEN") logger.info("{} connected to Echo device".format(self.friendly_name)) def on_disconnected(self, device_addr): """ Gadget disconnected from the paired Echo device. :param device_addr: the address of the device we disconnected from """ self.leds.set_color("LEFT", "BLACK") self.leds.set_color("RIGHT", "BLACK") logger.info("{} disconnected from Echo device".format(self.friendly_name)) def on_custom_mindstorms_gadget_control(self, directive): """ Handles the Custom.Mindstorms.Gadget control directive. :param directive: the custom directive with the matching namespace and name """ try: payload = json.loads(directive.payload.decode("utf-8")) print("Control payload: {}".format(payload), file=sys.stderr) control_type = payload["type"] if control_type == "move": # Expected params: [direction, duration, speed] self._move(payload["direction"], int(payload["duration"]), int(payload["speed"])) if control_type == "command": # Expected params: [command] self._activate(payload["command"]) if control_type == "dealcard": # Expected params: [command] = Number of Cards # Expected params: [player] num = payload["command"] player = payload["player"] speak = "Give "+ num + " card to " + player if player == "all": if (self.numPlayers == 0): self.numPlayers = 2 for i in range(int(num)): for j in range(self.numPlayers): self._dealcard(1,j) else: try: num = self.players.index(player) self._dealcard(int(payload["command"]),num) except ValueError: speak = "There is no user " + player print (speak,file=sys.stderr) self._send_event(EventName.SPEECH, {'speechOut': speak}) except KeyError: print("Missing expected parameters: {}".format(directive), file=sys.stderr) def _dealcard(self, num, player): """ Deal number of cards to player """ if num < 1: num = 1 degree = self._calcDegree(player) print("Give player : {} card : {} Move angle : {}".format(player, num,degree), file=sys.stderr) self.drive.on_to_position(SpeedPercent(10),degree) self.drive.wait_until_not_moving() for i in range(num): self.dealmotor.on_for_degrees(SpeedPercent(-100), 340) self.dealmotor.wait_until_not_moving() time.sleep(1) self.dealmotor.on_for_degrees(SpeedPercent(100), 270) self.dealmotor.wait_until_not_moving() time.sleep(1) def _calcDegree (self,player): degree = (player*self.degreeStep)+self.leftmargin return degree def _gameinit(self,game): """ Check and start game """ if (self.numPlayers == 0): self.numPlayers = 2 if game == "poker": self.numCards = 5 if game == "blackjack": self.numCards = 2 if game == "rummy": self.numCards = 7 if (self.numPlayers == 2): self.numCards = 10 speak = "" for i in range(self.numPlayers): print("Player : {} Color : {}".format(i,self.players[i]), file=sys.stderr) speak = speak + self.players[i] speak = "Start " + game + "with " + str(self.numPlayers) + "player " + speak self._send_event(EventName.SPEECH, {'speechOut': speak}) self._findboundary() self.drive.on_to_position(SpeedPercent(10),self.leftmargin) time.sleep(1) print("Game : {} Number of Card : {}".format(game,self.numCards), file=sys.stderr) for i in range(self.numCards): for j in range(self.numPlayers): self._dealcard(1,j) def _findboundary (self): "Move to left until sensor pressed " self.drive.on(SpeedPercent(10)) self.pushsensor.wait_for_pressed () self.drive.stop() "Get position" self.rightmargin = self.drive.position print("Right position : {} ".format(self.rightmargin), file=sys.stderr) self.drive.on(SpeedPercent(-10)) time.sleep(1) self.pushsensor.wait_for_pressed () self.drive.stop() "Get position + offset 45 for not to close limitation" self.leftmargin = self.drive.position self.leftmargin = self.leftmargin + 60 print("Left position : {} ".format(self.leftmargin), file=sys.stderr) self.degreeStep = int(abs((self.leftmargin - self.rightmargin)/self.numPlayers)) print("Degree steps : {} ".format(self.degreeStep), file=sys.stderr) def _addUser (self): if self.numPlayers == 0: self.players.clear() player = self.bcolor.color_name self.players.append(player.lower()) print("Player {} color: {}".format(self.players[self.numPlayers],player), file=sys.stderr) self.numPlayers += 1 self._send_event(EventName.PLAYER, {'player': player}) def _move(self, direction, duration: int, speed: int, is_blocking=False): """ Handles move commands from the directive. Right and left movement can under or over turn depending on the surface type. :param direction: the move direction :param duration: the duration in seconds :param speed: the speed percentage as an integer :param is_blocking: if set, motor run until duration expired before accepting another command """ print("Move command: ({}, {}, {}, {})".format(direction, speed, duration, is_blocking), file=sys.stderr) if direction in Direction.FORWARD.value: self.drive.on_for_degrees(SpeedPercent(10),90) if direction in Direction.BACKWARD.value: self.drive.on_for_degrees(SpeedPercent(-10),90) if direction in Direction.STOP.value: self.drive.off() def _send_event(self, name: EventName, payload): """ Sends a custom event to trigger a sentry action. :param name: the name of the custom event :param payload: the sentry JSON payload """ self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload) def _activate(self, command, speed=50): """ Handles preset commands. :param command: the preset command :param speed: the speed if applicable """ print("Activate command: ({}, {})".format(command, speed), file=sys.stderr) if command in Command.GAMES.value: self.game = command print("Play game: {}".format(self.game), file=sys.stderr) self._gameinit(self.game) if command in Command.RESET_GAME.value: # Reset game self._init_reset() if command in Command.ADD_CMD.value: self._addUser()
class myRobot: def __init__(self): self.tankDrive = MoveTank(OUTPUT_A, OUTPUT_D) self.motorA = LargeMotor(OUTPUT_A) self.motorD = LargeMotor(OUTPUT_D) self.myGyroSensor = GyroSensor() self.myGripper = RobotGripper() self.myUltrasonicSensor = UltrasonicSensor() self.myColorSensor = ColorSensor() self.Turning = False self.Moving = False self.myAngle = 0 self.positionX = 0 self.positionY = 0 self.myDefaultSpeed = 50 self.desiredBarCode = "BWWB" self.myGPS = IGPS() def getAverageDistance(self): #Returns average distance in inches average = (self.motorA.position + self.motorD.position) / 2 average = (average * 3.14 * (2.71 * 2)) return average def moveStraight(self, distance): self.motorA.reset() self.motorD.reset() self.tankDrive.reset() #self.myGyroSensor.angle = 0 self.myGyroSensor.mode = 'GYRO-ANG' currentDistance = 0 speedPercentLeft = .5 speedPercentRight = .5 while currentDistance < distance: currentDistance = self.getAverageDistance() if self.myGyroSensor.angle < 0: #Turn left, left gain gain = (-.00005 * (self.myGyroSensor.angle)) speedPercentLeft = speedPercentLeft - gain speedPercentRight = speedPercentRight + gain else: #Turn moe right, right gain gain = (.00005 * (self.myGyroSensor.angle)) speedPercentLeft = speedPercentLeft + gain speedPercentRight = speedPercentRight - gain self.tankDrive.on_for_degrees(self.myDefaultSpeed, self.myDefaultSpeed, 10, brake=False, block=True) sleep(.2) #movesteer('rotations',steer = , pwr=50, rots=.1) #Set to brake def collisonDistance(self): distance = self.myUltrasonicSensor.distance_inches_ping return distance def collisionAvoidance(self): if self.Turning == False: #Normal collision avoidance print('Checking for collisions') distance = self.collisonDistance() if distance < 5: self.motorA.stop() self.motorD.stop() print('Collision avoided') else: #Collision avoidance disabled print('Not checking for collisions') def turnToAngle(self, angle): #self.myGyroSensor.reset() if angle > 0: print('Positive angle') #Turns right self.Turning = True while self.myGyroSensor.value() < angle: self.tankDrive.on_for_degrees((-.2 * self.myDefaultSpeed), (.2 * self.myDefaultSpeed), 3, brake=False, block=True) sleep(.2) self.tankDrive.STOP_ACTION_BRAKE() self.Turning = False self.myAngle += self.myGyroSensor.value self.myGyroSensor.reset else: #do another thing print('Negative Angle') self.Turning = True while self.myGyroSensor.value() > angle: self.tankDrive.on_for_degrees((.2 * self.myDefaultSpeed), (-.2 * self.myDefaultSpeed), 2, brake=False, block=True) sleep(.1) self.tankDrive.STOP_ACTION_BRAKE() self.Turning = False self.myAngle -= self.myGyroSensor.value self.myGyroSensor.reset def readBarCode(self): #String kmessage is 3 letters, 'W' denotates white, "B" denotates black output = "" ev3.Sound.speak('starting').wait() for i in range(4): self.moveStraight(.25) self.myColorSensor.mode = 'COL-COLOR' if self.myColorSensor.value == 1: output = output + 'B' ev3.Sound.speak('black').wait() elif self.myColorSensor.value == 6: output = output + 'W' ev3.Sound.speak('white').wait() sleep(1) ev3.Sound.speak('Here is the barcode' + output).wait() print(output) sleep(5) return output
if( steps < 0 ): steps = 0 elif(btn.check_buttons(buttons=['enter'])): break # update the steps on screen lcd.text_pixels( str(steps), True, 80, 50, font='courB18') lcd.update() # btn.wait_for_released(buttons=['up','down', 'left', 'right','enter']) log.info('Climbing ......') while( steps > 0 ): oneStep() steps -= 1 # update the steps on screen lcd.text_pixels( str(steps), True, 80, 50, font='courB18') lcd.update() # move forward & stop mm_move.on(-90) lm_move.on(-100) sleep(0.5) mm_move.stop() lm_move.stop()
sound.play_file("/home/robot/sounds/Speed up.wav") console.reset_console() leds.animate_flash('GREEN', duration=None, block=False) state = "ready" log.debug("Starting control loop") while ok == True: time = get_time() calculate_control_loop_period() calculate_robot_body_angle_and_speed() calculate_wheel_angle_and_speed() calculate_output_power() drive_motors() check_if_robot_fell_down() state = "fell_over" right_motor.stop() left_motor.stop() leds.animate_stop() leds.reset() leds.animate_flash('RED') console.reset_console() sound.play_file("/home/robot/sounds/Speed down.wav") sleep(3.0) running = False log.info("Terminating Gyro Boy")
class IO(): """ Provides functions for high-level IO operations (move left, move forward, is left side free to move etc.). """ def __init__(self): # Large motors self.lm_left_port = "outB" self.lm_right_port = "outA" self.lm_left = LargeMotor(self.lm_left_port) self.lm_right = LargeMotor(self.lm_right_port) # distance at which sensor motor start moving self.move_sensor_check_degrees = 400 self.move_degrees = 570 # one tile distance self.move_speed = 35 self.after_crash_degrees = 200 self.steering_turn_speed = 30 # turning left or right self.steering_turn_degrees = 450 self.steering_turn_fwd_degrees = 150 # distance to move after turning # distance at which sensors start spinning self.steering_sensor_check_degrees = 50 self.btn = Button() # small motor self.sm_port = "outC" self.sm = Motor(self.sm_port) self.sm_turn_speed = 30 self.sm_center_turn_angle = 90 self.sm_side_turn_angle = 110 self.sm_is_left = False # color sensor self.color_sensor_port = "in1" self.color_sensor = ColorSensor(self.color_sensor_port) self.color_sensor.mode = ColorSensor.MODE_COL_REFLECT self.color_sensor_values = [] # regulations self.regulation_desired_value = 4 self.regulation_max_diff = 3 self.regulation_p = 1.5 self.regulation_tick = 0.03 # ultrasonic sensor self.ultrasonic_sensor_port = "in4" self.ultrasonic_sensor = UltrasonicSensor(self.ultrasonic_sensor_port) self.ultrasonic_sensor.mode = 'US-DIST-CM' self.ultrasonic_tile_length = 30 self.ultrasonic_max_value = 255 self.ultrasonic_sensor_values = [] # touch sensors self.touch_right = TouchSensor("in2") self.touch_left = TouchSensor("in3") def go_left(self): ok = self.__turn_left() if (ok): ok = self.__move_reg(self.steering_turn_fwd_degrees, self.steering_sensor_check_degrees) return ok def go_right(self): ok = self.__turn_right() if (ok): ok = self.__move_reg(self.steering_turn_fwd_degrees, self.steering_sensor_check_degrees) return ok def go_forward(self): return self.__move_reg(self.move_degrees, self.move_sensor_check_degrees) def go_back(self): self.__turn(stop_motor=self.lm_left, turn_motor=self.lm_right, degrees=self.steering_turn_degrees, speed=self.steering_turn_speed) return self.__turn(stop_motor=self.lm_right, turn_motor=self.lm_left, degrees=self.steering_turn_degrees, speed=-self.steering_turn_speed) def after_crash(self): debug_print("crash") self.__move(self.after_crash_degrees, self.move_speed) self.read_sensors() def __turn(self, stop_motor: Motor, turn_motor: Motor, degrees: int, speed: int): stop_motor.stop() start = turn_motor.degrees turn_motor.on(speed) while (abs(turn_motor.degrees - start) < degrees): if (not self.__check_button()): self.lm_left.stop() self.lm_right.stop() return False return True def __turn_left(self): return self.__turn(stop_motor=self.lm_left, turn_motor=self.lm_right, degrees=self.steering_turn_degrees, speed=-self.steering_turn_speed) def __turn_right(self): return self.__turn(stop_motor=self.lm_right, turn_motor=self.lm_left, degrees=self.steering_turn_degrees, speed=-self.steering_turn_speed) def __move(self, degrees, speed) -> None: self.lm_left.on_for_degrees(speed, degrees, block=False) self.lm_right.on_for_degrees(speed, degrees, block=True) def __reg(self): val = self.color_sensor.reflected_light_intensity diff = (self.regulation_desired_value - val) if diff >= 0 and val > 0: diff = min(diff, self.regulation_max_diff) elif val == 0: diff = 0 else: diff = -min(abs(diff), self.regulation_max_diff) diff *= self.regulation_p if self.sm_is_left: return (-self.move_speed + diff, -self.move_speed - diff) return (-self.move_speed - diff, -self.move_speed + diff) def __check_button(self): timeout = time.time() + self.regulation_tick while (time.time() <= timeout): # check for touch sensor if (self.touch_left.is_pressed or self.touch_right.is_pressed): return False if (self.btn.left or self.btn.right): debug_print("pressed") return None time.sleep(0.01) return True def __move_reg(self, degrees, sensor_degrees): t = Thread(target=self.read_sensors) start_l, start_r = (self.lm_left.degrees, self.lm_right.degrees) distance_l, distance_r = 0, 0 while (distance_l < degrees or distance_r < degrees): speed_l, speed_r = self.__reg() self.lm_left.on(speed_l, brake=True) self.lm_right.on(speed_r, brake=True) ok = self.__check_button() if (ok is False): self.lm_left.stop() self.lm_right.stop() return False elif (ok is None): debug_print("none") self.lm_left.stop() self.lm_right.stop() return None if ((distance_l >= sensor_degrees or distance_r >= sensor_degrees) and not t.isAlive()): t.start() distance_l = abs(start_l - self.lm_left.degrees) distance_r = abs(start_r - self.lm_right.degrees) t.join() return True def read_sensors(self): self.color_sensor_values = [] # List[float] self.ultrasonic_sensor_values = [] speed = self.sm_turn_speed if (self.sm_is_left): speed = -self.sm_turn_speed # side 1 self.color_sensor_values.append( self.color_sensor.reflected_light_intensity) self.ultrasonic_sensor_values.append( self.ultrasonic_sensor.distance_centimeters) # center self.sm.on_for_degrees(speed, self.sm_center_turn_angle) self.color_sensor_values.append( self.color_sensor.reflected_light_intensity) self.ultrasonic_sensor_values.append( self.ultrasonic_sensor.distance_centimeters) # side 2 self.sm.on_for_degrees(speed, self.sm_side_turn_angle) self.color_sensor_values.append( self.color_sensor.reflected_light_intensity) self.ultrasonic_sensor_values.append( self.ultrasonic_sensor.distance_centimeters) if not self.sm_is_left: self.ultrasonic_sensor_values.reverse() self.color_sensor_values.reverse() self.sm_is_left = not self.sm_is_left def directions_free(self) -> List[bool]: ''' Returns list of bools (left, center, right), representing if the directions are free to move. ''' return [a == 0 for a in self.color_sensor_values] def ghost_distance(self) -> List[int]: ''' Returns list of ints (left, center, right), representing the distance from closest ghost. ''' return [ int(a) // self.ultrasonic_tile_length if a < self.ultrasonic_max_value and a // self.ultrasonic_tile_length > 0 else None for a in self.ultrasonic_sensor_values ]
def main(): Sound.speak("").wait() MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") GY.mode = 'GYRO-ANG' GY.mode = 'GYRO-RATE' GY.mode = 'GYRO-ANG' tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) loop_gyro = 0 gyro_adjust = 1 starting_value = GY.value() gyro_correct_loops = 0 straight_correct_loops = 0 gyro_correct_straight = 0 # change this to whatever speed what you want left_wheel_speed = 100 right_wheel_speed = 100 # change the loop_gyro verses the defined value argument to however far you want to go # if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left # change the value to how far you want the robot to go while MB.position > -2201: if GY.value() == 0: left_wheel_speed = -300 right_wheel_speed = -300 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) gyro_adjust = 9 gyro_correct_loops = 0 gyro_correct_straight = 0 straight_correct_loops = 0 else: if GY.value() < 0: correct_rate = abs( GY.value() ) # This captures the gyro value at the beginning of the statement right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 if abs( GY.value() ) > correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time gyro_adjust = gyro_adjust + 1 gyro_correct_loops = gyro_correct_loops + 1 if GY.value() == 0 and gyro_correct_straight == 0: while straight_correct_loops < gyro_correct_loops + 1: right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 gyro_correct_loops = 0 straight_correct_loops = 0 else: correct_rate = abs( GY.value()) # Same idea as the other if statement left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 gyro_correct_loops = gyro_correct_loops + 1 if abs(GY.value()) > correct_rate: gyro_adjust = gyro_adjust + 1 if GY.value( ) == 0 and gyro_correct_straight == 0: #this code corrects the gyro back to the right line while straight_correct_loops < gyro_correct_loops + 1: left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 gyro_correct_loops = 0 straight_correct_loops = 0 # stop all motors MB.stop(stop_action="hold") MC.stop(stop_action="hold") while GY.value() < 46: left_wheel_speed = 200 right_wheel_speed = -200 #MB is left wheel & MC is right wheel MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) MB.stop(stop_action="hold") MC.stop(stop_action="hold") tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 3) while GY.value() > -1: left_wheel_speed = -200 right_wheel_speed = 200 tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 3)
def Krab(): MA = MediumMotor("outA") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("outD") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") #Setting the Gyro. V GY.mode = 'GYRO-ANG' GY.mode = 'GYRO-RATE' GY.mode = 'GYRO-ANG' tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) loop_gyro = 0 gyro_adjust = 1 starting_value = GY.value() gyro_correct_loops = 0 straight_correct_loops = 0 gyro_correct_straight = 0 # change this to whatever speed what you want. V left_wheel_speed = 100 right_wheel_speed = 100 # change the loop_gyro verses the defined value argument to however far you want to go # if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left # Wheel alignment. VVVV tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01) #Pulling out of Launch area. V tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 1.625) #Gyro Turn toowards the red circle. V while GY.value() < 80: left_wheel_speed = 100 right_wheel_speed = -100 #MB is left wheel & MC is right wheel MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) MB.stop(stop_action="hold") MC.stop(stop_action="hold") #Driving forward towards the safety factor. V while MB.position > -1700: if GY.value() == 90: left_wheel_speed = -500 right_wheel_speed = -500 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) gyro_adjust = 8 gyro_correct_loops = 0 gyro_correct_straight = 0 straight_correct_loops = 0 else: if GY.value() < 90: correct_rate = abs( GY.value() ) # This captures the gyro value at the beginning of the statement right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -500 right_wheel_speed = -500 if abs( GY.value() ) >= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time gyro_adjust = gyro_adjust + 1 gyro_correct_loops = gyro_correct_loops + 1 if GY.value() == 0 and gyro_correct_straight == 0: while straight_correct_loops < gyro_correct_loops + 1: right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 gyro_correct_loops = 0 straight_correct_loops = 0 else: correct_rate = abs( GY.value()) # Same idea as the other if statement left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -500 right_wheel_speed = -500 gyro_correct_loops = gyro_correct_loops + 1 if abs(GY.value()) >= correct_rate: gyro_adjust = gyro_adjust + 1 if GY.value( ) == 0 and gyro_correct_straight == 0: #this code corrects the gyro back to the right line while straight_correct_loops < gyro_correct_loops + 1: #runs this loop until it makes the gyro the opposite of what it was when it was wrong in the first place left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 #makes sure that when the gyro is corrected to both straight and the line it was on that gyro is not messed up again gyro_correct_loops = 0 straight_correct_loops = 0 # stop all motors MB.stop(stop_action="hold") MC.stop(stop_action="hold") #unlocking arm to get elevator MD.on_for_degrees(SpeedPercent(-50), 176.26) #pushing down the beams from safety factor tank_drive.on_for_rotations(SpeedPercent(-50), SpeedPercent(-50), 2.75) #going back to home. V while MB.position < 0: #2244 previously if GY.value() == 90: left_wheel_speed = 900 right_wheel_speed = 900 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) gyro_adjust = 8 gyro_correct_loops = 0 gyro_correct_straight = 0 straight_correct_loops = 0 else: if GY.value() < 90: correct_rate = abs( GY.value() ) # This captures the gyro value at the beginning of the statement right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = 900 right_wheel_speed = 900 if abs( GY.value() ) <= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time gyro_adjust = gyro_adjust + 1 gyro_correct_loops = gyro_correct_loops + 1 if GY.value() == 0 and gyro_correct_straight == 0: while straight_correct_loops > gyro_correct_loops + 1: right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 gyro_correct_loops = 0 straight_correct_loops = 0 else: correct_rate = abs( GY.value()) # Same idea as the other if statement left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = 900 right_wheel_speed = 900 gyro_correct_loops = gyro_correct_loops + 1 if abs(GY.value()) <= correct_rate: gyro_adjust = gyro_adjust + 1 if GY.value( ) == 0 and gyro_correct_straight == 0: #this code corrects the gyro back to the right line while straight_correct_loops > gyro_correct_loops + 1: #runs this loop until it makes the gyro the opposite of what it was when it was wrong in the first place left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 #makes sure that when the gyro is corrected to both straight and the line it was on that gyro is not messed up again gyro_correct_loops = 0 straight_correct_loops = 0 MB.stop(stop_action="hold") MC.stop(stop_action="hold") #still going home tank_drive.on_for_rotations(SpeedPercent(40), SpeedPercent(40), 1.5) Launchrun()
def Redcircle(): Sound.speak("").wait() MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") #Setting the Gyro. V GY.mode='GYRO-ANG' GY.mode='GYRO-RATE' GY.mode='GYRO-ANG' tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) loop_gyro = 0 gyro_adjust = 1 starting_value = GY.value() gyro_correct_loops = 0 straight_correct_loops = 0 gyro_correct_straight = 0 # change this to whatever speed what you want. V left_wheel_speed = 100 right_wheel_speed = 100 tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01) #wheel alignment # change the loop_gyro verses the defined value argument to however far you want to go # if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left # change the value to how far you want the robot to go. V #Pulling out of Launch area. V tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 1.5) #Gyro Turn toowards the red circle. V while GY.value() < 80: left_wheel_speed = 100 right_wheel_speed = -100 #MB is left wheel & MC is right wheel MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) MB.stop(stop_action="hold") MC.stop(stop_action="hold") #Driving forward towards the red circle. V while MB.position > -1270: #was -1280, tessa is changing it to 1270 to stay in circle better if GY.value() == 90: left_wheel_speed = -300 right_wheel_speed = -300 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) gyro_adjust = 8 gyro_correct_loops = 0 gyro_correct_straight = 0 straight_correct_loops = 0 else: if GY.value() < 90: correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 if abs (GY.value()) >= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time gyro_adjust = gyro_adjust + 1 gyro_correct_loops = gyro_correct_loops + 1 if GY.value() == 0 and gyro_correct_straight == 0: while straight_correct_loops < gyro_correct_loops + 1: right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 gyro_correct_loops = 0 straight_correct_loops = 0 else: correct_rate = abs (GY.value()) # Same idea as the other if statement left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 gyro_correct_loops = gyro_correct_loops + 1 if abs (GY.value()) >= correct_rate: gyro_adjust = gyro_adjust + 1 if GY.value() == 0 and gyro_correct_straight == 0: #this code corrects the gyro back to the right line while straight_correct_loops < gyro_correct_loops + 1: #runs this loop until it makes the gyro the opposite of what it was when it was wrong in the first place left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 #makes sure that when the gyro is corrected to both straight and the line it was on that gyro is not messed up again gyro_correct_loops = 0 straight_correct_loops = 0 # stop all motors MB.stop(stop_action="hold") MC.stop(stop_action="hold") #Pulling away from the Red circle and driving into home. V tank_drive.on_for_rotations(SpeedPercent(65), SpeedPercent(65), 5) program_running = 0 Launchrun()
class MindstormsGadget(AlexaGadget): def __init__(self): super().__init__(gadget_config_path='./auth.ini') # order queue self.queue = Queue() self.button = Button() self.leds = Leds() self.sound = Sound() self.console = Console() self.console.set_font("Lat15-TerminusBold16.psf.gz", True) self.dispense_motor = LargeMotor(OUTPUT_A) self.pump_motor = LargeMotor(OUTPUT_B) self.touch_sensor = TouchSensor(INPUT_1) # Start threads threading.Thread(target=self._handle_queue, daemon=True).start() threading.Thread(target=self._test, daemon=True).start() def on_connected(self, device_addr): self.leds.animate_rainbow(duration=3, block=False) self.sound.play_song((('C4', 'e3'), ('C5', 'e3'))) def on_disconnected(self, device_addr): self.leds.animate_police_lights('RED', 'ORANGE', duration=3, block=False) self.leds.set_color("LEFT", "BLACK") self.leds.set_color("RIGHT", "BLACK") self.sound.play_song((('C5', 'e3'), ('C4', 'e3'))) def _test(self): while 1: self.button.wait_for_pressed('up') order = { 'name': 'Test', 'tea': 'Jasmine', 'sugar': 100, 'ice': 100 } self.queue.put(order) sleep(1) def _handle_queue(self): while 1: if self.queue.empty(): continue order = self.queue.get() self._make(name=order['name'], tea=order['tea'], sugar=order['sugar'], ice=order['ice']) def _send_event(self, name, payload): self.send_custom_event('Custom.Mindstorms.Gadget', name, payload) def _affirm_receive(self): self.leds.animate_flash('GREEN', sleeptime=0.25, duration=0.5, block=False) self.sound.play_song((('C3', 'e3'), ('C3', 'e3'))) def on_custom_mindstorms_gadget_control(self, directive): try: payload = json.loads(directive.payload.decode("utf-8")) print("Control payload: {}".format(payload), file=sys.stderr) control_type = payload["type"] # regular voice commands if control_type == "automatic": self._affirm_receive() order = { "name": payload["name"] or "Anonymous", "tea": payload["tea"] or "Jasmine Milk Tea", "sugar": payload["sugar"] or 100, "ice": payload["ice"] or 100, } self.queue.put(order) # series of voice commands elif control_type == "manual": # Expected params: [command] control_command = payload["command"] if control_command == "dispense": self._affirm_receive() if payload['num']: self._dispense(payload['num']) else: self._dispense() elif control_command == "pour": self._affirm_receive() if payload['num']: self._pour(payload['num']) else: self._pour() except KeyError: print("Missing expected parameters: {}".format(directive), file=sys.stderr) def _make(self, name=None, tea="Jasmine Milk Tea", sugar=100, ice=100): if not self.touch_sensor.is_pressed: # cup is not in place self._send_event('CUP', None) self.touch_sensor.wait_for_pressed() sleep(3) # cup enter delay # mid_col = console.columns // 2 # mid_row = console.rows // 2 # mid_col = 1 # mid_row = 1 # alignment = "L" process = self.sound.play_file('mega.wav', 100, Sound.PLAY_NO_WAIT_FOR_COMPLETE) # dispense boba self._dispense() # dispense liquid self._pour(tea=tea) # self.console.text_at( # s, column=mid_col, row=mid_row, alignment=alignment, reset_console=True # ) # notify alexa that drink is finished payload = { "name": name, "tea": tea, "sugar": sugar, "ice": ice, } self._send_event("DONE", payload) process.kill() # kill song self.sound.play_song((('C4', 'q'), ('C4', 'q'), ('C4', 'q')), delay=0.1) self.touch_sensor.wait_for_released() # dispense liquid def _pour(self, time_in_s=10, tea="Jasmine Milk Tea"): # send event to alexa payload = {"time_in_s": time_in_s, "tea": tea} self._send_event("POUR", payload) self.pump_motor.run_forever(speed_sp=1000) sleep(time_in_s) self.pump_motor.stop() # dispense boba def _dispense(self, cycles=10): # send event to alexa payload = {"cycles": cycles} self._send_event("DISPENSE", payload) # ensure the dispenser resets to the correct position everytime if cycles % 2: cycles += 1 # agitate the boba to make it fall for i in range(cycles): deg = 45 if i % 2 else -45 self.dispense_motor.on_for_degrees(SpeedPercent(75), deg) sleep(0.5)
def Spider(): Sound.speak("").wait() MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") T1 = TouchSensor("") GY.mode='GYRO-ANG' GY.mode='GYRO-RATE' GY.mode='GYRO-ANG' tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) loop_gyro = 0 gyro_adjust = 1 starting_value = GY.value() gyro_correct_loops = 0 straight_correct_loops = 0 gyro_correct_straight = 0 # change this to whatever speed what you want left_wheel_speed = 100 right_wheel_speed = 100 tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01) #wheel alignment # change the loop_gyro verses the defined value argument to however far you want to go # if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left # change the value to how far you want the robot to go tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 1) #the robot starts out from base while GY.value() < 85: #gyro turns 85 degrees and faces towards the swing left_wheel_speed = 100 right_wheel_speed = -100 #MB is left wheel & MC is right wheel MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) MB.stop(stop_action="hold") MC.stop(stop_action="hold") while MB.position > -2326: #this is the gyro program, the first line tells the bot to continue loop until it reaches a defined position if GY.value() == 90: #this runs if the gyro is OK and already straight, sets a lot of variables as well left_wheel_speed = -300 right_wheel_speed = -300 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) gyro_adjust = 8 gyro_correct_loops = 0 gyro_correct_straight = 0 straight_correct_loops = 0 else: #if the gyro is off it runs this section of code if GY.value() < 90: correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement right_wheel_speed = right_wheel_speed - gyro_adjust #changes the wheel speeds accordingly left_wheel_speed = left_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 if abs (GY.value()) >= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time gyro_adjust = gyro_adjust + 1 gyro_correct_loops = gyro_correct_loops + 1 if GY.value() == 0 and gyro_correct_straight == 0: #runs only if bot isn't already on the original line it started from while straight_correct_loops < gyro_correct_loops + 1: #Basically this messes up the gyro again so the bot can correct back to the line it was orignally on right_wheel_speed = right_wheel_speed - gyro_adjust left_wheel_speed = left_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 #sets this to 1 so that it doesn't go off the line again gyro_correct_loops = 0 straight_correct_loops = 0 else: correct_rate = abs (GY.value()) # Same idea as the other if statement, just reversed left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) left_wheel_speed = -300 right_wheel_speed = -300 gyro_correct_loops = gyro_correct_loops + 1 if abs (GY.value()) >= correct_rate: gyro_adjust = gyro_adjust + 1 if GY.value() == 0 and gyro_correct_straight == 0: while straight_correct_loops < gyro_correct_loops + 1: left_wheel_speed = left_wheel_speed - gyro_adjust right_wheel_speed = right_wheel_speed + gyro_adjust straight_correct_loops = straight_correct_loops + 1 gyro_correct_straight = 1 gyro_correct_loops = 0 straight_correct_loops = 0 # stop all motors MB.stop(stop_action="hold") MC.stop(stop_action="hold") while GY.value() < 120: #this turns the blocks into the circle left_wheel_speed = 100 right_wheel_speed = -100 #MB is left wheel & MC is right wheel MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) MB.stop(stop_action="hold") MC.stop(stop_action="hold") tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 0.25) #goes forward slightly to move the blocks all the way into the circle tank_drive.on_for_rotations(SpeedPercent(30), SpeedPercent(30), 1) #drives back a bit while GY.value() > 110: #turns to face the launch area backwards left_wheel_speed = -100 #originaly 200 right_wheel_speed = 100 #originaly 200 MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) MB.stop(stop_action="hold") MC.stop(stop_action="hold") # Returning to home. V tank_drive.on_for_rotations(SpeedPercent(75), SpeedPercent(75), 8.5) #drives back to base. program_running = 0 Launchrun()
# SIG1 detected, control motors x = block[9] * 256 + block[8] # X-centroid of largest SIG1-object y = block[11] * 256 + block[10] # Y-centroid of largest SIG1-object dx = X_REF - x # Error in reference to X_REF integral_x = integral_x + dx # Calculate integral for PID derivative_x = dx - last_dx # Calculate derivative for PID speed_x = KP * dx + KI * integral_x + KD * derivative_x # Speed X-direction dy = Y_REF - y # Error in reference to Y_REF integral_y = integral_y + dy # Calculate integral for PID derivative_y = dy - last_dy # Calculate derivative for PID speed_y = KP * dy + KI * integral_y + KD * derivative_y # Speed Y-direction # Calculate motorspeed out of speed_x and speed_y # Use GAIN otherwise speed will be to slow, # but limit in range [-1000,1000] rspeed = limit_speed(GAIN * (speed_y - speed_x)) lspeed = limit_speed(GAIN * (speed_y + speed_x)) rmotor.run_forever(speed_sp=round(rspeed)) lmotor.run_forever(speed_sp=round(lspeed)) last_dx = dx # Set last error for x last_dy = dy # Set last error for y else: # SIG1 not detected, stop motors rmotor.stop() lmotor.stop() last_dx = 0 last_dy = 0 # TouchSensor pressed, stop motors rmotor.stop() lmotor.stop()
if __name__ == "__main__": #main() from time import perf_counter from ev3dev.ev3 import * #from ev3fast import * lMotor = LargeMotor('outA') rMotor = LargeMotor('outD') lSensor = ColorSensor('in1') rSensor = ColorSensor('in4') LOOPS = 1000 startTime = perf_counter() for a in range(0,LOOPS): valueL = lSensor.raw valueR = rSensor.raw totalL = (valueL[0] + valueL[1] + valueL[2]) totalR = (valueR[0] + valueR[1] + valueR[2]) error = totalL - totalR lMotor.speed_sp = 200 + error rMotor.speed_sp = 200 - error lMotor.run_forever() rMotor.run_forever() endTime = perf_counter() lMotor.stop() rMotor.stop() print(str(LOOPS / (endTime - startTime)))
class Balance: def __init__(self, motor, sensor, midPoint): self.motor = LargeMotor(motor) self.sensor = UltrasonicSensor(sensor) self.midPoint = midPoint self.motorPositionUp = self.motor.position - 20 self.motorPositionDown = self.motor.position + 20 self.integral = 0 self.integralCount = 0 def getSensorState(self): """ Returns sensor measure based on an axis positioned on the mid point """ return -1 * (self.sensor.distance_centimeters - self.midPoint) def rotateAxis(self, speed): """ Rotates axis on the tacho motor by speed value """ # Limits rotation to limit object speed if self.motor.position >= self.motorPositionDown and speed > 0: self.motor.stop(stop_action="brake") elif self.motor.position <= self.motorPositionUp and speed < 0: self.motor.stop(stop_action="brake") else: #print(speed) self.motor.on(speed, block=False) def calculateSpeed(self, Kp=1, Kd=0, Ki=0, verbose=False): """ Calculates speed in next cicle based on a PID control """ initialMeasure = self.getSensorState() sleep(0.015) finalMeasure = self.getSensorState() # Fix closeness to the sensor if finalMeasure < -30: finalMeasure = 8.4 speed = finalMeasure - initialMeasure if -3 < finalMeasure < 3 and (self.integral <= 10 and self.integral >= -10 and self.integralCount < 10): self.integral = self.integral + (finalMeasure) self.integralCount = self.integralCount + 1 else: self.integral = 0 self.integralCount = 0 value = (Kp * finalMeasure) + (Kd * speed) + self.integral * Ki # Fix unbalanced values if value > 100: value = 100.0 if value < -100: value = -100.0 #print("\n-Distancia: %f, Velocidad %f, Integracion %f" % (finalMeasure, speed, self.integral)) return value / 3.2