Skip to content

Commit ced6bac

Browse files
hairygaelhairygael
authored
Refactor left hand sensor script for clarity and efficiency
1 parent 2eb5299 commit ced6bac

1 file changed

Lines changed: 90 additions & 207 deletions

File tree

‎system/startScripts/i01_leftHandSensor_start.py‎

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

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

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

20-
left_thumbPin=ThisServicePartConfig.getint('MAIN', 'left_thumbPin')
21-
left_indexPin=ThisServicePartConfig.getint('MAIN', 'left_indexPin')
22-
left_majeurePin=ThisServicePartConfig.getint('MAIN', 'left_majeurePin')
23-
left_ringFingerPin=ThisServicePartConfig.getint('MAIN', 'left_ringFingerPin')
24-
left_pinkyPin=ThisServicePartConfig.getint('MAIN', 'left_pinkyPin')
25-
26-
left_thumb_Psi_min=ThisServicePartConfig.getint('MAIN', 'left_thumb_Psi_min')
27-
left_thumb_Psi_low=ThisServicePartConfig.getint('MAIN', 'left_thumb_Psi_low')
28-
left_thumb_Psi_mid=ThisServicePartConfig.getint('MAIN', 'left_thumb_Psi_mid')
29-
left_thumb_Psi_max=ThisServicePartConfig.getint('MAIN', 'left_thumb_Psi_max')
30-
31-
left_index_Psi_min=ThisServicePartConfig.getint('MAIN', 'left_index_Psi_min')
32-
left_index_Psi_low=ThisServicePartConfig.getint('MAIN', 'left_index_Psi_low')
33-
left_index_Psi_mid=ThisServicePartConfig.getint('MAIN', 'left_index_Psi_mid')
34-
left_index_Psi_max=ThisServicePartConfig.getint('MAIN', 'left_index_Psi_max')
35-
36-
left_majeure_Psi_min=ThisServicePartConfig.getint('MAIN', 'left_majeure_Psi_min')
37-
left_majeure_Psi_low=ThisServicePartConfig.getint('MAIN', 'left_majeure_Psi_low')
38-
left_majeure_Psi_mid=ThisServicePartConfig.getint('MAIN', 'left_majeure_Psi_mid')
39-
left_majeure_Psi_max=ThisServicePartConfig.getint('MAIN', 'left_majeure_Psi_max')
40-
41-
left_ringFinger_Psi_min=ThisServicePartConfig.getint('MAIN', 'left_ringFinger_Psi_min')
42-
left_ringFinger_Psi_low=ThisServicePartConfig.getint('MAIN', 'left_ringFinger_Psi_low')
43-
left_ringFinger_Psi_mid=ThisServicePartConfig.getint('MAIN', 'left_ringFinger_Psi_mid')
44-
left_ringFinger_Psi_max=ThisServicePartConfig.getint('MAIN', 'left_ringFinger_Psi_max')
45-
46-
left_pinky_Psi_min=ThisServicePartConfig.getint('MAIN', 'left_pinky_Psi_min')
47-
left_pinky_Psi_low=ThisServicePartConfig.getint('MAIN', 'left_pinky_Psi_low')
48-
left_pinky_Psi_mid=ThisServicePartConfig.getint('MAIN', 'left_pinky_Psi_mid')
49-
left_pinky_Psi_max=ThisServicePartConfig.getint('MAIN', 'left_pinky_Psi_max')
50-
51-
A0=left_thumbPin
52-
A1=left_indexPin
53-
A2=left_majeurePin
54-
A3=left_ringFingerPin
55-
A4=left_pinkyPin
56-
57-
# Used by finger sensors
58-
# Left Hand
59-
global leftThumbPressure
60-
leftThumbPressure=0
61-
global leftIndexPressure
62-
leftIndexPressure=0
63-
global leftMajeurePressure
64-
leftMajeurePressure=0
65-
global leftRingFingerPressure
66-
leftRingFingerPressure=0
67-
global leftPinkyPressure
68-
leftPinkyPressure=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+
'left_%sPin' % finger
35+
)
36+
37+
psi[finger] = {
38+
'min': ThisServicePartConfig.getint('MAIN', 'left_%s_Psi_min' % finger),
39+
'low': ThisServicePartConfig.getint('MAIN', 'left_%s_Psi_low' % finger),
40+
'mid': ThisServicePartConfig.getint('MAIN', 'left_%s_Psi_mid' % finger),
41+
'max': ThisServicePartConfig.getint('MAIN', 'left_%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_leftHand_thumb,
51+
"index": i01_leftHand_index,
52+
"majeure": i01_leftHand_majeure,
53+
"ringFinger": i01_leftHand_ringFinger,
54+
"pinky": i01_leftHand_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.left'):
7171
leftHandSensorStarted = 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("leftHandSensorStarted=False", "leftHandSensorStarted=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 left pin listener function
8974
def publishPinLeft(pinsLeft):
90-
#print(pinsLeft)
91-
92-
global leftThumbPressure
93-
global leftIndexPressure
94-
global leftMajeurePressure
95-
global leftRingFingerPressure
96-
global leftPinkyPressure
97-
98-
99-
100-
for pin in range(0, len(pinsLeft)):
101-
if pinsLeft[pin].pin==("A0"):
102-
#if pinsLeft[pin].value<=(left_thumb_Psi_min) and pinsLeft[pin].value<(left_thumb_Psi_low):leftThumbPressure=0
103-
if pinsLeft[pin].value>=(left_thumb_Psi_low)and pinsLeft[pin].value<(left_thumb_Psi_mid):
104-
if leftThumbPressure==1:
105-
i01_leftHand_thumb.stop()
106-
i01_leftHand_thumb.disable()
107-
print "---Stopping 1 leftThumbSensorPin"
108-
leftThumbPressure=0
109-
if pinsLeft[pin].value>=(left_thumb_Psi_mid):
110-
if leftThumbPressure<=2:
111-
i01_leftHand_thumb.stop()
112-
i01_leftHand_thumb.disable()
113-
print "---Stopping 2 leftThumbSensorPin"
114-
leftThumbPressure=0
115-
if pinsLeft[pin].value>=(left_thumb_Psi_max):
116-
if leftThumbPressure<=3:
117-
i01_leftHand_thumb.stop()
118-
i01_leftHand_thumb.disable()
119-
print "---Stopping 3 leftThumbSensorPin"
120-
leftThumbPressure=0
121-
print pinsLeft[pin].value,"Value:",left_thumbPin,"leftThumbSensorPin:"
122-
123-
if pinsLeft[pin].pin==("A1"):
124-
#if pinsLeft[pin].value<=(left_index_Psi_min) and pinsLeft[pin].value<(left_index_Psi_low):leftIndexPressure=0
125-
if pinsLeft[pin].value>=(left_index_Psi_low)and pinsLeft[pin].value<(left_index_Psi_mid):
126-
if leftIndexPressure==1:
127-
i01_leftHand_index.stop()
128-
i01_leftHand_index.disable()
129-
print "---Stopping 1 leftIndexSensorPin"
130-
leftIndexPressure=0
131-
if pinsLeft[pin].value>=(left_index_Psi_mid):
132-
if leftIndexPressure<=2:
133-
i01_leftHand_index.stop()
134-
i01_leftHand_index.disable()
135-
print "---Stopping 2 leftIndexSensorPin"
136-
leftIndexPressure=0
137-
if pinsLeft[pin].value>=(left_index_Psi_max):
138-
if leftIndexPressure<=3:
139-
i01_leftHand_index.stop()
140-
i01_leftHand_index.disable()
141-
print "---Stopping 3 leftIndexSensorPin"
142-
leftIndexPressure=0
143-
print pinsLeft[pin].value,"Value:",left_indexPin,"leftIndexSensorPin:"
144-
145-
if pinsLeft[pin].pin==("A2"):
146-
#if pinsLeft[pin].value<=(left_majeure_Psi_min) and pinsLeft[pin].value<(left_majeure_Psi_low):leftMajeurePressure=0
147-
if pinsLeft[pin].value>=(left_majeure_Psi_low)and pinsLeft[pin].value<(left_majeure_Psi_mid):
148-
if leftMajeurePressure==1:
149-
i01_leftHand_majeure.stop()
150-
i01_leftHand_majeure.disable()
151-
print "---Stopping 1 leftMajeureSensorPin"
152-
leftMajeurePressure=0
153-
if pinsLeft[pin].value>=(left_majeure_Psi_mid):
154-
if leftMajeurePressure<=2:
155-
i01_leftHand_majeure.stop()
156-
i01_leftHand_majeure.disable()
157-
print "---Stopping 2 leftMajeureSensorPin"
158-
leftMajeurePressure=0
159-
if pinsLeft[pin].value>=(left_majeure_Psi_max):
160-
if leftMajeurePressure<=3:
161-
i01_leftHand_majeure.stop()
162-
i01_leftHand_majeure.disable()
163-
print "---Stopping 3 leftMajeureSensorPin"
164-
leftMajeurePressure=0
165-
print pinsLeft[pin].value,"Value:",left_majeurePin,"leftMajeureSensorPin:"
166-
167-
if pinsLeft[pin].pin==("A3"):
168-
#if pinsLeft[pin].value<=(left_ringFinger_Psi_min) and pinsLeft[pin].value<(left_ringFinger_Psi_low):leftRingFingerPressure=0
169-
if pinsLeft[pin].value>=(left_ringFinger_Psi_low)and pinsLeft[pin].value<(left_ringFinger_Psi_mid):
170-
if leftRingFingerPressure==1:
171-
i01_leftHand_ringFinger.stop()
172-
i01_leftHand_ringFinger.disable()
173-
print "---Stopping 1 leftRingFingerSensorPin"
174-
leftRingFingerPressure=0
175-
if pinsLeft[pin].value>=(left_ringFinger_Psi_mid):
176-
if leftRingFingerPressure<=2:
177-
i01_leftHand_ringFinger.stop()
178-
i01_leftHand_ringFinger.disable()
179-
print "---Stopping 2 leftRingFingerSensorPin"
180-
leftRingFingerPressure=0
181-
if pinsLeft[pin].value>=(left_ringFinger_Psi_max):
182-
if leftRingFingerPressure<=3:
183-
i01_leftHand_ringFinger.stop()
184-
i01_leftHand_ringFinger.disable()
185-
print "---Stopping 3 leftRingFingerSensorPin"
186-
leftRingFingerPressure=0
187-
print pinsLeft[pin].value,"Value:",left_ringFingerPin,"leftRingFingerSensorPin:"
188-
189-
if pinsLeft[pin].pin==("A4"):
190-
#if pinsLeft[pin].value<=(left_pinky_Psi_min) and pinsLeft[pin].value<(left_pinky_Psi_low):leftPinkyPressure=0
191-
if pinsLeft[pin].value>=(left_pinky_Psi_low)and pinsLeft[pin].value<(left_pinky_Psi_mid):
192-
if leftPinkyPressure==1:
193-
i01_leftHand_pinky.stop()
194-
i01_leftHand_pinky.disable()
195-
print "---Stopping 1 leftPinkySensorPin"
196-
leftPinkyPressure=0
197-
if pinsLeft[pin].value>=(left_pinky_Psi_mid):
198-
if leftPinkyPressure<=2:
199-
i01_leftHand_pinky.stop()
200-
i01_leftHand_pinky.disable()
201-
print "---Stopping 2 leftPinkySensorPin"
202-
leftPinkyPressure=0
203-
if pinsLeft[pin].value>=(left_pinky_Psi_max):
204-
if leftPinkyPressure<=3:
205-
i01_leftHand_pinky.stop()
206-
i01_leftHand_pinky.disable()
207-
print "---Stopping 3 leftPinkySensorPin"
208-
leftPinkyPressure=0
209-
print pinsLeft[pin].value,"Value:",left_pinkyPin,"leftPinkySensorPin:"
210-
print "-----------Left-Finger-Sensors-----------"
75+
76+
for p in pinsLeft:
77+
78+
finger = pin_map.get(p.pin)
79+
80+
if finger is None:
81+
continue
82+
83+
value = p.value
84+
cfg = psi[finger]
85+
86+
if value >= cfg['max']:
87+
if pressure[finger] <= 3:
88+
stopFinger(finger, 3)
89+
pressure[finger] = 0
90+
91+
elif value >= cfg['mid']:
92+
if pressure[finger] <= 2:
93+
stopFinger(finger, 2)
94+
pressure[finger] = 0
95+
96+
elif value >= cfg['low']:
97+
if pressure[finger] == 1:
98+
stopFinger(finger, 1)
99+
pressure[finger] = 0
100+
101+
print("Value:", value, "Pin:", pins[finger], finger)
211102

212103
if runtime.isStarted('i01.chatBot'):
213104
i01_chatBot.getResponse("SYSTEM_EVENT STARTED LEFT HAND SENSOR")
214-
i01_left.addListener("publishPinArray","python","publishPinLeft")
105+
i01_left.addListener("publishPinArray","python","publishPinLeft")
215106

216107
def leftHandSensorON():
217108
if leftHandSensorStarted==1:
218-
print "=========LeftSensorON========"
219-
i01_left.enablePin(left_thumbPin, 2) #2 is the number of polls per seconds
220-
i01_left.enablePin(left_indexPin, 2)
221-
i01_left.enablePin(left_majeurePin, 2)
222-
i01_left.enablePin(left_ringFingerPin, 2)
223-
i01_left.enablePin(left_pinkyPin, 2)
224-
else:
225-
pass
109+
print "=========LeftSensorON========"
110+
111+
for pin in pins.values():
112+
i01_left.enablePin(pin, 1)#2 is the number of polls per seconds
226113

227114
def leftHandSensorOFF():
228-
#sleep(5)
229115
if leftHandSensorStarted==1:
230-
i01_left.disablePin(left_thumbPin)
231-
i01_left.disablePin(left_indexPin)
232-
i01_left.disablePin(left_majeurePin)
233-
i01_left.disablePin(left_ringFingerPin)
234-
i01_left.disablePin(left_pinkyPin)
116+
117+
for pin in pins.values():
118+
i01_left.disablePin(pin)
119+
235120
print "=========LeftSensorOFF======="
236-
else:
237-
pass
238121

239122
except:
240123
i01.error('could not start left hand sensor')
@@ -248,4 +131,4 @@ def leftHandSensorOFF():
248131
if runtime.isStarted('i01.chatBot'):
249132
i01_chatBot.getResponse("ALERT")
250133
i01_chatBot.getResponse("SYSTEM_ERROR_LEFTHANDSENSOR_2")
251-
leftHandSensorStarted = False
134+
leftHandSensorStarted = False

0 commit comments

Comments
 (0)