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():
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")
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")
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()
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()
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)
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
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 < 1:
if GY.value() == starting_value:
left_wheel_speed = 100
right_wheel_speed = 100
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() / 2
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = 100
right_wheel_speed = 100
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 = 100
right_wheel_speed = 100
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
t.start()
sleep(1)
while (True):
while ((z['0'] == 1 or q['0'] == 1) and q['0'] != 2 and q['0'] != 3 ):
error['0'] = target['0'] - int(cs.value())
integ['0'] += error['0']
deriv['0'] = error['0'] - lastError['0']
# u zero: on target, drive forward
# u positive: too bright, turn right
# u negative: too dark, turn left
u['0'] = Kp['0']*error['0'] + Ki['0']*integ['0'] + Kd['0']*deriv['0']
lastError['0'] = error['0']
if(u['0'] >= 0):
lm.run_forever(speed_sp=200+u['0'], stop_action="hold")
lm2.run_forever(speed_sp=200-u['0'], stop_action="hold")
else:
lm.run_forever(speed_sp=200-u['0'], stop_action="hold")
lm2.run_forever(speed_sp=200+u['0'], stop_action="hold")
if (z['0'] == 2 and q['0'] == 0):
while (z['0'] != 1 or q['0'] != 1):
lm.run_to_rel_pos(position_sp=0, stop_action="hold")
lm2.run_to_rel_pos(position_sp=1, speed_sp=400, stop_action="hold")
continue
elif (z['0'] == 3 and q['0'] == 0):
while (z['0'] != 1):
lm.run_to_rel_pos(position_sp=1, speed_sp=400, stop_action="hold")
lm2.run_to_rel_pos(position_sp=0, stop_action="hold")
continue
if (cs2.value() < 40):
>>>>>>> dad229498927371537254677a6bb6fc1881c73fc
cs2_black['0'] = True
if (cs2_black['0'] and cs_black['0'] == False):
i['0'] = 2
else:
cs2_black['0'] = False
t = Thread(target=notBlack, args=(cs,cs2))
t.start()
#keyboard.is_pressed('ENTER') !=
sleep(1)
while(True):
sleep(0.2)
while(cs_black['0'] or cs2_black['0']):
lm.run_forever(speed_sp=250, stop_action="hold")
lm2.run_forever(speed_sp=250, stop_action="hold")
# lm.run_to_rel_pos(position_sp=50, speed_sp=SpeedPercent(40), stop_action="hold")
# lm2.run_to_rel_pos(position_sp=50, speed_sp=SpeedPercent(40), stop_action="hold")
if(i['0']==1):
if(cs_black['0'] or cs2_black['0']):
pass
else:
while(cs_black['0']==False and cs2_black['0']==False):
<<<<<<< HEAD
lm.run_to_rel_pos(position_sp=0, stop_action="hold")
lm2.run_to_rel_pos(position_sp=4, speed_sp=900, stop_action="hold")
#lm.run_forever(speed_sp=0, stop_action="hold")
#lm2.run_forever(speed_sp=SpeedPercent(100), stop_action="hold")
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)'''
motorC.run_forever(speed_sp = -250)
time.sleep(6)
motorC.stop()
motorC.run_forever(speed_sp = 250)
time.sleep(10)
motorC.stop(stop_action = 'hold')
def main():
# start sensor and motor definitions
Sound.speak("").wait()
MA = MediumMotor("")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
# end sensor and motor definitions
# start calibration
GY.mode = 'GYRO-ANG'
GY.mode = 'GYRO-RATE'
GY.mode = 'GYRO-ANG'
# end calibration
# The following line would be if we used tank_drive
# tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
# start definition of driving parameters
loop_gyro = 0
starting_value = GY.value()
# end definition of driving parameters
# set initial speed parameters
speed = 20
adjust = 1
# change 999999999999 to however you want to go
while loop_gyro < 999999999999:
# while Gyro value is the same as the starting value, then go straigt.
while GY.value() == starting_value:
left_wheel_speed = speed
right_wheel_speed = speed
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
return
# while greater than starting value, then go left.
while GY.value() > starting_value:
left_wheel_speed = speed - adjust
right_wheel_speed = speed
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
return
# while less than starting value, then go right.
while GY.value() < starting_value:
left_wheel_speed = speed + adjust
right_wheel_speed = speed
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
return
return
car_direction = Direction(LargeMotor(OUTPUT_D))
line_sensor = LineSensor(ColorSensor(INPUT_4), ColorSensor(INPUT_1))
leds = LED()
speed_motor = LargeMotor(OUTPUT_A)
us = UltrasonicSensor(INPUT_3)
us.mode='US-DIST-CM'
units=us.units
speed_reduction = 1
steering_block = 0
direction = 0
while True:
provisional_direction = line_sensor.getMovement()
direction = provisional_direction if steering_block < 3.5 else direction
speed = getSpeed(us.value()/10)
if line_detected(provisional_direction): # Detecta LÃnea
steering_block = steering_block + 0.5 if steering_block + 0.5 <= 5 and (provisional_direction < -40 or provisional_direction > 40) else steering_block
elif not line_detected(provisional_direction): # No detecta lÃnea
steering_block = steering_block - 0.1 if steering_block - 0.1 >= 0 else steering_block
if line_detected(direction): # Detecta LÃnea
speed_reduction = speed_reduction + 0.5 if speed_reduction + 0.5 <= 5 else speed_reduction
elif not line_detected(direction): # No detecta lÃnea
speed_reduction = speed_reduction - 0.3 if speed_reduction - 0.3 >= 1 else speed_reduction
speed = speed / speed_reduction
speed_motor.run_forever(speed_sp = -MAX_SPEED*speed)
car_direction.steerToDeg(direction)
leds.updateLeds()
#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, SpeedPercent
from time import sleep
print("running")
m = LargeMotor(OUTPUT_A)
# while(True):
# m.run_forever()
# m.on_for_rotations(SpeedPercent(75), 100)
# m.speed_sp = 1000
m.run_forever(speed_sp=1000)
# m.on(100)
while True:
pass
print("ending")
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()
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 main():
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
# 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 > -750:
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
# pulling away from stacks. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 1.75)
# gyro 90 degree spin turn
while GY.value() < 50:
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)
# 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()
def stan(cs, cs2):
while (True):
if (cs.value() < 33 and cs2.value() < 33):
k['0'] = 1
elif (cs.value() < 33 and cs2.value() >= 33):
k['0'] = 2
elif (cs.value() >= 33 and cs2.value() < 33):
k['0'] = 3
elif (cs.value() >= 33 and cs2.value() >= 33):
k['0'] = 4
t = Thread(target=stan, args=(cs, cs2))
t.start()
sleep(1)
while (True):
while (k['0'] == 1):
sleep(0.2)
lm.run_forever(speed_sp=200, stop_action="brake")
lm2.run_forever(speed_sp=200, stop_action="brake")
if (k['0'] == 2):
while (k['0'] != 1):
lm.run_to_rel_pos(position_sp=0, stop_action="hold")
lm2.run_to_rel_pos(position_sp=3, speed_sp=600, stop_action="hold")
elif (k['0'] == 3):
while (k['0'] != 1):
lm.run_to_rel_pos(position_sp=3, speed_sp=600, stop_action="hold")
lm2.run_to_rel_pos(position_sp=0, stop_action="hold")
error = target_value - cs.value()
integral += (error * dt)
derivative = (error - previous_error) / dt
u = (Kp * error) + (Ki * integral) + (Kd * derivative)
t = Thread(target=notBlack, args=(cs, cs2))
t.start()
sleep(1)
while (True):
while (cs_black['0'] or cs2_black['0']):
sleep(1)
lm.run_forever(speed_sp=SpeedPercent(100), stop_action="hold")
lm2.run_forever(speed_sp=SpeedPercent(100), 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=6, speed_sp=300, stop_action="hold")
lm.run_to_rel_pos(position_sp=0, stop_action="hold")
lm2.run_to_rel_pos(position_sp=15, speed_sp=600, stop_action="hold")
elif (i['0'] == 2):
while (cs_black['0'] == False and cs2_black['0'] == False):
lm.run_to_rel_pos(position_sp=6, speed_sp=300, stop_action="hold")
while (cs_black['0'] or cs2_black['0']):
sleep(1)
error = target_value - cs.value()
integral += (error * dt)
derivative = (error - previous_error) / dt
u = (Kp * error) + (Ki * integral) + (Kd * derivative)
previous_error = error
if speed + abs(u) > 1000:
if u >= 0:
u = 1000 - speed
else:
u = speed - 1000
if u >= 0:
lm.run_forever(speed_sp=speed+u, stop_action="hold")
lm2.run_forever(speed_sp=speed-u, stop_action="hold")
else:
lm.run_forever(speed_sp=speed-u, stop_action="hold")
lm2.run_forever(speed_sp=speed+u, 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):
error = target_value - cs.value()
integral += (error * dt)
derivative = (error - previous_error) / dt
u = (Kp * error) + (Ki * integral) + (Kd * derivative)
previous_error = error
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()
# change this to whatever speed what you want. V
left_wheel_speed = -300
right_wheel_speed = -300
# if Gyro value is the same as the starting value, go straigt. if more turn right. if less turn left. V
# FIX THIS VALUE!!!!!!!!!! V
while MB.position >= -500: # consider adjusting the wheel speeds only if your current gyro value doesn't equal the starting value
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 = 1
else:
if GY.value() > starting_value:
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:
gyro_adjust = gyro_adjust + 1 # If gyro value has worsened despite the correction then change the adjust variable for next time
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
# wait for the block to drop. V
sleep(3)
# pulling away from the crane. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 0.25)
# Unlocking attachment. V
MD.on_for_degrees(SpeedPercent(50), 600)
# pulling away from unlocked attachment. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 1)
# gyro 90 degree spin turn
while GY.value() <= 45:
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)
def Gyro_straight():
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
tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01)
# change this to whatever speed what you want
while MB.position > -2000: # this is the gyro program, the first line tells the bot to continue loop until it reaches a defined position
if GY.value(
) == 0: #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() < 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 #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: #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:
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
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
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()
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)))
if (cs2.value() < 30):
cs2_black['0'] = True
if (cs2_black['0'] and cs_black['0'] == False):
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")
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()
# 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
# count_per_rot tells us the tacho count in one rotation of the motor.
while count_per_rot < 935
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 = 1
else:
if GY.value() > starting_value:
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
Sound.speak(count_per_rot)
loop_gyro = loop_gyro
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")