Skip to content

Commit 2eb5299

Browse files
hairygaelhairygael
authored
Refactor right hand sensor script for clarity
1 parent 146487b commit 2eb5299

1 file changed

Lines changed: 83 additions & 206 deletions

File tree

‎system/startScripts/i01_rightHandSensor_start.py‎

Lines changed: 83 additions & 206 deletions
Original file line numberDiff line numberDiff line change
@@ -8,239 +8,116 @@
88
import os
99
ThisServicePart = 'data/InMoov2/i01.rightHand.sensor.config'
1010

11-
def CheckFileExistRight(File):
12-
if not os.path.isfile(File):
13-
execfile('resource/InMoov2/system/startScripts/i01_rightHandSensor_config.py')
14-
i01.info("config file created : data/InMoov2/i01.rightHand.sensor.config")
11+
def CheckFileExistRight(filename):
12+
if not os.path.isfile(filename):
13+
execfile('resource/InMoov2/system/startScripts/i01_rightHandSensor_config.py')
14+
i01.info("config file created : %s" % filename)
1515

1616
CheckFileExistRight(ThisServicePart)
1717
ThisServicePartConfig = ConfigParser.ConfigParser()
1818
ThisServicePartConfig.read(ThisServicePart)
1919

20-
right_thumbPin=ThisServicePartConfig.getint('MAIN', 'right_thumbPin')
21-
right_indexPin=ThisServicePartConfig.getint('MAIN', 'right_indexPin')
22-
right_majeurePin=ThisServicePartConfig.getint('MAIN', 'right_majeurePin')
23-
right_ringFingerPin=ThisServicePartConfig.getint('MAIN', 'right_ringFingerPin')
24-
right_pinkyPin=ThisServicePartConfig.getint('MAIN', 'right_pinkyPin')
25-
26-
right_thumb_Psi_min=ThisServicePartConfig.getint('MAIN', 'right_thumb_Psi_min')
27-
right_thumb_Psi_low=ThisServicePartConfig.getint('MAIN', 'right_thumb_Psi_low')
28-
right_thumb_Psi_mid=ThisServicePartConfig.getint('MAIN', 'right_thumb_Psi_mid')
29-
right_thumb_Psi_max=ThisServicePartConfig.getint('MAIN', 'right_thumb_Psi_max')
30-
31-
right_index_Psi_min=ThisServicePartConfig.getint('MAIN', 'right_index_Psi_min')
32-
right_index_Psi_low=ThisServicePartConfig.getint('MAIN', 'right_index_Psi_low')
33-
right_index_Psi_mid=ThisServicePartConfig.getint('MAIN', 'right_index_Psi_mid')
34-
right_index_Psi_max=ThisServicePartConfig.getint('MAIN', 'right_index_Psi_max')
35-
36-
right_majeure_Psi_min=ThisServicePartConfig.getint('MAIN', 'right_majeure_Psi_min')
37-
right_majeure_Psi_low=ThisServicePartConfig.getint('MAIN', 'right_majeure_Psi_low')
38-
right_majeure_Psi_mid=ThisServicePartConfig.getint('MAIN', 'right_majeure_Psi_mid')
39-
right_majeure_Psi_max=ThisServicePartConfig.getint('MAIN', 'right_majeure_Psi_max')
40-
41-
right_ringFinger_Psi_min=ThisServicePartConfig.getint('MAIN', 'right_ringFinger_Psi_min')
42-
right_ringFinger_Psi_low=ThisServicePartConfig.getint('MAIN', 'right_ringFinger_Psi_low')
43-
right_ringFinger_Psi_mid=ThisServicePartConfig.getint('MAIN', 'right_ringFinger_Psi_mid')
44-
right_ringFinger_Psi_max=ThisServicePartConfig.getint('MAIN', 'right_ringFinger_Psi_max')
45-
46-
right_pinky_Psi_min=ThisServicePartConfig.getint('MAIN', 'right_pinky_Psi_min')
47-
right_pinky_Psi_low=ThisServicePartConfig.getint('MAIN', 'right_pinky_Psi_low')
48-
right_pinky_Psi_mid=ThisServicePartConfig.getint('MAIN', 'right_pinky_Psi_mid')
49-
right_pinky_Psi_max=ThisServicePartConfig.getint('MAIN', 'right_pinky_Psi_max')
50-
51-
A0=right_thumbPin
52-
A1=right_indexPin
53-
A2=right_majeurePin
54-
A3=right_ringFingerPin
55-
A4=right_pinkyPin
56-
57-
# Used by finger sensors
58-
# Right Hand
59-
global rightThumbPressure
60-
rightThumbPressure=0
61-
global rightIndexPressure
62-
rightIndexPressure=0
63-
global rightMajeurePressure
64-
rightMajeurePressure=0
65-
global rightRingFingerPressure
66-
rightRingFingerPressure=0
67-
global rightPinkyPressure
68-
rightPinkyPressure=0
20+
FINGERS = [
21+
"thumb",
22+
"index",
23+
"majeure",
24+
"ringFinger",
25+
"pinky"
26+
]
27+
28+
pins = {}
29+
psi = {}
30+
31+
for finger in FINGERS:
32+
pins[finger] = ThisServicePartConfig.getint(
33+
'MAIN',
34+
'right_%sPin' % finger
35+
)
36+
37+
psi[finger] = {
38+
'min': ThisServicePartConfig.getint('MAIN', 'right_%s_Psi_min' % finger),
39+
'low': ThisServicePartConfig.getint('MAIN', 'right_%s_Psi_low' % finger),
40+
'mid': ThisServicePartConfig.getint('MAIN', 'right_%s_Psi_mid' % finger),
41+
'max': ThisServicePartConfig.getint('MAIN', 'right_%s_Psi_max' % finger)
42+
}
43+
44+
pressure = {}
45+
46+
for finger in FINGERS:
47+
pressure[finger] = 0
48+
49+
finger_servos = {
50+
"thumb": i01_rightHand_thumb,
51+
"index": i01_rightHand_index,
52+
"majeure": i01_rightHand_majeure,
53+
"ringFinger": i01_rightHand_ringFinger,
54+
"pinky": i01_rightHand_pinky
55+
}
56+
57+
pin_map = {
58+
"A0": "thumb",
59+
"A1": "index",
60+
"A2": "majeure",
61+
"A3": "ringFinger",
62+
"A4": "pinky"
63+
}
64+
65+
def stopFinger(finger, level):
66+
finger_servos[finger].stop()
67+
finger_servos[finger].disable()
68+
print "---Stopping %s %sSensorPin" % (level, finger)
6969

