-
Notifications
You must be signed in to change notification settings - Fork 0
/
turning.py
86 lines (69 loc) · 3.09 KB
/
turning.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
#!/usr/bin/env pybricks-micropython
from robotsetup import ev3, driver, door, r_color, l_color, gyro, l_DriveMotor, r_DriveMotor
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
from linefollow import followBlack
import time
from robotsetup import ev3, driver, door, r_color, l_color, gyro, l_DriveMotor, r_DriveMotor, dumper
#######################################################################
## Run Code Starts Here ##
#######################################################################
def gyroRight (degrees):
startingAngle = gyro.angle()
finishAngle = startingAngle + degrees
print("Right --> Starting at: " + str(startingAngle) + " need to turn " + str(degrees) + " Will stop at: " + str(finishAngle))
turnSpeed = 150
while gyro.angle() <= finishAngle:
r_DriveMotor.run(-1 * turnSpeed)
l_DriveMotor.run(turnSpeed)
driver.stop()
def gyroLeft (degrees):
startingAngle = gyro.angle()
finishAngle = startingAngle - degrees
print("Left --> Starting at: " + str(startingAngle) + " need to turn " + str(degrees) + " Will stop at: " + str(finishAngle))
turnSpeed = 150
while gyro.angle() >= finishAngle:
r_DriveMotor.run(turnSpeed)
l_DriveMotor.run(-1 * turnSpeed)
driver.stop()
def trimExtraDegrees ():
currentAngle = gyro.angle()
print("Starting Trim at: " + str(currentAngle))
while currentAngle > 360:
currentAngle -= 360
while currentAngle < -360:
currentAngle += 360
gyro.reset_angle(currentAngle)
print("Returning angle: " + str(gyro.angle()))
return gyro.angle()
def gyroTurnTo (degree):
currentAngle = trimExtraDegrees()
while gyro.angle() != degree:
currentAngle = gyro.angle()
if currentAngle < degree:
if (degree-currentAngle) <= 180:
gyroRight(degree-currentAngle)
else:
gyroLeft((degree-180)+currentAngle)
else:
if (currentAngle-degree) <= 180:
gyroLeft(currentAngle-degree)
else:
gyroRight((360-currentAngle)+degree)
time.sleep(1)
def gyroRightTo (degree):
currentAngle = trimExtraDegrees()
if degree > currentAngle:
gyroRight(degree-currentAngle)
else:
gyroRight(degree+currentAngle)
def gyroLeftTo (degree):
currentAngle = trimExtraDegrees()
if currentAngle > degree:
gyroLeft(currentAngle-degree)
else:
gyroLeft(currentAngle-degree)