summaryrefslogtreecommitdiff
path: root/robot/problems/introduction/rotateback/old_naloga11_rotateBack.py
blob: a14c91d6e652aecc066276e6d048a9503aaba950 (plain)
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
#!/usr/bin/python
# coding=utf-8
import sys
sys.path.append('/home/user/codeq')
import time
from ev3dev import *
from mindstorms_widgets import mindstorms_widgets

# the sensor should be perfectly still while resetting the gyro
def resetGyro(gyro):
    gyro.mode = 'GYRO-RATE'
    gyro.mode = 'GYRO-ANG'

def sgn(x):
    return -1 if x<0 else (1 if x>0 else 0)

gyro = gyro_sensor()
assert gyro.connected
gyro.mode = 'GYRO-ANG'

touch = touch_sensor()
assert touch.connected

robot = mindstorms_widgets()
robot.connect_motor( 'left' )
robot.connect_motor( 'right' )

smer = hitrost = 0
resetGyro(gyro)
prev_smer = -1
while 1:
    smer = gyro.value()
    if smer!=prev_smer:
        print ("Kot:", smer)
        prev_smer = smer
    
    if touch.value():
        resetGyro(gyro)
        print( gyro.value())
    
    if abs(smer) < 10:
        hitrost = 10*sgn(smer)
    else:
        if abs(smer) > 100:
            hitrost = 100*sgn(smer)
        else:
            hitrost = smer
    
    if abs(smer) > 1:
        L, R = -hitrost, hitrost
    else:
        L = R = 0
    robot.move_tank('on', lr_power=[L,R])