7070
if runtime.isStarted('i01.right'):
7171
rightHandSensorStarted = True
72-
# configFilename="data/InMoov2/i01.life.yml"
73-
# # open the file
74-
# file = open(configFilename, "r")
75-
# # read the file
76-
# text = file.read()
77-
# # search & replace the word
78-
# replaced_text = text.replace("rightHandSensorStarted=False", "rightHandSensorStarted=True")
79-
80-
# # save the file
81-
# file = open(configFilename, "w")
82-
# file.write(replaced_text)
83-
# file.close()
84-
85-
# execfile('resource/InMoov2/life/0_inmoovLife.py')
8672

8773
try:
88-
# common right pin listener function
8974
def publishPinRight(pinsRight):
90-
#print(pinsRight)
91-
92-
global rightThumbPressure
93-
global rightIndexPressure
94-
global rightMajeurePressure
95-
global rightRingFingerPressure
96-
global rightPinkyPressure
9775

98-
#print (rightIndexPressure)
99-
76+
for p in pinsRight:
77+
78+
finger = pin_map.get(p.pin)
79+
80+
if finger is None:
81+
continue
10082

101-
102-
for pin in range(0, len(pinsRight)):
103-
if pinsRight[pin].pin==("A0"):
104-
#if pinsRight[pin].value<=(right_thumb_Psi_min) and pinsRight[pin].value<(right_thumb_Psi_low):rightThumbPressure=0
105-
if pinsRight[pin].value>=(right_thumb_Psi_low) and pinsRight[pin].value<(right_thumb_Psi_mid):
106-
if rightThumbPressure==1:
107-
i01_rightHand_thumb.stop()
108-
i01_rightHand_thumb.disable()
109-
print "---Stopping 1 rightThumbSensorPin"
110-
rightThumbPressure=0
111-
if pinsRight[pin].value>=(right_thumb_Psi_mid):
112-
if rightThumbPressure<=2:
113-
i01_rightHand_thumb.stop()
114-
i01_rightHand_thumb.disable()
115-
print "---Stopping 2 rightThumbSensorPin"
116-
rightThumbPressure=0
117-
if pinsRight[pin].value>=(right_thumb_Psi_max):
118-
if rightThumbPressure<=3:
119-
i01_rightHand_thumb.stop()
120-
i01_rightHand_thumb.disable()
121-
print "---Stopping 3 rightThumbSensorPin"
122-
rightThumbPressure=0
123-
print pinsRight[pin].value,"Value:",right_thumbPin,"rightThumbSensorPin:"
124-
#print "rightThumbSensorPin:",right_thumbPin,"Value:",pinsRight[pin].value
83+
value = p.value
84+
cfg = psi[finger]
12585

