-
Notifications
You must be signed in to change notification settings - Fork 0
/
Radley Robots Drive and Arm Code.py
65 lines (45 loc) · 1.39 KB
/
Radley Robots Drive and Arm Code.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
# ARC embedded Python with different conventions, including semicolons
while(1==1):
SpeedLeft = (getVar("$JoystickY1")*-100) - (getVar("$JoystickX1")*-100);
SpeedRight = (getVar("$JoystickY1")*-100) + (getVar("$JoystickX1")*-100);
if(SpeedLeft > 100):
PWM.Set(D5, 100);
SpeedLeft = 100;
if(SpeedLeft > 17):
Digital.Set(D6, 1);
Digital.Set(D7, 0);
PWM.Set(D5, int(SpeedLeft));
if(-17 < SpeedLeft < 17):
Digital.Set(D6, 0);
Digital.Set(D7, 0);
if(SpeedLeft < -100):
PWM.Set(D5, 100);
SpeedLeft = -100;
if(SpeedLeft <= -17):
Digital.Set(D6, 0);
Digital.Set(D7, 1);
PWM.Set(D5, int(SpeedLeft*-1));
if(SpeedRight > 100):
PWM.Set(D5, 100);
SpeedRight = 100;
if(SpeedRight > 17):
Digital.Set(D8, 1);
Digital.Set(D9, 0);
PWM.Set(D10, int(SpeedRight));
if(-17 < SpeedRight < 17):
Digital.Set(D8, 0);
Digital.Set(D9, 0);
if(SpeedRight < -100):
PWM.Set(D5, 100);
SpeedRight = -100;
if(SpeedRight <= -17):
Digital.Set(D8, 0);
Digital.Set(D9, 1);
PWM.Set(D10, int(SpeedRight*-1));
if(getVar("$JoystickX1")*-100 < -55):
SpeedRight = 0;
PWM.Set(D10, 0);
if(getVar("$JoystickX1")*-100 > 55):
SpeedLeft = 0;
PWM.Set(D5, 0);
print(SpeedLeft, SpeedRight);