-
Notifications
You must be signed in to change notification settings - Fork 0
/
touch_ctrl.py
139 lines (113 loc) · 3.8 KB
/
touch_ctrl.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
127
128
129
130
131
132
133
134
135
136
137
138
from serial import *
from sys import version_info
import time
import Maestro
class Done(Exception): pass
class Instruction():
flag = True
def __init__(self, target, target_val=6000, run_time=1):
self.target = target
self.target_val = target_val
self.run_time = run_time
self.textToSpeak = None
self.speechToText = None
def setTargetVal(self, target_val):
self.target_val = target_val
def setRunTime(self, run_time):
self.run_time = run_time
def changePos(self, array_pos):
instruct_arr[self.array_pos] = None
instruct_arr[array_pos] = self
self.array_pos = array_pos
def runInstruction(self, maestro):
if self.target == 7: #Special STT instruction
if self.speechToText == "GoHome":
pass
elif self.speechToText == "SpinCircle":
self.spinCircle(maestro)
else:
maestro.setTarget(self.target, self.target_val)
time.sleep(self.run_time)
slowDown(self.target, self.target_val, maestro)
def spinCircle(self, maestro):
print("---------------- Spin circle was called -------------")
maestro.setTarget(2, 5000)
time.sleep(3.5)
slowDown(2, 5000, maestro)
def getInstruction():
i = 0
while Instruction.instruct_arr[i] != None:
i = i + 1
user_in = input("Instruction: ")
# Add instruction to Instruction array
if user_in == "move":
Instruction.instruct_arr[i] = Instruction(i, 1, target_val=6000)
elif user_in == "turn":
Instruction.instruct_arr[i] = Instruction(i, 2, target_val=6000, run_time=1.5)
elif user_in == "quit":
Instruction.flag = False
raise Done
# Change instruction values
else:
split = user_in.split(' ')
inst = Instruction.instruct_arr[(int)(split[0])]
val = (int)(split[2])
if split[1] == "setTargetVal":
inst.setTargetVal(val)
elif split[1] == "setRunTime":
inst.setRunTime(val)
increment = 250
#Slow down motion on servo X going curSpeed by increment
def slowDown(servo, curSpeed, maestro):
if(servo == 1 or servo ==2):
forward = curSpeed < 6000
timeDelay = .05
decrementSpeed = curSpeed
if(forward):
while(decrementSpeed < 6000):
decrementSpeed = decrementSpeed + increment
maestro.setTarget(servo, decrementSpeed)
time.sleep(timeDelay)
maestro.setTarget(servo, 6000)
else:
while(decrementSpeed > 6000):
decrementSpeed = decrementSpeed - increment
maestro.setTarget(servo, decrementSpeed)
time.sleep(timeDelay)
maestro.setTarget(servo, 6000)
if __name__ == '__main__':
stdval = 6000
curvalFB = 6000
curvalLR = 6000
headtilt = 6000
headturn = 6000
x = Maestro.Controller()
x.setTarget(0, stdval)
x.setTarget(1, stdval)
x.setTarget(2, stdval)
x.setTarget(3, stdval)
x.setTarget(4, stdval)
# print("List of instructions:")
# print("Move: move")
# print("Turn: turn")
# print("Quit: quit")
# print()
# print("Change target_val: n setTargetVal n")
# print("Change run_time: n setRunTime n")
while Instruction.flag:
try:
# Break from while loop if we have 8 instructions
for i in range(8):
if Instruction.instruct_arr[i] == None:
break
if i == 7:
raise Done
getInstruction()
except Done:
break
runInstructions(x)
x.setTarget(0, stdval)
slowDown(1, curvalFB)
slowDown(2, curvalLR)
x.setTarget(3, stdval)
x.setTarget(4, stdval)