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
