-
Notifications
You must be signed in to change notification settings - Fork 0
/
lodz.py
126 lines (100 loc) · 3.14 KB
/
lodz.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent
from ev3dev2.sensor import INPUT_1,INPUT_2,INPUT_3,INPUT_4
from ev3dev2.sensor.lego import TouchSensor, LightSensor,UltrasonicSensor
from ev3dev2.button import Button
import os
from time import sleep
from random import randint, choice
from ev3dev2.led import Leds
os.system('setfont Lat15-TerminusBold14')
ultra = UltrasonicSensor(INPUT_1)
# mLT = LargeMotor(OUTPUT_A) #Silnik lewy tylny
# mLP = LargeMotor(OUTPUT_B) #Silnik lewy przedni
# mRT = LargeMotor(OUTPUT_C) #Silnik prawy tylny
# mRP = LargeMotor(OUTPUT_D) #Silnik prawy przedni
ramie = LargeMotor(OUTPUT_A);
napedLewy = LargeMotor(OUTPUT_C);
napedPrawy = LargeMotor(OUTPUT_D);
light = LightSensor(INPUT_2) #Czujnik koloru
leds = Leds()
button = Button()
def Rotate(POWER, TIME=.250):
mLT.run_forever(speed_sp=-POWER)
mLP.run_forever(speed_sp=(-POWER/1.667))
mRT.run_forever(speed_sp=POWER)
mRP.run_forever(speed_sp=(POWER/1.667))
sleep(TIME)
mLP.stop()
mLT.stop()
mRT.stop()
mRP.stop()
def motorOnRotations(LEFT_ROTATIONS,RIGHT_ROTATIONS,POWER):
mLT.on_for_rotations(SpeedPercent(POWER), LEFT_ROTATIONS)
mLP.on_for_rotations(SpeedPercent(POWER), LEFT_ROTATIONS*1.666667)
mRT.on_for_rotations(SpeedPercent(POWER), RIGHT_ROTATIONS)
mRP.on_for_rotations(SpeedPercent(POWER), RIGHT_ROTATIONS*1.666667)
def motorRunForever(POWER):
mLT.run_forever(speed_sp=-POWER)
mLP.run_forever(speed_sp=(-POWER/1.667))
mRT.run_forever(speed_sp=-POWER)
mRP.run_forever(speed_sp=(-POWER/1.667))
idleSpeed = 650
attackSpeed = 1050
baseDegrees = .950
def outOfBounds():
if light.reflected_light_intensity > 60:
rand = randint(0,1)
motorRunForever(-attackSpeed)
sleep(.4)
if rand == 0:
Rotate(attackSpeed, .5)
elif rand == 1:
Rotate(-attackSpeed, .5)
motorRunForever(idleSpeed)
return True
return False
while True:
leds.set_color("LEFT", "RED")
leds.set_color("RIGHT", "RED")
if tsP.is_pressed:
rand = choice((-1,1))
sleep(5)
Rotate(-attackSpeed, .200)
Rotate(attackSpeed, baseDegrees+rand*.100)
break
elif tsL.is_pressed:
rand = choice((-1,1))
sleep(5)
Rotate(attackSpeed, .200)
Rotate(-attackSpeed, baseDegrees+rand*.100)
break
motorRunForever(attackSpeed)
skan = True
while True:
if button.enter:
break
wart = ultra.distance_centimeters
if outOfBounds():
continue
elif tsL.is_pressed:
skan = True
Rotate(-attackSpeed, .5)
motorRunForever(attackSpeed)
elif tsP.is_pressed:
skan = True
Rotate(attackSpeed, .5)
motorRunForever(attackSpeed)
# if outOfBounds():
# continue
elif wart <= 20:
motorRunForever(attackSpeed)
skan = True
elif wart <= 55 and wart >= 30 and skan:
rand = randint(0,1)
if rand == 0:
Rotate(-attackSpeed,.15)
elif rand == 1:
Rotate(attackSpeed,.15)
motorRunForever(idleSpeed)
skan = False