126-
if pinsRight[pin].pin==("A1"):
127-
#if pinsRight[pin].value<(right_index_Psi_low):rightIndexPressure=0
128-
if pinsRight[pin].value>=(right_index_Psi_low) and pinsRight[pin].value<(right_index_Psi_mid):
129-
if rightIndexPressure==1:
130-
i01_rightHand_index.stop()
131-
i01_rightHand_index.disable()
132-
print "---Stopping 1 rightIndexSensorPin"
133-
rightIndexPressure=0
134-
if pinsRight[pin].value>=(right_index_Psi_mid):
135-
if rightIndexPressure<=2:
136-
i01_rightHand_index.stop()
137-
i01_rightHand_index.disable()
138-
print "---Stopping 2 rightIndexSensorPin"
139-
rightIndexPressure=0
140-
if pinsRight[pin].value>=(right_index_Psi_max):
141-
if rightIndexPressure<=3:
142-
i01_rightHand_index.stop()
143-
i01_rightHand_index.disable()
144-
print "---Stopping 3 rightIndexSensorPin"
145-
rightIndexPressure=0
146-
print pinsRight[pin].value,"Value:",right_indexPin,"rightIndexSensorPin:"
86+
if value >= cfg['max']:
87+
if pressure[finger] <= 3:
88+
stopFinger(finger, 3)
89+
pressure[finger] = 0
14790

148-
if pinsRight[pin].pin==("A2"):
149-
#if pinsRight[pin].value<=(right_majeure_Psi_min) and pinsRight[pin].value<(right_majeure_Psi_low):rightMajeurePressure=0
150-
if pinsRight[pin].value>=(right_majeure_Psi_low) and pinsRight[pin].value<(right_majeure_Psi_mid):
151-
if rightMajeurePressure==1:
152-
i01_rightHand_majeure.stop()
153-
i01_rightHand_majeure.disable()
154-
print "---Stopping 1 rightMajeureSensorPin"
155-
rightMajeurePressure=0
156-
if pinsRight[pin].value>=(right_majeure_Psi_mid):
157-
if rightMajeurePressure<=2:
158-
i01_rightHand_majeure.stop()
159-
i01_rightHand_majeure.disable()
160-
print "---Stopping 2 rightMajeureSensorPin"
161-
rightMajeurePressure=0
162-
if pinsRight[pin].value>=(right_majeure_Psi_max):
163-
if rightMajeurePressure<=3:
164-
i01_rightHand_majeure.stop()
165-
i01_rightHand_majeure.disable()
166-
print "---Stopping 3 rightMajeureSensorPin"
167-
rightMajeurePressure=0
168-
print pinsRight[pin].value,"Value:",right_majeurePin,"rightMajeureSensorPin:"
91+
elif value >= cfg['mid']:
92+
if pressure[finger] <= 2:
93+
stopFinger(finger, 2)
94+
pressure[finger] = 0
16995

