-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.py
81 lines (65 loc) · 2.2 KB
/
main.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
from hub import motion_sensor, port
import math, motor, motor_pair, time
perror = 0
I = 0
r = 2.7
yaw = 0.0
kp = 0.15
ki = 0.00
kd = 0.8
tp = 0
ti = 0
td = 0
motor_pair.pair(motor_pair.PAIR_1, port.C, port.D)
'''
motor.run(port.A, 400)
motor.run(port.B,400)
'''
motor.reset_relative_position(port.C, 0)
motor.reset_relative_position(port.D, 0)
motion_sensor.reset_yaw(0)
def PID(kp, ki, kd, Error):
global perror
global I
P = Error
I = Error - I
D = Error - perror
perror = P
PID = P * kp + I * ki + D * kd
return PID
def drive(cm: int, velocity: int, angle: int):
motion_sensor.reset_yaw(0)
motor.reset_relative_position(port.C, 0)
motor.reset_relative_position(port.D, 0)
if cm > 0:
while (((motor.relative_position(port.C) * -1 + motor.relative_position(port.D)) / 360 * r * math.pi) < cm):
v = int(PID(kp, ki, kd, motion_sensor.tilt_angles()[0] - angle))
motor_pair.move_tank(motor_pair.PAIR_1, velocity + v, velocity - v)
print(motion_sensor.tilt_angles()[0])
motor_pair.stop(motor_pair.PAIR_1)
else:
while (((motor.relative_position(port.C) * -1 + motor.relative_position(port.D)) / 360 * r * math.pi) > cm):
v = int(PID(tp, ti, td, motion_sensor.tilt_angles()[0] - angle))
motor_pair.move_tank(motor_pair.PAIR_1, -(velocity - v), -(velocity + v))
print(motion_sensor.tilt_angles()[0])
motor_pair.stop(motor_pair.PAIR_1)
def reset_yaw():
motion_sensor.reset_yaw(0)
def turn(angle):
global yaw
reset_yaw()
angle = angle * 10
# שמאל
while yaw < angle - 1 or yaw > angle + 1:
motor_pair.move_tank(motor_pair.PAIR_1, int((angle - yaw) / 2.1 + 60) * 1, int(((angle - yaw) / 2.1 + 60)))
yaw = motion_sensor.tilt_angles()[0]
motor_pair.stop(motor_pair.PAIR_1)
return
def move_arm(selected_motor: str, degrees):
if selected_motor == 'a':
motor.run_for_degrees(port.A, degrees, 500)
if selected_motor == 'b':
motor.run_for_degrees(port.B, degrees, 500)
drive(3456789023456, 700, 0)
move_arm('a', 2000)
move_arm('b', -150)