170-
if pinsRight[pin].pin==("A3"):
171-
#if pinsRight[pin].value<=(right_ringFinger_Psi_min) and pinsRight[pin].value<(right_ringFinger_Psi_low):rightRingFingerPressure=0
172-
if pinsRight[pin].value>=(right_ringFinger_Psi_low) and pinsRight[pin].value==(right_ringFinger_Psi_mid):
173-
if rightRingFingerPressure==1:
174-
print(rightRingFingerPressure)
175-
i01_rightHand_ringFinger.stop()
176-
i01_rightHand_ringFinger.disable()
177-
print "---Stopping 1 rightRingFingerSensorPin"
178-
rightRingFingerPressure=0
179-
if pinsRight[pin].value>=(right_ringFinger_Psi_mid):
180-
if rightRingFingerPressure<=2:
181-
print(rightRingFingerPressure)
182-
i01_rightHand_ringFinger.stop()
183-
i01_rightHand_ringFinger.disable()
184-
print "---Stopping 2 rightRingFingerSensorPin"
185-
rightRingFingerPressure=0
186-
if pinsRight[pin].value>=(right_ringFinger_Psi_max):
187-
if rightRingFingerPressure<=3:
188-
print(rightRingFingerPressure)
189-
i01_rightHand_ringFinger.stop()
190-
i01_rightHand_ringFinger.disable()
191-
print "---Stopping 3 rightRingFingerSensorPin"
192-
rightRingFingerPressure=0
193-
print pinsRight[pin].value,"Value:",right_ringFingerPin,"rightRingFingerSensorPin:"
96+
elif value >= cfg['low']:
97+
if pressure[finger] == 1:
98+
stopFinger(finger, 1)
99+
pressure[finger] = 0
194100

195-
if pinsRight[pin].pin==("A4"):
196-
#if pinsRight[pin].value<=(right_pinky_Psi_min) and pinsRight[pin].value<(right_pinky_Psi_low):rightPinkyPressure=0
197-
if pinsRight[pin].value>=(right_pinky_Psi_low) and pinsRight[pin].value<(right_pinky_Psi_mid):
198-
if rightPinkyPressure==1:
199-
i01_rightHand_pinky.stop()
200-
i01_rightHand_pinky.disable()
201-
print "---Stopping 1 rightPinkySensorPin"
202-
rightPinkyPressure=0
203-
if pinsRight[pin].value>=(right_pinky_Psi_mid) and pinsRight[pin].value<(right_pinky_Psi_max):
204-
if rightPinkyPressure<=2:
205-
i01_rightHand_pinky.stop()
206-
i01_rightHand_pinky.disable()
207-
print "---Stopping 2 rightPinkySensorPin"
208-
rightPinkyPressure=0
209-
if pinsRight[pin].value>=(right_pinky_Psi_max):
210-
if rightPinkyPressure<=3:
211-
i01_rightHand_pinky.stop()
212-
i01_rightHand_pinky.disable()
213-
print "---Stopping 3 rightPinkySensorPin"
214-
rightPinkyPressure=0
215-
print pinsRight[pin].value,"Value:",right_pinkyPin,"rightPinkySensorPin:"
216-
print "-----------Right-Finger-Sensors-----------"
101+
print("Value:", value, "Pin:", pins[finger], finger)
217102

218103
if runtime.isStarted('i01.chatBot'):
219104
i01_chatBot.getResponse("SYSTEM_EVENT STARTED RIGHT HAND SENSOR")
220-
i01_right.addListener("publishPinArray","python","publishPinRight")
105+
i01_right.addListener("publishPinArray","python","publishPinRight")
221106

222107
def rightHandSensorON():
223108
if rightHandSensorStarted==1:
224-
print "=========RightSensorON========"
225-
i01_right.enablePin(right_thumbPin, 2) #2 is the number of polls per seconds
226-
i01_right.enablePin(right_indexPin, 2)
227-
i01_right.enablePin(right_majeurePin, 2)
228-
i01_right.enablePin(right_ringFingerPin, 2)
229-
i01_right.enablePin(right_pinkyPin, 2)
230-
else:
231-
pass
109+
print "=========RightSensorON========"
110+
111+
for pin in pins.values():
112+
i01_right.enablePin(pin, 1)#2 is the number of polls per seconds
232113

233114
def rightHandSensorOFF():
234-
#sleep(5)
235115
if rightHandSensorStarted==1:
236-
i01_right.disablePin(right_thumbPin)
237-
i01_right.disablePin(right_indexPin)
238-
i01_right.disablePin(right_majeurePin)
239-
i01_right.disablePin(right_ringFingerPin)
240-
i01_right.disablePin(right_pinkyPin)
116+
117+
for pin in pins.values():
118+
i01_right.disablePin(pin)
119+
241120
print "=========RightSensorOFF======="
242-
else:
243-
pass
244121

245122
except:
246123
i01.error('could not start right hand sensor')

0 commit comments

Comments
 (0)